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