rviz學習系列---Markers:發送基本形狀（c++）

本文轉載自查看原文 2017-03-16 19:55 2672 rviz/ SLAM/ ROS

1.cd ~/catkin_ws/

catkin_create_pkg using_markers roscpp visualization_msgs

cd src/using_markers/src

gedit basic_shapes.cpp

2.寫代碼

 1 #include "ros/ros.h"
 2 #include "visualization_msgs/Marker.h"
 3 
 4 int main(int argc,char** argv)
 5 {
 6     ros::init(argc,argv,"basic_shapes");
 7     ros::NodeHandle n;
 8     ros::Rate r(1);
 9     ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker",1);
10 
11     uint32_t shape = visualization_msgs::Marker::CUBE;
12 
13     while(ros::ok())
14     {
15         visualization_msgs::Marker marker;
16         marker.header.frame_id = "/my_frame";
17         marker.header.stamp = ros::Time::now();
18         marker.ns = "basic_shapes";
19         marker.id = 0;
20         marker.type = shape;
21         marker.action = visualization_msgs::Marker::ADD;
22 
23         marker.pose.position.x = 0;
24         marker.pose.position.y = 0;
25         marker.pose.position.z = 0;
26         marker.pose.orientation.x = 0.0;
27         marker.pose.orientation.y = 0.0;
28         marker.pose.orientation.z = 0.0;
29         marker.pose.orientation.w = 1.0;
30 
31         marker.scale.x = 1.0;
32         marker.scale.y = 1.0;
33         marker.scale.z = 1.0;
34 
35         marker.color.r = 0.0f;
36         marker.color.g = 1.0f;
37         marker.color.b = 0.0f;
38         marker.color.a = 1.0;
39 
40         marker.lifetime = ros::Duration();
41 
42         while(marker_pub.getNumSubscribers() < 1)
43         {
44             if(!ros::ok())
45             {
46                 return 0;
47             }
48             ROS_WARN_ONCE("Please create a subscriber to the marker");
49             sleep(1);
50         }
51         marker_pub.publish(marker);
52 
53         switch(shape)
54         {
55             case visualization_msgs::Marker::CUBE:
56                 shape = visualization_msgs::Marker::SPHERE;
57                 break;
58             case visualization_msgs::Marker::SPHERE:
59                 shape = visualization_msgs::Marker::ARROW;
60                 break;
61             case visualization_msgs::Marker::ARROW:
62                 shape = visualization_msgs::Marker::CYLINDER;
63                 break;
64             case visualization_msgs::Marker::CYLINDER:
65                 shape = visualization_msgs::Marker::CUBE;
66                 break;
67 
68         }
69         r.sleep();
70     }
71 
72 }