#include <ros/ros.h>
#include <ros/console.h>
#include <visualization_msgs/MarkerArray.h>
#include <std_msgs/Int32.h>
#include <tf/transform_datatypes.h>
#include <iostream>
#include <vector>
#include <string>
#include "libwaypoint_follower/libwaypoint_follower.h"
#include "autoware_msgs/LaneArray.h"
#include "autoware_config_msgs/ConfigLaneStop.h"
#include "autoware_msgs/TrafficLight.h"
namespace
{
ros::Publisher g_local_mark_pub;
ros::Publisher g_global_mark_pub;
constexpr int32_t TRAFFIC_LIGHT_RED = 0; //红灯设置为0
constexpr int32_t TRAFFIC_LIGHT_GREEN = 1; //绿灯设置为1
constexpr int32_t TRAFFIC_LIGHT_UNKNOWN = 2; //其余状况设置为2
std_msgs::ColorRGBA _initial_color;
std_msgs::ColorRGBA _global_color;
std_msgs::ColorRGBA g_local_color;
const double g_global_alpha = 0.2;
const double g_local_alpha = 1.0;
int _closest_waypoint = -1;
visualization_msgs::MarkerArray g_global_marker_array;
visualization_msgs::MarkerArray g_local_waypoints_marker_array;
bool g_config_manual_detection = true;
enum class ChangeFlag : int32_t//十字路口三种行驶行为。
{
straight,
right,
left,
unknown = -1,
};
typedef std::underlying_type<ChangeFlag>::type ChangeFlagInteger;
void setLifetime(double sec, visualization_msgs::MarkerArray* marker_array)
{//设置传入的visualization_msgs:MarkerArray* marker_array的生存时间,生存时间表示在自动删除前维持多久,0表示永久。
ros::Duration lifetime(sec);
for (auto& marker : marker_array->markers)
{
marker.lifetime = lifetime;
}
}
void publishMarkerArray(const visualization_msgs::MarkerArray& marker_array, const ros::Publisher& publisher, bool delete_markers=false)
{/*
根据delete_markers选择在rviz中创建/删除marker_array 中的形状,publisher发布 marker_array.
*/
visualization_msgs::MarkerArray msg;
// insert local marker 插入标记
msg.markers.insert(msg.markers.end(), marker_array.markers.begin(), marker_array.markers.end());
if (delete_markers)
{
for (auto& marker : msg.markers)
{
/*visualization _msgs : :Marker中的成员变量action表示对标记的操作,是添加,修改还是删除*/
marker.action = visualization_msgs::Marker::DELETE;
}
}
publisher.publish(msg);
}
void createGlobalLaneArrayVelocityMarker(const autoware_msgs::LaneArray& lane_waypoints_array)
{/*
更新全局变量g _global_marker_array里的标记g_global_marker_array.markers,
其用于标记lane_waypoints_array.lanes中所有lane里的所有轨迹点的速度信息。
*/
visualization_msgs::MarkerArray tmp_marker_array;
visualization_msgs::Marker velocity_marker;// display by markers the velocity of each waypoint.通过标记显示每个轨迹点的速度
velocity_marker.header.frame_id = "map"; //设置帧工D和时间戳
velocity_marker.header.stamp = ros::Time::now();
velocity_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;//指定标记形状,TEXT_VIEw_FACING代表显示3D的文字
velocity_marker.action = visualization_msgs::Marker::ADD;//指定对marker的操作
//marker的颜色:类似std_msgs/colorRGBA
//每个数字介于0-1之间。a(alpha)表示透明度,0表示完全透明
velocity_marker.scale.z = 0.4;
velocity_marker.color.r = 1;
velocity_marker.color.g = 1;
velocity_marker.color.b = 1;
velocity_marker.color.a = 1.0;
velocity_marker.frame_locked = true;
int count = 1;
for (auto lane : lane_waypoints_array.lanes)
{
velocity_marker.ns = "global_velocity_lane_" + std::to_string(count);
for (int i = 0; i < static_cast<int>(lane.waypoints.size()); i++)
{
velocity_marker.id = i;
//命名空间(ns)和id为此marker创建一个唯一标识.
//如果rviz接收到相同命名空间和 id 的marker,新的将替换掉旧的.
geometry_msgs::Point relative_p;
relative_p.y = 0.5;
velocity_marker.pose.position = calcAbsoluteCoordinate(relative_p, lane.waypoints[i].pose.pose);
/*calcRelativeCoordinate函数的作用是计算全局坐标系中某点相对于无人车坐标系中的位置。
此处calcAbsoluteCoordinate函数的作用是计算current_pose坐标系上的点在世界坐标系下的绝对坐标。*/
velocity_marker.pose.position.z += 0.2;
// double to string
std::string vel = std::to_string(mps2kmph(lane.waypoints[i].twist.twist.linear.x));
velocity_marker.text = vel.erase(vel.find_first_of(".") + 2);//erase()函数的作用是删除下标vel.find_first_of(". ") +2开始及以后的字符
//这两行实现了速度单位从m/s到km/h 的换算,并且转换为字符串后保留小数点后一位。
tmp_marker_array.markers.push_back(velocity_marker);
}
count++;
}
g_global_marker_array.markers.insert(g_global_marker_array.markers.end(), tmp_marker_array.markers.begin(),
tmp_marker_array.markers.end());
}
void createGlobalLaneArrayChangeFlagMarker(const autoware_msgs::LaneArray& lane_waypoints_array)
{
visualization_msgs::MarkerArray tmp_marker_array;
// display by markers the velocity of each waypoint.
visualization_msgs::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time::now();
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.z = 0.4;
marker.color.r = 1;
marker.color.g = 1;
marker.color.b = 1;
marker.color.a = 1.0;
marker.frame_locked = true;
int count = 1;
for (auto lane : lane_waypoints_array.lanes)
{
marker.ns = "global_change_flag_lane_" + std::to_string(count);
for (int i = 0; i < static_cast<int>(lane.waypoints.size()); i++)
{
marker.id = i;
geometry_msgs::Point relative_p;
relative_p.x = -0.1;
marker.pose.position = calcAbsoluteCoordinate(relative_p, lane.waypoints[i].pose.pose);
marker.pose.position.z += 0.2;
// double to string
std::string str = "";
if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::straight))
{
str = "S";//直走 显示S 3D字母
}
else if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::right))
{
str = "R";//右转 显示R 3D字母
}
else if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::left))
{
str = "L";//左转 显示L 3D字母
}
else if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::unknown))
{
str = "U";
}
marker.text = str;
tmp_marker_array.markers.push_back(marker);
}
count++;
}
g_global_marker_array.markers.insert(g_global_marker_array.markers.end(), tmp_marker_array.markers.begin(),
tmp_marker_array.markers.end());
/*
#include<iostream>
using namespace std;
int main()
{
string str="hello";
string s="Hahah";
str.insert(1,s);//在原串下标为1的字符e前插入字符串s,结果为:hHahahello
cout<<str<<endl;
string str1="hello";
char c='w';
str1.insert(4,5,c);//在原串下标为4的字符o前插入5个字符c,结果为:hellwwwwwo
cout<<str1<<endl;
string str2="hello";
string s2="weakhaha";
str2.insert(0,s2,1,3);//将字符串s2从下标为1的e开始数3个字符,分别是eak,插入原串的下标为0的字符h前
cout<<str2<<endl; 结果为:eakhello
return 0;
}
*/
}
void createLocalWaypointVelocityMarker(std_msgs::ColorRGBA color, int closest_waypoint,
const autoware_msgs::Lane& lane_waypoint)
{//createLocalWaypointVelocityMarker函数(源码略)的功能跟createGlobalLane-ArrayVelocityMarker函数是差不多的,
//值得注意的是标记的颜色是根据传入的 std_msg::ColorRGBA color设置的,例如,如果前面路口是红灯,则标记也对应变为红色。
// display by markers the velocity of each waypoint.
visualization_msgs::Marker velocity;
velocity.header.frame_id = "map";
velocity.header.stamp = ros::Time::now();//时间戳
velocity.ns = "local_waypoint_velocity";
velocity.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
velocity.action = visualization_msgs::Marker::ADD;
velocity.scale.z = 0.4;
velocity.color = color;
velocity.frame_locked = true;
for (int i = 0; i < static_cast<int>(lane_waypoint.waypoints.size()); i++)
{
velocity.id = i;
geometry_msgs::Point relative_p;
relative_p.y = -0.65;
velocity.pose.position = calcAbsoluteCoordinate(relative_p, lane_waypoint.waypoints[i].pose.pose);
velocity.pose.position.z += 0.2;
// double to string
std::ostringstream oss;
oss << std::fixed << std::setprecision(1) << mps2kmph(lane_waypoint.waypoints[i].twist.twist.linear.x);
velocity.text = oss.str();
g_local_waypoints_marker_array.markers.push_back(velocity);
}
}
void createGlobalLaneArrayMarker(std_msgs::ColorRGBA color, const autoware_msgs::LaneArray& lane_waypoints_array)
{//createGlobalLaneArrayChangeFlagMarker函数的作用是 标记类型也是 TEXT_VIEW_FACING(3D的文字),用以显示 change_flag。
visualization_msgs::Marker lane_waypoint_marker;
lane_waypoint_marker.header.frame_id = "map";
lane_waypoint_marker.header.stamp = ros::Time::now();
lane_waypoint_marker.ns = "global_lane_array_marker";
lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
lane_waypoint_marker.scale.x = 1.0;
lane_waypoint_marker.color = color;
lane_waypoint_marker.frame_locked = true;
int count = 0;
for (auto lane : lane_waypoints_array.lanes)
{
lane_waypoint_marker.points.clear();
lane_waypoint_marker.id = count;
for (auto el : lane.waypoints)
{
geometry_msgs::Point point;
point = el.pose.pose.position;
lane_waypoint_marker.points.push_back(point);
}
g_global_marker_array.markers.push_back(lane_waypoint_marker);
count++;
}
}
void createGlobalLaneArrayOrientationMarker(const autoware_msgs::LaneArray& lane_waypoints_array)
{
//createGlobalLaneArrayOrientationMarker函数,这里的标记类型是 visualization_msgs::Marker:ARROW(箭头),用以指示车辆速度方向。
visualization_msgs::MarkerArray tmp_marker_array;
visualization_msgs::Marker lane_waypoint_marker;
lane_waypoint_marker.header.frame_id = "map";
lane_waypoint_marker.header.stamp = ros::Time::now();
lane_waypoint_marker.type = visualization_msgs::Marker::ARROW;
lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
lane_waypoint_marker.scale.x = 0.25;
lane_waypoint_marker.scale.y = 0.05;
lane_waypoint_marker.scale.z = 0.05;
lane_waypoint_marker.color.r = 1.0;
lane_waypoint_marker.color.a = 1.0;
lane_waypoint_marker.frame_locked = true;
int count = 1;
for (auto lane : lane_waypoints_array.lanes)
{
lane_waypoint_marker.ns = "global_lane_waypoint_orientation_marker_" + std::to_string(count);
for (int i = 0; i < static_cast<int>(lane.waypoints.size()); i++)
{
lane_waypoint_marker.id = i;
lane_waypoint_marker.pose = lane.waypoints[i].pose.pose;
tmp_marker_array.markers.push_back(lane_waypoint_marker);
}
count++;
}
g_global_marker_array.markers.insert(g_global_marker_array.markers.end(), tmp_marker_array.markers.begin(),
tmp_marker_array.markers.end());
}
void createLocalPathMarker(std_msgs::ColorRGBA color, const autoware_msgs::Lane& lane_waypoint)
{//createLocalPathMarker函数以线条(点的连线)形状标记出轨迹,线条穿过各个轨迹点。
visualization_msgs::Marker lane_waypoint_marker;
lane_waypoint_marker.header.frame_id = "map";
lane_waypoint_marker.header.stamp = ros::Time::now();
lane_waypoint_marker.ns = "local_path_marker";
lane_waypoint_marker.id = 0;
lane_waypoint_marker.type = visualization_msgs::Marker::LINE_STRIP;
lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
lane_waypoint_marker.scale.x = 0.2;
lane_waypoint_marker.color = color;
lane_waypoint_marker.frame_locked = true;
for (unsigned int i = 0; i < lane_waypoint.waypoints.size(); i++)
{
geometry_msgs::Point point;
point = lane_waypoint.waypoints[i].pose.pose.position;
lane_waypoint_marker.points.push_back(point);
}
g_local_waypoints_marker_array.markers.push_back(lane_waypoint_marker);
}
void createLocalPointMarker(const autoware_msgs::Lane& lane_waypoint)
{//createLocalPointMarker函数以立方体的形状标记出轨迹点。
visualization_msgs::Marker lane_waypoint_marker;
lane_waypoint_marker.header.frame_id = "map";
lane_waypoint_marker.header.stamp = ros::Time::now();
lane_waypoint_marker.ns = "local_point_marker";
lane_waypoint_marker.id = 0;
lane_waypoint_marker.type = visualization_msgs::Marker::CUBE_LIST;
lane_waypoint_marker.action = visualization_msgs::Marker::ADD;
lane_waypoint_marker.scale.x = 0.2;
lane_waypoint_marker.scale.y = 0.2;
lane_waypoint_marker.scale.z = 0.2;
lane_waypoint_marker.color.r = 1.0;
lane_waypoint_marker.color.a = 1.0;
lane_waypoint_marker.frame_locked = true;
for (unsigned int i = 0; i < lane_waypoint.waypoints.size(); i++)
{
geometry_msgs::Point point;
point = lane_waypoint.waypoints[i].pose.pose.position;
lane_waypoint_marker.points.push_back(point);
}
g_local_waypoints_marker_array.markers.push_back(lane_waypoint_marker);
}
void lightCallback(const autoware_msgs::TrafficLightConstPtr& msg)
{
std_msgs::ColorRGBA global_color; //ROS自带消息格式,用于传递RGBA数据
global_color.a = g_global_alpha;
/*
RGBA模式与RGB相同,仅在 RGB模式上新增了Alpha透明度
R:红色值。正整数|百分数
G:绿色值。正整数|百分数
B:蓝色值。正整数|百分数
A: Alpha透明度,取值0~1之间
*/
std_msgs::ColorRGBA local_color;
local_color.a = g_local_alpha;
switch (msg->traffic_light)//根据灯色设置global_color和 local_color
{
case TRAFFIC_LIGHT_RED:
global_color.r = 1.0;
_global_color = global_color;
local_color.r = 1.0;
g_local_color = local_color;
break;
case TRAFFIC_LIGHT_GREEN:
global_color.g = 1.0;
_global_color = global_color;
local_color.g = 1.0;
g_local_color = local_color;
break;
case TRAFFIC_LIGHT_UNKNOWN:
global_color.b = 1.0;
global_color.g = 0.7;
_global_color = global_color;
local_color.b = 1.0;
local_color.g = 0.7;
g_local_color = local_color;
break;
default:
ROS_ERROR("unknown traffic_light");
return;
}
}
//
void receiveAutoDetection(const autoware_msgs::TrafficLightConstPtr& msg)
{
if (!g_config_manual_detection)
lightCallback(msg);
}
void receiveManualDetection(const autoware_msgs::TrafficLightConstPtr& msg)
{
if (g_config_manual_detection)
lightCallback(msg);
}
/*
receiveAutoDetection函数是话题为"light_color"的回调函数
receiveManualDetection函数是话题为"light_color_managed"的回调函数,
结合函数名可知它们是跟交通信号灯灯色识别可视化相关的函数。receiveAutoDetection函数和
receiveManualDetection函数内部都调用了lightCallback函数。区别是根据设置的
g_config_manual_detection保证只有一个函数调用lightCallback函数。
*/
void configParameter(const autoware_config_msgs::ConfigLaneStopConstPtr& msg)
{//configParameter 函数更新g_config_manual_detection
g_config_manual_detection = msg->manual_detection;
}
void laneArrayCallback(const autoware_msgs::LaneArrayConstPtr& msg)
{
/*
laneArrayCallback函数跟 lightCallback函数一样,也是被两个话题所调用,
它对应于两个话题“lane_waypoints_array”和“traffic_waypoints_array"。
*/
publishMarkerArray(g_global_marker_array, g_global_mark_pub, true);//首先删除rviz中上一次发布的可视化标记
g_global_marker_array.markers.clear();
createGlobalLaneArrayVelocityMarker(*msg);
createGlobalLaneArrayOrientationMarker(*msg);
createGlobalLaneArrayChangeFlagMarker(*msg);
publishMarkerArray(g_global_marker_array, g_global_mark_pub);
}
void finalCallback(const autoware_msgs::LaneConstPtr& msg)
{//finalCallback函数是对应于话题"final_waypoints"的回调函数。
g_local_waypoints_marker_array.markers.clear();
if (_closest_waypoint != -1)//默认值为-1,在对应于话题"closest_waypoint"的回调函数closestcallback中被更新
createLocalWaypointVelocityMarker(g_local_color, _closest_waypoint, *msg);
/*g_local_color在 lightcallback函数中被更新而lightcallback函数被话题为
"light_color"和"light_color_managed"的回调函数receiveAutoDetection函数和receiveManualDetection函数分别调用*/
createLocalPathMarker(g_local_color, *msg);
createLocalPointMarker(*msg);
setLifetime(0.5, &g_local_waypoints_marker_array);
publishMarkerArray(g_local_waypoints_marker_array, g_local_mark_pub);
}
void closestCallback(const std_msgs::Int32ConstPtr& msg)
{//closestCallback configParameter 函数分别更新_closest_waypoint和g_config_manual_detection。
_closest_waypoint = msg->data;
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "waypoints_marker_publisher");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
// subscribe traffic light 订阅信号灯消息
ros::Subscriber light_sub = nh.subscribe("light_color", 10, receiveAutoDetection);
ros::Subscriber light_managed_sub = nh.subscribe("light_color_managed", 10, receiveManualDetection);
// subscribe global waypoints 订阅全局轨迹点消息
ros::Subscriber lane_array_sub = nh.subscribe("lane_waypoints_array", 10, laneArrayCallback);
ros::Subscriber traffic_array_sub = nh.subscribe("traffic_waypoints_array", 10, laneArrayCallback);
// subscribe local waypoints 订阅本地轨迹点消息
ros::Subscriber final_sub = nh.subscribe("final_waypoints", 10, finalCallback);
ros::Subscriber closest_sub = nh.subscribe("closest_waypoint", 10, closestCallback);
// subscribe config
ros::Subscriber config_sub = nh.subscribe("config/lane_stop", 10, configParameter);
g_local_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("local_waypoints_mark", 10, true);
g_global_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("global_waypoints_mark", 10, true);
// initialize path color
_initial_color.g = 0.7;
_initial_color.b = 1.0;
_global_color = _initial_color;
_global_color.a = g_global_alpha;
g_local_color = _initial_color;
g_local_color.a = g_local_alpha;
ros::spin();
}
/*
在rviz中标记无人车的轨迹需要用到类visualization_msgs::Marker,其重要成员如下。
1.uint8 ARRQW=0 //箭头
2.uint8 cUBE=1 //立方体
3.uint8 SPHERE=2 //球
4.uint8 CYLINDER=3 //圆柱体
5.uint8 LINE_STRIP=4 //线条(点的连线)
6.uint8 LINE_LIST=5 //线条序列
7.uint8 CUBE_LIST=6 //立方体序列
8.uint8 SPHERE_LIST=7 //球序列
9.uint8 POINTS=8 //点集
10.uint8 TEXT_VIEw_FACING=9 //3D文字
11.uint8 MESH_RESOURCE=10 //网格
12.uint8 TRIANGLE_LIST=11 //三角形序列
对标记的操作,为枚举值
1. uint8 ADD=0
2. uint8 MODIFY=0
3. uint8 DELETE=2
4. uint8 DELETEALL=3
重要成员变量
Header header
string ns //命名空间namespace
int32 id
注:(id与命名空间联合起来,形成唯一的标识,可以区分各个标志物,
使得程序可以对指定的标志物进行操作,如果再次执行相同id,会删除上一次的标记.)
int32 type //类型,指形状
int32 action //操作,是添加、修改还是删除
geometry_msgs/Pose pose //位姿
geometry_msgs/vector3 scalel //尺寸,默认1
std_msgs/ColorRGBA colorl //颜色[0.0-1.0]
duration lifetime //自动删除前的维持时间,0表示永久
bool frame_locked //用于锁定帧,即每个时间步重新转教
geometry_msgs/Point[ ] points //用于指明序列中每个点的位置
std_msgs/colorRGBA [ ] colors
string text //只用于文本标识
string mesh_resource
bool mesh_use_embedded_materials //仅用于MESH_RESOURCE标记
*/
参考书目《Autoware与自动驾驶技术》
注:可以下载Visual Studio Coded代码编辑软件,展示性好点。