interactive_marker.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, 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 #include <boost/make_shared.hpp>
00031 
00032 #include <QMenu>
00033 
00034 #include <OgreSceneNode.h>
00035 #include <OgreSceneManager.h>
00036 #include <OgreMaterialManager.h>
00037 #include <OgreResourceGroupManager.h>
00038 #include <OgreSubEntity.h>
00039 #include <OgreMath.h>
00040 #include <OgreRenderWindow.h>
00041 
00042 #include <ros/ros.h>
00043 #include <interactive_markers/tools.h>
00044 
00045 #include "rviz/frame_manager.h"
00046 #include "rviz/display_context.h"
00047 #include "rviz/selection/selection_manager.h"
00048 #include "rviz/frame_manager.h"
00049 #include "rviz/render_panel.h"
00050 #include "rviz/geometry.h"
00051 
00052 #include "rviz/default_plugin/interactive_markers/integer_action.h"
00053 #include "rviz/default_plugin/interactive_markers/interactive_marker.h"
00054 
00055 namespace rviz
00056 {
00057 
00058 InteractiveMarker::InteractiveMarker( Ogre::SceneNode* scene_node, DisplayContext* context ) :
00059   context_(context)
00060 , pose_changed_(false)
00061 , time_since_last_feedback_(0)
00062 , dragging_(false)
00063 , pose_update_requested_(false)
00064 , heart_beat_t_(0)
00065 , show_visual_aids_(false)
00066 {
00067   reference_node_ = scene_node->createChildSceneNode();
00068   axes_ = new Axes( context->getSceneManager(), reference_node_, 1, 0.05 );
00069 }
00070 
00071 InteractiveMarker::~InteractiveMarker()
00072 {
00073   delete axes_;
00074   context_->getSceneManager()->destroySceneNode( reference_node_ );
00075 }
00076 
00077 void InteractiveMarker::processMessage( const visualization_msgs::InteractiveMarkerPose& message )
00078 {
00079   boost::recursive_mutex::scoped_lock lock(mutex_);
00080   Ogre::Vector3 position( message.pose.position.x, message.pose.position.y, message.pose.position.z );
00081   Ogre::Quaternion orientation( message.pose.orientation.w, message.pose.orientation.x,
00082       message.pose.orientation.y, message.pose.orientation.z );
00083 
00084   if ( orientation.w == 0 && orientation.x == 0 && orientation.y == 0 && orientation.z == 0 )
00085   {
00086     orientation.w = 1;
00087   }
00088 
00089   reference_time_ = message.header.stamp;
00090   reference_frame_ = message.header.frame_id;
00091   frame_locked_ = (message.header.stamp == ros::Time(0));
00092 
00093   requestPoseUpdate( position, orientation );
00094   context_->queueRender();
00095 }
00096 
00097 bool InteractiveMarker::processMessage( const visualization_msgs::InteractiveMarker& message )
00098 {
00099   boost::recursive_mutex::scoped_lock lock(mutex_);
00100 
00101   // copy values
00102   name_ = message.name;
00103   description_ = message.description;
00104 
00105   if ( message.controls.size() == 0 )
00106   {
00107     Q_EMIT statusUpdate( StatusProperty::Ok, name_, "Marker empty.");
00108     return false;
00109   }
00110 
00111   scale_ = message.scale;
00112 
00113   reference_frame_ = message.header.frame_id;
00114   reference_time_ = message.header.stamp;
00115   frame_locked_ = (message.header.stamp == ros::Time(0));
00116 
00117   position_ = Ogre::Vector3(
00118       message.pose.position.x,
00119       message.pose.position.y,
00120       message.pose.position.z );
00121 
00122   orientation_ = Ogre::Quaternion(
00123       message.pose.orientation.w,
00124       message.pose.orientation.x,
00125       message.pose.orientation.y,
00126       message.pose.orientation.z );
00127 
00128   pose_changed_ =false;
00129   time_since_last_feedback_ = 0;
00130 
00131   // setup axes
00132   axes_->setPosition(position_);
00133   axes_->setOrientation(orientation_);
00134   axes_->set( scale_, scale_*0.05 );
00135 
00136   has_menu_ = message.menu_entries.size() > 0;
00137 
00138   updateReferencePose();
00139 
00140   // Instead of just erasing all the old controls and making new ones
00141   // here, we want to preserve as much as possible from the old ones,
00142   // so that we don't lose the drag action in progress if a control is
00143   // being dragged when this update comes in.
00144   //
00145   // Controls are stored in a map from control name to control
00146   // pointer, so we loop over the incoming control messages looking
00147   // for names we already know about.  When we find them, we just call
00148   // the control's processMessage() function to update it.  When we
00149   // don't find them, we create a new Control.  We also keep track of
00150   // which control names we used to have but which are not present in
00151   // the incoming message, which we use to delete the unwanted
00152   // controls.
00153 
00154   // Make set of old-names called old-names-to-delete.
00155   std::set<std::string> old_names_to_delete;
00156   M_ControlPtr::const_iterator ci;
00157   for( ci = controls_.begin(); ci != controls_.end(); ci++ )
00158   {
00159     old_names_to_delete.insert( (*ci).first );
00160   }
00161 
00162   // Loop over new array:
00163   for ( unsigned i = 0; i < message.controls.size(); i++ )
00164   {
00165     const visualization_msgs::InteractiveMarkerControl& control_message = message.controls[ i ];
00166     M_ControlPtr::iterator search_iter = controls_.find( control_message.name );
00167     InteractiveMarkerControlPtr control;
00168 
00169     // If message->name in map,
00170     if( search_iter != controls_.end() )
00171     {    
00172       // Use existing control
00173       control = (*search_iter).second;
00174     }
00175     else
00176     {
00177       // Else make new control
00178       control = boost::make_shared<InteractiveMarkerControl>( context_, reference_node_, this );
00179       controls_[ control_message.name ] = control;
00180     }
00181     // Update the control with the message data
00182     control->processMessage( control_message );
00183     control->setShowVisualAids( show_visual_aids_ );
00184 
00185     // Remove message->name from old-names-to-delete
00186     old_names_to_delete.erase( control_message.name );
00187   }
00188 
00189   // Loop over old-names-to-delete
00190   std::set<std::string>::iterator si;
00191   for( si = old_names_to_delete.begin(); si != old_names_to_delete.end(); si++ )
00192   {
00193     // Remove Control object from map for name-to-delete
00194     controls_.erase( *si );
00195   }
00196 
00197   description_control_ =
00198     boost::make_shared<InteractiveMarkerControl>( context_,
00199                                                   reference_node_, this );
00200 
00201   description_control_->processMessage( interactive_markers::makeTitle( message ));
00202 
00203   //create menu
00204   menu_entries_.clear();
00205   menu_.reset();
00206   if ( has_menu_ )
00207   {
00208     menu_.reset( new QMenu() );
00209     top_level_menu_ids_.clear();
00210 
00211     // Put all menu entries into the menu_entries_ map and create the
00212     // tree of menu entry ids.
00213     for ( unsigned m=0; m < message.menu_entries.size(); m++ )
00214     {
00215       const visualization_msgs::MenuEntry& entry = message.menu_entries[ m ];
00216       MenuNode node;
00217       node.entry = entry;
00218       menu_entries_[ entry.id ] = node;
00219       if( entry.parent_id == 0 )
00220       {
00221         top_level_menu_ids_.push_back( entry.id );
00222       }
00223       else
00224       {
00225         // Find the parent node and add this entry to the parent's list of children.
00226         std::map< uint32_t, MenuNode >::iterator parent_it = menu_entries_.find( entry.parent_id );
00227         if( parent_it == menu_entries_.end() ) {
00228           ROS_ERROR("interactive marker menu entry %u found before its parent id %u.  Ignoring.", entry.id, entry.parent_id);
00229         }
00230         else
00231         {
00232           (*parent_it).second.child_ids.push_back( entry.id );
00233         }
00234       }
00235     }
00236     populateMenu( menu_.get(), top_level_menu_ids_ );
00237   }
00238 
00239   if ( frame_locked_ )
00240   {
00241     std::ostringstream s;
00242     s << "Locked to frame " << reference_frame_;
00243     Q_EMIT statusUpdate( StatusProperty::Ok, name_, s.str() );
00244   }
00245   else
00246   {
00247     Q_EMIT statusUpdate( StatusProperty::Ok, name_, "Position is fixed." );
00248   }
00249   return true;
00250 }
00251 
00252 // Recursively append menu and submenu entries to menu, based on a
00253 // vector of menu entry id numbers describing the menu entries at the
00254 // current level.
00255 void InteractiveMarker::populateMenu( QMenu* menu, std::vector<uint32_t>& ids )
00256 {
00257   for( size_t id_index = 0; id_index < ids.size(); id_index++ )
00258   {
00259     uint32_t id = ids[ id_index ];
00260     std::map< uint32_t, MenuNode >::iterator node_it = menu_entries_.find( id );
00261     ROS_ASSERT_MSG(node_it != menu_entries_.end(), "interactive marker menu entry %u not found during populateMenu().", id);
00262     MenuNode node = (*node_it).second;
00263 
00264     if ( node.child_ids.empty() )
00265     {
00266       IntegerAction* action = new IntegerAction( makeMenuString( node.entry.title ),
00267                                                  menu,
00268                                                  (int) node.entry.id );
00269       connect( action, SIGNAL( triggered( int )), this, SLOT( handleMenuSelect( int )));
00270       menu->addAction( action );
00271     }
00272     else
00273     {
00274       // make sub-menu
00275       QMenu* sub_menu = menu->addMenu( makeMenuString( node.entry.title ));
00276       populateMenu( sub_menu, node.child_ids );
00277     }
00278   }
00279 }
00280 
00281 QString InteractiveMarker::makeMenuString( const std::string &entry )
00282 {
00283   QString menu_entry;
00284   if ( entry.find( "[x]" ) == 0 )
00285   {
00286     menu_entry = QChar( 0x2611 ) + QString::fromStdString( entry.substr( 3 ) );
00287   }
00288   else if ( entry.find( "[ ]" ) == 0 )
00289   {
00290     menu_entry = QChar( 0x2610 ) + QString::fromStdString( entry.substr( 3 ) );
00291   }
00292   else
00293   {
00294     menu_entry = QChar( 0x3000 ) + QString::fromStdString( entry );
00295   }
00296   return menu_entry;
00297 }
00298 
00299 
00300 void InteractiveMarker::updateReferencePose()
00301 {
00302   boost::recursive_mutex::scoped_lock lock(mutex_);
00303   Ogre::Vector3 reference_position;
00304   Ogre::Quaternion reference_orientation;
00305 
00306   // if we're frame-locked, we need to find out what the most recent transformation time
00307   // actually is so we send back correct feedback
00308   if ( frame_locked_ )
00309   {
00310     std::string fixed_frame = context_->getFrameManager()->getFixedFrame();
00311     if ( reference_frame_ == fixed_frame )
00312     {
00313       // if the two frames are identical, we don't need to do anything.
00314       // This should be ros::Time::now(), but then the computer running
00315       // RViz has to be time-synced with the server
00316       reference_time_ = ros::Time();
00317     }
00318     else
00319     {
00320       std::string error;
00321       int retval = context_->getFrameManager()->getTFClient()->getLatestCommonTime(
00322           reference_frame_, fixed_frame, reference_time_, &error );
00323       if ( retval != tf::NO_ERROR )
00324       {
00325         std::ostringstream s;
00326         s <<"Error getting time of latest transform between " << reference_frame_
00327             << " and " << fixed_frame << ": " << error << " (error code: " << retval << ")";
00328         Q_EMIT statusUpdate( StatusProperty::Error, name_, s.str() );
00329         reference_node_->setVisible( false );
00330         return;
00331       }
00332     }
00333   }
00334 
00335   if (!context_->getFrameManager()->getTransform( reference_frame_, ros::Time(),
00336       reference_position, reference_orientation ))
00337   {
00338     std::string error;
00339     context_->getFrameManager()->transformHasProblems(reference_frame_, ros::Time(), error);
00340     Q_EMIT statusUpdate( StatusProperty::Error, name_, error);
00341     reference_node_->setVisible( false );
00342     return;
00343   }
00344 
00345   reference_node_->setPosition( reference_position );
00346   reference_node_->setOrientation( reference_orientation );
00347   reference_node_->setVisible( true, false );
00348 
00349   context_->queueRender();
00350 }
00351 
00352 
00353 void InteractiveMarker::update(float wall_dt)
00354 {
00355   boost::recursive_mutex::scoped_lock lock(mutex_);
00356   time_since_last_feedback_ += wall_dt;
00357   if ( frame_locked_ )
00358   {
00359     updateReferencePose();
00360   }
00361 
00362   M_ControlPtr::iterator it;
00363   for ( it = controls_.begin(); it != controls_.end(); it++ )
00364   {
00365     (*it).second->update();
00366   }
00367   if( description_control_ )
00368   {
00369     description_control_->update();
00370   }
00371 
00372   if ( dragging_ )
00373   {
00374     if ( pose_changed_ )
00375     {
00376       publishPose();
00377     }
00378     else if ( time_since_last_feedback_ > 0.25 )
00379     {
00380       //send keep-alive so we don't use control over the marker
00381       visualization_msgs::InteractiveMarkerFeedback feedback;
00382       feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::KEEP_ALIVE;
00383       publishFeedback( feedback );
00384     }
00385   }
00386 }
00387 
00388 void InteractiveMarker::publishPose()
00389 {
00390   boost::recursive_mutex::scoped_lock lock(mutex_);
00391   visualization_msgs::InteractiveMarkerFeedback feedback;
00392   feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
00393   feedback.control_name = last_control_name_;
00394   publishFeedback( feedback );
00395   pose_changed_ = false;
00396 }
00397 
00398 void InteractiveMarker::requestPoseUpdate( Ogre::Vector3 position, Ogre::Quaternion orientation )
00399 {
00400   boost::recursive_mutex::scoped_lock lock(mutex_);
00401   if ( dragging_ )
00402   {
00403     pose_update_requested_ = true;
00404     requested_position_ = position;
00405     requested_orientation_ = orientation;
00406   }
00407   else
00408   {
00409     updateReferencePose();
00410     setPose( position, orientation, "" );
00411   }
00412 }
00413 
00414 void InteractiveMarker::setPose( Ogre::Vector3 position, Ogre::Quaternion orientation, const std::string &control_name )
00415 {
00416   boost::recursive_mutex::scoped_lock lock(mutex_);
00417   position_ = position;
00418   orientation_ = orientation;
00419   pose_changed_ = true;
00420   last_control_name_ = control_name;
00421 
00422   axes_->setPosition(position_);
00423   axes_->setOrientation(orientation_);
00424 
00425   M_ControlPtr::iterator it;
00426   for ( it = controls_.begin(); it != controls_.end(); it++ )
00427   {
00428     (*it).second->interactiveMarkerPoseChanged( position_, orientation_ );
00429   }
00430   if( description_control_ )
00431   {
00432     description_control_->interactiveMarkerPoseChanged( position_, orientation_ );
00433   }
00434 }
00435 
00436 void InteractiveMarker::setShowDescription( bool show )
00437 {
00438   boost::recursive_mutex::scoped_lock lock(mutex_);
00439   if ( description_control_.get() )
00440   {
00441     description_control_->setVisible( show );
00442   }
00443 }
00444 
00445 void InteractiveMarker::setShowAxes( bool show )
00446 {
00447   boost::recursive_mutex::scoped_lock lock(mutex_);
00448   axes_->getSceneNode()->setVisible( show );
00449 }
00450 
00451 void InteractiveMarker::setShowVisualAids( bool show )
00452 {
00453   boost::recursive_mutex::scoped_lock lock(mutex_);
00454   M_ControlPtr::iterator it;
00455   for ( it = controls_.begin(); it != controls_.end(); it++ )
00456   {
00457     (*it).second->setShowVisualAids( show );
00458   }
00459   show_visual_aids_ = show;
00460 }
00461 
00462 void InteractiveMarker::translate( Ogre::Vector3 delta_position, const std::string &control_name )
00463 {
00464   boost::recursive_mutex::scoped_lock lock(mutex_);
00465   setPose( position_+delta_position, orientation_, control_name );
00466 }
00467 
00468 void InteractiveMarker::rotate( Ogre::Quaternion delta_orientation, const std::string &control_name )
00469 {
00470   boost::recursive_mutex::scoped_lock lock(mutex_);
00471   setPose( position_, delta_orientation * orientation_, control_name );
00472 }
00473 
00474 void InteractiveMarker::startDragging()
00475 {
00476   boost::recursive_mutex::scoped_lock lock(mutex_);
00477   dragging_ = true;
00478   pose_changed_ = false;
00479 }
00480 
00481 void InteractiveMarker::stopDragging()
00482 {
00483   boost::recursive_mutex::scoped_lock lock(mutex_);
00484   dragging_ = false;
00485   if ( pose_update_requested_ )
00486   {
00487     updateReferencePose();
00488     setPose( requested_position_, requested_orientation_, "" );
00489     pose_update_requested_ = false;
00490   }
00491 }
00492 
00493 bool InteractiveMarker::handle3DCursorEvent(ViewportMouseEvent& event, const Ogre::Vector3& cursor_pos, const Ogre::Quaternion& cursor_rot, const std::string &control_name)
00494 {
00495   boost::recursive_mutex::scoped_lock lock(mutex_);
00496 
00497   if( event.acting_button == Qt::LeftButton )
00498   {
00499     Ogre::Vector3 point_rel_world = cursor_pos;
00500     bool got_3D_point = true;
00501 
00502     visualization_msgs::InteractiveMarkerFeedback feedback;
00503     feedback.control_name = control_name;
00504     feedback.marker_name = name_;
00505 
00506     // make sure we've published a last pose update
00507     feedback.event_type = ((uint8_t)visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
00508     publishFeedback( feedback, got_3D_point, point_rel_world );
00509 
00510     feedback.event_type = (event.type == QEvent::MouseButtonPress ?
00511                            (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN :
00512                            (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP);
00513     publishFeedback( feedback, got_3D_point, point_rel_world );
00514   }
00515 
00516   if( !dragging_ && menu_.get() )
00517   {
00518     // Event.right() will be false during a right-button-up event.  We
00519     // want to swallow (with the "return true") all other
00520     // right-button-related mouse events.
00521     if( event.right() )
00522     {
00523       return true;
00524     }
00525     if( event.rightUp() && event.buttons_down == Qt::NoButton )
00526     {
00527       // Save the 3D mouse point to send with the menu feedback, if any.
00528       Ogre::Vector3 three_d_point = cursor_pos;
00529       bool valid_point = true;
00530       Ogre::Vector2 mouse_pos = project3DPointToViewportXY(event.viewport, cursor_pos);
00531       QCursor::setPos(event.panel->mapToGlobal(QPoint(mouse_pos.x, mouse_pos.y)));
00532       showMenu( event, control_name, three_d_point, valid_point );
00533       return true;
00534     }
00535   }
00536 
00537   return false;
00538 }
00539 
00540 bool InteractiveMarker::handleMouseEvent(ViewportMouseEvent& event, const std::string &control_name)
00541 {
00542   boost::recursive_mutex::scoped_lock lock(mutex_);
00543 
00544   if( event.acting_button == Qt::LeftButton )
00545   {
00546     Ogre::Vector3 point_rel_world;
00547     bool got_3D_point =
00548       context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, point_rel_world );
00549 
00550     visualization_msgs::InteractiveMarkerFeedback feedback;
00551     feedback.control_name = control_name;
00552     feedback.marker_name = name_;
00553 
00554     // make sure we've published a last pose update
00555     feedback.event_type = ((uint8_t)visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
00556     publishFeedback( feedback, got_3D_point, point_rel_world );
00557 
00558     feedback.event_type = (event.type == QEvent::MouseButtonPress ?
00559                            (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN :
00560                            (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP);
00561     publishFeedback( feedback, got_3D_point, point_rel_world );
00562   }
00563 
00564   if( !dragging_ && menu_.get() )
00565   {
00566     // Event.right() will be false during a right-button-up event.  We
00567     // want to swallow (with the "return true") all other
00568     // right-button-related mouse events.
00569     if( event.right() )
00570     {
00571       return true;
00572     }
00573     if( event.rightUp() && event.buttons_down == Qt::NoButton )
00574     {
00575       // Save the 3D mouse point to send with the menu feedback, if any.
00576       Ogre::Vector3 three_d_point;
00577       bool valid_point = context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, three_d_point );
00578       showMenu( event, control_name, three_d_point, valid_point );
00579       return true;
00580     }
00581   }
00582 
00583   return false;
00584 }
00585 
00586 
00587 void InteractiveMarker::showMenu( ViewportMouseEvent& event, const std::string &control_name, const Ogre::Vector3 &three_d_point, bool valid_point )
00588 {
00589   // Save the 3D mouse point to send with the menu feedback, if any.
00590   got_3d_point_for_menu_ = valid_point;
00591   three_d_point_for_menu_ = three_d_point;
00592 
00593   event.panel->showContextMenu( menu_ );
00594   last_control_name_ = control_name;
00595 }
00596 
00597 
00598 void InteractiveMarker::handleMenuSelect( int menu_item_id )
00599 {
00600   boost::recursive_mutex::scoped_lock lock(mutex_);
00601 
00602   std::map< uint32_t, MenuNode >::iterator it = menu_entries_.find( menu_item_id );
00603 
00604   if ( it != menu_entries_.end() )
00605   {
00606     visualization_msgs::MenuEntry& entry = it->second.entry;
00607 
00608     std::string command = entry.command;
00609     uint8_t command_type = entry.command_type;
00610 
00611     if ( command_type == visualization_msgs::MenuEntry::FEEDBACK )
00612     {
00613       visualization_msgs::InteractiveMarkerFeedback feedback;
00614       feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT;
00615       feedback.menu_entry_id = entry.id;
00616       feedback.control_name = last_control_name_;
00617       publishFeedback( feedback, got_3d_point_for_menu_, three_d_point_for_menu_ );
00618     }
00619     else if ( command_type == visualization_msgs::MenuEntry::ROSRUN )
00620     {
00621       std::string sys_cmd = "rosrun " + command;
00622       ROS_INFO_STREAM( "Running system command: " << sys_cmd );
00623       sys_thread_ = boost::shared_ptr<boost::thread>( new boost::thread( boost::bind( &system, sys_cmd.c_str() ) ) );
00624       //system( sys_cmd.c_str() );
00625     }
00626     else if ( command_type == visualization_msgs::MenuEntry::ROSLAUNCH )
00627     {
00628       std::string sys_cmd = "roslaunch " + command;
00629       ROS_INFO_STREAM( "Running system command: " << sys_cmd );
00630       sys_thread_ = boost::shared_ptr<boost::thread>( new boost::thread( boost::bind( &system, sys_cmd.c_str() ) ) );
00631       //system( sys_cmd.c_str() );
00632     }
00633   }
00634 }
00635 
00636 
00637 void InteractiveMarker::publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback,
00638                                         bool mouse_point_valid,
00639                                         const Ogre::Vector3& mouse_point_rel_world )
00640 {
00641   boost::recursive_mutex::scoped_lock lock(mutex_);
00642 
00643   feedback.marker_name = name_;
00644 
00645   if ( frame_locked_ )
00646   {
00647     // frame-locked IMs will return their pose in the same coordinate frame
00648     // as they were set up with (the "reference frame").
00649     // The transformation's timestamp will be the one used for placing
00650     // the reference frame into the fixed frame
00651     feedback.header.frame_id = reference_frame_;
00652     feedback.header.stamp = reference_time_;
00653     feedback.pose.position.x = position_.x;
00654     feedback.pose.position.y = position_.y;
00655     feedback.pose.position.z = position_.z;
00656     feedback.pose.orientation.x = orientation_.x;
00657     feedback.pose.orientation.y = orientation_.y;
00658     feedback.pose.orientation.z = orientation_.z;
00659     feedback.pose.orientation.w = orientation_.w;
00660 
00661     feedback.mouse_point_valid = mouse_point_valid;
00662     if( mouse_point_valid )
00663     {
00664       Ogre::Vector3 mouse_rel_reference = reference_node_->convertWorldToLocalPosition( mouse_point_rel_world );
00665       feedback.mouse_point.x = mouse_rel_reference.x;
00666       feedback.mouse_point.y = mouse_rel_reference.y;
00667       feedback.mouse_point.z = mouse_rel_reference.z;
00668     }
00669   }
00670   else
00671   {
00672     // Timestamped IMs will return feedback in RViz's fixed frame
00673     feedback.header.frame_id = context_->getFixedFrame().toStdString();
00674     // This should be ros::Time::now(), but then the computer running
00675     // RViz has to be time-synced with the server
00676     feedback.header.stamp = ros::Time();
00677 
00678     Ogre::Vector3 world_position = reference_node_->convertLocalToWorldPosition( position_ );
00679     Ogre::Quaternion world_orientation = reference_node_->convertLocalToWorldOrientation( orientation_ );
00680 
00681     feedback.pose.position.x = world_position.x;
00682     feedback.pose.position.y = world_position.y;
00683     feedback.pose.position.z = world_position.z;
00684     feedback.pose.orientation.x = world_orientation.x;
00685     feedback.pose.orientation.y = world_orientation.y;
00686     feedback.pose.orientation.z = world_orientation.z;
00687     feedback.pose.orientation.w = world_orientation.w;
00688 
00689     feedback.mouse_point_valid = mouse_point_valid;
00690     feedback.mouse_point.x = mouse_point_rel_world.x;
00691     feedback.mouse_point.y = mouse_point_rel_world.y;
00692     feedback.mouse_point.z = mouse_point_rel_world.z;
00693   }
00694 
00695   Q_EMIT userFeedback( feedback );
00696 
00697   time_since_last_feedback_ = 0;
00698 }
00699 
00700 } // end namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:30