Advertisement
Guest User

ROS marker

a guest
Mar 28th, 2017
70
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 2.22 KB | None | 0 0
  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.   // Set our initial shape type to be a cube
  12.   uint32_t shape = visualization_msgs::Marker::CYLINDER;
  13.  
  14.   while (ros::ok())
  15.   {
  16.     visualization_msgs::Marker marker;
  17.     // Set the frame ID and timestamp.  See the TF tutorials for information on these.
  18.     marker.header.frame_id = "/base_link";
  19.     marker.header.stamp = ros::Time::now();
  20.  
  21.     // Set the namespace and id for this marker.  This serves to create a unique ID
  22.     // Any marker sent with the same namespace and id will overwrite the old one
  23.     marker.ns = "basic_shapes";
  24.     marker.id = 0;
  25.  
  26.     // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
  27.     marker.type = shape;
  28.  
  29.     // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
  30.     marker.action = visualization_msgs::Marker::ADD;
  31.  
  32.     // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
  33.     marker.pose.position.x = 0.4;
  34.     marker.pose.position.y = 0;
  35.     marker.pose.position.z = 0.04;
  36.     marker.pose.orientation.x = 0.0;
  37.     marker.pose.orientation.y = 0.0;
  38.     marker.pose.orientation.z = 0.0;
  39.     marker.pose.orientation.w = 1.0;
  40.  
  41.     // Set the scale of the marker -- 1x1x1 here means 1m on a side
  42.     marker.scale.x = 0.04;
  43.     marker.scale.y = 0.04;
  44.     marker.scale.z = 0.08;
  45.  
  46.     // Set the color -- be sure to set alpha to something non-zero!
  47.     marker.color.r = 0.95f;
  48.     marker.color.g = 0.95f;
  49.     marker.color.b = 0.75f;
  50.     marker.color.a = 1.0;
  51.  
  52.     marker.lifetime = ros::Duration();
  53.  
  54.     // Publish the marker
  55.     while (marker_pub.getNumSubscribers() < 1)
  56.     {
  57.       if (!ros::ok())
  58.       {
  59.         return 0;
  60.       }
  61.       ROS_WARN_ONCE("Please create a subscriber to the marker");
  62.       sleep(1);
  63.     }
  64.     marker_pub.publish(marker);
  65. marker.pose.position.x = 0.8;
  66. marker.id = 1;
  67. marker_pub.publish(marker);
  68.  
  69.  
  70.  
  71.     r.sleep();
  72.   }
  73. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement