interactive_marker.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 #include <boost/make_shared.hpp>
31 
32 #include <QMenu>
33 
34 #include <OgreSceneNode.h>
35 #include <OgreSceneManager.h>
36 #include <OgreMaterialManager.h>
37 #include <OgreResourceGroupManager.h>
38 #include <OgreSubEntity.h>
39 #include <OgreMath.h>
40 #include <OgreRenderWindow.h>
41 
42 #include <ros/ros.h>
44 
45 #include "rviz/frame_manager.h"
46 #include "rviz/display_context.h"
48 #include "rviz/frame_manager.h"
49 #include "rviz/render_panel.h"
50 #include "rviz/geometry.h"
52 
55 
56 namespace rviz
57 {
58 
59 InteractiveMarker::InteractiveMarker( Ogre::SceneNode* scene_node, DisplayContext* context ) :
60  context_(context)
61 , pose_changed_(false)
62 , time_since_last_feedback_(0)
63 , dragging_(false)
64 , pose_update_requested_(false)
65 , heart_beat_t_(0)
66 , show_visual_aids_(false)
67 {
68  reference_node_ = scene_node->createChildSceneNode();
69  axes_ = new Axes( context->getSceneManager(), reference_node_, 1, 0.05 );
70 }
71 
73 {
74  delete axes_;
75  context_->getSceneManager()->destroySceneNode( reference_node_ );
76 }
77 
78 void InteractiveMarker::processMessage( const visualization_msgs::InteractiveMarkerPose& message )
79 {
80  boost::recursive_mutex::scoped_lock lock(mutex_);
81  Ogre::Vector3 position( message.pose.position.x, message.pose.position.y, message.pose.position.z );
82  Ogre::Quaternion orientation( message.pose.orientation.w, message.pose.orientation.x,
83  message.pose.orientation.y, message.pose.orientation.z );
84 
85  if ( orientation.w == 0 && orientation.x == 0 && orientation.y == 0 && orientation.z == 0 )
86  {
87  orientation.w = 1;
88  }
89 
90  reference_time_ = message.header.stamp;
91  reference_frame_ = message.header.frame_id;
92  frame_locked_ = (message.header.stamp == ros::Time(0));
93 
94  requestPoseUpdate( position, orientation );
96 }
97 
98 bool InteractiveMarker::processMessage( const visualization_msgs::InteractiveMarker& message )
99 {
100  boost::recursive_mutex::scoped_lock lock(mutex_);
101 
102  // copy values
103  name_ = message.name;
104  description_ = message.description;
105 
106  if ( message.controls.size() == 0 )
107  {
108  Q_EMIT statusUpdate( StatusProperty::Ok, name_, "Marker empty.");
109  return false;
110  }
111 
112  scale_ = message.scale;
113 
114  reference_frame_ = message.header.frame_id;
115  reference_time_ = message.header.stamp;
116  frame_locked_ = (message.header.stamp == ros::Time(0));
117 
118  position_ = Ogre::Vector3(
119  message.pose.position.x,
120  message.pose.position.y,
121  message.pose.position.z );
122 
123  normalizeQuaternion(message.pose.orientation, orientation_);
124 
125  pose_changed_ =false;
127 
128  // setup axes
131  axes_->set( scale_, scale_*0.05 );
132 
133  has_menu_ = message.menu_entries.size() > 0;
134 
136 
137  // Instead of just erasing all the old controls and making new ones
138  // here, we want to preserve as much as possible from the old ones,
139  // so that we don't lose the drag action in progress if a control is
140  // being dragged when this update comes in.
141  //
142  // Controls are stored in a map from control name to control
143  // pointer, so we loop over the incoming control messages looking
144  // for names we already know about. When we find them, we just call
145  // the control's processMessage() function to update it. When we
146  // don't find them, we create a new Control. We also keep track of
147  // which control names we used to have but which are not present in
148  // the incoming message, which we use to delete the unwanted
149  // controls.
150 
151  // Make set of old-names called old-names-to-delete.
152  std::set<std::string> old_names_to_delete;
153  M_ControlPtr::const_iterator ci;
154  for( ci = controls_.begin(); ci != controls_.end(); ci++ )
155  {
156  old_names_to_delete.insert( (*ci).first );
157  }
158 
159  // Loop over new array:
160  for ( unsigned i = 0; i < message.controls.size(); i++ )
161  {
162  const visualization_msgs::InteractiveMarkerControl& control_message = message.controls[ i ];
163  M_ControlPtr::iterator search_iter = controls_.find( control_message.name );
165 
166  // If message->name in map,
167  if( search_iter != controls_.end() )
168  {
169  // Use existing control
170  control = (*search_iter).second;
171  }
172  else
173  {
174  // Else make new control
175  control = boost::make_shared<InteractiveMarkerControl>( context_, reference_node_, this );
176  controls_[ control_message.name ] = control;
177  }
178  // Update the control with the message data
179  control->processMessage( control_message );
180  control->setShowVisualAids( show_visual_aids_ );
181 
182  // Remove message->name from old-names-to-delete
183  old_names_to_delete.erase( control_message.name );
184  }
185 
186  // Loop over old-names-to-delete
187  std::set<std::string>::iterator si;
188  for( si = old_names_to_delete.begin(); si != old_names_to_delete.end(); si++ )
189  {
190  // Remove Control object from map for name-to-delete
191  controls_.erase( *si );
192  }
193 
195  boost::make_shared<InteractiveMarkerControl>( context_,
196  reference_node_, this );
197 
198  description_control_->processMessage( interactive_markers::makeTitle( message ));
199 
200  //create menu
201  menu_entries_.clear();
202  menu_.reset();
203  if ( has_menu_ )
204  {
205  menu_.reset( new QMenu() );
206  top_level_menu_ids_.clear();
207 
208  // Put all menu entries into the menu_entries_ map and create the
209  // tree of menu entry ids.
210  for ( unsigned m=0; m < message.menu_entries.size(); m++ )
211  {
212  const visualization_msgs::MenuEntry& entry = message.menu_entries[ m ];
213  MenuNode node;
214  node.entry = entry;
215  menu_entries_[ entry.id ] = node;
216  if( entry.parent_id == 0 )
217  {
218  top_level_menu_ids_.push_back( entry.id );
219  }
220  else
221  {
222  // Find the parent node and add this entry to the parent's list of children.
223  std::map< uint32_t, MenuNode >::iterator parent_it = menu_entries_.find( entry.parent_id );
224  if( parent_it == menu_entries_.end() ) {
225  ROS_ERROR("interactive marker menu entry %u found before its parent id %u. Ignoring.", entry.id, entry.parent_id);
226  }
227  else
228  {
229  (*parent_it).second.child_ids.push_back( entry.id );
230  }
231  }
232  }
234  }
235 
236  if ( frame_locked_ )
237  {
238  std::ostringstream s;
239  s << "Locked to frame " << reference_frame_;
240  Q_EMIT statusUpdate( StatusProperty::Ok, name_, s.str() );
241  }
242  else
243  {
244  Q_EMIT statusUpdate( StatusProperty::Ok, name_, "Position is fixed." );
245  }
246  return true;
247 }
248 
249 // Recursively append menu and submenu entries to menu, based on a
250 // vector of menu entry id numbers describing the menu entries at the
251 // current level.
252 void InteractiveMarker::populateMenu( QMenu* menu, std::vector<uint32_t>& ids )
253 {
254  for( size_t id_index = 0; id_index < ids.size(); id_index++ )
255  {
256  uint32_t id = ids[ id_index ];
257  std::map< uint32_t, MenuNode >::iterator node_it = menu_entries_.find( id );
258  ROS_ASSERT_MSG(node_it != menu_entries_.end(), "interactive marker menu entry %u not found during populateMenu().", id);
259  MenuNode node = (*node_it).second;
260 
261  if ( node.child_ids.empty() )
262  {
263  IntegerAction* action = new IntegerAction( makeMenuString( node.entry.title ),
264  menu,
265  (int) node.entry.id );
266  connect( action, SIGNAL( triggered( int )), this, SLOT( handleMenuSelect( int )));
267  menu->addAction( action );
268  }
269  else
270  {
271  // make sub-menu
272  QMenu* sub_menu = menu->addMenu( makeMenuString( node.entry.title ));
273  populateMenu( sub_menu, node.child_ids );
274  }
275  }
276 }
277 
278 QString InteractiveMarker::makeMenuString( const std::string &entry )
279 {
280  QString menu_entry;
281  if ( entry.find( "[x]" ) == 0 )
282  {
283  menu_entry = QChar( 0x2611 ) + QString::fromStdString( entry.substr( 3 ) );
284  }
285  else if ( entry.find( "[ ]" ) == 0 )
286  {
287  menu_entry = QChar( 0x2610 ) + QString::fromStdString( entry.substr( 3 ) );
288  }
289  else
290  {
291  menu_entry = QChar( 0x3000 ) + QString::fromStdString( entry );
292  }
293  return menu_entry;
294 }
295 
296 
298 {
299  boost::recursive_mutex::scoped_lock lock(mutex_);
300  Ogre::Vector3 reference_position;
301  Ogre::Quaternion reference_orientation;
302 
303  // if we're frame-locked, we need to find out what the most recent transformation time
304  // actually is so we send back correct feedback
305  if ( frame_locked_ )
306  {
307  std::string fixed_frame = context_->getFrameManager()->getFixedFrame();
308  if ( reference_frame_ == fixed_frame )
309  {
310  // if the two frames are identical, we don't need to do anything.
311  // This should be ros::Time::now(), but then the computer running
312  // RViz has to be time-synced with the server
314  }
315  else
316  {
317  std::string error;
319  reference_frame_, fixed_frame, reference_time_, &error );
320  if ( retval != tf::NO_ERROR )
321  {
322  std::ostringstream s;
323  s <<"Error getting time of latest transform between " << reference_frame_
324  << " and " << fixed_frame << ": " << error << " (error code: " << retval << ")";
325  Q_EMIT statusUpdate( StatusProperty::Error, name_, s.str() );
326  reference_node_->setVisible( false );
327  return;
328  }
329  }
330  }
331 
333  reference_position, reference_orientation ))
334  {
335  std::string error;
337  Q_EMIT statusUpdate( StatusProperty::Error, name_, error);
338  reference_node_->setVisible( false );
339  return;
340  }
341 
342  reference_node_->setPosition( reference_position );
343  reference_node_->setOrientation( reference_orientation );
344  reference_node_->setVisible( true, false );
345 
347 }
348 
349 
350 void InteractiveMarker::update(float wall_dt)
351 {
352  boost::recursive_mutex::scoped_lock lock(mutex_);
353  time_since_last_feedback_ += wall_dt;
354  if ( frame_locked_ )
355  {
357  }
358 
359  M_ControlPtr::iterator it;
360  for ( it = controls_.begin(); it != controls_.end(); it++ )
361  {
362  (*it).second->update();
363  }
365  {
366  description_control_->update();
367  }
368 
369  if ( dragging_ )
370  {
371  if ( pose_changed_ )
372  {
373  publishPose();
374  }
375  else if ( time_since_last_feedback_ > 0.25 )
376  {
377  //send keep-alive so we don't use control over the marker
378  visualization_msgs::InteractiveMarkerFeedback feedback;
379  feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::KEEP_ALIVE;
380  publishFeedback( feedback );
381  }
382  }
383 }
384 
386 {
387  boost::recursive_mutex::scoped_lock lock(mutex_);
388  visualization_msgs::InteractiveMarkerFeedback feedback;
389  feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
390  feedback.control_name = last_control_name_;
391  publishFeedback( feedback );
392  pose_changed_ = false;
393 }
394 
395 void InteractiveMarker::requestPoseUpdate( Ogre::Vector3 position, Ogre::Quaternion orientation )
396 {
397  boost::recursive_mutex::scoped_lock lock(mutex_);
398  if ( dragging_ )
399  {
400  pose_update_requested_ = true;
401  requested_position_ = position;
402  requested_orientation_ = orientation;
403  }
404  else
405  {
407  setPose( position, orientation, "" );
408  }
409 }
410 
411 void InteractiveMarker::setPose( Ogre::Vector3 position, Ogre::Quaternion orientation, const std::string &control_name )
412 {
413  boost::recursive_mutex::scoped_lock lock(mutex_);
414  position_ = position;
415  orientation_ = orientation;
416  pose_changed_ = true;
417  last_control_name_ = control_name;
418 
421 
422  M_ControlPtr::iterator it;
423  for ( it = controls_.begin(); it != controls_.end(); it++ )
424  {
425  (*it).second->interactiveMarkerPoseChanged( position_, orientation_ );
426  }
428  {
429  description_control_->interactiveMarkerPoseChanged( position_, orientation_ );
430  }
431 }
432 
434 {
435  boost::recursive_mutex::scoped_lock lock(mutex_);
436  if ( description_control_.get() )
437  {
438  description_control_->setVisible( show );
439  }
440 }
441 
443 {
444  boost::recursive_mutex::scoped_lock lock(mutex_);
445  axes_->getSceneNode()->setVisible( show );
446 }
447 
449 {
450  boost::recursive_mutex::scoped_lock lock(mutex_);
451  M_ControlPtr::iterator it;
452  for ( it = controls_.begin(); it != controls_.end(); it++ )
453  {
454  (*it).second->setShowVisualAids( show );
455  }
456  show_visual_aids_ = show;
457 }
458 
459 void InteractiveMarker::translate( Ogre::Vector3 delta_position, const std::string &control_name )
460 {
461  boost::recursive_mutex::scoped_lock lock(mutex_);
462  setPose( position_+delta_position, orientation_, control_name );
463 }
464 
465 void InteractiveMarker::rotate( Ogre::Quaternion delta_orientation, const std::string &control_name )
466 {
467  boost::recursive_mutex::scoped_lock lock(mutex_);
468  setPose( position_, delta_orientation * orientation_, control_name );
469 }
470 
472 {
473  boost::recursive_mutex::scoped_lock lock(mutex_);
474  dragging_ = true;
475  pose_changed_ = false;
476 }
477 
479 {
480  boost::recursive_mutex::scoped_lock lock(mutex_);
481  dragging_ = false;
483  {
486  pose_update_requested_ = false;
487  }
488 }
489 
490 bool InteractiveMarker::handle3DCursorEvent(ViewportMouseEvent& event, const Ogre::Vector3& cursor_pos, const Ogre::Quaternion& cursor_rot, const std::string &control_name)
491 {
492  boost::recursive_mutex::scoped_lock lock(mutex_);
493 
494  if( event.acting_button == Qt::LeftButton )
495  {
496  Ogre::Vector3 point_rel_world = cursor_pos;
497  bool got_3D_point = true;
498 
499  visualization_msgs::InteractiveMarkerFeedback feedback;
500  feedback.control_name = control_name;
501  feedback.marker_name = name_;
502 
503  // make sure we've published a last pose update
504  feedback.event_type = ((uint8_t)visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
505  publishFeedback( feedback, got_3D_point, point_rel_world );
506 
507  feedback.event_type = (event.type == QEvent::MouseButtonPress ?
508  (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN :
509  (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP);
510  publishFeedback( feedback, got_3D_point, point_rel_world );
511  }
512 
513  if( !dragging_ && menu_.get() )
514  {
515  // Event.right() will be false during a right-button-up event. We
516  // want to swallow (with the "return true") all other
517  // right-button-related mouse events.
518  if( event.right() )
519  {
520  return true;
521  }
522  if( event.rightUp() && event.buttons_down == Qt::NoButton )
523  {
524  // Save the 3D mouse point to send with the menu feedback, if any.
525  Ogre::Vector3 three_d_point = cursor_pos;
526  bool valid_point = true;
527  Ogre::Vector2 mouse_pos = project3DPointToViewportXY(event.viewport, cursor_pos);
528  QCursor::setPos(event.panel->mapToGlobal(QPoint(mouse_pos.x, mouse_pos.y)));
529  showMenu( event, control_name, three_d_point, valid_point );
530  return true;
531  }
532  }
533 
534  return false;
535 }
536 
537 bool InteractiveMarker::handleMouseEvent(ViewportMouseEvent& event, const std::string &control_name)
538 {
539  boost::recursive_mutex::scoped_lock lock(mutex_);
540 
541  if( event.acting_button == Qt::LeftButton )
542  {
543  Ogre::Vector3 point_rel_world;
544  bool got_3D_point =
545  context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, point_rel_world );
546 
547  visualization_msgs::InteractiveMarkerFeedback feedback;
548  feedback.control_name = control_name;
549  feedback.marker_name = name_;
550 
551  // make sure we've published a last pose update
552  feedback.event_type = ((uint8_t)visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
553  publishFeedback( feedback, got_3D_point, point_rel_world );
554 
555  feedback.event_type = (event.type == QEvent::MouseButtonPress ?
556  (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN :
557  (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP);
558  publishFeedback( feedback, got_3D_point, point_rel_world );
559  }
560 
561  if( !dragging_ && menu_.get() )
562  {
563  // Event.right() will be false during a right-button-up event. We
564  // want to swallow (with the "return true") all other
565  // right-button-related mouse events.
566  if( event.right() )
567  {
568  return true;
569  }
570  if( event.rightUp() && event.buttons_down == Qt::NoButton )
571  {
572  // Save the 3D mouse point to send with the menu feedback, if any.
573  Ogre::Vector3 three_d_point;
574  bool valid_point = context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, three_d_point );
575  showMenu( event, control_name, three_d_point, valid_point );
576  return true;
577  }
578  }
579 
580  return false;
581 }
582 
583 
584 void InteractiveMarker::showMenu( ViewportMouseEvent& event, const std::string &control_name, const Ogre::Vector3 &three_d_point, bool valid_point )
585 {
586  // Save the 3D mouse point to send with the menu feedback, if any.
587  got_3d_point_for_menu_ = valid_point;
588  three_d_point_for_menu_ = three_d_point;
589 
590  event.panel->showContextMenu( menu_ );
591  last_control_name_ = control_name;
592 }
593 
594 
595 void InteractiveMarker::handleMenuSelect( int menu_item_id )
596 {
597  boost::recursive_mutex::scoped_lock lock(mutex_);
598 
599  std::map< uint32_t, MenuNode >::iterator it = menu_entries_.find( menu_item_id );
600 
601  if ( it != menu_entries_.end() )
602  {
603  visualization_msgs::MenuEntry& entry = it->second.entry;
604 
605  std::string command = entry.command;
606  uint8_t command_type = entry.command_type;
607 
608  if ( command_type == visualization_msgs::MenuEntry::FEEDBACK )
609  {
610  visualization_msgs::InteractiveMarkerFeedback feedback;
611  feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT;
612  feedback.menu_entry_id = entry.id;
613  feedback.control_name = last_control_name_;
615  }
616  else if ( command_type == visualization_msgs::MenuEntry::ROSRUN )
617  {
618  std::string sys_cmd = "rosrun " + command;
619  ROS_INFO_STREAM( "Running system command: " << sys_cmd );
620  sys_thread_ = boost::shared_ptr<boost::thread>( new boost::thread( boost::bind( &system, sys_cmd.c_str() ) ) );
621  //system( sys_cmd.c_str() );
622  }
623  else if ( command_type == visualization_msgs::MenuEntry::ROSLAUNCH )
624  {
625  std::string sys_cmd = "roslaunch " + command;
626  ROS_INFO_STREAM( "Running system command: " << sys_cmd );
627  sys_thread_ = boost::shared_ptr<boost::thread>( new boost::thread( boost::bind( &system, sys_cmd.c_str() ) ) );
628  //system( sys_cmd.c_str() );
629  }
630  }
631 }
632 
633 
634 void InteractiveMarker::publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback,
635  bool mouse_point_valid,
636  const Ogre::Vector3& mouse_point_rel_world )
637 {
638  boost::recursive_mutex::scoped_lock lock(mutex_);
639 
640  feedback.marker_name = name_;
641 
642  if ( frame_locked_ )
643  {
644  // frame-locked IMs will return their pose in the same coordinate frame
645  // as they were set up with (the "reference frame").
646  // The transformation's timestamp will be the one used for placing
647  // the reference frame into the fixed frame
648  feedback.header.frame_id = reference_frame_;
649  feedback.header.stamp = reference_time_;
650  feedback.pose.position.x = position_.x;
651  feedback.pose.position.y = position_.y;
652  feedback.pose.position.z = position_.z;
653  feedback.pose.orientation.x = orientation_.x;
654  feedback.pose.orientation.y = orientation_.y;
655  feedback.pose.orientation.z = orientation_.z;
656  feedback.pose.orientation.w = orientation_.w;
657 
658  feedback.mouse_point_valid = mouse_point_valid;
659  if( mouse_point_valid )
660  {
661  Ogre::Vector3 mouse_rel_reference = reference_node_->convertWorldToLocalPosition( mouse_point_rel_world );
662  feedback.mouse_point.x = mouse_rel_reference.x;
663  feedback.mouse_point.y = mouse_rel_reference.y;
664  feedback.mouse_point.z = mouse_rel_reference.z;
665  }
666  }
667  else
668  {
669  // Timestamped IMs will return feedback in RViz's fixed frame
670  feedback.header.frame_id = context_->getFixedFrame().toStdString();
671  // This should be ros::Time::now(), but then the computer running
672  // RViz has to be time-synced with the server
673  feedback.header.stamp = ros::Time();
674 
675  Ogre::Vector3 world_position = reference_node_->convertLocalToWorldPosition( position_ );
676  Ogre::Quaternion world_orientation = reference_node_->convertLocalToWorldOrientation( orientation_ );
677 
678  feedback.pose.position.x = world_position.x;
679  feedback.pose.position.y = world_position.y;
680  feedback.pose.position.z = world_position.z;
681  feedback.pose.orientation.x = world_orientation.x;
682  feedback.pose.orientation.y = world_orientation.y;
683  feedback.pose.orientation.z = world_orientation.z;
684  feedback.pose.orientation.w = world_orientation.w;
685 
686  feedback.mouse_point_valid = mouse_point_valid;
687  feedback.mouse_point.x = mouse_point_rel_world.x;
688  feedback.mouse_point.y = mouse_point_rel_world.y;
689  feedback.mouse_point.z = mouse_point_rel_world.z;
690  }
691 
692  Q_EMIT userFeedback( feedback );
693 
695 }
696 
697 } // end namespace rviz
boost::recursive_mutex mutex_
void translate(Ogre::Vector3 delta_position, const std::string &control_name)
std::map< uint32_t, MenuNode > menu_entries_
bool transformHasProblems(const std::string &frame, ros::Time time, std::string &error)
Check to see if a transform is known between a given frame and the fixed frame.
void setShowDescription(bool show)
void rotate(Ogre::Quaternion delta_orientation, const std::string &control_name)
XmlRpcServer s
int getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time &time, std::string *error_string) const
visualization_msgs::InteractiveMarkerControl makeTitle(const visualization_msgs::InteractiveMarker &msg)
Ogre::Vector3 three_d_point_for_menu_
void populateMenu(QMenu *menu, std::vector< uint32_t > &ids)
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
Definition: axes.cpp:84
Ogre::Vector2 project3DPointToViewportXY(const Ogre::Viewport *view, const Ogre::Vector3 &pos)
Given a viewport and a 3D position in world coordinates, project that point into the view plane...
Definition: geometry.cpp:75
QString makeMenuString(const std::string &entry)
void publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback, bool mouse_point_valid=false, const Ogre::Vector3 &mouse_point_rel_world=Ogre::Vector3(0, 0, 0))
Pure-virtual base class for objects which give Display subclasses context in which to work...
void handleMenuSelect(int menu_item_id)
ROSLIB_DECL std::string command(const std::string &cmd)
bool handle3DCursorEvent(ViewportMouseEvent &event, const Ogre::Vector3 &cursor_pos, const Ogre::Quaternion &cursor_rot, const std::string &control_name)
bool processMessage(const visualization_msgs::InteractiveMarker &message)
#define ROS_ASSERT_MSG(cond,...)
InteractiveMarker(Ogre::SceneNode *scene_node, DisplayContext *context)
Ogre::Quaternion requested_orientation_
InteractiveMarkerControlPtr description_control_
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
virtual SelectionManager * getSelectionManager() const =0
Return a pointer to the SelectionManager.
void showMenu(ViewportMouseEvent &event, const std::string &control_name, const Ogre::Vector3 &three_d_point, bool valid_point)
tf::TransformListener * getTFClient()
Return the tf::TransformListener used to receive transform data.
const std::string & getFixedFrame()
Return the current fixed frame name.
float normalizeQuaternion(float &w, float &x, float &y, float &z)
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
Definition: axes.h:58
action
visualization_msgs::MenuEntry entry
void userFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
std::vector< uint32_t > top_level_menu_ids_
boost::shared_ptr< boost::thread > sys_thread_
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
virtual QString getFixedFrame() const =0
Return the fixed frame name.
#define ROS_INFO_STREAM(args)
void setPose(Ogre::Vector3 position, Ogre::Quaternion orientation, const std::string &control_name)
void statusUpdate(StatusProperty::Level level, const std::string &name, const std::string &text)
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
void set(float length, float radius)
Set the parameters on this object.
Definition: axes.cpp:69
boost::shared_ptr< QMenu > menu_
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
Definition: axes.cpp:89
Ogre::Quaternion orientation_
void update(float wall_dt)
void requestPoseUpdate(Ogre::Vector3 position, Ogre::Quaternion orientation)
bool handleMouseEvent(ViewportMouseEvent &event, const std::string &control_name)
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
Definition: axes.h:90
bool get3DPoint(Ogre::Viewport *viewport, const int x, const int y, Ogre::Vector3 &result_point)
#define ROS_ERROR(...)
Ogre::SceneNode * reference_node_
NO_ERROR


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:41