$search
00001 /* 00002 * Copyright (c) 2010, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 // %Tag(FULLTEXT)% 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 // %Tag(MARKER_INIT)% 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 // %EndTag(MARKER_INIT)% 00055 00056 // %Tag(ID)% 00057 points.id = 0; 00058 line_strip.id = 1; 00059 line_list.id = 2; 00060 // %EndTag(ID)% 00061 00062 // %Tag(TYPE)% 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 // %EndTag(TYPE)% 00067 00068 // %Tag(SCALE)% 00069 // POINTS markers use x and y scale for width/height respectively 00070 points.scale.x = 0.2; 00071 points.scale.y = 0.2; 00072 00073 // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width 00074 line_strip.scale.x = 0.1; 00075 line_list.scale.x = 0.1; 00076 // %EndTag(SCALE)% 00077 00078 // %Tag(COLOR)% 00079 // Points are green 00080 points.color.g = 1.0f; 00081 points.color.a = 1.0; 00082 00083 // Line strip is blue 00084 line_strip.color.b = 1.0; 00085 line_strip.color.a = 1.0; 00086 00087 // Line list is red 00088 line_list.color.r = 1.0; 00089 line_list.color.a = 1.0; 00090 // %EndTag(COLOR)% 00091 00092 // %Tag(HELIX)% 00093 // Create the vertices for the points and lines 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 // The line list needs two points for each line 00108 line_list.points.push_back(p); 00109 p.z += 1.0; 00110 line_list.points.push_back(p); 00111 } 00112 // %EndTag(HELIX)% 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 // %EndTag(FULLTEXT)% 00124