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 <OGRE/OgreSceneNode.h>
00031 #include <OGRE/OgreSceneManager.h>
00032
00033 #include <tf/transform_listener.h>
00034
00035 #include "rviz/frame_manager.h"
00036 #include "rviz/properties/bool_property.h"
00037 #include "rviz/properties/ros_topic_property.h"
00038 #include "rviz/selection/selection_manager.h"
00039 #include "rviz/validate_floats.h"
00040 #include "rviz/display_context.h"
00041
00042 #include "rviz/default_plugin/interactive_marker_display.h"
00043
00044 namespace rviz
00045 {
00046
00048 bool validateFloats(const visualization_msgs::InteractiveMarker& msg)
00049 {
00050 bool valid = true;
00051 valid = valid && validateFloats(msg.pose);
00052 valid = valid && validateFloats(msg.scale);
00053 for ( unsigned c=0; c<msg.controls.size(); c++)
00054 {
00055 valid = valid && validateFloats( msg.controls[c].orientation );
00056 for ( unsigned m=0; m<msg.controls[c].markers.size(); m++ )
00057 {
00058 valid = valid && validateFloats(msg.controls[c].markers[m].pose);
00059 valid = valid && validateFloats(msg.controls[c].markers[m].scale);
00060 valid = valid && validateFloats(msg.controls[c].markers[m].color);
00061 valid = valid && validateFloats(msg.controls[c].markers[m].points);
00062 }
00063 }
00064 return valid;
00065 }
00067
00068
00069
00070 InteractiveMarkerDisplay::InteractiveMarkerDisplay()
00071 : Display()
00072 , im_client_( this )
00073 {
00074 marker_update_topic_property_ = new RosTopicProperty( "Update Topic", "",
00075 ros::message_traits::datatype<visualization_msgs::InteractiveMarkerUpdate>(),
00076 "visualization_msgs::InteractiveMarkerUpdate topic to subscribe to.",
00077 this, SLOT( updateTopic() ));
00078
00079 show_descriptions_property_ = new BoolProperty( "Show Descriptions", true,
00080 "Whether or not to show the descriptions of each Interactive Marker.",
00081 this, SLOT( updateShowDescriptions() ));
00082
00083 show_axes_property_ = new BoolProperty( "Show Axes", false,
00084 "Whether or not to show the axes of each Interactive Marker.",
00085 this, SLOT( updateShowAxes() ));
00086 }
00087
00088 void InteractiveMarkerDisplay::onInitialize()
00089 {
00090 tf_filter_ = new tf::MessageFilter<visualization_msgs::InteractiveMarker>(*context_->getTFClient(),
00091 fixed_frame_.toStdString(), 100, update_nh_);
00092 tf_pose_filter_ = new tf::MessageFilter<visualization_msgs::InteractiveMarkerPose>(*context_->getTFClient(),
00093 fixed_frame_.toStdString(), 100, update_nh_);
00094 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00095
00096
00097
00098 tf_filter_->registerCallback(boost::bind(&InteractiveMarkerDisplay::tfMarkerSuccess, this, _1));
00099 tf_filter_->registerFailureCallback(boost::bind(&InteractiveMarkerDisplay::tfMarkerFail, this, _1, _2));
00100 tf_pose_filter_->registerCallback(boost::bind(&InteractiveMarkerDisplay::tfPoseSuccess, this, _1));
00101 tf_pose_filter_->registerFailureCallback(boost::bind(&InteractiveMarkerDisplay::tfPoseFail, this, _1, _2));
00102
00103 client_id_ = ros::this_node::getName() + "/" + getNameStd();
00104 }
00105
00106 InteractiveMarkerDisplay::~InteractiveMarkerDisplay()
00107 {
00108 unsubscribe();
00109 scene_manager_->destroySceneNode( scene_node_ );
00110 delete tf_filter_;
00111 delete tf_pose_filter_;
00112 }
00113
00114 void InteractiveMarkerDisplay::onEnable()
00115 {
00116 subscribe();
00117 scene_node_->setVisible( true );
00118 }
00119
00120 void InteractiveMarkerDisplay::onDisable()
00121 {
00122 unsubscribe();
00123 tf_filter_->clear();
00124 tf_pose_filter_->clear();
00125 scene_node_->setVisible( false );
00126 }
00127
00128 void InteractiveMarkerDisplay::updateTopic()
00129 {
00130 unsubscribe();
00131 subscribe();
00132 }
00133
00134 void InteractiveMarkerDisplay::subscribe()
00135 {
00136 if ( !isEnabled() )
00137 {
00138 return;
00139 }
00140
00141 marker_update_sub_.shutdown();
00142 num_publishers_ = 0;
00143
00144 try
00145 {
00146 std::string topic = marker_update_topic_property_->getTopicStd();
00147 if( !topic.empty() )
00148 {
00149 ROS_DEBUG( "Subscribing to %s", topic.c_str() );
00150 marker_update_sub_ = update_nh_.subscribe( topic, 100, &InteractiveMarkerClient::processMarkerUpdate, &im_client_ );
00151 }
00152
00153 setStatus( StatusProperty::Ok, "Topic", "OK" );
00154 }
00155 catch( ros::Exception& e )
00156 {
00157 setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00158 }
00159
00160 im_client_.clear();
00161 }
00162
00163 bool InteractiveMarkerDisplay::subscribeToInit()
00164 {
00165 bool result = false;
00166
00167 ROS_DEBUG( "subscribeToInit()" );
00168 try
00169 {
00170 std::string update_topic = marker_update_topic_property_->getTopicStd();
00171 if( !update_topic.empty() )
00172 {
00173 std::string init_topic = update_topic + "_full";
00174 ROS_DEBUG( "Subscribing to %s", init_topic.c_str() );
00175 marker_init_sub_ = update_nh_.subscribe( init_topic, 100, &InteractiveMarkerClient::processMarkerInit, &im_client_ );
00176 result = true;
00177 }
00178
00179 setStatus( StatusProperty::Ok, "InitTopic", "OK" );
00180 }
00181 catch( ros::Exception& e )
00182 {
00183 setStatus( StatusProperty::Error, "InitTopic", QString("Error subscribing: ") + e.what() );
00184 }
00185 return result;
00186 }
00187
00188 void InteractiveMarkerDisplay::clearMarkers()
00189 {
00190 interactive_markers_.clear();
00191 tf_filter_->clear();
00192 tf_pose_filter_->clear();
00193 }
00194
00195 void InteractiveMarkerDisplay::unsubscribe()
00196 {
00197 marker_update_sub_.shutdown();
00198 marker_init_sub_.shutdown();
00199 clearMarkers();
00200 im_client_.unsubscribedFromInit();
00201 }
00202
00203 void InteractiveMarkerDisplay::unsubscribeFromInit()
00204 {
00205 marker_init_sub_.shutdown();
00206 }
00207
00208 void InteractiveMarkerDisplay::processMarkerChanges( const std::vector<visualization_msgs::InteractiveMarker>* markers,
00209 const std::vector<visualization_msgs::InteractiveMarkerPose>* poses,
00210 const std::vector<std::string>* erases )
00211 {
00212 std::set<std::string> names;
00213
00214 if( markers != NULL )
00215 {
00216 std::vector<visualization_msgs::InteractiveMarker>::const_iterator marker_it = markers->begin();
00217 std::vector<visualization_msgs::InteractiveMarker>::const_iterator marker_end = markers->end();
00218 for( ; marker_it != marker_end; ++marker_it )
00219 {
00220 if( !names.insert( marker_it->name ).second )
00221 {
00222 setStatus( StatusProperty::Error, "Marker array",
00223 "The name '" + QString::fromStdString( marker_it->name ) + "' was used multiple times.");
00224 }
00225
00226 visualization_msgs::InteractiveMarker::ConstPtr marker_ptr( new visualization_msgs::InteractiveMarker( *marker_it ));
00227
00228 if ( marker_it->header.stamp == ros::Time(0) )
00229 {
00230
00231 updateMarker( marker_ptr );
00232 context_->queueRender();
00233 }
00234 else
00235 {
00236 ROS_DEBUG("Forwarding %s to tf filter.", marker_it->name.c_str());
00237 tf_filter_->add( marker_ptr );
00238 }
00239 }
00240 }
00241
00242 if( poses != NULL )
00243 {
00244
00245 std::vector<visualization_msgs::InteractiveMarkerPose>::const_iterator pose_it = poses->begin();
00246 std::vector<visualization_msgs::InteractiveMarkerPose>::const_iterator pose_end = poses->end();
00247
00248 for (; pose_it != pose_end; ++pose_it)
00249 {
00250 if ( !names.insert( pose_it->name ).second )
00251 {
00252 setStatusStd( StatusProperty::Error, "Marker array", "The name '" + pose_it->name + "' was used multiple times.");
00253 }
00254
00255 visualization_msgs::InteractiveMarkerPose::ConstPtr pose_ptr(new visualization_msgs::InteractiveMarkerPose(*pose_it));
00256
00257 if ( pose_it->header.stamp == ros::Time(0) )
00258 {
00259 updatePose( pose_ptr );
00260 context_->queueRender();
00261 }
00262 else
00263 {
00264 ROS_DEBUG("Forwarding pose for %s to tf filter.", pose_it->name.c_str());
00265 tf_pose_filter_->add( pose_ptr );
00266 }
00267 }
00268 }
00269
00270 if( erases != NULL )
00271 {
00272
00273 std::vector<std::string>::const_iterator erase_it = erases->begin();
00274 std::vector<std::string>::const_iterator erase_end = erases->end();
00275
00276 for (; erase_it != erase_end; ++erase_it)
00277 {
00278 interactive_markers_.erase( *erase_it );
00279 }
00280 }
00281 }
00282
00283 void InteractiveMarkerDisplay::tfMarkerSuccess( const visualization_msgs::InteractiveMarker::ConstPtr& marker )
00284 {
00285 ROS_DEBUG("Queueing %s", marker->name.c_str());
00286 boost::mutex::scoped_lock lock(queue_mutex_);
00287 marker_queue_.push_back(marker);
00288 context_->queueRender();
00289 }
00290
00291 void InteractiveMarkerDisplay::tfMarkerFail(const visualization_msgs::InteractiveMarker::ConstPtr& marker, tf::FilterFailureReason reason)
00292 {
00293 std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, marker->__connection_header ? (*marker->__connection_header)["callerid"] : "unknown", reason);
00294 setStatusStd( StatusProperty::Error, marker->name, error);
00295 }
00296
00297 void InteractiveMarkerDisplay::tfPoseSuccess(const visualization_msgs::InteractiveMarkerPose::ConstPtr& marker_pose)
00298 {
00299 ROS_DEBUG("Queueing pose for %s", marker_pose->name.c_str());
00300 boost::mutex::scoped_lock lock(queue_mutex_);
00301 pose_queue_.push_back(marker_pose);
00302 context_->queueRender();
00303 }
00304
00305 void InteractiveMarkerDisplay::tfPoseFail(const visualization_msgs::InteractiveMarkerPose::ConstPtr& marker_pose, tf::FilterFailureReason reason)
00306 {
00307 std::string error = context_->getFrameManager()->discoverFailureReason(
00308 marker_pose->header.frame_id, marker_pose->header.stamp,
00309 marker_pose->__connection_header ? (*marker_pose->__connection_header)["callerid"] : "unknown", reason);
00310 setStatusStd( StatusProperty::Error, marker_pose->name, error);
00311 }
00312
00313
00314 void InteractiveMarkerDisplay::update(float wall_dt, float ros_dt)
00315 {
00316
00317 if ( !im_client_.isPublisherListEmpty() )
00318 {
00319
00320 unsigned num_pub = marker_update_sub_.getNumPublishers();
00321 if ( num_pub < num_publishers_ )
00322 {
00323 reset();
00324 }
00325 else
00326 {
00327 num_publishers_ = num_pub;
00328 }
00329
00330 im_client_.flagLateConnections();
00331 }
00332
00333 V_InteractiveMarkerMessage local_marker_queue;
00334 V_InteractiveMarkerPoseMessage local_pose_queue;
00335
00336
00337
00338 {
00339 boost::mutex::scoped_lock lock(queue_mutex_);
00340 local_marker_queue.swap( marker_queue_ );
00341 local_pose_queue.swap( pose_queue_ );
00342 }
00343
00344 if ( !local_marker_queue.empty() )
00345 {
00346 V_InteractiveMarkerMessage::iterator message_it = local_marker_queue.begin();
00347 V_InteractiveMarkerMessage::iterator message_end = local_marker_queue.end();
00348 for ( ; message_it != message_end; ++message_it )
00349 {
00350 updateMarker( *message_it );
00351 }
00352 }
00353
00354 if ( !local_pose_queue.empty() )
00355 {
00356 V_InteractiveMarkerPoseMessage::iterator message_it = local_pose_queue.begin();
00357 V_InteractiveMarkerPoseMessage::iterator message_end = local_pose_queue.end();
00358 for ( ; message_it != message_end; ++message_it )
00359 {
00360 updatePose( *message_it );
00361 }
00362 }
00363
00364 M_StringToInteractiveMarkerPtr::iterator it;
00365 for ( it = interactive_markers_.begin(); it != interactive_markers_.end(); it++ )
00366 {
00367 it->second->update( wall_dt );
00368 }
00369 }
00370
00371 void InteractiveMarkerDisplay::updateMarker( visualization_msgs::InteractiveMarker::ConstPtr& marker )
00372 {
00373 if ( !validateFloats( *marker ) )
00374 {
00375 setStatusStd( StatusProperty::Error, marker->name, "Message contains invalid floats!" );
00376 return;
00377 }
00378 ROS_DEBUG("Processing interactive marker '%s'. %d", marker->name.c_str(), (int)marker->controls.size() );
00379
00380 std::map< std::string, InteractiveMarkerPtr >::iterator int_marker_entry = interactive_markers_.find( marker->name );
00381
00382 std::string topic = marker_update_topic_property_->getTopicStd();
00383
00384 topic = ros::names::clean( topic );
00385 topic = topic.substr( 0, topic.find_last_of( '/' ) );
00386
00387 if ( int_marker_entry == interactive_markers_.end() )
00388 {
00389 int_marker_entry = interactive_markers_.insert( std::make_pair(marker->name, InteractiveMarkerPtr ( new InteractiveMarker(this, context_, topic, client_id_) ) ) ).first;
00390 }
00391
00392 if ( int_marker_entry->second->processMessage( marker ) )
00393 {
00394 int_marker_entry->second->setShowAxes( show_axes_property_->getBool() );
00395 int_marker_entry->second->setShowDescription( show_descriptions_property_->getBool() );
00396 }
00397 }
00398
00399 void InteractiveMarkerDisplay::updatePose( visualization_msgs::InteractiveMarkerPose::ConstPtr& marker_pose )
00400 {
00401 if ( !validateFloats( marker_pose->pose ) )
00402 {
00403 setStatusStd( StatusProperty::Error, marker_pose->name, "Pose message contains invalid floats!" );
00404 return;
00405 }
00406
00407 std::map< std::string, InteractiveMarkerPtr >::iterator int_marker_entry = interactive_markers_.find( marker_pose->name );
00408
00409 if ( int_marker_entry != interactive_markers_.end() )
00410 {
00411 int_marker_entry->second->processMessage( marker_pose );
00412 }
00413 }
00414
00415 void InteractiveMarkerDisplay::fixedFrameChanged()
00416 {
00417 tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00418 tf_pose_filter_->setTargetFrame( fixed_frame_.toStdString() );
00419 reset();
00420 }
00421
00422 void InteractiveMarkerDisplay::reset()
00423 {
00424 ROS_DEBUG("reset");
00425 Display::reset();
00426 unsubscribe();
00427 subscribe();
00428 }
00429
00430 void InteractiveMarkerDisplay::updateShowDescriptions()
00431 {
00432 bool show = show_descriptions_property_->getBool();
00433
00434 M_StringToInteractiveMarkerPtr::iterator it;
00435 for ( it = interactive_markers_.begin(); it != interactive_markers_.end(); it++ )
00436 {
00437 it->second->setShowDescription( show );
00438 }
00439 }
00440
00441 void InteractiveMarkerDisplay::updateShowAxes()
00442 {
00443 bool show = show_axes_property_->getBool();
00444
00445 M_StringToInteractiveMarkerPtr::iterator it;
00446 for ( it = interactive_markers_.begin(); it != interactive_markers_.end(); it++ )
00447 {
00448 it->second->setShowAxes( show );
00449 }
00450 }
00451
00452 void InteractiveMarkerDisplay::setStatusOk(const std::string& name, const std::string& text)
00453 {
00454 setStatusStd( StatusProperty::Ok, name, text );
00455 }
00456
00457 void InteractiveMarkerDisplay::setStatusWarn(const std::string& name, const std::string& text)
00458 {
00459 setStatusStd( StatusProperty::Warn, name, text );
00460 }
00461
00462 void InteractiveMarkerDisplay::setStatusError(const std::string& name, const std::string& text)
00463 {
00464 setStatusStd( StatusProperty::Error, name, text );
00465 }
00466
00467 }
00468
00469 #include <pluginlib/class_list_macros.h>
00470 PLUGINLIB_DECLARE_CLASS( rviz, InteractiveMarker, rviz::InteractiveMarkerDisplay, rviz::Display )