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都已經分析結束，後面將繼續分析剩下的操作所對應的代碼實現過程。