point_cloud.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 
31 // %Tag(fullSource)%
32 #include <math.h>
33 
34 #include <ros/ros.h>
35 
37 
38 namespace vm = visualization_msgs;
39 
40 void processFeedback( const vm::InteractiveMarkerFeedbackConstPtr &feedback )
41 {
42  uint8_t type = feedback->event_type;
43 
44  if( type == vm::InteractiveMarkerFeedback::BUTTON_CLICK ||
45  type == vm::InteractiveMarkerFeedback::MOUSE_DOWN ||
46  type == vm::InteractiveMarkerFeedback::MOUSE_UP )
47  {
48  const char* type_str = (type == vm::InteractiveMarkerFeedback::BUTTON_CLICK ? "button click" :
49  (type == vm::InteractiveMarkerFeedback::MOUSE_DOWN ? "mouse down" : "mouse up"));
50 
51  if( feedback->mouse_point_valid )
52  {
53  ROS_INFO( "%s at %f, %f, %f in frame %s",
54  type_str,
55  feedback->mouse_point.x, feedback->mouse_point.y, feedback->mouse_point.z,
56  feedback->header.frame_id.c_str() );
57  }
58  else
59  {
60  ROS_INFO( "%s", type_str );
61  }
62  }
63  else if( type == vm::InteractiveMarkerFeedback::POSE_UPDATE )
64  {
65  ROS_INFO_STREAM( feedback->marker_name << " is now at "
66  << feedback->pose.position.x << ", " << feedback->pose.position.y
67  << ", " << feedback->pose.position.z );
68  }
69 }
70 
71 void makePoints( std::vector<geometry_msgs::Point>& points_out, int num_points )
72 {
73  double radius = 3;
74  points_out.resize(num_points);
75  for( int i = 0; i < num_points; i++ )
76  {
77  double angle = (i / (double) num_points * 50 * M_PI);
78  double height = (i / (double) num_points * 10);
79  points_out[i].x = radius * cos( angle );
80  points_out[i].y = radius * sin( angle );
81  points_out[i].z = height;
82  }
83 }
84 
85 vm::InteractiveMarker makeMarker( std::string name, std::string description, int32_t type, float x, int num_points = 10000, float scale = 0.1f )
86 {
87  // create an interactive marker for our server
88  vm::InteractiveMarker int_marker;
89  int_marker.header.frame_id = "base_link";
90  int_marker.name = name;
91  int_marker.description = description;
92 
93  // create a point cloud marker
94  vm::Marker points_marker;
95  points_marker.type = type;
96  points_marker.scale.x = scale;
97  points_marker.scale.y = scale;
98  points_marker.scale.z = scale;
99  points_marker.color.r = 0.5;
100  points_marker.color.g = 0.5;
101  points_marker.color.b = 0.5;
102  points_marker.color.a = 1.0;
103  makePoints( points_marker.points, num_points );
104 
105  // create a control which contains the point cloud which acts like a button.
106  vm::InteractiveMarkerControl points_control;
107  points_control.always_visible = true;
108  points_control.interaction_mode = vm::InteractiveMarkerControl::BUTTON;
109  points_control.markers.push_back( points_marker );
110 
111  // add the control to the interactive marker
112  int_marker.controls.push_back( points_control );
113 
114  // create a control which will move the box
115  // this control does not contain any markers,
116  // which will cause RViz to insert two arrows
117  vm::InteractiveMarkerControl rotate_control;
118  rotate_control.name = "move_x";
119  rotate_control.interaction_mode =
120  vm::InteractiveMarkerControl::MOVE_AXIS;
121 
122  // add the control to the interactive marker
123  int_marker.controls.push_back(rotate_control);
124 
125  int_marker.pose.position.x = x;
126 
127  return int_marker;
128 }
129 
130 int main(int argc, char** argv)
131 {
132  ros::init(argc, argv, "point_cloud");
133 
134  // create an interactive marker server on the topic namespace simple_marker
136 
137  server.insert(makeMarker("points", "Points marker", vm::Marker::POINTS, 0), &processFeedback);
138  // LINE_STRIP and LINE_LIST are not actually selectable, and they won't highlight or detect mouse clicks like the others (yet).
139  server.insert(makeMarker("line_strip", "Line Strip marker", vm::Marker::LINE_STRIP, 10, 1000), &processFeedback);
140  server.insert(makeMarker("line_list", "Line List marker", vm::Marker::LINE_LIST, 20), &processFeedback);
141  server.insert(makeMarker("cube_list", "Cube List marker", vm::Marker::CUBE_LIST, 30), &processFeedback);
142  server.insert(makeMarker("sphere_list", "Sphere List marker", vm::Marker::SPHERE_LIST, 40), &processFeedback);
143  server.insert(makeMarker("triangle_list", "Triangle List marker", vm::Marker::TRIANGLE_LIST, 50, 201, 1.0f), &processFeedback);
144 
145  // 'commit' changes and send to all clients
146  server.applyChanges();
147 
148  // start the ROS main loop
149  ros::spin();
150 }
151 // %Tag(fullSource)%
int main(int argc, char **argv)
f
void processFeedback(const vm::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: point_cloud.cpp:40
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
ROSCPP_DECL void spin(Spinner &spinner)
vm::InteractiveMarker makeMarker(std::string name, std::string description, int32_t type, float x, int num_points=10000, float scale=0.1f)
Definition: point_cloud.cpp:85
#define ROS_INFO(...)
void makePoints(std::vector< geometry_msgs::Point > &points_out, int num_points)
Definition: point_cloud.cpp:71
void insert(const visualization_msgs::InteractiveMarker &int_marker)
#define ROS_INFO_STREAM(args)


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Fri Feb 1 2019 03:33:34