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 "rviz/properties/ros_topic_property.h"
00031 #include "rviz/properties/int_property.h"
00032
00033 #include "marker_array_display.h"
00034
00035 namespace rviz
00036 {
00037
00038 MarkerArrayDisplay::MarkerArrayDisplay()
00039 : MarkerDisplay()
00040 {
00041 marker_topic_property_->setMessageType( QString::fromStdString( ros::message_traits::datatype<visualization_msgs::MarkerArray>() ));
00042 marker_topic_property_->setValue( "visualization_marker_array" );
00043 marker_topic_property_->setDescription( "visualization_msgs::MarkerArray topic to subscribe to." );
00044
00045 queue_size_property_->setDescription( "Advanced: set the size of the incoming Marker message queue. "
00046 " This should generally be at least a few times larger than the number of Markers in each MarkerArray." );
00047 }
00048
00049 void MarkerArrayDisplay::subscribe()
00050 {
00051 if ( !isEnabled() )
00052 {
00053 return;
00054 }
00055
00056 std::string topic = marker_topic_property_->getTopicStd();
00057 if( !topic.empty() )
00058 {
00059 array_sub_.shutdown();
00060
00061 try
00062 {
00063 array_sub_ = update_nh_.subscribe( topic, 1000, &MarkerArrayDisplay::handleMarkerArray, this );
00064 setStatus( StatusProperty::Ok, "Topic", "OK" );
00065 }
00066 catch( ros::Exception& e )
00067 {
00068 setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00069 }
00070 }
00071 }
00072
00073 void MarkerArrayDisplay::unsubscribe()
00074 {
00075 array_sub_.shutdown();
00076 }
00077
00078
00079
00080 void MarkerArrayDisplay::handleMarkerArray( const visualization_msgs::MarkerArray::ConstPtr& array )
00081 {
00082 incomingMarkerArray( array );
00083 }
00084
00085 }
00086
00087 #include <pluginlib/class_list_macros.h>
00088 PLUGINLIB_DECLARE_CLASS( rviz, MarkerArray, rviz::MarkerArrayDisplay, rviz::Display )