positionstringlist_display.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Nico Blodow (blodow@cs.tum.edu)
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "positionstringlist_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 PositionStringListDisplay::PositionStringListDisplay(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 , new_msg_(true)
00055 , tf_filter_(*manager->getTFClient(), "", 2, update_nh_)
00056 {
00057   static int count = 0;
00058   std::stringstream ss;
00059   ss << "PositionStringList " << count++;
00060 
00061   tf_filter_.connectInput(sub_);
00062   tf_filter_.registerCallback(boost::bind(&PositionStringListDisplay::incomingMessage, this, _1));
00063 }
00064 
00065 PositionStringListDisplay::~PositionStringListDisplay()
00066 {
00067   unsubscribe();
00068   clearTextItems ();
00069 }
00070 
00071 void PositionStringListDisplay::setNumberTextItems (unsigned int n)
00072 {
00073   if (n == scene_nodes_.size())
00074     return;
00075   if (n < scene_nodes_.size())
00076   {
00077     unsigned int needed = scene_nodes_.size() - n;
00078     for (unsigned int too_many = 0; too_many < needed; too_many++)
00079     {
00080       //TODO: Is this enough? This a memory leak?
00081       scene_manager_->getRootSceneNode ()->removeChild (scene_nodes_.back());
00082       scene_nodes_.pop_back();
00083       text_objects_.pop_back();
00084     }
00085   }
00086   else 
00087   {
00088     unsigned int needed = n - scene_nodes_.size();
00089     for (unsigned int additional = 0; additional < needed; additional++)
00090     {
00091       Ogre::SceneNode* scene_node = scene_manager_->getRootSceneNode()->createChildSceneNode();
00092       ogre_tools::MovableText *text = new ogre_tools::MovableText("(empty)", "Arial", character_height_, Ogre::ColourValue(color_.r_, color_.g_, color_.b_));
00093       text->showOnTop (true);
00094       scene_node->attachObject(text);
00095   
00096       text_objects_.push_back (text);
00097       scene_nodes_.push_back (scene_node);
00098     }
00099   }
00100 }
00101 
00102 void PositionStringListDisplay::clearTextItems ()
00103 {
00104   // TODO: This a memory leak??
00105   for (std::vector<Ogre::SceneNode*>::iterator it = scene_nodes_.begin(); it != scene_nodes_.end(); it++)
00106     (*it)->detachAllObjects();
00107 
00108   for (std::vector<ogre_tools::MovableText*>::iterator it = text_objects_.begin(); it != text_objects_.end(); it++)
00109     delete (*it);
00110 }
00111 
00112 void PositionStringListDisplay::setTopic(const std::string & topic)
00113 {
00114   unsubscribe();
00115   topic_ = topic;
00116   subscribe();
00117 
00118   propertyChanged(topic_property_);
00119 
00120   causeRender();
00121 }
00122 
00123 void PositionStringListDisplay::setColor(const rviz::Color & color)
00124 {
00125   color_ = color;
00126 
00127   propertyChanged(color_property_);
00128   processMessage(current_message_);
00129   causeRender();
00130 }
00131 
00132 void PositionStringListDisplay::setCharacterHeight(const double & h)
00133 {
00134   character_height_ = h;
00135 
00136   propertyChanged(character_height_property_);
00137   processMessage(current_message_);
00138   causeRender();
00139 }
00140 
00141 void PositionStringListDisplay::subscribe()
00142 {
00143   if (!isEnabled())
00144     return;
00145 
00146   if (!topic_.empty())
00147   {
00148     sub_.subscribe(update_nh_, topic_, 1);
00149   }
00150 }
00151 
00152 void PositionStringListDisplay::unsubscribe()
00153 {
00154   sub_.unsubscribe();
00155 }
00156 
00157 void PositionStringListDisplay::onEnable()
00158 {
00159   for (std::vector<Ogre::SceneNode*>::iterator it = scene_nodes_.begin(); it != scene_nodes_.end(); it++)
00160     (*it)->setVisible(true);
00161   subscribe();
00162 }
00163 
00164 void PositionStringListDisplay::onDisable()
00165 {
00166   unsubscribe();
00167   for (std::vector<Ogre::SceneNode*>::iterator it = scene_nodes_.begin(); it != scene_nodes_.end(); it++)
00168     (*it)->setVisible(false);
00169 }
00170 
00171 void PositionStringListDisplay::fixedFrameChanged()
00172 {
00173   tf_filter_.setTargetFrame(fixed_frame_);
00174 }
00175 
00176 void PositionStringListDisplay::update(float wall_dt, float ros_dt)
00177 {
00178 }
00179 
00180 void PositionStringListDisplay::processMessage(const position_string_msgs::PositionStringList::ConstPtr& msg)
00181 {
00182   if (!msg)
00183   {
00184     return;
00185   }
00186   
00187   if (new_msg_) // new message means new text
00188   {
00189     setNumberTextItems (msg->texts.size ());
00190     for (unsigned int i = 0; i < msg->texts.size(); i++)
00191     {
00192       text_objects_[i]->setCaption(msg->texts[i]);
00193       if (i < msg->poses.size() && (fabs(msg->poses[i].x) != std::numeric_limits<double>::infinity())
00194                                 && (fabs(msg->poses[i].y) != std::numeric_limits<double>::infinity())
00195                                 && (fabs(msg->poses[i].z) != std::numeric_limits<double>::infinity()))
00196       {
00197         msg->poses[i];
00198         tf::Stamped<tf::Pose> pose(btTransform(btQuaternion(1.0f, 0.0f, 0.0f, 0.0f),
00199             btVector3(msg->poses[i].x, msg->poses[i].y, msg->poses[i].z)), msg->header.stamp,
00200             msg->header.frame_id);
00201       
00202         try
00203         {
00204           vis_manager_->getTFClient()->transformPose(fixed_frame_, pose, pose);
00205         }
00206         catch (tf::TransformException & e)
00207         {
00208           ROS_ERROR("Error transforming from frame '%s' to frame '%s'",
00209               msg->header.frame_id.c_str(), fixed_frame_.c_str());
00210         }
00211       
00212         Ogre::Vector3 position = Ogre::Vector3(pose.getOrigin().x(),
00213             pose.getOrigin().y(), pose.getOrigin().z());
00214       
00215         rviz::robotToOgre(position);
00216 
00217         scene_nodes_[i]->setPosition(position);
00218       }
00219       else // this just catches the case that not enough poses were given in the message.
00220         scene_nodes_[i]->setPosition (Ogre::Vector3(0,0,0));
00221     }
00222   }
00223   else  // otherwise, only settings (color/fontsize) has changed
00224   {
00225     for (std::vector<ogre_tools::MovableText*>::iterator it = text_objects_.begin(); it != text_objects_.end(); it++)
00226     {
00227       (*it)->setCharacterHeight (character_height_);
00228       (*it)->setColor (Ogre::ColourValue(color_.r_, color_.g_, color_.b_));
00229     }
00230   }
00231 }
00232 
00233 void PositionStringListDisplay::incomingMessage(const position_string_msgs::PositionStringList::ConstPtr& message)
00234 {
00235   current_message_ = message;
00236   new_msg_ = true;
00237   processMessage(message);
00238   new_msg_ = false;
00239 }
00240 
00241 void PositionStringListDisplay::reset()
00242 {
00243 }
00244 
00245 void PositionStringListDisplay::targetFrameChanged()
00246 {
00247 }
00248 
00249 void PositionStringListDisplay::createProperties()
00250 {
00251   character_height_property_ = property_manager_->createProperty<rviz::DoubleProperty> ("Character Height", property_prefix_, boost::bind(&PositionStringListDisplay::getCharacterHeight, this),
00252                                                                             boost::bind(&PositionStringListDisplay::setCharacterHeight, this, _1), parent_category_, this);
00253   color_property_ = property_manager_->createProperty<rviz::ColorProperty> ("Color", property_prefix_, boost::bind(&PositionStringListDisplay::getColor, this),
00254                                                                             boost::bind(&PositionStringListDisplay::setColor, this, _1), parent_category_, this);
00255   topic_property_ = property_manager_->createProperty<rviz::ROSTopicStringProperty> ("Topic", property_prefix_, boost::bind(&PositionStringListDisplay::getTopic, this),
00256                                                                                boost::bind(&PositionStringListDisplay::setTopic, this, _1), parent_category_, this);
00257   rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00258   topic_prop->setMessageType(ros::message_traits::DataType<position_string_msgs::PositionStringList>().value());
00259 }
00260 
00261 } // namespace position_string_rviz_plugin
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


position_string_rviz_plugin
Author(s): Nico Blodow
autogenerated on Thu May 23 2013 10:07:24