selection.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 <ros/ros.h>
33 #include <stdlib.h>
34 
37 
38 #include <tf/LinearMath/Vector3.h>
39 #include <tf/tf.h>
40 
41 bool testPointAgainstAabb2(const tf::Vector3 &aabbMin1, const tf::Vector3 &aabbMax1,
42  const tf::Vector3 &point)
43 {
44  bool overlap = true;
45  overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
46  overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
47  overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
48  return overlap;
49 }
50 
51 namespace vm = visualization_msgs;
52 
54 {
55 public:
57  std::vector<tf::Vector3>& points ) :
58  server_( server ),
59  min_sel_( -1, -1, -1 ),
60  max_sel_( 1, 1, 1 ),
61  points_( points )
62  {
63  updateBox( );
65 
67  }
68 
70  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
71  {
72  ROS_INFO_STREAM( feedback->marker_name << " is now at "
73  << feedback->pose.position.x << ", " << feedback->pose.position.y
74  << ", " << feedback->pose.position.z );
75 
76  if ( feedback->marker_name == "min_x" ) min_sel_.setX( feedback->pose.position.x );
77  if ( feedback->marker_name == "max_x" ) max_sel_.setX( feedback->pose.position.x );
78  if ( feedback->marker_name == "min_y" ) min_sel_.setY( feedback->pose.position.y );
79  if ( feedback->marker_name == "max_y" ) max_sel_.setY( feedback->pose.position.y );
80  if ( feedback->marker_name == "min_z" ) min_sel_.setZ( feedback->pose.position.z );
81  if ( feedback->marker_name == "max_z" ) max_sel_.setZ( feedback->pose.position.z );
82 
83  updateBox( );
85 
86  if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP )
87  {
89  }
90 
91  server_->applyChanges();
92  }
93 
94  vm::Marker makeBox( vm::InteractiveMarker &msg,
95  tf::Vector3 min_bound, tf::Vector3 max_bound )
96  {
97  vm::Marker marker;
98 
99  marker.type = vm::Marker::CUBE;
100  marker.scale.x = max_bound.x() - min_bound.x();
101  marker.scale.y = max_bound.y() - min_bound.y();
102  marker.scale.z = max_bound.z() - min_bound.z();
103  marker.pose.position.x = 0.5 * ( max_bound.x() + min_bound.x() );
104  marker.pose.position.y = 0.5 * ( max_bound.y() + min_bound.y() );
105  marker.pose.position.z = 0.5 * ( max_bound.z() + min_bound.z() );
106  marker.color.r = 0.5;
107  marker.color.g = 0.5;
108  marker.color.b = 0.5;
109  marker.color.a = 0.5;
110 
111  return marker;
112  }
113 
114  void updateBox( )
115  {
116  vm::InteractiveMarker msg;
117  msg.header.frame_id = "base_link";
118 
119  vm::InteractiveMarkerControl control;
120  control.always_visible = false;
121  control.markers.push_back( makeBox(msg, min_sel_, max_sel_) );
122  msg.controls.push_back( control );
123 
124  server_->insert( msg );
125  }
126 
127  void updatePointCloud( std::string name, std_msgs::ColorRGBA color, std::vector<tf::Vector3> &points )
128  {
129  // create an interactive marker for our server
130  vm::InteractiveMarker int_marker;
131  int_marker.header.frame_id = "base_link";
132  int_marker.name = name;
133 
134  // create a point cloud marker
135  vm::Marker points_marker;
136  points_marker.type = vm::Marker::SPHERE_LIST;
137  points_marker.scale.x = 0.05;
138  points_marker.scale.y = 0.05;
139  points_marker.scale.z = 0.05;
140  points_marker.color = color;
141 
142  for ( unsigned i=0; i<points.size(); i++ )
143  {
144  geometry_msgs::Point p;
145  p.x = points[i].x();
146  p.y = points[i].y();
147  p.z = points[i].z();
148  points_marker.points.push_back( p );
149  }
150 
151  // create container control
152  vm::InteractiveMarkerControl points_control;
153  points_control.always_visible = true;
154  points_control.interaction_mode = vm::InteractiveMarkerControl::NONE;
155  points_control.markers.push_back( points_marker );
156 
157  // add the control to the interactive marker
158  int_marker.controls.push_back( points_control );
159 
160  server_->insert( int_marker );
161  }
162 
164  {
165  std::vector<tf::Vector3> points_in, points_out;
166  points_in.reserve( points_.size() );
167  points_out.reserve( points_.size() );
168 
169  // determine which points are selected (i.e. inside the selection box)
170  for ( unsigned i=0; i<points_.size(); i++ )
171  {
173  {
174  points_in.push_back( points_[i] );
175  }
176  else
177  {
178  points_out.push_back( points_[i] );
179  }
180  }
181 
182  std_msgs::ColorRGBA in_color;
183  in_color.r = 1.0;
184  in_color.g = 0.8;
185  in_color.b = 0.0;
186  in_color.a = 1.0;
187 
188  std_msgs::ColorRGBA out_color;
189  out_color.r = 0.5;
190  out_color.g = 0.5;
191  out_color.b = 0.5;
192  out_color.a = 1.0;
193 
194  updatePointCloud( "selected_points", in_color, points_in );
195  updatePointCloud( "unselected_points", out_color, points_out );
196  }
197 
199  {
200  for ( int axis=0; axis<3; axis++ )
201  {
202  for ( int sign=-1; sign<=1; sign+=2 )
203  {
204  vm::InteractiveMarker int_marker;
205  int_marker.header.frame_id = "base_link";
206  int_marker.scale = 1.0;
207 
208  vm::InteractiveMarkerControl control;
209  control.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
210  control.orientation_mode = vm::InteractiveMarkerControl::INHERIT;
211  control.always_visible = false;
212 
213  tf::Quaternion orien;
214 
215  switch ( axis )
216  {
217  case 0:
218  int_marker.name = sign>0 ? "max_x" : "min_x";
219  int_marker.pose.position.x = sign>0 ? max_sel_.x() : min_sel_.x();
220  int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
221  int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
222  orien = tf::Quaternion(1.0, 0.0, 0.0, 1.0);
223  orien.normalize();
224  tf::quaternionTFToMsg(orien, control.orientation);
225  break;
226  case 1:
227  int_marker.name = sign>0 ? "max_y" : "min_y";
228  int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
229  int_marker.pose.position.y = sign>0 ? max_sel_.y() : min_sel_.y();
230  int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
231  orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
232  orien.normalize();
233  tf::quaternionTFToMsg(orien, control.orientation);
234  break;
235  default:
236  int_marker.name = sign>0 ? "max_z" : "min_z";
237  int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
238  int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
239  int_marker.pose.position.z = sign>0 ? max_sel_.z() : min_sel_.z();
240  orien = tf::Quaternion(0.0, -1.0, 0.0, 1.0);
241  orien.normalize();
242  tf::quaternionTFToMsg(orien, control.orientation);
243  break;
244  }
245 
246  interactive_markers::makeArrow( int_marker, control, 0.5 * sign );
247 
248  int_marker.controls.push_back( control );
249  server_->insert( int_marker, boost::bind( &PointCouldSelector::processAxisFeedback, this, _1 ) );
250  }
251  }
252  }
253 
255  {
256  for ( int axis=0; axis<3; axis++ )
257  {
258  for ( int sign=-1; sign<=1; sign+=2 )
259  {
260  std::string name;
261  geometry_msgs::Pose pose;
262 
263  switch ( axis )
264  {
265  case 0:
266  name = sign>0 ? "max_x" : "min_x";
267  pose.position.x = sign>0 ? max_sel_.x() : min_sel_.x();
268  pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
269  pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
270  break;
271  case 1:
272  name = sign>0 ? "max_y" : "min_y";
273  pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
274  pose.position.y = sign>0 ? max_sel_.y() : min_sel_.y();
275  pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
276  break;
277  default:
278  name = sign>0 ? "max_z" : "min_z";
279  pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
280  pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
281  pose.position.z = sign>0 ? max_sel_.z() : min_sel_.z();
282  break;
283  }
284 
285  server_->setPose( name, pose );
286  }
287  }
288  }
289 
290 private:
292 
294  std::vector<tf::Vector3> points_;
295 
296  vm::InteractiveMarker sel_points_marker_;
297  vm::InteractiveMarker unsel_points_marker_;
298 };
299 
300 
301 
302 
303 double rand( double min, double max )
304 {
305  double t = (double)rand() / (double)RAND_MAX;
306  return min + t*(max-min);
307 }
308 
309 
310 void makePoints( std::vector<tf::Vector3>& points_out, int num_points )
311 {
312  double radius = 3;
313  double scale = 0.2;
314  points_out.resize(num_points);
315  for( int i = 0; i < num_points; i++ )
316  {
317  points_out[i].setX( scale * rand( -radius, radius ) );
318  points_out[i].setY( scale * rand( -radius, radius ) );
319  points_out[i].setZ( scale * radius * 0.2 * ( sin( 10.0 / radius * points_out[i].x() ) + cos( 10.0 / radius * points_out[i].y() ) ) );
320  }
321 }
322 
323 
324 int main(int argc, char** argv)
325 {
326  ros::init(argc, argv, "selection");
327 
328  // create an interactive marker server on the topic namespace simple_marker
331 
332  std::vector<tf::Vector3> points;
333  makePoints( points, 10000 );
334 
335  PointCouldSelector selector( server, points );
336 
337  // 'commit' changes and send to all clients
338  server->applyChanges();
339 
340  // start the ROS main loop
341  ros::spin();
342 }
343 // %Tag(fullSource)%
tf::Vector3 min_sel_
Definition: selection.cpp:293
tf::Vector3 max_sel_
Definition: selection.cpp:293
void updatePointCloud(std::string name, std_msgs::ColorRGBA color, std::vector< tf::Vector3 > &points)
Definition: selection.cpp:127
TFSIMD_FORCE_INLINE void setX(tfScalar x)
TFSIMD_FORCE_INLINE void setY(tfScalar y)
bool testPointAgainstAabb2(const tf::Vector3 &aabbMin1, const tf::Vector3 &aabbMax1, const tf::Vector3 &point)
Definition: selection.cpp:41
vm::InteractiveMarker unsel_points_marker_
Definition: selection.cpp:297
void makePoints(std::vector< tf::Vector3 > &points_out, int num_points)
Definition: selection.cpp:310
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
Quaternion & normalize()
void updatePointClouds()
Definition: selection.cpp:163
vm::InteractiveMarker sel_points_marker_
Definition: selection.cpp:296
vm::Marker makeBox(vm::InteractiveMarker &msg, tf::Vector3 min_bound, tf::Vector3 max_bound)
Definition: selection.cpp:94
void updateSizeHandles()
Definition: selection.cpp:254
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
int main(int argc, char **argv)
Definition: selection.cpp:324
ROSCPP_DECL void spin(Spinner &spinner)
TFSIMD_FORCE_INLINE const tfScalar & x() const
double rand(double min, double max)
Definition: selection.cpp:303
TFSIMD_FORCE_INLINE const tfScalar & z() const
PointCouldSelector(boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server, std::vector< tf::Vector3 > &points)
Definition: selection.cpp:56
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
TFSIMD_FORCE_INLINE const tfScalar & y() const
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
Definition: selection.cpp:291
#define ROS_INFO_STREAM(args)
TFSIMD_FORCE_INLINE const tfScalar & getX() const
INTERACTIVE_MARKERS_PUBLIC void makeArrow(const visualization_msgs::InteractiveMarker &msg, visualization_msgs::InteractiveMarkerControl &control, float pos)
std::vector< tf::Vector3 > points_
Definition: selection.cpp:294
void processAxisFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: selection.cpp:69


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Sat May 16 2020 03:49:16