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