在rviz中使用點和線可視化軌跡（Markers: Points and Lines）

1 介紹

在做位姿相關的研究時，我們希望直觀的可視化軌跡，尤其是對比不同的軌跡。

本教程將向您介紹POINTS，LINE_STRIP和LINE_LIST標記類型。有關類型的完整列表，請參見“標記顯示”頁面。

2 使用Points, Line Strips, and Line Lists

POINTS，LINE_STRIP和LINE_LIST標記都使用visualization_msgs/Marker消息的點成員。
POINTS類型在添加的每個點處放置一個點。LINE_STRIP類型將每個點用作一組連接的線中的頂點，其中點0連接到點1，點1連接到點2，點2連接到點3等。LINE_LIST類型在每對點（即點0到1、2到3等）中創建未連接的線。

2.1 代碼

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

#include <cmath>

int main( int argc, char** argv )
{
  ros::init(argc, argv, "points_and_lines");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);

  ros::Rate r(30);

  float f = 0.0;
  while (ros::ok())
  {
    visualization_msgs::Marker points, line_strip, line_list;
    points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame";
    points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
    points.ns = line_strip.ns = line_list.ns = "points_and_lines";
    points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;

    points.id = 0;
    line_strip.id = 1;
    line_list.id = 2;

    points.type = visualization_msgs::Marker::POINTS;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    line_list.type = visualization_msgs::Marker::LINE_LIST;

    // POINTS markers use x and y scale for width/height respectively
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
    line_strip.scale.x = 0.1;
    line_list.scale.x = 0.1;

    // Points are green
    points.color.g = 1.0f;
    points.color.a = 1.0;

    // Line strip is blue
    line_strip.color.b = 1.0;
    line_strip.color.a = 1.0;

    // Line list is red
    line_list.color.r = 1.0;
    line_list.color.a = 1.0;

    // Create the vertices for the points and lines
    for (uint32_t i = 0; i < 100; ++i)
    {
      float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
      float z = 5 * cos(f + i / 100.0f * 2 * M_PI);
      
      geometry_msgs::Point p;
      p.x = (int32_t)i - 50;
      p.y = y;
      p.z = z;

      points.points.push_back(p);
      line_strip.points.push_back(p);

      // The line list needs two points for each line
      line_list.points.push_back(p);
      p.z += 1.0;
      line_list.points.push_back(p);
    }

    marker_pub.publish(points);
    marker_pub.publish(line_strip);
    marker_pub.publish(line_list);

    r.sleep();

    f += 0.04;
  }
}

2.2 代碼解釋

現在，讓我們分解代碼，跳過上一教程中解釋的內容。產生的總體效果是一個旋轉的螺旋線，其線從每個頂點向上伸出。

    visualization_msgs::Marker points, line_strip, line_list;
    points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame";
    points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
    points.ns = line_strip.ns = line_list.ns = "points_and_lines";
    points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;

在這裏，我們創建三個visualization_msgs/Marker消息並初始化它們的所有共享數據。我們利用消息成員默認爲0且僅設置位姿的成員w這一事實。

    points.id = 0;
    line_strip.id = 1;
    line_list.id = 2;

我們爲三個標記分配了三個不同的ID。使用points_and_lines命名空間可確保它們不會與其他廣播發生衝突。

    points.type = visualization_msgs::Marker::POINTS;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    line_list.type = visualization_msgs::Marker::LINE_LIST;

在這裏，我們將標記類型設置爲POINTS，LINE_STRIP和LINE_LIST。

    // POINTS markers use x and y scale for width/height respectively
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
    line_strip.scale.x = 0.1;
    line_list.scale.x = 0.1;

標尺（scale）成員對這些標記類型表示不同的含義。POINTS標記分別將x和y成員用於寬度和高度，而LINE_STRIP和LINE_LIST標記僅使用x分量來定義線寬。比例尺值以米爲單位。

    // Points are green
    points.color.g = 1.0f;
    points.color.a = 1.0;

    // Line strip is blue
    line_strip.color.b = 1.0;
    line_strip.color.a = 1.0;

    // Line list is red
    line_list.color.r = 1.0;
    line_list.color.a = 1.0;

在這裏，我們將點設置爲綠色，將線條設置爲藍色，並將線列表設置爲紅色。

    // Create the vertices for the points and lines
    for (uint32_t i = 0; i < 100; ++i)
    {
      float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
      float z = 5 * cos(f + i / 100.0f * 2 * M_PI);

      geometry_msgs::Point p;
      p.x = (int32_t)i - 50;
      p.y = y;
      p.z = z;

      points.points.push_back(p);
      line_strip.points.push_back(p);

      // The line list needs two points for each line
      line_list.points.push_back(p);
      p.z += 1.0;
      line_list.points.push_back(p);
    }

我們使用正弦和餘弦生成一個螺旋。POINTS和LINE_STRIP標記每個頂點只需要一個點，而LINE_LIST標記則需要2個。

2.3 查看標記

（1）創建一個名爲using_markers的包

catkin_create_pkg using_markers roscpp visualization_msgs

（2）編輯using_markers包中的CMakeLists.txt文件，並將其添加到底部

add_executable(points_and_lines src/points_and_lines.cpp)
target_link_libraries(points_and_lines ${catkin_LIBRARIES})

（3）編譯

$ catkin_make

（4）運行

$ rosrun rviz rviz＆
$ rosrun using_markers points_and_lines

點擊【Add】，添加【Marker】，將【Fixed Frame】設置爲【my_frame】，最後應該看到一個看起來像這樣的旋轉螺旋：

3 參考

http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Points%20and%20Lines