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::onEnable()
00108 {
00109 subscribe();
00110 }
00111
00112 void InteractiveMarkerDisplay::onDisable()
00113 {
00114 unsubscribe();
00115 }
00116
00117 void InteractiveMarkerDisplay::updateTopic()
00118 {
00119 unsubscribe();
00120
00121 std::string update_topic = marker_update_topic_property_->getTopicStd();
00122
00123 size_t idx = update_topic.find( "/update" );
00124 if ( idx != std::string::npos )
00125 {
00126 topic_ns_ = update_topic.substr( 0, idx );
00127 subscribe();
00128 }
00129 else
00130 {
00131 setStatusStd( StatusProperty::Error, "Topic", "Invalid topic name: " + update_topic );
00132 }
00133
00134 }
00135
00136 void InteractiveMarkerDisplay::subscribe()
00137 {
00138 if ( isEnabled() )
00139 {
00140 im_client_->subscribe(topic_ns_);
00141
00142 std::string feedback_topic = topic_ns_+"/feedback";
00143 feedback_pub_ = update_nh_.advertise<visualization_msgs::InteractiveMarkerFeedback>( feedback_topic, 100, false );
00144 }
00145 }
00146
00147 void InteractiveMarkerDisplay::publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
00148 {
00149 feedback.client_id = client_id_;
00150 feedback_pub_.publish( feedback );
00151 }
00152
00153 void InteractiveMarkerDisplay::onStatusUpdate( StatusProperty::Level level, const std::string& name, const std::string& text )
00154 {
00155 setStatusStd(level,name,text);
00156 }
00157
00158 void InteractiveMarkerDisplay::unsubscribe()
00159 {
00160 if (im_client_)
00161 {
00162 im_client_->shutdown();
00163 }
00164 feedback_pub_.shutdown();
00165 Display::reset();
00166 }
00167
00168 void InteractiveMarkerDisplay::update(float wall_dt, float ros_dt)
00169 {
00170 im_client_->update();
00171
00172 M_StringToStringToIMPtr::iterator server_it;
00173 for ( server_it = interactive_markers_.begin(); server_it != interactive_markers_.end(); server_it++ )
00174 {
00175 M_StringToIMPtr::iterator im_it;
00176 for ( im_it = server_it->second.begin(); im_it != server_it->second.end(); im_it++ )
00177 {
00178 im_it->second->update( wall_dt );
00179 }
00180 }
00181 }
00182
00183 InteractiveMarkerDisplay::M_StringToIMPtr& InteractiveMarkerDisplay::getImMap( std::string server_id )
00184 {
00185 M_StringToStringToIMPtr::iterator im_map_it = interactive_markers_.find( server_id );
00186
00187 if ( im_map_it == interactive_markers_.end() )
00188 {
00189 im_map_it = interactive_markers_.insert( std::make_pair( server_id, M_StringToIMPtr() ) ).first;
00190 }
00191
00192 return im_map_it->second;
00193 }
00194
00195 void InteractiveMarkerDisplay::updateMarkers(
00196 const std::string& server_id,
00197 const std::vector<visualization_msgs::InteractiveMarker>& markers
00198 )
00199 {
00200 M_StringToIMPtr& im_map = getImMap( server_id );
00201
00202 for ( size_t i=0; i<markers.size(); i++ )
00203 {
00204 const visualization_msgs::InteractiveMarker& marker = markers[i];
00205
00206 if ( !validateFloats( marker ) )
00207 {
00208 setStatusStd( StatusProperty::Error, marker.name, "Marker contains invalid floats!" );
00209
00210 continue;
00211 }
00212 ROS_DEBUG("Processing interactive marker '%s'. %d", marker.name.c_str(), (int)marker.controls.size() );
00213
00214 std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker.name );
00215
00216 if ( int_marker_entry == im_map.end() )
00217 {
00218 int_marker_entry = im_map.insert( std::make_pair(marker.name, IMPtr ( new InteractiveMarker(getSceneNode(), context_) ) ) ).first;
00219 connect( int_marker_entry->second.get(),
00220 SIGNAL( userFeedback(visualization_msgs::InteractiveMarkerFeedback&) ),
00221 this,
00222 SLOT( publishFeedback(visualization_msgs::InteractiveMarkerFeedback&) ));
00223 connect( int_marker_entry->second.get(),
00224 SIGNAL( statusUpdate(StatusProperty::Level, const std::string&, const std::string&) ),
00225 this,
00226 SLOT( onStatusUpdate(StatusProperty::Level, const std::string&, const std::string&) ) );
00227 }
00228
00229 if ( int_marker_entry->second->processMessage( marker ) )
00230 {
00231 int_marker_entry->second->setShowAxes( show_axes_property_->getBool() );
00232 int_marker_entry->second->setShowVisualAids( show_visual_aids_property_->getBool() );
00233 int_marker_entry->second->setShowDescription( show_descriptions_property_->getBool() );
00234 }
00235 else
00236 {
00237 unsubscribe();
00238 return;
00239 }
00240 }
00241 }
00242
00243 void InteractiveMarkerDisplay::eraseMarkers(
00244 const std::string& server_id,
00245 const std::vector<std::string>& erases )
00246 {
00247 M_StringToIMPtr& im_map = getImMap( server_id );
00248
00249 for ( size_t i=0; i<erases.size(); i++ )
00250 {
00251 im_map.erase( erases[i] );
00252 deleteStatusStd( erases[i] );
00253 }
00254 }
00255
00256 void InteractiveMarkerDisplay::updatePoses(
00257 const std::string& server_id,
00258 const std::vector<visualization_msgs::InteractiveMarkerPose>& marker_poses )
00259 {
00260 M_StringToIMPtr& im_map = getImMap( server_id );
00261
00262 for ( size_t i=0; i<marker_poses.size(); i++ )
00263 {
00264 const visualization_msgs::InteractiveMarkerPose& marker_pose = marker_poses[i];
00265
00266 if ( !validateFloats( marker_pose.pose ) )
00267 {
00268 setStatusStd( StatusProperty::Error, marker_pose.name, "Pose message contains invalid floats!" );
00269 return;
00270 }
00271
00272 std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker_pose.name );
00273
00274 if ( int_marker_entry != im_map.end() )
00275 {
00276 int_marker_entry->second->processMessage( marker_pose );
00277 }
00278 else
00279 {
00280 setStatusStd( StatusProperty::Error, marker_pose.name, "Pose received for non-existing marker '" + marker_pose.name );
00281 unsubscribe();
00282 return;
00283 }
00284 }
00285 }
00286
00287 void InteractiveMarkerDisplay::initCb( visualization_msgs::InteractiveMarkerInitConstPtr msg )
00288 {
00289 resetCb( msg->server_id );
00290 updateMarkers( msg->server_id, msg->markers );
00291 }
00292
00293 void InteractiveMarkerDisplay::updateCb( visualization_msgs::InteractiveMarkerUpdateConstPtr msg )
00294 {
00295 updateMarkers( msg->server_id, msg->markers );
00296 updatePoses( msg->server_id, msg->poses );
00297 eraseMarkers( msg->server_id, msg->erases );
00298 }
00299
00300 void InteractiveMarkerDisplay::resetCb( std::string server_id )
00301 {
00302 interactive_markers_.erase( server_id );
00303 deleteStatusStd(server_id);
00304 }
00305
00306 void InteractiveMarkerDisplay::statusCb(
00307 interactive_markers::InteractiveMarkerClient::StatusT status,
00308 const std::string& server_id,
00309 const std::string& msg )
00310 {
00311 setStatusStd( static_cast<StatusProperty::Level>(status), server_id, msg );
00312 }
00313
00314 void InteractiveMarkerDisplay::fixedFrameChanged()
00315 {
00316 if (im_client_)
00317 im_client_->setTargetFrame( fixed_frame_.toStdString() );
00318 reset();
00319 }
00320
00321 void InteractiveMarkerDisplay::reset()
00322 {
00323 Display::reset();
00324 unsubscribe();
00325 subscribe();
00326 }
00327
00328 void InteractiveMarkerDisplay::updateShowDescriptions()
00329 {
00330 bool show = show_descriptions_property_->getBool();
00331
00332 M_StringToStringToIMPtr::iterator server_it;
00333 for ( server_it = interactive_markers_.begin(); server_it != interactive_markers_.end(); server_it++ )
00334 {
00335 M_StringToIMPtr::iterator im_it;
00336 for ( im_it = server_it->second.begin(); im_it != server_it->second.end(); im_it++ )
00337 {
00338 im_it->second->setShowDescription( show );
00339 }
00340 }
00341 }
00342
00343 void InteractiveMarkerDisplay::updateShowAxes()
00344 {
00345 bool show = show_axes_property_->getBool();
00346
00347 M_StringToStringToIMPtr::iterator server_it;
00348 for ( server_it = interactive_markers_.begin(); server_it != interactive_markers_.end(); server_it++ )
00349 {
00350 M_StringToIMPtr::iterator im_it;
00351 for ( im_it = server_it->second.begin(); im_it != server_it->second.end(); im_it++ )
00352 {
00353 im_it->second->setShowAxes( show );
00354 }
00355 }
00356 }
00357
00358 void InteractiveMarkerDisplay::updateShowVisualAids()
00359 {
00360 bool show = show_visual_aids_property_->getBool();
00361
00362 M_StringToStringToIMPtr::iterator server_it;
00363 for ( server_it = interactive_markers_.begin(); server_it != interactive_markers_.end(); server_it++ )
00364 {
00365 M_StringToIMPtr::iterator im_it;
00366 for ( im_it = server_it->second.begin(); im_it != server_it->second.end(); im_it++ )
00367 {
00368 im_it->second->setShowVisualAids( show );
00369 }
00370 }
00371 }
00372
00373 void InteractiveMarkerDisplay::updateEnableTransparency()
00374 {
00375
00376 unsubscribe();
00377 im_client_->setEnableAutocompleteTransparency( enable_transparency_property_->getBool() );
00378 subscribe();
00379 }
00380
00381 }
00382
00383 #include <pluginlib/class_list_macros.h>
00384 PLUGINLIB_EXPORT_CLASS( rviz::InteractiveMarkerDisplay, rviz::Display )