Доступ к статусу светофора в Webots

Question

Доступ к статусу светофора в Webots

Мне нужно получить доступ к Текущее состояние светофоров в 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-связанные с Field,

Большое спасибо за вашу помощь

0

c++webots

Решение

Другие решения

Других решений пока нет …

Источник

Accepted Answer

Одним из решений, позволяющих получить состояние светофора от супервизора, было бы установить состояние в поле «данные» робота с контроллера.

Тогда вы сможете прочитать его от руководителя. Вот, например, модифицированная версия ‘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;
}

Затем вам нужно изменить PROT GenericTrafficLight, чтобы сделать поле данных видимым, вот модифицированная версия:

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

С уважением,

Дэвид

0