进入Webots中的交通灯状态

Question

我需要访问当前状态的交通灯在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。

非常感谢你的帮助

Answer 1

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

然后你就可以从监狱长那里读出来了。例如，修改后的“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
  }
}

由衷地，

大卫