How to draw a straight line, curve or trajectory from a set of scattered points and display it in Rviz
1 Introduction
When doing pose-related research, we hope to visualize trajectories intuitively, especially to compare different trajectories.
This tutorial will introduce you to POINTS , LINE_STRIP and LINE_LIST tag types. For a complete list of types, see the "Marker Display" page.
2 使用Points, Line Strips, and Line Lists
POINTS , LINE_STRIP and LINE_LIST markers all use the point member of the visualization_msgs/Marker message.
POINTS类型在添加的每个点处放置一个点。
LINE_STRIP类型将每个点用作一组连接的线中的顶点，其中点0连接到点1，点1连接到点2，点2连接到点3等。
LINE_LIST类型在每对点（即点0到1、2到3等）中创建未连接的线。
2.1 Code
#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 Code explanation
Now, let's break down the code and skip the content explained in the previous tutorial. The overall effect produced is a rotating spiral with lines extending upward from each vertex.
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;
Here, we create three visualization_msgs/Marker messages and initialize all their shared data. We take advantage of the fact that the message member defaults to 0 and only the pose member w is set.
points.id = 0;
line_strip.id = 1;
line_list.id = 2;
We assigned three different IDs to the three tags. Using the points_and_lines namespace ensures that they will not conflict with other broadcasts.
points.type = visualization_msgs::Marker::POINTS;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
line_list.type = visualization_msgs::Marker::LINE_LIST;
Here, we set the mark type to POINTS , LINE_STRIP and 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;
The scale members have different meanings for these mark types. The POINTS tag uses the x and y members for width and height, respectively, while the LINE_STRIP and LINE_LIST tags only use the x component to define the line width. The scale value is in meters.
// 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;
Here, we set the point to green, the line to blue, and the line list to red.
// 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);
}
We use sine and cosine to generate a spiral. The POINTS and LINE_STRIP markers only need one point per vertex, while the LINE_LIST marker requires two.
2.3 View mark
(1) Create a package named using_markers
catkin_create_pkg using_markers roscpp visualization_msgs
(2) Edit the CMakeLists.txt file in the using_markers package and add it to the bottom
add_executable(points_and_lines src/points_and_lines.cpp)
target_link_libraries(points_and_lines ${catkin_LIBRARIES})
(3) Compile
$ catkin_make
(4) Operation
$ rosrun rviz rviz＆
$ rosrun using_markers points_and_lines
Click [Add], add [Marker], set [Fixed Frame] to [my_frame], and finally you should see a rotating spiral that looks like this:
3 Reference
http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Points%20and%20Lines