Go to the documentation of this file.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 #include "marker_array_display.h"
00030
00031 #include "rviz/properties/property_manager.h"
00032 #include "rviz/properties/property.h"
00033
00034 namespace rviz
00035 {
00036
00037 MarkerArrayDisplay::MarkerArrayDisplay()
00038 : MarkerDisplay()
00039 , topic_("visualization_marker_array")
00040 {
00041 }
00042
00043 MarkerArrayDisplay::~MarkerArrayDisplay()
00044 {
00045 }
00046
00047 void MarkerArrayDisplay::setTopic(const std::string& topic)
00048 {
00049 unsubscribe();
00050 topic_ = topic;
00051 subscribe();
00052 propertyChanged(topic_property_);
00053 }
00054
00055 void MarkerArrayDisplay::createProperties()
00056 {
00057 topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Marker Array Topic", property_prefix_,
00058 boost::bind( &MarkerArrayDisplay::getTopic, this ),
00059 boost::bind( &MarkerArrayDisplay::setTopic, this, _1 ),
00060 parent_category_, this );
00061 setPropertyHelpText( topic_property_,
00062 "visualization_msgs::MarkerArray topic to subscribe to.");
00063 ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00064 topic_prop->setMessageType(ros::message_traits::datatype<visualization_msgs::MarkerArray>());
00065
00066 queue_size_property_ = property_manager_->createProperty<IntProperty>( "Queue Size", property_prefix_,
00067 boost::bind( &MarkerArrayDisplay::getQueueSize, this ),
00068 boost::bind( &MarkerArrayDisplay::setQueueSize, this, _1 ),
00069 parent_category_, this );
00070 setPropertyHelpText( queue_size_property_, "Advanced: set the size of the incoming Marker message queue. This should generally be at least a few times larger than the number of Markers in each MarkerArray." );
00071
00072 namespaces_category_ = property_manager_->createCategory("Namespaces", property_prefix_, parent_category_, this);
00073 }
00074
00078 void MarkerArrayDisplay::subscribe()
00079 {
00080 if ( !isEnabled() )
00081 {
00082 return;
00083 }
00084
00085 if (!topic_.empty())
00086 {
00087 array_sub_.shutdown();
00088
00089 try
00090 {
00091 array_sub_ = update_nh_.subscribe(topic_, 1000, &MarkerArrayDisplay::handleMarkerArray, this);
00092 setStatus(status_levels::Ok, "Topic", "OK");
00093 }
00094 catch (ros::Exception& e)
00095 {
00096 setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00097 }
00098 }
00099 }
00100
00101 void MarkerArrayDisplay::unsubscribe()
00102 {
00103 array_sub_.shutdown();
00104 }
00105
00106
00107
00108 void MarkerArrayDisplay::handleMarkerArray(const visualization_msgs::MarkerArray::ConstPtr& array)
00109 {
00110 incomingMarkerArray( array );
00111 }
00112
00113 }