（五）学习笔记autoware源码core_planning（waypoint_marker_publisher)

#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代码编辑软件，展示性好点。