00001 #include "nxt_color_display.h"
00002 #include "rviz/visualization_manager.h"
00003 #include "rviz/properties/property.h"
00004 #include "rviz/properties/property_manager.h"
00005 #include "rviz/common.h"
00006 #include "rviz/frame_manager.h"
00007 #include "rviz/validate_floats.h"
00008
00009 #include <tf/transform_listener.h>
00010
00011 #include <ogre_tools/shape.h>
00012
00013 #include <OGRE/OgreSceneNode.h>
00014 #include <OGRE/OgreSceneManager.h>
00015
00016 namespace nxt_rviz_plugin
00017 {
00018 NXTColorDisplay::NXTColorDisplay( const std::string& name, rviz::VisualizationManager* manager )
00019 : Display( name, manager )
00020 , messages_received_(0)
00021 , tf_filter_(*manager->getTFClient(), "", 10, update_nh_)
00022 {
00023 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00024
00025 cylinder_ = new ogre_tools::Shape(ogre_tools::Shape::Cylinder, vis_manager_->getSceneManager(), scene_node_);
00026
00027 scene_node_->setVisible( false );
00028
00029 setAlpha( 0.5f );
00030 setDisplayLength( 0.003f );
00031
00032 Ogre::Vector3 scale( 0, 0, 0);
00033 rviz::scaleRobotToOgre( scale );
00034 cylinder_->setScale(scale);
00035
00036 tf_filter_.connectInput(sub_);
00037 tf_filter_.registerCallback(boost::bind(&NXTColorDisplay::incomingMessage, this, _1));
00038 vis_manager_->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter_, this);
00039 }
00040
00041 NXTColorDisplay::~NXTColorDisplay()
00042 {
00043 unsubscribe();
00044 clear();
00045 delete cylinder_;
00046 }
00047
00048 void NXTColorDisplay::clear()
00049 {
00050
00051 messages_received_ = 0;
00052 setStatus(rviz::status_levels::Warn, "Topic", "No messages received");
00053 }
00054
00055 void NXTColorDisplay::setTopic( const std::string& topic )
00056 {
00057 unsubscribe();
00058
00059 topic_ = topic;
00060
00061 subscribe();
00062
00063 propertyChanged(topic_property_);
00064
00065 causeRender();
00066 }
00067
00068 void NXTColorDisplay::setAlpha( float alpha )
00069 {
00070 alpha_ = alpha;
00071
00072 propertyChanged(alpha_property_);
00073
00074 processMessage(current_message_);
00075 causeRender();
00076 }
00077
00078 void NXTColorDisplay::setDisplayLength( float displayLength )
00079 {
00080 displayLength_ = displayLength;
00081
00082 propertyChanged(display_property_);
00083
00084 processMessage(current_message_);
00085 causeRender();
00086 }
00087
00088
00089 void NXTColorDisplay::subscribe()
00090 {
00091 if ( !isEnabled() )
00092 {
00093 return;
00094 }
00095
00096 sub_.subscribe(update_nh_, topic_, 10);
00097 }
00098
00099 void NXTColorDisplay::unsubscribe()
00100 {
00101 sub_.unsubscribe();
00102 }
00103
00104 void NXTColorDisplay::onEnable()
00105 {
00106 scene_node_->setVisible( true );
00107 subscribe();
00108 }
00109
00110 void NXTColorDisplay::onDisable()
00111 {
00112 unsubscribe();
00113 clear();
00114 scene_node_->setVisible( false );
00115 }
00116
00117 void NXTColorDisplay::fixedFrameChanged()
00118 {
00119 clear();
00120
00121 tf_filter_.setTargetFrame( fixed_frame_ );
00122 }
00123
00124 void NXTColorDisplay::update(float wall_dt, float ros_dt)
00125 {
00126 }
00127
00128
00129 void NXTColorDisplay::processMessage(const nxt_msgs::Color::ConstPtr& msg)
00130 {
00131 if (!msg)
00132 {
00133 return;
00134 }
00135
00136 ++messages_received_;
00137
00138 {
00139 std::stringstream ss;
00140 ss << messages_received_ << " messages received";
00141 setStatus(rviz::status_levels::Ok, "Topic", ss.str());
00142 }
00143
00144 Ogre::Vector3 position;
00145 Ogre::Quaternion orientation;
00146 geometry_msgs::Pose pose;
00147 pose.position.z = -0.0033;
00148 pose.position.y = 0;
00149 pose.position.x = 0.0185 + displayLength_/2;
00150 pose.orientation.x = 0.707;
00151 pose.orientation.z = -0.707;
00152 if (!vis_manager_->getFrameManager()->transform(msg->header.frame_id,msg->header.stamp,pose, position, orientation, true))
00153 {
00154 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), fixed_frame_.c_str() );
00155 }
00156
00157 cylinder_->setPosition(position);
00158 cylinder_->setOrientation(orientation);
00159 Ogre::Vector3 scale( 0.0155, 0.0155, displayLength_);
00160 rviz::scaleRobotToOgre( scale );
00161 cylinder_->setScale(scale);
00162 cylinder_->setColor(msg->r, msg->g, msg->b, alpha_);
00163
00164 }
00165
00166 void NXTColorDisplay::incomingMessage(const nxt_msgs::Color::ConstPtr& msg)
00167 {
00168 processMessage(msg);
00169 }
00170
00171 void NXTColorDisplay::reset()
00172 {
00173 Display::reset();
00174 clear();
00175 }
00176
00177 void NXTColorDisplay::createProperties()
00178 {
00179 topic_property_ = property_manager_->createProperty<rviz::ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &NXTColorDisplay::getTopic, this ),
00180 boost::bind( &NXTColorDisplay::setTopic, this, _1 ), parent_category_, this );
00181 setPropertyHelpText(topic_property_, "nxt_msgs::Color topic to subscribe to.");
00182 rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00183 topic_prop->setMessageType(ros::message_traits::datatype<nxt_msgs::Color>());
00184
00185
00186 alpha_property_ = property_manager_->createProperty<rviz::FloatProperty>( "Alpha", property_prefix_, boost::bind( &NXTColorDisplay::getAlpha, this ),
00187 boost::bind( &NXTColorDisplay::setAlpha, this, _1 ), parent_category_, this );
00188
00189 display_property_ = property_manager_->createProperty<rviz::FloatProperty>( "Display Length", property_prefix_, boost::bind( &NXTColorDisplay::getDisplayLength, this ),
00190 boost::bind( &NXTColorDisplay::setDisplayLength, this, _1 ), parent_category_, this );
00191
00192 setPropertyHelpText(alpha_property_, "Amount of transparency to apply to the circle.");
00193 }
00194
00195 const char* NXTColorDisplay::getDescription()
00196 {
00197 return "Displays data from a nxt_msgs::Color message as a cirle of color.";
00198 }
00199 }