00001 #include "range_display.h"
00002 #include "rviz/visualization_manager.h"
00003 #include "rviz/properties/property.h"
00004 #include "rviz/properties/property_manager.h"
00005 #include "rviz/frame_manager.h"
00006 #include "rviz/validate_floats.h"
00007
00008 #include <tf/transform_listener.h>
00009
00010 #include <rviz/ogre_helpers/shape.h>
00011
00012 #include <OGRE/OgreSceneNode.h>
00013 #include <OGRE/OgreSceneManager.h>
00014
00015 namespace rviz
00016 {
00017 RangeDisplay::RangeDisplay()
00018 : Display()
00019 , color_( 1.0f, 1.0f, 1.0f )
00020 , messages_received_(0)
00021 {
00022 }
00023
00024 void RangeDisplay::onInitialize()
00025 {
00026 tf_filter_ = new tf::MessageFilter<sensor_msgs::Range>(*vis_manager_->getTFClient(), "", 10, update_nh_);
00027
00028 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00029 scene_node_->setVisible( false );
00030
00031 setBuffer( 1 );
00032 Ogre::Vector3 scale( 0, 0, 0);
00033
00034 tf_filter_->connectInput(sub_);
00035 tf_filter_->registerCallback(boost::bind(&RangeDisplay::incomingMessage, this, _1));
00036 vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00037 setAlpha( 0.5f );
00038 }
00039
00040 RangeDisplay::~RangeDisplay()
00041 {
00042 unsubscribe();
00043 clear();
00044 for (size_t i = 0; i < cones_.size(); i++) {
00045 delete cones_[i];
00046 }
00047
00048 delete tf_filter_;
00049 }
00050
00051 void RangeDisplay::clear()
00052 {
00053 setBuffer( cones_.size() );
00054 tf_filter_->clear();
00055 messages_received_ = 0;
00056 setStatus(rviz::status_levels::Warn, "Topic", "No messages received");
00057 }
00058
00059 void RangeDisplay::setTopic( const std::string& topic )
00060 {
00061 unsubscribe();
00062
00063 topic_ = topic;
00064
00065 subscribe();
00066
00067 propertyChanged(topic_property_);
00068
00069 causeRender();
00070 }
00071
00072 void RangeDisplay::setColor( const rviz::Color& color )
00073 {
00074 color_ = color;
00075
00076 propertyChanged(color_property_);
00077
00078 processMessage(current_message_);
00079 causeRender();
00080 }
00081
00082 void RangeDisplay::setBuffer( int buffer )
00083 {
00084 if(buffer < 1)
00085 buffer = 1;
00086 buffer_len_ = buffer;
00087
00088 propertyChanged(bufferLen_property_);
00089
00090 for (size_t i = 0; i < cones_.size(); i++) {
00091 delete cones_[i];
00092 }
00093 cones_.resize(buffer_len_);
00094 for (size_t i = 0; i < cones_.size(); i++) {
00095 cones_[i] = new Shape(Shape::Cone, vis_manager_->getSceneManager(), scene_node_);
00096 Shape* cone = cones_[i];
00097
00098 Ogre::Vector3 position;
00099 Ogre::Quaternion orientation;
00100 geometry_msgs::Pose pose;
00101 pose.position.z = pose.position.y = 0;
00102 pose.position.x = 0;
00103 pose.orientation.x = 0;
00104 pose.orientation.z = 0;
00105 Ogre::Vector3 scale( 0, 0, 0);
00106 cone->setScale(scale);
00107 cone->setColor(color_.r_, color_.g_, color_.b_, 0);
00108 }
00109 }
00110
00111 void RangeDisplay::setAlpha( float alpha )
00112 {
00113 alpha_ = alpha;
00114
00115 propertyChanged(alpha_property_);
00116
00117 processMessage(current_message_);
00118 causeRender();
00119 }
00120
00121 void RangeDisplay::subscribe()
00122 {
00123 if ( !isEnabled() )
00124 {
00125 return;
00126 }
00127
00128 try
00129 {
00130 sub_.subscribe(update_nh_, topic_, 10);
00131 setStatus(status_levels::Ok, "Topic", "OK");
00132 }
00133 catch (ros::Exception& e)
00134 {
00135 setStatus(status_levels::Error, "Topic", std::string("Error subscribing: ") + e.what());
00136 }
00137 }
00138
00139 void RangeDisplay::unsubscribe()
00140 {
00141 sub_.unsubscribe();
00142 }
00143
00144 void RangeDisplay::onEnable()
00145 {
00146 scene_node_->setVisible( true );
00147 subscribe();
00148 }
00149
00150 void RangeDisplay::onDisable()
00151 {
00152 unsubscribe();
00153 clear();
00154 scene_node_->setVisible( false );
00155 }
00156
00157 void RangeDisplay::fixedFrameChanged()
00158 {
00159 tf_filter_->setTargetFrame( fixed_frame_ );
00160 clear();
00161 }
00162
00163 void RangeDisplay::update(float wall_dt, float ros_dt)
00164 {
00165 }
00166
00167
00168 void RangeDisplay::processMessage(const sensor_msgs::Range::ConstPtr& msg)
00169 {
00170 if (!msg)
00171 {
00172 return;
00173 }
00174
00175 ++messages_received_;
00176
00177 Shape* cone = cones_[messages_received_ % buffer_len_];
00178
00179 {
00180 std::stringstream ss;
00181 ss << messages_received_ << " messages received";
00182 setStatus(rviz::status_levels::Ok, "Topic", ss.str());
00183 }
00184
00185 Ogre::Vector3 position;
00186 Ogre::Quaternion orientation;
00187 geometry_msgs::Pose pose;
00188 pose.position.z = pose.position.y = 0;
00189 pose.position.x = msg->range/2 - .008824 * msg->range;
00190 pose.orientation.z = 0.707;
00191 pose.orientation.w = 0.707;
00192 if( !vis_manager_->getFrameManager()->transform( msg->header.frame_id, msg->header.stamp, pose, position, orientation ))
00193 {
00194 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), fixed_frame_.c_str() );
00195 }
00196
00197 cone->setPosition(position);
00198 cone->setOrientation(orientation);
00199
00200 double cone_width = 2.0 * msg->range * tan( msg->field_of_view / 2.0 );
00201 Ogre::Vector3 scale( cone_width, msg->range, cone_width );
00202 cone->setScale(scale);
00203 cone->setColor(color_.r_, color_.g_, color_.b_, alpha_);
00204
00205 }
00206
00207 void RangeDisplay::incomingMessage(const sensor_msgs::Range::ConstPtr& msg)
00208 {
00209 processMessage(msg);
00210 }
00211
00212 void RangeDisplay::reset()
00213 {
00214 Display::reset();
00215 clear();
00216 }
00217
00218 void RangeDisplay::createProperties()
00219 {
00220 topic_property_ = property_manager_->createProperty<rviz::ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &RangeDisplay::getTopic, this ),
00221 boost::bind( &RangeDisplay::setTopic, this, _1 ), parent_category_, this );
00222 setPropertyHelpText(topic_property_, "sensor_msgs::Range topic to subscribe to.");
00223 rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00224 topic_prop->setMessageType(ros::message_traits::datatype<sensor_msgs::Range>());
00225 color_property_ = property_manager_->createProperty<rviz::ColorProperty>( "Color", property_prefix_, boost::bind( &RangeDisplay::getColor, this ),
00226 boost::bind( &RangeDisplay::setColor, this, _1 ), parent_category_, this );
00227 setPropertyHelpText(color_property_, "Color to draw the range.");
00228 alpha_property_ = property_manager_->createProperty<rviz::FloatProperty>( "Alpha", property_prefix_, boost::bind( &RangeDisplay::getAlpha, this ),
00229 boost::bind( &RangeDisplay::setAlpha, this, _1 ), parent_category_, this );
00230 setPropertyHelpText(alpha_property_, "Amount of transparency to apply to the range.");
00231 bufferLen_property_ = property_manager_->createProperty<rviz::IntProperty>( "Buffer Length", property_prefix_, boost::bind( &RangeDisplay::getBuffer, this ),
00232 boost::bind( &RangeDisplay::setBuffer, this, _1 ), parent_category_, this );
00233 setPropertyHelpText(bufferLen_property_, "Number of prior measurements to display.");
00234
00235 }
00236
00237 const char* RangeDisplay::getDescription()
00238 {
00239 return "Displays data from a sensor_msgs::Range message as a cone.";
00240 }
00241 }