0

我需要访问Webots中红绿灯的当前状态。

检查通用交通灯的“generic_traffic_light.c”控制器，我们有：

WbDeviceTag red_light;
red_light = wb_robot_get_device("ref light");
...
wb_led_set(red_light, 1);

假设我使用了几个通用红绿灯，是否可以访问每个红绿灯状态？

我可以通过以下方式获取通用交通信号灯的节点：

Node *traffic_node = supervisor->getFromDef("traffic_light_1");

但Node没有任何Device方法或任何LED-related Field。

非常感谢你的帮助

4

1 回答 1

0

能够从主管获取交通灯状态的一种解决方案是从控制器在机器人的“数据”字段中设置状态。

然后您将能够从主管那里读取它。例如，这里是“generic_traffic_light.c”的修改版本（我刚刚添加了一些对“wb_robot_set_data”的调用）：

#include <webots/robot.h>
#include <webots/led.h>

#include <stdio.h>
#include <string.h>

#define TIME_STEP 512
enum {GREEN_STATE, RED_STATE, ORANGE_STATE_TO_RED, ORANGE_STATE_TO_GREEN};

int main(int argc, char **argv) {
  wb_robot_init();
  double red_time = 20.0;
  double green_time = 20.0;
  double orange_time = 1.5;
  int current_state = GREEN_STATE;

  if (argc > 1) {
    sscanf(argv[1],"%lf",&red_time);
    if (argc > 2) {
      sscanf(argv[2],"%lf",&green_time);
      if (argc > 3) {
        if (strcmp(argv[3], "r") == 0)
          current_state = RED_STATE;
        else if (strcmp(argv[3], "g") == 0)
          current_state = GREEN_STATE;
        else if (strcmp(argv[3], "og") == 0)
          current_state = ORANGE_STATE_TO_GREEN;
        else if (strcmp(argv[3], "or") == 0)
          current_state = ORANGE_STATE_TO_RED;
      }
    }
    else
      green_time = red_time;
  }

  WbDeviceTag red_light, orange_light, green_light;
  red_light = wb_robot_get_device("red light");
  orange_light = wb_robot_get_device("orange light");
  green_light = wb_robot_get_device("green light");
  double last_phase_change_time = 0.0;

  if (current_state == GREEN_STATE) {
    wb_led_set(green_light, 1);
    wb_robot_set_data("green");
  } else if (current_state == RED_STATE) {
    wb_led_set(red_light, 1);
    wb_robot_set_data("red");
  } else {
    wb_led_set(orange_light, 1);
    wb_robot_set_data("orange");
  }

  while (wb_robot_step(TIME_STEP) != -1) {
    double current_time =  wb_robot_get_time();

    if (current_state == GREEN_STATE) {
      if ((current_time - last_phase_change_time) >=  green_time) {
        current_state = ORANGE_STATE_TO_RED;
        last_phase_change_time = current_time;
        wb_led_set(green_light, 0);
        wb_led_set(orange_light, 1);
        wb_robot_set_data("orange");
      }
    } else if (current_state == RED_STATE) {
      if ((current_time - last_phase_change_time) >=  red_time) {
        current_state = ORANGE_STATE_TO_GREEN;
        last_phase_change_time = current_time;
        wb_led_set(red_light, 0);
        wb_led_set(orange_light, 1);
        wb_robot_set_data("orange");
      }
    } else if (current_state == ORANGE_STATE_TO_RED) {
      if ((current_time - last_phase_change_time) >=  orange_time) {
        current_state = RED_STATE;
        last_phase_change_time = current_time;
        wb_led_set(orange_light, 0);
        wb_led_set(red_light, 1);
        wb_robot_set_data("red");
      }
    } else { //current_state == ORANGE_STATE_TO_GREEN
      if ((current_time - last_phase_change_time) >=  orange_time) {
        current_state = GREEN_STATE;
        last_phase_change_time = current_time;
        wb_led_set(orange_light, 0);
        wb_led_set(green_light, 1);
        wb_robot_set_data("green");
      }
    }
  };

  wb_robot_cleanup();

  return 0;
}

然后您需要更改 'GenericTrafficLight' PROTO 以使 'data' 字段可见，这是一个修改后的版本：

PROTO GenericTrafficLight [
  field SFVec3f    translation 0 0 0
  field SFRotation rotation    0 1 0 0
  field SFBool     startGreen  TRUE
  field SFFloat    greenTime   60
  field SFFloat    redTime     15
  field SFString   state       "off"
]
{
%{
  local greenTime = fields.greenTime.value
  if greenTime <= 0.0 then
    greenTime = fields.greenTime.defaultValue
    io.stderr:write("'greenTime' should be strictly positive.\n")
    io.stderr:write("'greenTime' was reset to '" .. greenTime .. "'.\n")
  end
  local redTime = fields.redTime.value
  if redTime <= 0.0 then
    redTime = fields.redTime.defaultValue
    io.stderr:write("'redTime' should be strictly positive.\n")
    io.stderr:write("'redTime' was reset to '" .. redTime .. "'.\n")
  end
  local controllerArgs = '"' .. redTime .. ' ' .. greenTime .. ' '
  if fields.startGreen.value then
    controllerArgs = controllerArgs .. 'g'
  else
    controllerArgs = controllerArgs .. 'r'
  end
  controllerArgs = controllerArgs .. '"'
}%
  Robot {
    translation IS translation
    rotation IS rotation
    children [
      Pole {
        slot [
          TrafficLight {
            lamp_geometry TrafficLightStandardLampGeometry {
            }
          }
        ]
      }
    ]
    controller "generic_traffic_light"
    controllerArgs %{= controllerArgs }%
    data IS state
  }
}

真挚地，

大卫

于 2017-08-07T09:21:22.580 回答