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 "positionstring_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/properties/property.h"
00033 #include "rviz/properties/property_manager.h"
00034 #include "rviz/common.h"
00035
00036 #include <tf/transform_listener.h>
00037
00038 #include <boost/bind.hpp>
00039
00040 #include <OGRE/OgreSceneNode.h>
00041 #include <OGRE/OgreSceneManager.h>
00042 #include <OGRE/OgreManualObject.h>
00043 #include <OGRE/OgreBillboardSet.h>
00044
00045 #include <ogre_tools/movable_text.h>
00046
00047 namespace position_string_rviz_plugin
00048 {
00049
00050 PositionStringDisplay::PositionStringDisplay(const std::string & name, rviz::VisualizationManager * manager)
00051 : Display(name, manager)
00052 , color_(0.1f, 1.0f, 0.0f)
00053 , character_height_(0.1)
00054 , tf_filter_(*manager->getTFClient(), "", 2, update_nh_)
00055 {
00056 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00057
00058 static int count = 0;
00059 std::stringstream ss;
00060 ss << "PositionString " << count++;
00061
00062 text_ = new ogre_tools::MovableText("positionstring", "Arial", character_height_, Ogre::ColourValue::White);
00063 text_->showOnTop (true);
00064 scene_node_->attachObject(text_);
00065
00066 tf_filter_.connectInput(sub_);
00067 tf_filter_.registerCallback(boost::bind(&PositionStringDisplay::incomingMessage, this, _1));
00068 }
00069
00070 PositionStringDisplay::~PositionStringDisplay()
00071 {
00072 unsubscribe();
00073
00074 delete text_;
00075 }
00076
00077
00078 void PositionStringDisplay::setTopic(const std::string & topic)
00079 {
00080 unsubscribe();
00081 topic_ = topic;
00082 subscribe();
00083
00084 propertyChanged(topic_property_);
00085
00086 causeRender();
00087 }
00088
00089 void PositionStringDisplay::setColor(const rviz::Color & color)
00090 {
00091 color_ = color;
00092
00093 propertyChanged(color_property_);
00094 processMessage(current_message_);
00095 causeRender();
00096 }
00097
00098 void PositionStringDisplay::setCharacterHeight(const double & h)
00099 {
00100 character_height_ = h;
00101
00102 propertyChanged(character_height_property_);
00103 processMessage(current_message_);
00104 causeRender();
00105 }
00106
00107 void PositionStringDisplay::subscribe()
00108 {
00109 if (!isEnabled())
00110 return;
00111
00112 if (!topic_.empty())
00113 {
00114 sub_.subscribe(update_nh_, topic_, 1);
00115 }
00116 }
00117
00118 void PositionStringDisplay::unsubscribe()
00119 {
00120 sub_.unsubscribe();
00121 }
00122
00123 void PositionStringDisplay::onEnable()
00124 {
00125 scene_node_->setVisible(true);
00126 subscribe();
00127 }
00128
00129 void PositionStringDisplay::onDisable()
00130 {
00131 unsubscribe();
00132 scene_node_->setVisible(false);
00133 }
00134
00135 void PositionStringDisplay::fixedFrameChanged()
00136 {
00137 tf_filter_.setTargetFrame(fixed_frame_);
00138 }
00139
00140 void PositionStringDisplay::update(float wall_dt, float ros_dt)
00141 {
00142 }
00143
00144 void PositionStringDisplay::processMessage(const position_string_msgs::PositionString::ConstPtr& msg)
00145 {
00146 if (!msg)
00147 {
00148 return;
00149 }
00150
00151 text_->setCaption (msg->text);
00152 text_->setCharacterHeight (character_height_);
00153 text_->setColor (Ogre::ColourValue(color_.r_, color_.g_, color_.b_));
00154
00155 tf::Stamped<tf::Pose> pose(btTransform(btQuaternion(1.0f, 0.0f, 0.0f, 0.0f),
00156 btVector3(msg->pose.x, msg->pose.y, msg->pose.z)), msg->header.stamp,
00157 msg->header.frame_id);
00158
00159 try
00160 {
00161 vis_manager_->getTFClient()->transformPose(fixed_frame_, pose, pose);
00162 }
00163 catch (tf::TransformException & e)
00164 {
00165 ROS_ERROR("Error transforming from frame '%s' to frame '%s'",
00166 msg->header.frame_id.c_str(), fixed_frame_.c_str());
00167 }
00168
00169 Ogre::Vector3 position = Ogre::Vector3(pose.getOrigin().x(),
00170 pose.getOrigin().y(), pose.getOrigin().z());
00171
00172 rviz::robotToOgre(position);
00173
00174 scene_node_->setPosition(position);
00175 }
00176
00177 void PositionStringDisplay::incomingMessage(const position_string_msgs::PositionString::ConstPtr& message)
00178 {
00179 current_message_ = message;
00180 processMessage(message);
00181 }
00182
00183 void PositionStringDisplay::reset()
00184 {
00185 }
00186
00187 void PositionStringDisplay::targetFrameChanged()
00188 {
00189 }
00190
00191 void PositionStringDisplay::createProperties()
00192 {
00193 character_height_property_ = property_manager_->createProperty<rviz::DoubleProperty> ("Character Height", property_prefix_, boost::bind(&PositionStringDisplay::getCharacterHeight, this),
00194 boost::bind(&PositionStringDisplay::setCharacterHeight, this, _1), parent_category_, this);
00195 color_property_ = property_manager_->createProperty<rviz::ColorProperty> ("Color", property_prefix_, boost::bind(&PositionStringDisplay::getColor, this),
00196 boost::bind(&PositionStringDisplay::setColor, this, _1), parent_category_, this);
00197 topic_property_ = property_manager_->createProperty<rviz::ROSTopicStringProperty> ("Topic", property_prefix_, boost::bind(&PositionStringDisplay::getTopic, this),
00198 boost::bind(&PositionStringDisplay::setTopic, this, _1), parent_category_, this);
00199 rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00200 topic_prop->setMessageType(ros::message_traits::DataType<position_string_msgs::PositionString>().value());
00201 }
00202
00203 }