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 "pose_array_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/properties/property.h"
00033 #include "rviz/properties/property_manager.h"
00034 #include "rviz/frame_manager.h"
00035 #include "rviz/validate_floats.h"
00036
00037 #include "rviz/ogre_helpers/arrow.h"
00038
00039 #include <tf/transform_listener.h>
00040
00041 #include <boost/bind.hpp>
00042
00043 #include <OGRE/OgreSceneNode.h>
00044 #include <OGRE/OgreSceneManager.h>
00045 #include <OGRE/OgreManualObject.h>
00046
00047 namespace rviz
00048 {
00049
00050 PoseArrayDisplay::PoseArrayDisplay()
00051 : Display()
00052 , color_( 1.0f, 0.1f, 0.0f )
00053 , length_( 0.3 )
00054 , messages_received_(0)
00055 {
00056 }
00057
00058 PoseArrayDisplay::~PoseArrayDisplay()
00059 {
00060 unsubscribe();
00061 clear();
00062
00063 scene_manager_->destroyManualObject( manual_object_ );
00064 delete tf_filter_;
00065 }
00066
00067 void PoseArrayDisplay::onInitialize()
00068 {
00069 tf_filter_ = new tf::MessageFilter<geometry_msgs::PoseArray>(*vis_manager_->getTFClient(), "", 2, update_nh_);
00070 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00071
00072 static int count = 0;
00073 std::stringstream ss;
00074 ss << "ParticleCloud2D" << count++;
00075 manual_object_ = scene_manager_->createManualObject( ss.str() );
00076 manual_object_->setDynamic( true );
00077 scene_node_->attachObject( manual_object_ );
00078
00079 tf_filter_->connectInput(sub_);
00080 tf_filter_->registerCallback(boost::bind(&PoseArrayDisplay::incomingMessage, this, _1));
00081 vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00082 }
00083
00084 void PoseArrayDisplay::clear()
00085 {
00086 manual_object_->clear();
00087
00088 messages_received_ = 0;
00089 setStatus(status_levels::Warn, "Topic", "No messages received");
00090 }
00091
00092 void PoseArrayDisplay::setTopic( const std::string& topic )
00093 {
00094 unsubscribe();
00095
00096 topic_ = topic;
00097
00098 subscribe();
00099
00100 propertyChanged(topic_property_);
00101
00102 causeRender();
00103 }
00104
00105 void PoseArrayDisplay::setColor( const Color& color )
00106 {
00107 color_ = color;
00108
00109 propertyChanged(color_property_);
00110
00111 causeRender();
00112 }
00113
00114 void PoseArrayDisplay::setLength( float length )
00115 {
00116 length_ = length;
00117 propertyChanged( length_property_ );
00118 causeRender();
00119 }
00120
00121 void PoseArrayDisplay::subscribe()
00122 {
00123 if ( !isEnabled() )
00124 {
00125 return;
00126 }
00127
00128 try
00129 {
00130 sub_.subscribe(update_nh_, topic_, 5);
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 PoseArrayDisplay::unsubscribe()
00140 {
00141 sub_.unsubscribe();
00142 }
00143
00144 void PoseArrayDisplay::onEnable()
00145 {
00146 scene_node_->setVisible( true );
00147 subscribe();
00148 }
00149
00150 void PoseArrayDisplay::onDisable()
00151 {
00152 unsubscribe();
00153 clear();
00154 scene_node_->setVisible( false );
00155 }
00156
00157 void PoseArrayDisplay::createProperties()
00158 {
00159 topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &PoseArrayDisplay::getTopic, this ),
00160 boost::bind( &PoseArrayDisplay::setTopic, this, _1 ), parent_category_, this );
00161 setPropertyHelpText(topic_property_, "geometry_msgs::PoseArray topic to subscribe to.");
00162 ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00163 topic_prop->setMessageType(ros::message_traits::datatype<geometry_msgs::PoseArray>());
00164
00165 color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &PoseArrayDisplay::getColor, this ),
00166 boost::bind( &PoseArrayDisplay::setColor, this, _1 ), parent_category_, this );
00167 setPropertyHelpText(color_property_, "Color to draw the arrows.");
00168
00169 length_property_ = property_manager_->createProperty<FloatProperty>( "Arrow Length", property_prefix_, boost::bind( &PoseArrayDisplay::getLength, this ),
00170 boost::bind( &PoseArrayDisplay::setLength, this, _1 ), parent_category_, this );
00171 setPropertyHelpText(length_property_, "Length of the arrows.");
00172 }
00173
00174 void PoseArrayDisplay::fixedFrameChanged()
00175 {
00176 clear();
00177 tf_filter_->setTargetFrame( fixed_frame_ );
00178 }
00179
00180 void PoseArrayDisplay::update(float wall_dt, float ros_dt)
00181 {
00182 }
00183
00184 bool validateFloats(const geometry_msgs::PoseArray& msg)
00185 {
00186 return validateFloats(msg.poses);
00187 }
00188
00189 void PoseArrayDisplay::processMessage(const geometry_msgs::PoseArray::ConstPtr& msg)
00190 {
00191 ++messages_received_;
00192
00193 if (!validateFloats(*msg))
00194 {
00195 setStatus(status_levels::Error, "Topic", "Message contained invalid floating point values (nans or infs)");
00196 return;
00197 }
00198
00199 {
00200 std::stringstream ss;
00201 ss << messages_received_ << " messages received";
00202 setStatus(status_levels::Ok, "Topic", ss.str());
00203 }
00204
00205 manual_object_->clear();
00206
00207 Ogre::Vector3 position;
00208 Ogre::Quaternion orientation;
00209 if (!vis_manager_->getFrameManager()->getTransform(msg->header, position, orientation))
00210 {
00211 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), fixed_frame_.c_str() );
00212 }
00213
00214 scene_node_->setPosition( position );
00215 scene_node_->setOrientation( orientation );
00216
00217 manual_object_->clear();
00218
00219 Ogre::ColourValue color( color_.r_, color_.g_, color_.b_, 1.0f );
00220 size_t num_poses = msg->poses.size();
00221 manual_object_->estimateVertexCount( num_poses * 6 );
00222 manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
00223 for( size_t i=0; i < num_poses; ++i)
00224 {
00225 Ogre::Vector3 pos(msg->poses[i].position.x, msg->poses[i].position.y, msg->poses[i].position.z);
00226 tf::Quaternion quat;
00227 tf::quaternionMsgToTF(msg->poses[i].orientation, quat);
00228 Ogre::Quaternion orient( quat.w(), quat.x(), quat.y(), quat.z() );
00229
00230
00231
00232 Ogre::Vector3 vertices[6];
00233 vertices[0] = pos;
00234 vertices[1] = pos + orient * Ogre::Vector3(length_, 0, 0);
00235 vertices[2] = vertices[1];
00236 vertices[3] = pos + orient * Ogre::Vector3(0.75*length_, 0.2*length_, 0);
00237 vertices[4] = vertices[1];
00238 vertices[5] = pos + orient * Ogre::Vector3(0.75*length_, -0.2*length_, 0);
00239
00240 for ( int i = 0; i < 6; ++i )
00241 {
00242 manual_object_->position( vertices[i] );
00243 manual_object_->colour( color );
00244 }
00245 }
00246 manual_object_->end();
00247
00248 causeRender();
00249 }
00250
00251 void PoseArrayDisplay::incomingMessage(const geometry_msgs::PoseArray::ConstPtr& msg)
00252 {
00253 processMessage(msg);
00254 }
00255
00256 void PoseArrayDisplay::reset()
00257 {
00258 Display::reset();
00259 clear();
00260 }
00261
00262 }
00263