00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
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
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
00170 if( search_iter != controls_.end() )
00171 {
00172
00173 control = (*search_iter).second;
00174 }
00175 else
00176 {
00177
00178 control = boost::make_shared<InteractiveMarkerControl>( context_, reference_node_, this );
00179 controls_[ control_message.name ] = control;
00180 }
00181
00182 control->processMessage( control_message );
00183 control->setShowVisualAids( show_visual_aids_ );
00184
00185
00186 old_names_to_delete.erase( control_message.name );
00187 }
00188
00189
00190 std::set<std::string>::iterator si;
00191 for( si = old_names_to_delete.begin(); si != old_names_to_delete.end(); si++ )
00192 {
00193
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
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
00212
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
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
00253
00254
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
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
00307
00308 if ( frame_locked_ )
00309 {
00310 std::string fixed_frame = context_->getFrameManager()->getFixedFrame();
00311 if ( reference_frame_ == fixed_frame )
00312 {
00313
00314
00315
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
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
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
00519
00520
00521 if( event.right() )
00522 {
00523 return true;
00524 }
00525 if( event.rightUp() && event.buttons_down == Qt::NoButton )
00526 {
00527
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
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
00567
00568
00569 if( event.right() )
00570 {
00571 return true;
00572 }
00573 if( event.rightUp() && event.buttons_down == Qt::NoButton )
00574 {
00575
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
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
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
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
00648
00649
00650
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
00673 feedback.header.frame_id = context_->getFixedFrame().toStdString();
00674
00675
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 }