Access to Traffic Light status in Webots

Question

I need to get access to the current status of traffic lights in Webots.

Checking the "generic_traffic_light.c" controller for a Generic traffic light, we have:

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

Assuming I use several generic traffic lights, is it possible to have access to each traffic light status?

I can get the nodes of my generic traffic lights by:

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

but Node doesn't have any Device method or any LED-related Field.

Thanks a lot for your help

score 0 · Answer 1 · answered Aug 07 '17 at 09:21

One solution to be able to get the traffic light state from the Supervisor would be to set the state in the 'data' field of the Robot from the controller.

Then you will be able to read it from the Supervisor. Here is for example the modified version of 'generic_traffic_light.c' (I just added a few call to '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;
}

Then you need to change the 'GenericTrafficLight' PROTO in order to render the 'data' field visible, here is a modified version:

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
  }
}

Sincerely,

David

Access to Traffic Light status in Webots

1 Answers1