ROS+Gazebo中红绿黄交通灯如何实现？

交通灯如何实现？"/>

ROS+Gazebo中红绿黄交通灯如何实现？

红灯：

绿灯：

黄灯：

交通灯+车道识别，是最简单的自动驾驶仿真。

参考代码如下：

#include "GazeboTrafficLight.hpp"namespace gazebo {GazeboTrafficLight::GazeboTrafficLight() {sequence_timestamp_ = 0.0;}void GazeboTrafficLight::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {ROS_INFO_STREAM("Loaded traffic light plugin for model: " << model->GetName());std::vector<geometry_msgs::TransformStamped> traffic_light_transforms;for (auto model_link : model->GetLinks()) {if (model_link->GetName().find("light_fixture") != std::string::npos) {geometry_msgs::TransformStamped new_light_transform;new_light_transform.header.frame_id = "world";new_light_transform.child_frame_id = parseLinkName(model_link->GetName());new_light_transform.transform.translation.x = model_link->WorldPose().Pos().X();new_light_transform.transform.translation.y = model_link->WorldPose().Pos().Y();new_light_transform.transform.translation.z = model_link->WorldPose().Pos().Z();new_light_transform.transform.rotation.w = model_link->WorldPose().Rot().W();new_light_transform.transform.rotation.x = model_link->WorldPose().Rot().X();new_light_transform.transform.rotation.y = model_link->WorldPose().Rot().Y();new_light_transform.transform.rotation.z = model_link->WorldPose().Rot().Z();traffic_light_transforms.push_back(new_light_transform);}}if (traffic_light_transforms.size() > 0) {broadcaster_.sendTransform(traffic_light_transforms);}for (auto model_joint : model->GetJoints()) {if (model_joint->GetName().find("switch") == std::string::npos) {continue;}std::string traffic_light_name;std::string joint_name;parseJointName(model_joint->GetName(), traffic_light_name, joint_name);if (joint_name.find("red") != std::string::npos) {traffic_lights_[traffic_light_name][RED] = model_joint;} else if (joint_name.find("yellow") != std::string::npos) {traffic_lights_[traffic_light_name][YELLOW] = model_joint;} else if (joint_name.find("green") != std::string::npos) {traffic_lights_[traffic_light_name][GREEN] = model_joint;}}n_.reset(new ros::NodeHandle(model->GetName()));light_timer_ = n_->createTimer(ros::Duration(0.1), &GazeboTrafficLight::timerCb, this);srv_.reset(new dynamic_reconfigure::Server<gazebo_traffic_light::GazeboTrafficLightConfig>(*n_));srv_->setCallback(boost::bind(&GazeboTrafficLight::reconfig, this, _1, _2));XmlRpc::XmlRpcValue sequence_list;if (n_->getParam("light_sequence", sequence_list)) {for (int i = 0; i < sequence_list.size(); i++) {XmlRpc::XmlRpcValue param_dict_list = sequence_list[i];LightSequenceEntry new_entry;bool parsable = true;if (param_dict_list["color"].getType() != XmlRpc::XmlRpcValue::TypeInvalid) {if (param_dict_list["color"].getType() == XmlRpc::XmlRpcValue::TypeString) {std::string color = param_dict_list["color"];if (color == "green") {new_entry.color = LightColor::GREEN;} else if (color == "yellow") {new_entry.color = LightColor::YELLOW;} else { // rednew_entry.color = LightColor::RED;}} else {parsable = false;ROS_ERROR("Color attribute is not a string!");}}if (param_dict_list["duration"].getType() != XmlRpc::XmlRpcValue::TypeInvalid) {if (param_dict_list["duration"].getType() == XmlRpc::XmlRpcValue::TypeInt) {int temp_val = param_dict_list["duration"];new_entry.duration = (double)temp_val;} else if (param_dict_list["duration"].getType() == XmlRpc::XmlRpcValue::TypeDouble) {new_entry.duration = param_dict_list["duration"];} else {parsable = false;ROS_ERROR("Duration attribute is not a numeric type!");}}if (param_dict_list["flashing"].getType() != XmlRpc::XmlRpcValue::TypeInvalid) {if (param_dict_list["flashing"].getType() == XmlRpc::XmlRpcValue::TypeBoolean) {new_entry.flashing = param_dict_list["flashing"];} else {parsable = false;ROS_ERROR("Flashing attribute is not a boolean type!");}}if (parsable) {light_sequence_.push_back(new_entry);}}} else {ROS_INFO_STREAM("light_sequence parameter not found for model " << model->GetName() << "... Using default");light_sequence_.push_back(LightSequenceEntry(LightColor::GREEN, 4.0, false));light_sequence_.push_back(LightSequenceEntry(LightColor::YELLOW, 2.0, false));light_sequence_.push_back(LightSequenceEntry(LightColor::RED, 6.0, false));}computeCycleTime();}void GazeboTrafficLight::parseJointName(const std::string& input_str, std::string& traffic_light_name, std::string& joint_name) {std::string s = input_str;size_t pos = s.rfind("::");if (pos == std::string::npos) {traffic_light_name = "gazebo_traffic_light";joint_name = s;} else {traffic_light_name = s.substr(0, pos);joint_name = s.substr(pos + 2);}}std::string GazeboTrafficLight::parseLinkName(const std::string& input_str) {std::string output;std::string s = input_str;int count = std::count(input_str.begin(), input_str.end(), ':') / 2;if (count == 0) {output = s;} else {int limit = std::max(1, count - 1);for (int i = 0; i < limit; i++) {size_t pos = s.find("::");output += (s.substr(0, pos) + "_");s = s.substr(pos + 2);}output = output.substr(0, output.length() - 1); // Remove trailing underscore}return output;}void GazeboTrafficLight::computeCycleTime() {cycle_time_ = 0.0;for (auto& stage : light_sequence_) {cycle_time_ += stage.duration;}}void GazeboTrafficLight::timerCb(const ros::TimerEvent& event) {for (auto light : traffic_lights_) {switch (cfg_.override) {case gazebo_traffic_light::GazeboTrafficLight_NO_FORCE:advanceSequence(light.first);break;case gazebo_traffic_light::GazeboTrafficLight_RED:setColor(light.first, LightColor::RED, false);break;case gazebo_traffic_light::GazeboTrafficLight_RED_FLASH:setColor(light.first, LightColor::RED, true);break;case gazebo_traffic_light::GazeboTrafficLight_YELLOW:setColor(light.first, LightColor::YELLOW, false);break;case gazebo_traffic_light::GazeboTrafficLight_YELLOW_FLASH:setColor(light.first, LightColor::YELLOW, true);break;case gazebo_traffic_light::GazeboTrafficLight_GREEN:setColor(light.first, LightColor::GREEN, false);break;case gazebo_traffic_light::GazeboTrafficLight_GREEN_FLASH:setColor(light.first, LightColor::GREEN, true);break;}}if (event.last_expected == ros::Time(0)) {return;}sequence_timestamp_ += (event.current_real - event.last_real).toSec();if (sequence_timestamp_ >= cycle_time_) {sequence_timestamp_ -= cycle_time_;}}void GazeboTrafficLight::advanceSequence(const std::string& traffic_light_name) {double time_thres = cycle_time_;for (auto it = light_sequence_.rbegin(); it != light_sequence_.rend(); ++it) {time_thres -= it->duration;if (sequence_timestamp_ >= time_thres) {setColor(traffic_light_name, it->color, it->flashing);break;}}}void GazeboTrafficLight::setColor(const std::string& traffic_light_name, LightColor color, bool flashing) {for (int i = LightColor::RED; i <= LightColor::GREEN; i++) {if ( (flashing && ( (int)(1.25 * sequence_timestamp_) % 2 )) || (i != color) ){traffic_lights_[traffic_light_name][i]->SetPosition(0, LIGHT_OFF_POS_);} else {traffic_lights_[traffic_light_name][i]->SetPosition(0, LIGHT_ON_POS_);}}}void GazeboTrafficLight::reconfig(gazebo_traffic_light::GazeboTrafficLightConfig& config, uint32_t level) {sequence_timestamp_ = 0.0;cfg_ = config;}void GazeboTrafficLight::Reset() {}GazeboTrafficLight::~GazeboTrafficLight() {n_->shutdown();}} // namespace gazebo

如何实现红黄绿等使用主题控制呢？

#! /usr/bin/env pythonPACKAGE='gazebo_traffic_light'from dynamic_reconfigure.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *gen = ParameterGenerator()force_enum = gen.enum( [gen.const("NO_FORCE",     int_t, -1, ""),gen.const("RED",          int_t, 0, ""),gen.const("RED_FLASH",    int_t, 1, ""),gen.const("YELLOW",       int_t, 2, ""),gen.const("YELLOW_FLASH", int_t, 3, ""),gen.const("GREEN",        int_t, 4, ""),gen.const("GREEN_FLASH",  int_t, 5, ""),], "Bypass programmed sequence and force light state")#       Name        Type   Level     Description                             Default   Min  Max
gen.add("override", int_t, 0,        "Force light into a certain state",     -1,       -1,  5, edit_method=force_enum)exit(gen.generate(PACKAGE, PACKAGE, "GazeboTrafficLight"))

如下图所示，如何设置：

/gazebo/auto_disable_bodies
/gazebo/cfm
/gazebo/contact_max_correcting_vel
/gazebo/contact_surface_layer
/gazebo/enable_ros_network
/gazebo/erp
/gazebo/gravity_x
/gazebo/gravity_y
/gazebo/gravity_z
/gazebo/max_contacts
/gazebo/max_update_rate
/gazebo/sor_pgs_iters
/gazebo/sor_pgs_precon_iters
/gazebo/sor_pgs_rms_error_tol
/gazebo/sor_pgs_w
/gazebo/time_step
/gazebo_traffic_light/override
/rosdistro/gazebo/auto_disable_bodies
/gazebo/cfm
/gazebo/contact_max_correcting_vel
/gazebo/contact_surface_layer
/gazebo/enable_ros_network
/gazebo/erp
/gazebo/gravity_x
/gazebo/gravity_y
/gazebo/gravity_z
/gazebo/max_contacts
/gazebo/max_update_rate
/gazebo/sor_pgs_iters
/gazebo/sor_pgs_precon_iters
/gazebo/sor_pgs_rms_error_tol
/gazebo/sor_pgs_w
/gazebo/time_step
/gazebo_traffic_light/override
/rosdistro
/roslaunch/uris/host_ros__34939
/rosversion
/run_id
/test_cantilevered_traffic_light/light_sequence
/test_cantilevered_traffic_light/override
/test_included_traffic_light/override
/use_sim_time