ptu_interactive_control.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
35 #include <ros/ros.h>
36 #include <interactive_markers/interactive_marker_server.h>
37 //commented next line isince it appears not needed and was not found in path! V. Santos, 24-Mai-2013,10:51
38 //#include <interactive_markers/interactive_marker_client.h>
39 #include <interactive_markers/menu_handler.h>
40 #include <tf/transform_broadcaster.h>
41 #include <tf/tf.h>
42 #include <math.h>
43 #include <sensor_msgs/JointState.h>
44 
45 using namespace visualization_msgs;
46 
47 
48 // %Tag(vars)%
49 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
50 float marker_pos = 0;
51 interactive_markers::MenuHandler menu_handler;
52 ros::Publisher pub;
53 // %EndTag(vars)%
54 
55 // %Tag(frameCallback)%
56 void frameCallback(const ros::TimerEvent&)
57 {
58  //static uint32_t counter = 0;
59 
60  //static tf::TransformBroadcaster br;
61 
62  //tf::Transform t;
63 
64  //ros::Time time = ros::Time::now();
65 
66  //t.setOrigin(tf::Vector3(0.0, 0.0, sin(float(counter)/140.0) * 2.0));
67  //t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
68  //br.sendTransform(tf::StampedTransform(t, time, "base_link", "moving_frame"));
69 
70  //t.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
71  //t.setRotation(tf::createQuaternionFromRPY(0.0, float(counter)/140.0, 0.0));
72  //br.sendTransform(tf::StampedTransform(t, time, "base_link", "rotating_frame"));
73 
74  //++counter;
75 }
76 // %EndTag(frameCallback)%
77 
78 // %Tag(processFeedback)%
79 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
80 {
81  std::ostringstream s;
82  s << "Feedback from marker '" << feedback->marker_name << "' "
83  << " / control '" << feedback->control_name << "'";
84 
85  std::ostringstream mouse_point_ss;
86  if( feedback->mouse_point_valid )
87  {
88  mouse_point_ss << " at " << feedback->mouse_point.x
89  << ", " << feedback->mouse_point.y
90  << ", " << feedback->mouse_point.z
91  << " in frame " << feedback->header.frame_id;
92  }
93 
94  switch ( feedback->event_type )
95  {
96  //case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
97  //ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." );
98  //break;
99 
100  //case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
101  //ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." );
102  //break;
103 
104  case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
105 
106  tf::Quaternion q;
107  q[0] = feedback->pose.orientation.x;
108  q[1] = feedback->pose.orientation.y;
109  q[2] = feedback->pose.orientation.z;
110  q[3] = feedback->pose.orientation.w;
111  tf::Matrix3x3 M(q);
112  double r,p,y;
113 #if ROS_VERSION_MINIMUM(1, 8, 0) // if current ros version is >= 1.8.0
114  M.getEulerYPR(y, p, r); //Changed by V. Santos, 15-Abr-2013,09:25
115 #else // Code for earlier versions (Electric, diamondback, ...)
116  M.getRPY(r,p,y);
117 #endif
118 
119  sensor_msgs::JointState joint_state;
120  joint_state.header.stamp = ros::Time::now();
121  joint_state.name.resize(2);
122  joint_state.position.resize(2);
123  joint_state.name[0] =ros::names::remap("pan");
124  joint_state.position[0] = y;
125  joint_state.name[1] =ros::names::remap("tilt");
126  joint_state.position[1] = p;
127  pub.publish(joint_state);
128 
129  //btQuaternion q = feedback->pose.orientation;
130 
131  ROS_INFO_STREAM( s.str() << ": pose changed"
132  << "\nposition = "
133  << feedback->pose.position.x
134  << ", " << feedback->pose.position.y
135  << ", " << feedback->pose.position.z
136  << "\norientation = "
137  << feedback->pose.orientation.w
138  << ", " << feedback->pose.orientation.x
139  << ", " << feedback->pose.orientation.y
140  << ", " << feedback->pose.orientation.z
141  << "\nframe: " << feedback->header.frame_id
142  << " time: " << feedback->header.stamp.sec << "sec, "
143  << feedback->header.stamp.nsec << " nsec" );
144  break;
145 
146  //case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
147  //ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." );
148  //break;
149 
150  //case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
151  //ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." );
152  //break;
153  }
154 
155  server->applyChanges();
156 }
157 // %EndTag(processFeedback)%
158 
159 // %Tag(alignMarker)%
160 void alignMarker( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
161 {
162  geometry_msgs::Pose pose = feedback->pose;
163 
164  pose.position.x = round(pose.position.x-0.5)+0.5;
165  pose.position.y = round(pose.position.y-0.5)+0.5;
166 
167  ROS_INFO_STREAM( feedback->marker_name << ":"
168  << " aligning position = "
169  << feedback->pose.position.x
170  << ", " << feedback->pose.position.y
171  << ", " << feedback->pose.position.z
172  << " to "
173  << pose.position.x
174  << ", " << pose.position.y
175  << ", " << pose.position.z );
176 
177  server->setPose( feedback->marker_name, pose );
178  server->applyChanges();
179 }
180 // %EndTag(alignMarker)%
181 
182 double rand( double min, double max )
183 {
184  double t = (double)rand() / (double)RAND_MAX;
185  return min + t*(max-min);
186 }
187 
188 // %Tag(Box)%
189 Marker makeBox( InteractiveMarker &msg )
190 {
191  Marker marker;
192 
193  marker.type = Marker::SPHERE;
194  marker.scale.x = msg.scale * 0.45;
195  marker.scale.y = msg.scale * 0.45;
196  marker.scale.z = msg.scale * 0.45;
197  marker.color.r = 0;
198  marker.color.g = 0.7;
199  marker.color.b = 0;
200  marker.color.a = 0.6;
201 
202  return marker;
203 }
204 
205 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
206 {
207  InteractiveMarkerControl control;
208  control.always_visible = true;
209  control.markers.push_back( makeBox(msg) );
210  msg.controls.push_back( control );
211 
212  return msg.controls.back();
213 }
214 // %EndTag(Box)%
215 
216 void saveMarker( InteractiveMarker int_marker )
217 {
218  server->insert(int_marker);
219  server->setCallback(int_marker.name, &processFeedback);
220 }
221 
223 
224 // %Tag(6DOF)%
225 void make6DofMarker( bool fixed )
226 {
227  InteractiveMarker int_marker;
228  int_marker.header.frame_id = "/atc/vehicle/center_bumper";
229  int_marker.pose.position.x = 2;
230  int_marker.pose.position.y = 0;
231  int_marker.pose.position.z = 0.5;
232  int_marker.scale = 0.5;
233 
234  int_marker.name = "Fovetation Control Target";
235  int_marker.description = "Foveation_control will try \nto aim the PTU towards\n the marker";
236 
237  // insert a box
238  makeBoxControl(int_marker);
239  InteractiveMarkerControl control;
240 
241  if ( fixed )
242  {
243  //int_marker.name += "_fixed";
244  //int_marker.description += "\n(fixed orientation)";
245  //control.orientation_mode = InteractiveMarkerControl::FIXED;
246  }
247 
248  control.orientation.w = 1;
249  control.orientation.x = 1;
250  control.orientation.y = 0;
251  control.orientation.z = 0;
252  //control.name = "rotate_x";
253  //control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
254  //int_marker.controls.push_back(control);
255  control.name = "move_x";
256  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
257  int_marker.controls.push_back(control);
258 
259  control.orientation.w = 1;
260  control.orientation.x = 0;
261  control.orientation.y = 1;
262  control.orientation.z = 0;
263  //control.name = "rotate_z";
264  //control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
265  //int_marker.controls.push_back(control);
266  control.name = "move_z";
267  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
268  int_marker.controls.push_back(control);
269 
270  control.orientation.w = 1;
271  control.orientation.x = 0;
272  control.orientation.y = 0;
273  control.orientation.z = 1;
274  //control.name = "rotate_y";
275  //control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
276  //int_marker.controls.push_back(control);
277  control.name = "move_y";
278  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
279  int_marker.controls.push_back(control);
280 
281  server->insert(int_marker);
282  server->setCallback(int_marker.name, &processFeedback);
283 }
284 // %EndTag(6DOF)%
285 
286 // %Tag(RandomDof)%
287 void makeRandomDofMarker( )
288 {
289  InteractiveMarker int_marker;
290  int_marker.header.frame_id = "/base_link";
291  int_marker.pose.position.y = -3.0 * marker_pos++;;
292  int_marker.scale = 1;
293 
294  int_marker.name = "6dof_random_axes";
295  int_marker.description = "6-DOF\n(Arbitrary Axes)";
296 
297  makeBoxControl(int_marker);
298 
299  InteractiveMarkerControl control;
300 
301  for ( int i=0; i<3; i++ )
302  {
303  control.orientation.w = rand(-1,1);
304  control.orientation.x = rand(-1,1);
305  control.orientation.y = rand(-1,1);
306  control.orientation.z = rand(-1,1);
307  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
308  int_marker.controls.push_back(control);
309  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
310  int_marker.controls.push_back(control);
311  }
312 
313  server->insert(int_marker);
314  server->setCallback(int_marker.name, &processFeedback);
315 }
316 // %EndTag(RandomDof)%
317 
318 
319 // %Tag(ViewFacing)%
320 void makeViewFacingMarker( )
321 {
322  InteractiveMarker int_marker;
323  int_marker.header.frame_id = "/base_link";
324  int_marker.pose.position.y = -3.0 * marker_pos++;;
325  int_marker.scale = 1;
326 
327  int_marker.name = "view_facing";
328  int_marker.description = "View Facing 6-DOF";
329 
330  InteractiveMarkerControl control;
331 
332  // make a control that rotates around the view axis
333  control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
334  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
335  control.orientation.w = 1;
336  control.name = "rotate";
337 
338  int_marker.controls.push_back(control);
339 
340  // create a box in the center which should not be view facing,
341  // but move in the camera plane.
342  control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
343  control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
344  control.independent_marker_orientation = true;
345  control.name = "move";
346 
347  control.markers.push_back( makeBox(int_marker) );
348  control.always_visible = true;
349 
350  int_marker.controls.push_back(control);
351 
352  server->insert(int_marker);
353  server->setCallback(int_marker.name, &processFeedback);
354 }
355 // %EndTag(ViewFacing)%
356 
357 
358 // %Tag(Quadrocopter)%
359 void makeQuadrocopterMarker( )
360 {
361  InteractiveMarker int_marker;
362  int_marker.header.frame_id = "/base_link";
363  int_marker.pose.position.y = -3.0 * marker_pos++;;
364  int_marker.scale = 1;
365 
366  int_marker.name = "quadrocopter";
367  int_marker.description = "Quadrocopter";
368 
369  makeBoxControl(int_marker);
370 
371  InteractiveMarkerControl control;
372 
373  control.orientation.w = 1;
374  control.orientation.x = 0;
375  control.orientation.y = 1;
376  control.orientation.z = 0;
377  control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
378  int_marker.controls.push_back(control);
379  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
380  int_marker.controls.push_back(control);
381 
382  server->insert(int_marker);
383  server->setCallback(int_marker.name, &processFeedback);
384 }
385 // %EndTag(Quadrocopter)%
386 
387 // %Tag(ChessPiece)%
388 void makeChessPieceMarker( )
389 {
390  InteractiveMarker int_marker;
391  int_marker.header.frame_id = "/base_link";
392  int_marker.pose.position.y = -3.0 * marker_pos++;;
393  int_marker.scale = 1;
394 
395  int_marker.name = "chess_piece";
396  int_marker.description = "Chess Piece\n(2D Move + Alignment)";
397 
398  InteractiveMarkerControl control;
399 
400  control.orientation.w = 1;
401  control.orientation.x = 0;
402  control.orientation.y = 1;
403  control.orientation.z = 0;
404  control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
405  int_marker.controls.push_back(control);
406 
407  // make a box which also moves in the plane
408  control.markers.push_back( makeBox(int_marker) );
409  control.always_visible = true;
410  int_marker.controls.push_back(control);
411 
412  // we want to use our special callback function
413  server->insert(int_marker);
414  server->setCallback(int_marker.name, &processFeedback);
415 
416  // set different callback for POSE_UPDATE feedback
417  server->setCallback(int_marker.name, &alignMarker, visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE );
418 }
419 // %EndTag(ChessPiece)%
420 
421 // %Tag(PanTilt)%
422 void makePanTiltMarker( )
423 {
424  InteractiveMarker int_marker;
425  int_marker.header.frame_id = "/atc/ptu/base";
426  int_marker.pose.position.x = 0;
427  int_marker.pose.position.y = 0;
428  int_marker.pose.position.z = 0;
429  int_marker.scale = 0.5;
430 
431  int_marker.name = "/marker";
432  int_marker.description = " Pan/Tilt control";
433 
434  makeBoxControl(int_marker);
435 
436  InteractiveMarkerControl control;
437 
438  control.orientation.w = 1;
439  control.orientation.x = 0;
440  control.orientation.y = 1;
441  control.orientation.z = 0;
442  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
443  control.orientation_mode = InteractiveMarkerControl::FIXED;
444  int_marker.controls.push_back(control);
445 
446  control.orientation.w = 1;
447  control.orientation.x = 0;
448  control.orientation.y = 0;
449  control.orientation.z = 1;
450  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
451  control.orientation_mode = InteractiveMarkerControl::INHERIT;
452  int_marker.controls.push_back(control);
453 
454  server->insert(int_marker);
455  server->setCallback(int_marker.name, &processFeedback);
456 }
457 // %EndTag(PanTilt)%
458 
459 // %Tag(Menu)%
460 void makeMenuMarker()
461 {
462  InteractiveMarker int_marker;
463  int_marker.header.frame_id = "/atc/ptu/base";
464  int_marker.pose.position.y = -3.0 * marker_pos++;;
465  int_marker.scale = 1;
466 
467  int_marker.name = "context_menu";
468  int_marker.description = "Context Menu\n(Right Click)";
469 
470  InteractiveMarkerControl control;
471 
472  // make one control using default visuals
473  control.interaction_mode = InteractiveMarkerControl::MENU;
474  control.description="Options";
475  control.name = "menu_only_control";
476  int_marker.controls.push_back(control);
477 
478  // make one control showing a box
479  Marker marker = makeBox( int_marker );
480  control.markers.push_back( marker );
481  control.always_visible = true;
482  int_marker.controls.push_back(control);
483 
484  server->insert(int_marker);
485  server->setCallback(int_marker.name, &processFeedback);
486  menu_handler.apply( *server, int_marker.name );
487 }
488 // %EndTag(Menu)%
489 
490 // %Tag(Moving)%
491 void makeMovingMarker()
492 {
493  InteractiveMarker int_marker;
494  int_marker.header.frame_id = "/moving_frame";
495  int_marker.pose.position.y = -3.0 * marker_pos++;;
496  int_marker.scale = 1;
497 
498  int_marker.name = "moving";
499  int_marker.description = "Marker Attached to a\nMoving Frame";
500 
501  InteractiveMarkerControl control;
502 
503  control.orientation.w = 1;
504  control.orientation.x = 1;
505  control.orientation.y = 0;
506  control.orientation.z = 0;
507  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
508  int_marker.controls.push_back(control);
509 
510  control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
511  control.always_visible = true;
512  control.markers.push_back( makeBox(int_marker) );
513  int_marker.controls.push_back(control);
514 
515  server->insert(int_marker);
516  server->setCallback(int_marker.name, &processFeedback);
517 }
518 // %EndTag(Moving)%
519 
520 // %Tag(main)%
521 int main(int argc, char** argv)
522 {
523  ros::init(argc, argv, "ptu_interactive_control_node");
524  ros::NodeHandle n;
525 
526  //declare the publisher
527  pub = n.advertise<sensor_msgs::JointState>("/ptu_cmd", 1);
528 
529 
530  // create a timer to update the published transforms
531  //ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
532 
533  server.reset( new interactive_markers::InteractiveMarkerServer("direct/im","",false) );
534 
535  ros::Duration(0.1).sleep();
536 
537 
538  menu_handler.insert( "First Entry", &processFeedback );
539  menu_handler.insert( "Second Entry", &processFeedback );
540  interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Submenu" );
541  menu_handler.insert( sub_menu_handle, "First Entry", &processFeedback );
542  menu_handler.insert( sub_menu_handle, "Second Entry", &processFeedback );
543 
544  //make6DofMarker( false );
545  //make6DofMarker( true );
546  //makeRandomDofMarker( );
547  //makeViewFacingMarker( );
548  //makeQuadrocopterMarker( );
549  //makeChessPieceMarker( );
550  makePanTiltMarker( );
551  //makeMenuMarker( );
552  //makeMovingMarker( );
553 
554  server->applyChanges();
555 
556  ros::spin();
557 
558  server.reset();
559 }
560 // %EndTag(main)%
561 
562 
563 
makeQuadrocopterMarker
void makeQuadrocopterMarker()
Definition: ptu_interactive_control.cpp:359
makePanTiltMarker
void makePanTiltMarker()
Definition: ptu_interactive_control.cpp:422
main
int main(int argc, char **argv)
Definition: ptu_interactive_control.cpp:521
makeViewFacingMarker
void makeViewFacingMarker()
Definition: ptu_interactive_control.cpp:320
frameCallback
void frameCallback(const ros::TimerEvent &)
Definition: ptu_interactive_control.cpp:56
makeRandomDofMarker
void makeRandomDofMarker()
Definition: ptu_interactive_control.cpp:287
pub
ros::Publisher pub
Definition: ptu_interactive_control.cpp:52
processFeedback
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: ptu_interactive_control.cpp:79
makeMenuMarker
void makeMenuMarker()
Definition: ptu_interactive_control.cpp:460
alignMarker
void alignMarker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: ptu_interactive_control.cpp:160
marker_pos
float marker_pos
Definition: ptu_interactive_control.cpp:50
rand
double rand(double min, double max)
Definition: ptu_interactive_control.cpp:182
makeChessPieceMarker
void makeChessPieceMarker()
Definition: ptu_interactive_control.cpp:388
makeBoxControl
InteractiveMarkerControl & makeBoxControl(InteractiveMarker &msg)
Definition: ptu_interactive_control.cpp:205
saveMarker
void saveMarker(InteractiveMarker int_marker)
Definition: ptu_interactive_control.cpp:216
makeMovingMarker
void makeMovingMarker()
Definition: ptu_interactive_control.cpp:491
makeBox
Marker makeBox(InteractiveMarker &msg)
Definition: ptu_interactive_control.cpp:189
make6DofMarker
void make6DofMarker(bool fixed)
Definition: ptu_interactive_control.cpp:225
server
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server
Definition: ptu_interactive_control.cpp:49
menu_handler
interactive_markers::MenuHandler menu_handler
Definition: ptu_interactive_control.cpp:51

atlascar_ptu
Author(s): Miguel Oliveira
autogenerated on Mon Mar 2 2015 01:31:26