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 "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/common.h"
00035 #include "rviz/selection/selection_manager.h"
00036 #include "rviz/frame_manager.h"
00037 #include "rviz/validate_floats.h"
00038
00039 #include "markers/shape_marker.h"
00040 #include "markers/arrow_marker.h"
00041 #include "markers/line_list_marker.h"
00042 #include "markers/line_strip_marker.h"
00043 #include "markers/sphere_list_marker.h"
00044 #include "markers/points_marker.h"
00045 #include "markers/text_view_facing_marker.h"
00046 #include "markers/mesh_resource_marker.h"
00047 #include "markers/triangle_list_marker.h"
00048
00049 #include <ogre_tools/arrow.h>
00050 #include <ogre_tools/shape.h>
00051 #include <ogre_tools/billboard_line.h>
00052
00053 #include <tf/transform_listener.h>
00054
00055 #include <OGRE/OgreSceneNode.h>
00056 #include <OGRE/OgreSceneManager.h>
00057
00058 namespace rviz
00059 {
00060
00064
00065 MarkerDisplay::MarkerDisplay( const std::string& name, VisualizationManager* manager )
00066 : Display( name, manager )
00067 , tf_filter_(*manager->getTFClient(), "", 100, update_nh_)
00068 , marker_topic_("visualization_marker")
00069 {
00070 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00071
00072 tf_filter_.connectInput(sub_);
00073 tf_filter_.registerCallback(boost::bind(&MarkerDisplay::incomingMarker, this, _1));
00074 tf_filter_.registerFailureCallback(boost::bind(&MarkerDisplay::failedMarker, this, _1, _2));
00075 }
00076
00077 MarkerDisplay::~MarkerDisplay()
00078 {
00079 unsubscribe();
00080
00081 clearMarkers();
00082 }
00083
00084 MarkerBasePtr MarkerDisplay::getMarker(MarkerID id)
00085 {
00086 M_IDToMarker::iterator it = markers_.find(id);
00087 if (it != markers_.end())
00088 {
00089 return it->second;
00090 }
00091
00092 return MarkerBasePtr();
00093 }
00094
00095 void MarkerDisplay::clearMarkers()
00096 {
00097 markers_.clear();
00098 markers_with_expiration_.clear();
00099 frame_locked_markers_.clear();
00100 tf_filter_.clear();
00101
00102 if (property_manager_)
00103 {
00104 M_Namespace::iterator it = namespaces_.begin();
00105 M_Namespace::iterator end = namespaces_.end();
00106 for (; it != end; ++it)
00107 {
00108 property_manager_->deleteProperty(it->second.prop.lock());
00109 }
00110 }
00111
00112 namespaces_.clear();
00113 }
00114
00115 void MarkerDisplay::onEnable()
00116 {
00117 subscribe();
00118
00119 scene_node_->setVisible( true );
00120 }
00121
00122 void MarkerDisplay::onDisable()
00123 {
00124 unsubscribe();
00125 tf_filter_.clear();
00126
00127 clearMarkers();
00128
00129 scene_node_->setVisible( false );
00130 }
00131
00132 void MarkerDisplay::setMarkerTopic(const std::string& topic)
00133 {
00134 unsubscribe();
00135 marker_topic_ = topic;
00136 subscribe();
00137
00138 propertyChanged(marker_topic_property_);
00139 }
00140
00141 void MarkerDisplay::subscribe()
00142 {
00143 if ( !isEnabled() )
00144 {
00145 return;
00146 }
00147
00148 if (!marker_topic_.empty())
00149 {
00150 array_sub_.shutdown();
00151 sub_.unsubscribe();
00152
00153 try
00154 {
00155 sub_.subscribe(update_nh_, marker_topic_, 1000);
00156 array_sub_ = update_nh_.subscribe(marker_topic_ + "_array", 1000, &MarkerDisplay::incomingMarkerArray, this);
00157 setStatus(status_levels::Ok, "Topic", "OK");
00158 }
00159 catch (ros::Exception& e)
00160 {
00161 setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00162 }
00163 }
00164 }
00165
00166 void MarkerDisplay::unsubscribe()
00167 {
00168 sub_.unsubscribe();
00169 array_sub_.shutdown();
00170 }
00171
00172 void MarkerDisplay::deleteMarker(MarkerID id)
00173 {
00174 deleteMarkerStatus(id);
00175
00176 M_IDToMarker::iterator it = markers_.find(id);
00177 if (it != markers_.end())
00178 {
00179 markers_with_expiration_.erase(it->second);
00180 frame_locked_markers_.erase(it->second);
00181 markers_.erase(it);
00182 }
00183 }
00184
00185 void MarkerDisplay::setNamespaceEnabled(const std::string& ns, bool enabled)
00186 {
00187 M_Namespace::iterator it = namespaces_.find(ns);
00188 if (it != namespaces_.end())
00189 {
00190 it->second.enabled = enabled;
00191
00192 std::vector<MarkerID> to_delete;
00193
00194
00195 M_IDToMarker::iterator marker_it = markers_.begin();
00196 M_IDToMarker::iterator marker_end = markers_.end();
00197 for (; marker_it != marker_end; ++marker_it)
00198 {
00199 if (marker_it->first.first == ns)
00200 {
00201 to_delete.push_back(marker_it->first);
00202 }
00203 }
00204
00205 {
00206 std::vector<MarkerID>::iterator it = to_delete.begin();
00207 std::vector<MarkerID>::iterator end = to_delete.end();
00208 for (; it != end; ++it)
00209 {
00210 deleteMarker(*it);
00211 }
00212 }
00213 }
00214 }
00215
00216 bool MarkerDisplay::isNamespaceEnabled(const std::string& ns)
00217 {
00218 M_Namespace::iterator it = namespaces_.find(ns);
00219 if (it != namespaces_.end())
00220 {
00221 return it->second.enabled;
00222 }
00223
00224 return true;
00225 }
00226
00227 void MarkerDisplay::setMarkerStatus(MarkerID id, StatusLevel level, const std::string& text)
00228 {
00229 std::stringstream ss;
00230 ss << id.first << "/" << id.second;
00231 std::string marker_name = ss.str();
00232 setStatus(level, marker_name, text);
00233 }
00234
00235 void MarkerDisplay::deleteMarkerStatus(MarkerID id)
00236 {
00237 std::stringstream ss;
00238 ss << id.first << "/" << id.second;
00239 std::string marker_name = ss.str();
00240 deleteStatus(marker_name);
00241 }
00242
00243 void MarkerDisplay::incomingMarkerArray(const visualization_msgs::MarkerArray::ConstPtr& array)
00244 {
00245 std::vector<visualization_msgs::Marker>::const_iterator it = array->markers.begin();
00246 std::vector<visualization_msgs::Marker>::const_iterator end = array->markers.end();
00247 for (; it != end; ++it)
00248 {
00249 const visualization_msgs::Marker& marker = *it;
00250 tf_filter_.add(visualization_msgs::Marker::Ptr(new visualization_msgs::Marker(marker)));
00251 }
00252 }
00253
00254 void MarkerDisplay::incomingMarker( const visualization_msgs::Marker::ConstPtr& marker )
00255 {
00256 boost::mutex::scoped_lock lock(queue_mutex_);
00257
00258 message_queue_.push_back(marker);
00259 }
00260
00261 void MarkerDisplay::failedMarker(const visualization_msgs::Marker::ConstPtr& marker, tf::FilterFailureReason reason)
00262 {
00263 std::string error = FrameManager::instance()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, marker->__connection_header ? (*marker->__connection_header)["callerid"] : "unknown", reason);
00264 setMarkerStatus(MarkerID(marker->ns, marker->id), status_levels::Error, error);
00265 }
00266
00267 bool validateFloats(const visualization_msgs::Marker& msg)
00268 {
00269 bool valid = true;
00270 valid = valid && validateFloats(msg.pose);
00271 valid = valid && validateFloats(msg.scale);
00272 valid = valid && validateFloats(msg.color);
00273 valid = valid && validateFloats(msg.points);
00274 return valid;
00275 }
00276
00277 void MarkerDisplay::processMessage( const visualization_msgs::Marker::ConstPtr& message )
00278 {
00279 if (!validateFloats(*message))
00280 {
00281 setMarkerStatus(MarkerID(message->ns, message->id), status_levels::Error, "Contains invalid floating point values (nans or infs)");
00282 return;
00283 }
00284
00285 switch ( message->action )
00286 {
00287 case visualization_msgs::Marker::ADD:
00288 processAdd( message );
00289 break;
00290
00291 case visualization_msgs::Marker::DELETE:
00292 processDelete( message );
00293 break;
00294
00295 default:
00296 ROS_ERROR( "Unknown marker action: %d\n", message->action );
00297 }
00298 }
00299
00300 void MarkerDisplay::processAdd( const visualization_msgs::Marker::ConstPtr& message )
00301 {
00302
00303 M_Namespace::iterator ns_it = namespaces_.find(message->ns);
00304 if (ns_it == namespaces_.end())
00305 {
00306 Namespace ns;
00307 ns.name = message->ns;
00308 ns.enabled = true;
00309
00310 if (property_manager_)
00311 {
00312 ns.prop = property_manager_->createProperty<BoolProperty>(ns.name, property_prefix_, boost::bind(&MarkerDisplay::isNamespaceEnabled, this, ns.name),
00313 boost::bind(&MarkerDisplay::setNamespaceEnabled, this, ns.name, _1), namespaces_category_, this);
00314 setPropertyHelpText(ns.prop, "Enable/disable all markers in this namespace.");
00315 }
00316
00317 ns_it = namespaces_.insert(std::make_pair(ns.name, ns)).first;
00318 }
00319
00320 if (!ns_it->second.enabled)
00321 {
00322 return;
00323 }
00324
00325 deleteMarkerStatus(MarkerID(message->ns, message->id));
00326
00327 bool create = true;
00328 MarkerBasePtr marker;
00329
00330 M_IDToMarker::iterator it = markers_.find( MarkerID(message->ns, message->id) );
00331 if ( it != markers_.end() )
00332 {
00333 marker = it->second;
00334 markers_with_expiration_.erase(marker);
00335 if ( message->type == marker->getMessage()->type )
00336 {
00337 create = false;
00338 }
00339 else
00340 {
00341 markers_.erase( it );
00342 }
00343 }
00344
00345 if ( create )
00346 {
00347 switch ( message->type )
00348 {
00349 case visualization_msgs::Marker::CUBE:
00350 case visualization_msgs::Marker::CYLINDER:
00351 case visualization_msgs::Marker::SPHERE:
00352 {
00353 marker.reset(new ShapeMarker(this, vis_manager_, scene_node_));
00354 }
00355 break;
00356
00357 case visualization_msgs::Marker::ARROW:
00358 {
00359 marker.reset(new ArrowMarker(this, vis_manager_, scene_node_));
00360 }
00361 break;
00362
00363 case visualization_msgs::Marker::LINE_STRIP:
00364 {
00365 marker.reset(new LineStripMarker(this, vis_manager_, scene_node_));
00366 }
00367 break;
00368 case visualization_msgs::Marker::LINE_LIST:
00369 {
00370 marker.reset(new LineListMarker(this, vis_manager_, scene_node_));
00371 }
00372 break;
00373 case visualization_msgs::Marker::SPHERE_LIST:
00374 {
00375 marker.reset(new SphereListMarker(this, vis_manager_, scene_node_));
00376 }
00377 break;
00378 case visualization_msgs::Marker::CUBE_LIST:
00379 case visualization_msgs::Marker::POINTS:
00380 {
00381 marker.reset(new PointsMarker(this, vis_manager_, scene_node_));
00382 }
00383 break;
00384 case visualization_msgs::Marker::TEXT_VIEW_FACING:
00385 {
00386 marker.reset(new TextViewFacingMarker(this, vis_manager_, scene_node_));
00387 }
00388 break;
00389 case visualization_msgs::Marker::MESH_RESOURCE:
00390 {
00391 marker.reset(new MeshResourceMarker(this, vis_manager_, scene_node_));
00392 }
00393 break;
00394
00395 case visualization_msgs::Marker::TRIANGLE_LIST:
00396 {
00397 marker.reset(new TriangleListMarker(this, vis_manager_, scene_node_));
00398 }
00399 break;
00400 default:
00401 ROS_ERROR( "Unknown marker type: %d", message->type );
00402 }
00403
00404 markers_.insert(std::make_pair(MarkerID(message->ns, message->id), marker));
00405 }
00406
00407 if (marker)
00408 {
00409 marker->setMessage(message);
00410
00411 if (message->lifetime.toSec() > 0.0001f)
00412 {
00413 markers_with_expiration_.insert(marker);
00414 }
00415
00416 if (message->frame_locked)
00417 {
00418 frame_locked_markers_.insert(marker);
00419 }
00420
00421 causeRender();
00422 }
00423 }
00424
00425 void MarkerDisplay::processDelete( const visualization_msgs::Marker::ConstPtr& message )
00426 {
00427 deleteMarker(MarkerID(message->ns, message->id));
00428 causeRender();
00429 }
00430
00431 void MarkerDisplay::update(float wall_dt, float ros_dt)
00432 {
00433 V_MarkerMessage local_queue;
00434
00435 {
00436 boost::mutex::scoped_lock lock(queue_mutex_);
00437
00438 local_queue.swap( message_queue_ );
00439 }
00440
00441 if ( !local_queue.empty() )
00442 {
00443 V_MarkerMessage::iterator message_it = local_queue.begin();
00444 V_MarkerMessage::iterator message_end = local_queue.end();
00445 for ( ; message_it != message_end; ++message_it )
00446 {
00447 visualization_msgs::Marker::ConstPtr& marker = *message_it;
00448
00449 processMessage( marker );
00450 }
00451 }
00452
00453 {
00454 S_MarkerBase::iterator it = markers_with_expiration_.begin();
00455 S_MarkerBase::iterator end = markers_with_expiration_.end();
00456 for (; it != end;)
00457 {
00458 MarkerBasePtr marker = *it;
00459 if (marker->expired())
00460 {
00461 S_MarkerBase::iterator copy = it;
00462 ++it;
00463 deleteMarker(marker->getID());
00464 }
00465 else
00466 {
00467 ++it;
00468 }
00469 }
00470 }
00471
00472 {
00473 S_MarkerBase::iterator it = frame_locked_markers_.begin();
00474 S_MarkerBase::iterator end = frame_locked_markers_.end();
00475 for (; it != end; ++it)
00476 {
00477 MarkerBasePtr marker = *it;
00478 marker->updateFrameLocked();
00479 }
00480 }
00481 }
00482
00483 void MarkerDisplay::targetFrameChanged()
00484 {
00485 }
00486
00487 void MarkerDisplay::fixedFrameChanged()
00488 {
00489 tf_filter_.setTargetFrame( fixed_frame_ );
00490
00491 clearMarkers();
00492 }
00493
00494 void MarkerDisplay::reset()
00495 {
00496 Display::reset();
00497 clearMarkers();
00498 }
00499
00500 void MarkerDisplay::createProperties()
00501 {
00502 marker_topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Marker Topic", property_prefix_, boost::bind( &MarkerDisplay::getMarkerTopic, this ),
00503 boost::bind( &MarkerDisplay::setMarkerTopic, this, _1 ), parent_category_, this );
00504 setPropertyHelpText(marker_topic_property_, "visualization_msgs::Marker topic to subscribe to. <topic>_array will also automatically be subscribed with type visualization_msgs::MarkerArray.");
00505 ROSTopicStringPropertyPtr topic_prop = marker_topic_property_.lock();
00506 topic_prop->setMessageType(ros::message_traits::datatype<visualization_msgs::Marker>());
00507
00508 namespaces_category_ = property_manager_->createCategory("Namespaces", property_prefix_, parent_category_, this);
00509 }
00510
00511 }
