Autoware planning模块学习笔记（二）：路径规划（3）- 节点waypoint_marker_publisher
Autoware planning模块学习笔记（二）：路径规划（3）- 节点waypoint_marker_publisher
前面两篇博客Autoware planning模块学习笔记（二）：路径规划（1）和Autoware planning模块学习笔记（二）：路径规划（2）分别分析了执行“勾选waypoint_loader”这一操作时所启动的waypoint_loader节点和waypoint_replanner节点，这篇博客继续分析剩下的waypoint_marker_publisher节点。
节点waypoint_marker_publisher的源文件是nodes/waypoint_marker_publisher/waypoint_marker_publisher.cpp
老规矩，先找到main函数。main函数在waypoint_marker_publisher.cpp中，main函数中先订阅了一批Topic，接着设置了两个发布者。接着咱们就详细分析下这些回调函数的作用。（开场词基本定型了哈哈～）
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();
}
1. receiveAutoDetection函数和receiveManualDetection函数
receiveAutoDetection函数和receiveManualDetection函数分别是对应topic为"light_color"和"light_color_managed"的回调函数，结合函数名我们猜测它们是跟交通信号灯灯色识别可视化相关的函数。receiveAutoDetection函数和receiveManualDetection函数内部都调用了lightCallback函数。区别是根据设置的g_config_manual_detection保证只有一个函数调用lightCallback函数。g_config_manual_detection默认设置是true，但在topic "config/lane_stop" 对应的回调函数configParameter中会被重新设置。
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);
}
lightCallback函数
void lightCallback(const autoware_msgs::TrafficLightConstPtr& msg)
{
std_msgs::ColorRGBA global_color;//ROS自带消息格式，用于传递RGBA数据。
//RGBA色彩模式与RGB相同，（防盗标记：zhengkunxian）只是在RGB模式上新增了Alpha透明度。
//R：红色值。正整数 | 百分数；G：绿色值。正整数 | 百分数；B：蓝色值。正整数 | 百分数；A：Alpha透明度,取值0~1之间
global_color.a = g_global_alpha;//const double g_global_alpha = 0.2;
std_msgs::ColorRGBA local_color;
local_color.a = g_local_alpha;//const double g_local_alpha = 1.0;
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;
}
}
2. laneArrayCallback函数
laneArrayCallback函数跟lightCallback函数一样，也是被两个topic所调用，此处它对应于两个topic “lane_waypoints_array”和“traffic_waypoints_array”。
void laneArrayCallback(const autoware_msgs::LaneArrayConstPtr& msg)
{
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);
}
2.1 publishMarkerArray函数
根据delete_markers选择在rviz中创建/删除marker_array中的形状，publisher发布marker_array。
void publishMarkerArray(const visualization_msgs::MarkerArray& marker_array, const ros::Publisher& publisher, bool delete_markers=false)
{
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)
{
marker.action = visualization_msgs::Marker::DELETE;//这个是用来指定我们要对marker做什么。
//visualization_msgs::Marker中的成员变量action表示对标记的操作，是添加，修改还是删除：
//uint8 ADD=0
//uint8 MODIFY=0
//uint8 DELETE=2
//uint8 DELETEALL=3
}
}
publisher.publish(msg);
}
2.2 createGlobalLaneArrayVelocityMarker函数
更新全局变量g_global_marker_array里的标记g_global_marker_array.markers，其用于标记lane_waypoints_array.lanes中所有lane里的所有轨迹点的速度信息。
void createGlobalLaneArrayVelocityMarker(const autoware_msgs::LaneArray& lane_waypoints_array)
{
visualization_msgs::MarkerArray tmp_marker_array;
// display by markers the velocity of each waypoint.
visualization_msgs::Marker velocity_marker;
//设置帧ID和时间戳
velocity_marker.header.frame_id = "map";
velocity_marker.header.stamp = ros::Time::now();
//这个就是指定我们会发送哪种形状过去。TEXT_VIEW_FACING代表显示3D的文字。
velocity_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
//这个是用来指定我们要对marker做什么的。
velocity_marker.action = visualization_msgs::Marker::ADD;
//指定了标记的规模，对于基本形状，（防盗标记：zhengkunxian）0.4表示在这边长度是0.4米。
velocity_marker.scale.z = 0.4;
//marker的颜色:类似std_msgs/ColorRGBA。每个数字介于0-1之间。最后一个a(alpha)表示透明度，0表示完全透明。
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创建一个唯一的名字的。
//如果接收到一个相同命名空间和id的marker，那新的就会把老的替换掉。
geometry_msgs::Point relative_p;
relative_p.y = 0.5;
velocity_marker.pose.position = calcAbsoluteCoordinate(relative_p, lane.waypoints[i].pose.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);
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());
}
2.2.1 visualization_msgs::Marker
在rviz中标记无人车的轨迹需要用到类visualization_msgs::Marker，其重要成员如下（参考官方文档visualization_msgs/Marker Message ）：
uint8 ARROW=0//箭头
uint8 CUBE=1//立方体
uint8 SPHERE=2//球
uint8 CYLINDER=3//圆柱体
uint8 LINE_STRIP=4//线条（点的连线）
uint8 LINE_LIST=5//线条序列
uint8 CUBE_LIST=6//立方体序列
uint8 SPHERE_LIST=7//球序列
uint8 POINTS=8//点集
uint8 TEXT_VIEW_FACING=9//显示3D的文字
uint8 MESH_RESOURCE=10//网格？
uint8 TRIANGLE_LIST=11//三角形序列
//对标记的操作，也是枚举值
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3
// 重要成员变量
Header header
string ns //命名空间namespace
int32 id //与命名空间联合起来，形成唯一的id，这个唯一的id可以将各个标志物区分开来，
//使得程序可以对指定的标志物进行操作，如果再次执行相同id，会删除上一次的标记
int32 type //类型，指哪种形状
int32 action //操作，是添加还是修改还是删除
geometry_msgs/Pose pose // 位姿
geometry_msgs/Vector3 scale // 默认1，指1米大小。一般要小于1
std_msgs/ColorRGBA color // Color [0.0-1.0]，其实是普通颜色值除以255
duration lifetime // 在自动删除前维持多久，0表示永久
bool frame_locked // 如果此标记应锁定帧，即每个时间步重新转换为其帧
geometry_msgs/Point[] points//这个是在序列、点集中才会用到，指明序列中每个点的位置
// 数量必须是0或与点数相同，alpha还不能用
std_msgs/ColorRGBA[] colors
// 只用于文本标识
string text
// 仅用于MESH_RESOURCE标记
string mesh_resource
bool mesh_use_embedded_materials
2.2.2 calcAbsoluteCoordinate函数
calcAbsoluteCoordinate函数位于src/autoware/common/waypoint_follower/lib/libwaypoint_follower.cpp。在Autoware planning模块学习笔记（二）：路径规划（2）- 节点waypoint_replanner（上）中对同一文件内的calcRelativeCoordinate函数进行了分析，其作用是计算全局坐标系中某点相对于机器人坐标系中的位置。这里对calcAbsoluteCoordinate函数的作用做出分析：（防盗标记：zhengkunxian）计算current_pose框架上的点的绝对坐标。我们仍然设计一个小实验验证这个函数的作用，实验条件跟我们测试calcRelativeCoordinate函数的作用时一致，可以看到在current_pose坐标系中坐标(1,2,0)处所对应的点在绝对坐标系中的坐标为(5,9,0)，而代码运行结果与我们的计算相符，因此得证代码的作用。
geometry_msgs::Point calcAbsoluteCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose)
{
tf::Transform inverse;
tf::poseMsgToTF(current_pose, inverse);
tf::Point p;
pointMsgToTF(point_msg, p);
tf::Point tf_p = inverse * p;
geometry_msgs::Point tf_point_msg;
pointTFToMsg(tf_p, tf_point_msg);
return tf_point_msg;
}
2.2.3 erase函数
对createGlobalLaneArrayVelocityMarker函数中下面两行代码进行解析：
std::string vel = std::to_string(mps2kmph(lane.waypoints[i].twist.twist.linear.x));
velocity_marker.text = vel.erase(vel.find_first_of(".") + 2);//删除下标vel.find_first_of(".") + 2开始及以后的字符
从下面的示例可以看出，这两行实现了速度单位从m/s到km/h的换算，并且转换为字符串后保留小数点后一位。
2.3 createGlobalLaneArrayOrientationMarker函数
createGlobalLaneArrayOrientationMarker函数的作用可以参考2.2 createGlobalLaneArrayVelocityMarker函数进行
分析，这里的标记类型是visualization_msgs::Marker::ARROW（箭头），用以指示车辆速度方向。
void createGlobalLaneArrayOrientationMarker(const autoware_msgs::LaneArray& lane_waypoints_array)
{
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());
}
2.4 createGlobalLaneArrayChangeFlagMarker函数
createGlobalLaneArrayChangeFlagMarker函数的作用参考2.2 createGlobalLaneArrayVelocityMarker函数和2.3 createGlobalLaneArrayOrientationMarker函数，这里的标记类型跟2.2 createGlobalLaneArrayVelocityMarker函数中是一样的，是TEXT_VIEW_FACING（3D的文字），用以显示change_flag。
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";
}
else if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::right))
{
str = "R";
}
else if (lane.waypoints[i].change_flag == static_cast<ChangeFlagInteger>(ChangeFlag::left))
{
str = "L";
}
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());
}
3. finalCallback函数
finalCallback函数是对应于topic "final_waypoints"的回调函数。
void finalCallback(const autoware_msgs::LaneConstPtr& msg)
{
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);
}
3.1 createLocalWaypointVelocityMarker函数
createLocalWaypointVelocityMarker函数的功能跟createGlobalLaneArrayVelocityMarker函数是差不多的，值得注意的是标记的颜色是根据传入的std_msgs::ColorRGBA color设置的，举个例子如果前面路口是红灯，则标记也对应变为红色。
void createLocalWaypointVelocityMarker(std_msgs::ColorRGBA color, int closest_waypoint,
const autoware_msgs::Lane& lane_waypoint)
{
// 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);
}
}
浮点值可以四舍五入到若干位有效数或精度，这是出现在小数点前后的总位数。可以通过使用 setprecision 操作符来控制显示浮点数值的有效数的数量。
std::ostringstream oss;
oss << std::fixed << std::setprecision(1) << mps2kmph(lane_waypoint.waypoints[i].twist.twist.linear.x);
3.2 createLocalPathMarker函数
以线条（点的连线）形状标记出轨迹，线条穿过各个轨迹点。
void createLocalPathMarker(std_msgs::ColorRGBA color, const autoware_msgs::Lane& lane_waypoint)
{
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);
}
3.3 createLocalPointMarker函数
以立方体的形状标记出轨迹点。
void createLocalPointMarker(const autoware_msgs::Lane& lane_waypoint)
{
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);
}
3.4 setLifetime函数
设置传入的visualization_msgs::MarkerArray* marker_array的生存时间，生存时间表示在自动删除前维持多久，0表示永久。
void setLifetime(double sec, visualization_msgs::MarkerArray* marker_array)
{
ros::Duration lifetime(sec);
for (auto& marker : marker_array->markers)
{
marker.lifetime = lifetime;
}
}
4. closestCallback函数
更新设置_closest_waypoint。
void closestCallback(const std_msgs::Int32ConstPtr& msg)
{
_closest_waypoint = msg->data;
}
5. configParameter函数
更新设置g_config_manual_detection。
void configParameter(const autoware_config_msgs::ConfigLaneStopConstPtr& msg)
{
g_config_manual_detection = msg->manual_detection;
}
至此，“勾选waypoint_loader”这一操作时所启动的三个节点waypoint_loader，waypoint_replanner和waypoint_marker_publisher都已经分析结束，后面将继续分析剩下的操作所对应的代码实现过程。