Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <ros/ros.h>
00032 #include <visualization_msgs/Marker.h>
00033
00034 #include <cmath>
00035
00036 int main( int argc, char** argv )
00037 {
00038 ros::init(argc, argv, "points_and_lines");
00039 ros::NodeHandle n;
00040 ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
00041
00042 ros::Rate r(30);
00043
00044 float f = 0.0;
00045 while (ros::ok())
00046 {
00047
00048 visualization_msgs::Marker points, line_strip, line_list;
00049 points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame";
00050 points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
00051 points.ns = line_strip.ns = line_list.ns = "points_and_lines";
00052 points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
00053 points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;
00054
00055
00056
00057 points.id = 0;
00058 line_strip.id = 1;
00059 line_list.id = 2;
00060
00061
00062
00063 points.type = visualization_msgs::Marker::POINTS;
00064 line_strip.type = visualization_msgs::Marker::LINE_STRIP;
00065 line_list.type = visualization_msgs::Marker::LINE_LIST;
00066
00067
00068
00069
00070 points.scale.x = 0.2;
00071 points.scale.y = 0.2;
00072
00073
00074 line_strip.scale.x = 0.1;
00075 line_list.scale.x = 0.1;
00076
00077
00078
00079
00080 points.color.g = 1.0f;
00081 points.color.a = 1.0;
00082
00083
00084 line_strip.color.b = 1.0;
00085 line_strip.color.a = 1.0;
00086
00087
00088 line_list.color.r = 1.0;
00089 line_list.color.a = 1.0;
00090
00091
00092
00093
00094 for (uint32_t i = 0; i < 100; ++i)
00095 {
00096 float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
00097 float z = 5 * cos(f + i / 100.0f * 2 * M_PI);
00098
00099 geometry_msgs::Point p;
00100 p.x = (int32_t)i - 50;
00101 p.y = y;
00102 p.z = z;
00103
00104 points.points.push_back(p);
00105 line_strip.points.push_back(p);
00106
00107
00108 line_list.points.push_back(p);
00109 p.z += 1.0;
00110 line_list.points.push_back(p);
00111 }
00112
00113
00114 marker_pub.publish(points);
00115 marker_pub.publish(line_strip);
00116 marker_pub.publish(line_list);
00117
00118 r.sleep();
00119
00120 f += 0.04;
00121 }
00122 }
00123
00124