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 <tf/transform_listener.h>
00031
00032 #include "rviz/frame_manager.h"
00033 #include "rviz/properties/bool_property.h"
00034 #include "rviz/properties/ros_topic_property.h"
00035 #include "rviz/selection/selection_manager.h"
00036 #include "rviz/validate_floats.h"
00037 #include "rviz/display_context.h"
00038
00039 #include "rviz/default_plugin/interactive_marker_display.h"
00040
00041
00042 namespace rviz
00043 {
00044
00046 bool validateFloats(const visualization_msgs::InteractiveMarker& msg)
00047 {
00048 bool valid = true;
00049 valid = valid && validateFloats(msg.pose);
00050 valid = valid && validateFloats(msg.scale);
00051 for ( unsigned c=0; c<msg.controls.size(); c++)
00052 {
00053 valid = valid && validateFloats( msg.controls[c].orientation );
00054 for ( unsigned m=0; m<msg.controls[c].markers.size(); m++ )
00055 {
00056 valid = valid && validateFloats(msg.controls[c].markers[m].pose);
00057 valid = valid && validateFloats(msg.controls[c].markers[m].scale);
00058 valid = valid && validateFloats(msg.controls[c].markers[m].color);
00059 valid = valid && validateFloats(msg.controls[c].markers[m].points);
00060 }
00061 }
00062 return valid;
00063 }
00065
00066
00067
00068 InteractiveMarkerDisplay::InteractiveMarkerDisplay()
00069 : Display()
00070 {
00071 marker_update_topic_property_ = new RosTopicProperty( "Update Topic", "",
00072 ros::message_traits::datatype<visualization_msgs::InteractiveMarkerUpdate>(),
00073 "visualization_msgs::InteractiveMarkerUpdate topic to subscribe to.",
00074 this, SLOT( updateTopic() ));
00075
00076 show_descriptions_property_ = new BoolProperty( "Show Descriptions", true,
00077 "Whether or not to show the descriptions of each Interactive Marker.",
00078 this, SLOT( updateShowDescriptions() ));
00079
00080 show_axes_property_ = new BoolProperty( "Show Axes", false,
00081 "Whether or not to show the axes of each Interactive Marker.",
00082 this, SLOT( updateShowAxes() ));
00083
00084 show_visual_aids_property_ = new BoolProperty( "Show Visual Aids", false,
00085 "Whether or not to show visual helpers while moving/rotating Interactive Markers.",
00086 this, SLOT( updateShowVisualAids() ));
00087 enable_transparency_property_ = new BoolProperty( "Enable Transparency", true,
00088 "Whether or not to allow transparency for auto-completed markers (e.g. rings and arrows).",
00089 this, SLOT( updateEnableTransparency() ));
00090 }
00091
00092 void InteractiveMarkerDisplay::onInitialize()
00093 {
00094 tf::Transformer* tf = context_->getFrameManager()->getTFClient();
00095 im_client_.reset( new interactive_markers::InteractiveMarkerClient( *tf, fixed_frame_.toStdString() ) );
00096
00097 im_client_->setInitCb( boost::bind( &InteractiveMarkerDisplay::initCb, this, _1 ) );
00098 im_client_->setUpdateCb( boost::bind( &InteractiveMarkerDisplay::updateCb, this, _1 ) );
00099 im_client_->setResetCb( boost::bind( &InteractiveMarkerDisplay::resetCb, this, _1 ) );
00100 im_client_->setStatusCb( boost::bind( &InteractiveMarkerDisplay::statusCb, this, _1, _2, _3 ) );
00101
00102 client_id_ = ros::this_node::getName() + "/" + getNameStd();
00103
00104 onEnable();
00105 }
00106
00107 void InteractiveMarkerDisplay::setTopic( const QString &topic, const QString &datatype )
00108 {
00109 marker_update_topic_property_->setString( topic );
00110 }
00111
00112 void InteractiveMarkerDisplay::onEnable()
00113 {
00114 subscribe();
00115 }
00116
00117 void InteractiveMarkerDisplay::onDisable()
00118 {
00119 unsubscribe();
00120 }
00121
00122 void InteractiveMarkerDisplay::updateTopic()
00123 {
00124 unsubscribe();
00125
00126 std::string update_topic = marker_update_topic_property_->getTopicStd();
00127
00128 size_t idx = update_topic.find( "/update" );
00129 if ( idx != std::string::npos )
00130 {
00131 topic_ns_ = update_topic.substr( 0, idx );
00132 subscribe();
00133 }
00134 else
00135 {
00136 setStatusStd( StatusProperty::Error, "Topic", "Invalid topic name: " + update_topic );
00137 }
00138
00139 }
00140
00141 void InteractiveMarkerDisplay::subscribe()
00142 {
00143 if ( isEnabled() )
00144 {
00145 im_client_->subscribe(topic_ns_);
00146
00147 std::string feedback_topic = topic_ns_+"/feedback";
00148 feedback_pub_ = update_nh_.advertise<visualization_msgs::InteractiveMarkerFeedback>( feedback_topic, 100, false );
00149 }
00150 }
00151
00152 void InteractiveMarkerDisplay::publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
00153 {
00154 feedback.client_id = client_id_;
00155 feedback_pub_.publish( feedback );
00156 }
00157
00158 void InteractiveMarkerDisplay::onStatusUpdate( StatusProperty::Level level, const std::string& name, const std::string& text )
00159 {
00160 setStatusStd(level,name,text);
00161 }
00162
00163 void InteractiveMarkerDisplay::unsubscribe()
00164 {
00165 if (im_client_)
00166 {
00167 im_client_->shutdown();
00168 }
00169 feedback_pub_.shutdown();
00170 Display::reset();
00171 }
00172
00173 void InteractiveMarkerDisplay::update(float wall_dt, float ros_dt)
00174 {
00175 im_client_->update();
00176
00177 M_StringToStringToIMPtr::iterator server_it;
00178 for ( server_it = interactive_markers_.begin(); server_it != interactive_markers_.end(); server_it++ )
00179 {
00180 M_StringToIMPtr::iterator im_it;
00181 for ( im_it = server_it->second.begin(); im_it != server_it->second.end(); im_it++ )
00182 {
00183 im_it->second->update( wall_dt );
00184 }
00185 }
00186 }
00187
00188 InteractiveMarkerDisplay::M_StringToIMPtr& InteractiveMarkerDisplay::getImMap( std::string server_id )
00189 {
00190 M_StringToStringToIMPtr::iterator im_map_it = interactive_markers_.find( server_id );
00191
00192 if ( im_map_it == interactive_markers_.end() )
00193 {
00194 im_map_it = interactive_markers_.insert( std::make_pair( server_id, M_StringToIMPtr() ) ).first;
00195 }
00196
00197 return im_map_it->second;
00198 }
00199
00200 void InteractiveMarkerDisplay::updateMarkers(
00201 const std::string& server_id,
00202 const std::vector<visualization_msgs::InteractiveMarker>& markers
00203 )
00204 {
00205 M_StringToIMPtr& im_map = getImMap( server_id );
00206
00207 for ( size_t i=0; i<markers.size(); i++ )
00208 {
00209 const visualization_msgs::InteractiveMarker& marker = markers[i];
00210
00211 if ( !validateFloats( marker ) )
00212 {
00213 setStatusStd( StatusProperty::Error, marker.name, "Marker contains invalid floats!" );
00214
00215 continue;
00216 }
00217 ROS_DEBUG("Processing interactive marker '%s'. %d", marker.name.c_str(), (int)marker.controls.size() );
00218
00219 std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker.name );
00220
00221 if ( int_marker_entry == im_map.end() )
00222 {
00223 int_marker_entry = im_map.insert( std::make_pair(marker.name, IMPtr ( new InteractiveMarker(getSceneNode(), context_) ) ) ).first;
00224 connect( int_marker_entry->second.get(),
00225 SIGNAL( userFeedback(visualization_msgs::InteractiveMarkerFeedback&) ),
00226 this,
00227 SLOT( publishFeedback(visualization_msgs::InteractiveMarkerFeedback&) ));
00228 connect( int_marker_entry->second.get(),
00229 SIGNAL( statusUpdate(StatusProperty::Level, const std::string&, const std::string&) ),
00230 this,
00231 SLOT( onStatusUpdate(StatusProperty::Level, const std::string&, const std::string&) ) );
00232 }
00233
00234 if ( int_marker_entry->second->processMessage( marker ) )
00235 {
00236 int_marker_entry->second->setShowAxes( show_axes_property_->getBool() );
00237 int_marker_entry->second->setShowVisualAids( show_visual_aids_property_->getBool() );
00238 int_marker_entry->second->setShowDescription( show_descriptions_property_->getBool() );
00239 }
00240 else
00241 {
00242 unsubscribe();
00243 return;
00244 }
00245 }
00246 }
00247
00248 void InteractiveMarkerDisplay::eraseMarkers(
00249 const std::string& server_id,
00250 const std::vector<std::string>& erases )
00251 {
00252 M_StringToIMPtr& im_map = getImMap( server_id );
00253
00254 for ( size_t i=0; i<erases.size(); i++ )
00255 {
00256 im_map.erase( erases[i] );
00257 deleteStatusStd( erases[i] );
00258 }
00259 }
00260
00261 void InteractiveMarkerDisplay::updatePoses(
00262 const std::string& server_id,
00263 const std::vector<visualization_msgs::InteractiveMarkerPose>& marker_poses )
00264 {
00265 M_StringToIMPtr& im_map = getImMap( server_id );
00266
00267 for ( size_t i=0; i<marker_poses.size(); i++ )
00268 {
00269 const visualization_msgs::InteractiveMarkerPose& marker_pose = marker_poses[i];
00270
00271 if ( !validateFloats( marker_pose.pose ) )
00272 {
00273 setStatusStd( StatusProperty::Error, marker_pose.name, "Pose message contains invalid floats!" );
00274 return;
00275 }
00276
00277 std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker_pose.name );
00278
00279 if ( int_marker_entry != im_map.end() )
00280 {
00281 int_marker_entry->second->processMessage( marker_pose );
00282 }
00283 else
00284 {
00285 setStatusStd( StatusProperty::Error, marker_pose.name, "Pose received for non-existing marker '" + marker_pose.name );
00286 unsubscribe();
00287 return;
00288 }
00289 }
00290 }
00291
00292 void InteractiveMarkerDisplay::initCb( visualization_msgs::InteractiveMarkerInitConstPtr msg )
00293 {
00294 resetCb( msg->server_id );
00295 updateMarkers( msg->server_id, msg->markers );
00296 }
00297
00298 void InteractiveMarkerDisplay::updateCb( visualization_msgs::InteractiveMarkerUpdateConstPtr msg )
00299 {
00300 updateMarkers( msg->server_id, msg->markers );
00301 updatePoses( msg->server_id, msg->poses );
00302 eraseMarkers( msg->server_id, msg->erases );
00303 }
00304
00305 void InteractiveMarkerDisplay::resetCb( std::string server_id )
00306 {
00307 interactive_markers_.erase( server_id );
00308 deleteStatusStd(server_id);
00309 }
00310
00311 void InteractiveMarkerDisplay::statusCb(
00312 interactive_markers::InteractiveMarkerClient::StatusT status,
00313 const std::string& server_id,
00314 const std::string& msg )
00315 {
00316 setStatusStd( static_cast<StatusProperty::Level>(status), server_id, msg );
00317 }
00318
00319 void InteractiveMarkerDisplay::fixedFrameChanged()
00320 {
00321 if (im_client_)
00322 im_client_->setTargetFrame( fixed_frame_.toStdString() );
00323 reset();
00324 }
00325
00326 void InteractiveMarkerDisplay::reset()
00327 {
00328 Display::reset();
00329 unsubscribe();
00330 subscribe();
00331 }
00332
00333 void InteractiveMarkerDisplay::updateShowDescriptions()
00334 {
00335 bool show = show_descriptions_property_->getBool();
00336
00337 M_StringToStringToIMPtr::iterator server_it;
00338 for ( server_it = interactive_markers_.begin(); server_it != interactive_markers_.end(); server_it++ )
00339 {
00340 M_StringToIMPtr::iterator im_it;
00341 for ( im_it = server_it->second.begin(); im_it != server_it->second.end(); im_it++ )
00342 {
00343 im_it->second->setShowDescription( show );
00344 }
00345 }
00346 }
00347
00348 void InteractiveMarkerDisplay::updateShowAxes()
00349 {
00350 bool show = show_axes_property_->getBool();
00351
00352 M_StringToStringToIMPtr::iterator server_it;
00353 for ( server_it = interactive_markers_.begin(); server_it != interactive_markers_.end(); server_it++ )
00354 {
00355 M_StringToIMPtr::iterator im_it;
00356 for ( im_it = server_it->second.begin(); im_it != server_it->second.end(); im_it++ )
00357 {
00358 im_it->second->setShowAxes( show );
00359 }
00360 }
00361 }
00362
00363 void InteractiveMarkerDisplay::updateShowVisualAids()
00364 {
00365 bool show = show_visual_aids_property_->getBool();
00366
00367 M_StringToStringToIMPtr::iterator server_it;
00368 for ( server_it = interactive_markers_.begin(); server_it != interactive_markers_.end(); server_it++ )
00369 {
00370 M_StringToIMPtr::iterator im_it;
00371 for ( im_it = server_it->second.begin(); im_it != server_it->second.end(); im_it++ )
00372 {
00373 im_it->second->setShowVisualAids( show );
00374 }
00375 }
00376 }
00377
00378 void InteractiveMarkerDisplay::updateEnableTransparency()
00379 {
00380
00381 unsubscribe();
00382 im_client_->setEnableAutocompleteTransparency( enable_transparency_property_->getBool() );
00383 subscribe();
00384 }
00385
00386 }
00387
00388 #include <pluginlib/class_list_macros.h>
00389 PLUGINLIB_EXPORT_CLASS( rviz::InteractiveMarkerDisplay, rviz::Display )