红灯：
绿灯：
黄灯：
交通灯 车道识别，是最简单的自动驾驶仿真。
参考代码如下：
代码语言：javascript复制#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 { // red
new_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如何实现红黄绿等使用主题控制呢？
代码语言：javascript复制#! /usr/bin/env python
PACKAGE='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
