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 <OGRE/OgreSceneNode.h>
00035 #include <OGRE/OgreSceneManager.h>
00036 #include <OGRE/OgreMaterialManager.h>
00037 #include <OGRE/OgreResourceGroupManager.h>
00038 #include <OGRE/OgreSubEntity.h>
00039 #include <OGRE/OgreMath.h>
00040
00041 #include <ros/ros.h>
00042 #include <interactive_markers/tools.h>
00043
00044 #include "rviz/frame_manager.h"
00045 #include "rviz/visualization_manager.h"
00046 #include "rviz/selection/selection_manager.h"
00047 #include "rviz/frame_manager.h"
00048 #include "rviz/default_plugin/interactive_marker_display.h"
00049 #include "rviz/render_panel.h"
00050
00051 #include "interactive_markers/integer_action.h"
00052 #include "interactive_marker.h"
00053
00054 namespace rviz
00055 {
00056
00057 InteractiveMarker::InteractiveMarker( InteractiveMarkerDisplay *owner, VisualizationManager *vis_manager, std::string topic_ns, std::string client_id ) :
00058 owner_(owner)
00059 , vis_manager_(vis_manager)
00060 , pose_changed_(false)
00061 , time_since_last_feedback_(0)
00062 , dragging_(false)
00063 , pose_update_requested_(false)
00064 , heart_beat_t_(0)
00065 , topic_ns_(topic_ns)
00066 , client_id_(client_id)
00067 {
00068 ros::NodeHandle nh;
00069 std::string feedback_topic = topic_ns+"/feedback";
00070 feedback_pub_ = nh.advertise<visualization_msgs::InteractiveMarkerFeedback>( feedback_topic, 100, false );
00071
00072 reference_node_ = vis_manager->getSceneManager()->getRootSceneNode()->createChildSceneNode();
00073
00074 axes_node_ = reference_node_->createChildSceneNode();
00075 axes_ = new Axes( vis_manager->getSceneManager(), axes_node_, 1, 0.05 );
00076 }
00077
00078 InteractiveMarker::~InteractiveMarker()
00079 {
00080 delete axes_;
00081 vis_manager_->getSceneManager()->destroySceneNode( axes_node_ );
00082 vis_manager_->getSceneManager()->destroySceneNode( reference_node_ );
00083 }
00084
00085 void InteractiveMarker::hideVisible()
00086 {
00087 M_ControlPtr::iterator it;
00088 for( it = controls_.begin(); it != controls_.end(); it++ )
00089 {
00090 it->second->hideVisible();
00091 }
00092 }
00093
00094 void InteractiveMarker::restoreVisible()
00095 {
00096 M_ControlPtr::iterator it;
00097 for( it = controls_.begin(); it != controls_.end(); it++ )
00098 {
00099 it->second->restoreVisible();
00100 }
00101 }
00102
00103 void InteractiveMarker::processMessage( visualization_msgs::InteractiveMarkerPoseConstPtr message )
00104 {
00105 boost::recursive_mutex::scoped_lock lock(mutex_);
00106 Ogre::Vector3 position( message->pose.position.x, message->pose.position.y, message->pose.position.z );
00107 Ogre::Quaternion orientation( message->pose.orientation.w, message->pose.orientation.x,
00108 message->pose.orientation.y, message->pose.orientation.z );
00109
00110 if ( orientation.w == 0 && orientation.x == 0 && orientation.y == 0 && orientation.z == 0 )
00111 {
00112 orientation.w = 1;
00113 }
00114
00115 reference_time_ = message->header.stamp;
00116 reference_frame_ = message->header.frame_id;
00117 frame_locked_ = (message->header.stamp == ros::Time(0));
00118
00119 requestPoseUpdate( position, orientation );
00120 vis_manager_->queueRender();
00121 }
00122
00123 bool InteractiveMarker::processMessage( visualization_msgs::InteractiveMarkerConstPtr message )
00124 {
00125 boost::recursive_mutex::scoped_lock lock(mutex_);
00126
00127 visualization_msgs::InteractiveMarker auto_message = *message;
00128 interactive_markers::autoComplete( auto_message );
00129
00130
00131
00132 name_ = auto_message.name;
00133 description_ = auto_message.description;
00134
00135 if ( auto_message.controls.size() == 0 )
00136 {
00137 owner_->setStatus( status_levels::Ok, name_, "Marker empty.");
00138 return false;
00139 }
00140
00141 scale_ = auto_message.scale;
00142
00143 reference_frame_ = auto_message.header.frame_id;
00144 reference_time_ = auto_message.header.stamp;
00145 frame_locked_ = (auto_message.header.stamp == ros::Time(0));
00146
00147 position_ = Ogre::Vector3(
00148 auto_message.pose.position.x,
00149 auto_message.pose.position.y,
00150 auto_message.pose.position.z );
00151
00152 orientation_ = Ogre::Quaternion(
00153 auto_message.pose.orientation.w,
00154 auto_message.pose.orientation.x,
00155 auto_message.pose.orientation.y,
00156 auto_message.pose.orientation.z );
00157
00158 pose_changed_ =false;
00159 time_since_last_feedback_ = 0;
00160
00161
00162 axes_->setPosition(position_);
00163 axes_->setOrientation(orientation_);
00164 axes_->set( scale_, scale_*0.05 );
00165
00166 updateReferencePose();
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183 std::set<std::string> old_names_to_delete;
00184 M_ControlPtr::const_iterator ci;
00185 for( ci = controls_.begin(); ci != controls_.end(); ci++ )
00186 {
00187 old_names_to_delete.insert( (*ci).first );
00188 }
00189
00190
00191 for ( unsigned i = 0; i < auto_message.controls.size(); i++ )
00192 {
00193 visualization_msgs::InteractiveMarkerControl& control_message = auto_message.controls[ i ];
00194 M_ControlPtr::iterator search_iter = controls_.find( control_message.name );
00195 InteractiveMarkerControlPtr control;
00196
00197
00198 if( search_iter != controls_.end() )
00199 {
00200
00201 control = (*search_iter).second;
00202 }
00203 else
00204 {
00205
00206 control = boost::make_shared<InteractiveMarkerControl>( vis_manager_, reference_node_, this );
00207 controls_[ control_message.name ] = control;
00208 }
00209
00210 control->processMessage( control_message );
00211
00212
00213 old_names_to_delete.erase( control_message.name );
00214 }
00215
00216
00217 std::set<std::string>::iterator si;
00218 for( si = old_names_to_delete.begin(); si != old_names_to_delete.end(); si++ )
00219 {
00220
00221 controls_.erase( *si );
00222 }
00223
00224 description_control_ =
00225 boost::make_shared<InteractiveMarkerControl>( vis_manager_,
00226 reference_node_, this );
00227
00228 description_control_->processMessage( interactive_markers::makeTitle( auto_message ));
00229
00230
00231 menu_entries_.clear();
00232 menu_.reset();
00233 if ( message->menu_entries.size() > 0 )
00234 {
00235 menu_.reset( new QMenu() );
00236 top_level_menu_ids_.clear();
00237
00238
00239
00240 for ( unsigned m=0; m < message->menu_entries.size(); m++ )
00241 {
00242 const visualization_msgs::MenuEntry& entry = message->menu_entries[ m ];
00243 MenuNode node;
00244 node.entry = entry;
00245 menu_entries_[ entry.id ] = node;
00246 if( entry.parent_id == 0 )
00247 {
00248 top_level_menu_ids_.push_back( entry.id );
00249 }
00250 else
00251 {
00252
00253 std::map< uint32_t, MenuNode >::iterator parent_it = menu_entries_.find( entry.parent_id );
00254 if( parent_it == menu_entries_.end() ) {
00255 ROS_ERROR("interactive marker menu entry %u found before its parent id %u. Ignoring.", entry.id, entry.parent_id);
00256 }
00257 else
00258 {
00259 (*parent_it).second.child_ids.push_back( entry.id );
00260 }
00261 }
00262 }
00263 populateMenu( menu_.get(), top_level_menu_ids_ );
00264 }
00265
00266 owner_->setStatus( status_levels::Ok, name_, "OK");
00267 return true;
00268 }
00269
00270
00271
00272
00273 void InteractiveMarker::populateMenu( QMenu* menu, std::vector<uint32_t>& ids )
00274 {
00275 for( size_t id_index = 0; id_index < ids.size(); id_index++ )
00276 {
00277 uint32_t id = ids[ id_index ];
00278 std::map< uint32_t, MenuNode >::iterator node_it = menu_entries_.find( id );
00279 ROS_ASSERT_MSG(node_it != menu_entries_.end(), "interactive marker menu entry %u not found during populateMenu().", id);
00280 MenuNode node = (*node_it).second;
00281
00282 if ( node.child_ids.empty() )
00283 {
00284 IntegerAction* action = new IntegerAction( makeMenuString( node.entry.title ),
00285 menu,
00286 (int) node.entry.id );
00287 connect( action, SIGNAL( triggered( int )), this, SLOT( handleMenuSelect( int )));
00288 menu->addAction( action );
00289 }
00290 else
00291 {
00292
00293 QMenu* sub_menu = menu->addMenu( makeMenuString( node.entry.title ));
00294 populateMenu( sub_menu, node.child_ids );
00295 }
00296 }
00297 }
00298
00299 QString InteractiveMarker::makeMenuString( const std::string &entry )
00300 {
00301 QString menu_entry;
00302 if ( entry.find( "[x]" ) == 0 )
00303 {
00304 menu_entry = QChar( 0x2611 ) + QString::fromStdString( entry.substr( 3 ) );
00305 }
00306 else if ( entry.find( "[ ]" ) == 0 )
00307 {
00308 menu_entry = QChar( 0x2610 ) + QString::fromStdString( entry.substr( 3 ) );
00309 }
00310 else
00311 {
00312 menu_entry = QChar( 0x3000 ) + QString::fromStdString( entry );
00313 }
00314 return menu_entry;
00315 }
00316
00317 void InteractiveMarker::updateReferencePose()
00318 {
00319 boost::recursive_mutex::scoped_lock lock(mutex_);
00320 Ogre::Vector3 reference_position;
00321 Ogre::Quaternion reference_orientation;
00322
00323
00324
00325 if ( frame_locked_ )
00326 {
00327 std::string fixed_frame = FrameManager::instance()->getFixedFrame();
00328 if ( reference_frame_ == fixed_frame )
00329 {
00330
00331 reference_time_ = ros::Time::now();
00332 }
00333 else
00334 {
00335 std::string error;
00336 int retval = FrameManager::instance()->getTFClient()->getLatestCommonTime(
00337 reference_frame_, fixed_frame, reference_time_, &error );
00338 if ( retval != tf::NO_ERROR )
00339 {
00340 std::ostringstream s;
00341 s <<"Error getting time of latest transform between " << reference_frame_
00342 << " and " << fixed_frame << ": " << error << " (error code: " << retval << ")";
00343 owner_->setStatus( status_levels::Error, name_, s.str() );
00344 reference_node_->setVisible( false );
00345 return;
00346 }
00347 }
00348 }
00349
00350 if (!FrameManager::instance()->getTransform( reference_frame_, reference_time_,
00351 reference_position, reference_orientation ))
00352 {
00353 std::string error;
00354 FrameManager::instance()->transformHasProblems(reference_frame_, reference_time_, error);
00355 owner_->setStatus( status_levels::Error, name_, error);
00356 reference_node_->setVisible( false );
00357 return;
00358 }
00359
00360 reference_node_->setPosition( reference_position );
00361 reference_node_->setOrientation( reference_orientation );
00362 reference_node_->setVisible( true, false );
00363
00364 vis_manager_->queueRender();
00365 }
00366
00367 void InteractiveMarker::update(float wall_dt)
00368 {
00369 boost::recursive_mutex::scoped_lock lock(mutex_);
00370 time_since_last_feedback_ += wall_dt;
00371 if ( frame_locked_ )
00372 {
00373 updateReferencePose();
00374 }
00375
00376 M_ControlPtr::iterator it;
00377 for ( it = controls_.begin(); it != controls_.end(); it++ )
00378 {
00379 (*it).second->update();
00380 }
00381 if( description_control_ )
00382 {
00383 description_control_->update();
00384 }
00385
00386 if ( dragging_ )
00387 {
00388 if ( pose_changed_ )
00389 {
00390 publishPose();
00391 }
00392 else if ( time_since_last_feedback_ > 0.25 )
00393 {
00394
00395 visualization_msgs::InteractiveMarkerFeedback feedback;
00396 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::KEEP_ALIVE;
00397 publishFeedback( feedback );
00398 }
00399 }
00400 }
00401
00402 void InteractiveMarker::publishPose()
00403 {
00404 boost::recursive_mutex::scoped_lock lock(mutex_);
00405 visualization_msgs::InteractiveMarkerFeedback feedback;
00406 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
00407 feedback.control_name = last_control_name_;
00408 publishFeedback( feedback );
00409 pose_changed_ = false;
00410 }
00411
00412 void InteractiveMarker::requestPoseUpdate( Ogre::Vector3 position, Ogre::Quaternion orientation )
00413 {
00414 boost::recursive_mutex::scoped_lock lock(mutex_);
00415 if ( dragging_ )
00416 {
00417 pose_update_requested_ = true;
00418 requested_position_ = position;
00419 requested_orientation_ = orientation;
00420 }
00421 else
00422 {
00423 updateReferencePose();
00424 setPose( position, orientation, "" );
00425 }
00426 }
00427
00428 void InteractiveMarker::setPose( Ogre::Vector3 position, Ogre::Quaternion orientation, const std::string &control_name )
00429 {
00430 boost::recursive_mutex::scoped_lock lock(mutex_);
00431 position_ = position;
00432 orientation_ = orientation;
00433 pose_changed_ = true;
00434 last_control_name_ = control_name;
00435
00436 axes_->setPosition(position_);
00437 axes_->setOrientation(orientation_);
00438
00439 M_ControlPtr::iterator it;
00440 for ( it = controls_.begin(); it != controls_.end(); it++ )
00441 {
00442 (*it).second->interactiveMarkerPoseChanged( position_, orientation_ );
00443 }
00444 if( description_control_ )
00445 {
00446 description_control_->interactiveMarkerPoseChanged( position_, orientation_ );
00447 }
00448 }
00449
00450 void InteractiveMarker::setShowDescription( bool show )
00451 {
00452 boost::recursive_mutex::scoped_lock lock(mutex_);
00453 if ( description_control_.get() )
00454 {
00455 description_control_->setVisible( show );
00456 }
00457 }
00458
00459 void InteractiveMarker::setShowAxes( bool show )
00460 {
00461 boost::recursive_mutex::scoped_lock lock(mutex_);
00462 axes_node_->setVisible( show );
00463 }
00464
00465 void InteractiveMarker::translate( Ogre::Vector3 delta_position, const std::string &control_name )
00466 {
00467 boost::recursive_mutex::scoped_lock lock(mutex_);
00468 setPose( position_+delta_position, orientation_, control_name );
00469 }
00470
00471 void InteractiveMarker::rotate( Ogre::Quaternion delta_orientation, const std::string &control_name )
00472 {
00473 boost::recursive_mutex::scoped_lock lock(mutex_);
00474 setPose( position_, delta_orientation * orientation_, control_name );
00475 }
00476
00477 void InteractiveMarker::startDragging()
00478 {
00479 boost::recursive_mutex::scoped_lock lock(mutex_);
00480 dragging_ = true;
00481 pose_changed_ = false;
00482 }
00483
00484 void InteractiveMarker::stopDragging()
00485 {
00486 boost::recursive_mutex::scoped_lock lock(mutex_);
00487 if ( pose_changed_ )
00488 {
00489 publishPose();
00490 }
00491 dragging_ = false;
00492 if ( pose_update_requested_ )
00493 {
00494 updateReferencePose();
00495 setPose( requested_position_, requested_orientation_, "" );
00496 pose_update_requested_ = false;
00497 }
00498 }
00499
00500 bool InteractiveMarker::handleMouseEvent(ViewportMouseEvent& event, const std::string &control_name)
00501 {
00502 boost::recursive_mutex::scoped_lock lock(mutex_);
00503
00504 if( event.acting_button == Qt::LeftButton )
00505 {
00506 Ogre::Vector3 point_rel_world;
00507 bool got_3D_point =
00508 vis_manager_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, point_rel_world );
00509
00510 visualization_msgs::InteractiveMarkerFeedback feedback;
00511 feedback.event_type = (event.type == QEvent::MouseButtonPress ?
00512 (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN :
00513 (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP);
00514
00515 feedback.control_name = control_name;
00516 feedback.marker_name = name_;
00517 publishFeedback( feedback, got_3D_point, point_rel_world );
00518 }
00519
00520 if( menu_.get() )
00521 {
00522
00523
00524
00525 if( event.right() )
00526 {
00527 return true;
00528 }
00529 if( event.rightUp() )
00530 {
00531
00532 got_3d_point_for_menu_ =
00533 vis_manager_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, three_d_point_for_menu_ );
00534
00535 event.panel->showContextMenu( menu_ );
00536
00537 last_control_name_ = control_name;
00538 return true;
00539 }
00540 }
00541
00542 return false;
00543 }
00544
00545
00546 void InteractiveMarker::handleMenuSelect( int menu_item_id )
00547 {
00548 boost::recursive_mutex::scoped_lock lock(mutex_);
00549
00550 std::map< uint32_t, MenuNode >::iterator it = menu_entries_.find( menu_item_id );
00551
00552 if ( it != menu_entries_.end() )
00553 {
00554 visualization_msgs::MenuEntry& entry = it->second.entry;
00555
00556 std::string command = entry.command;
00557 uint8_t command_type = entry.command_type;
00558
00559 if ( command_type == visualization_msgs::MenuEntry::FEEDBACK )
00560 {
00561 visualization_msgs::InteractiveMarkerFeedback feedback;
00562 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT;
00563 feedback.menu_entry_id = entry.id;
00564 feedback.control_name = last_control_name_;
00565 publishFeedback( feedback, got_3d_point_for_menu_, three_d_point_for_menu_ );
00566 }
00567 else if ( command_type == visualization_msgs::MenuEntry::ROSRUN )
00568 {
00569 std::string sys_cmd = "rosrun " + command;
00570 ROS_INFO_STREAM( "Running system command: " << sys_cmd );
00571 sys_thread_ = boost::shared_ptr<boost::thread>( new boost::thread( boost::bind( &system, sys_cmd.c_str() ) ) );
00572
00573 }
00574 else if ( command_type == visualization_msgs::MenuEntry::ROSLAUNCH )
00575 {
00576 std::string sys_cmd = "roslaunch " + command;
00577 ROS_INFO_STREAM( "Running system command: " << sys_cmd );
00578 sys_thread_ = boost::shared_ptr<boost::thread>( new boost::thread( boost::bind( &system, sys_cmd.c_str() ) ) );
00579
00580 }
00581 }
00582 }
00583
00584
00585 void InteractiveMarker::publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback,
00586 bool mouse_point_valid,
00587 const Ogre::Vector3& mouse_point_rel_world )
00588 {
00589 boost::recursive_mutex::scoped_lock lock(mutex_);
00590
00591 feedback.client_id = client_id_;
00592 feedback.marker_name = name_;
00593
00594 if ( frame_locked_ )
00595 {
00596 feedback.header.frame_id = reference_frame_;
00597 feedback.header.stamp = reference_time_;
00598 feedback.pose.position.x = position_.x;
00599 feedback.pose.position.y = position_.y;
00600 feedback.pose.position.z = position_.z;
00601 feedback.pose.orientation.x = orientation_.x;
00602 feedback.pose.orientation.y = orientation_.y;
00603 feedback.pose.orientation.z = orientation_.z;
00604 feedback.pose.orientation.w = orientation_.w;
00605
00606 feedback.mouse_point_valid = mouse_point_valid;
00607 if( mouse_point_valid )
00608 {
00609 Ogre::Vector3 mouse_rel_reference = reference_node_->convertWorldToLocalPosition( mouse_point_rel_world );
00610 feedback.mouse_point.x = mouse_rel_reference.x;
00611 feedback.mouse_point.y = mouse_rel_reference.y;
00612 feedback.mouse_point.z = mouse_rel_reference.z;
00613 }
00614 }
00615 else
00616 {
00617 feedback.header.frame_id = vis_manager_->getFixedFrame();
00618 feedback.header.stamp = ros::Time::now();
00619
00620 Ogre::Vector3 world_position = reference_node_->convertLocalToWorldPosition( position_ );
00621 Ogre::Quaternion world_orientation = reference_node_->convertLocalToWorldOrientation( orientation_ );
00622
00623 feedback.pose.position.x = world_position.x;
00624 feedback.pose.position.y = world_position.y;
00625 feedback.pose.position.z = world_position.z;
00626 feedback.pose.orientation.x = world_orientation.x;
00627 feedback.pose.orientation.y = world_orientation.y;
00628 feedback.pose.orientation.z = world_orientation.z;
00629 feedback.pose.orientation.w = world_orientation.w;
00630
00631 feedback.mouse_point_valid = mouse_point_valid;
00632 feedback.mouse_point.x = mouse_point_rel_world.x;
00633 feedback.mouse_point.y = mouse_point_rel_world.y;
00634 feedback.mouse_point.z = mouse_point_rel_world.z;
00635 }
00636
00637 feedback_pub_.publish( feedback );
00638
00639 time_since_last_feedback_ = 0;
00640 }
00641
00642 }