cop_rviz_plugin: cop_rviz_plugin: jlo_display

00001 /*
00002  * Copyright (c) 2008, U. Klank
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 the 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 
00031 #ifndef JLO_DISPLAY_BASE_H
00032 #define JLO_DISPLAY_BASE_H
00033 
00034 #include "rviz/display.h"
00035 #include "rviz/helpers/color.h"
00036 #include "rviz/properties/forwards.h"
00037 #include "rviz/visualization_manager.h"
00038 #include "rviz/frame_manager.h"
00039 #include "rviz/common.h"
00040 
00041 #include <vision_msgs/cop_answer.h>
00042 #include <vision_srvs/srvjlo.h>
00043 
00044 #include <tf/transform_listener.h>
00045 
00046 #include <ogre_tools/axes.h>
00047 #include <ogre_tools/movable_text.h>
00048 #include <OGRE/OgreSceneNode.h>
00049 #include <OGRE/OgreSceneManager.h>
00050 
00051 #include <list>
00052 
00053 namespace Ogre
00054 {
00055 class SceneNode;
00056 }
00057 
00058 namespace rviz_shows_cop
00059 {
00060 
00065 class JloDisplayBase : public rviz::Display
00066 {
00067 public:
00068   struct JloDescription
00069   {
00070     unsigned long pose;
00071     std::string label;
00072   };
00073   JloDisplayBase(const std::string& name, rviz::VisualizationManager* manager);
00074 
00075   virtual ~JloDisplayBase();
00076 
00077   virtual void setTopic(const std::string& topic);
00078   const std::string& getTopic()
00079   {
00080     return (topic_);
00081   }
00082 
00083   void setKeep(uint32_t keep);
00084   uint32_t getKeep() { return keep_; }
00085 
00086   void setST( bool show_text );
00087   bool getST() { return show_text_; }
00088   void setSA( bool show_axis );
00089   bool getSA() { return show_axis_; }
00090   void setSC( bool show_cov);
00091   bool getSC() { return show_cov_; }
00092   void setAL(double axis_length);
00093   double getAL() { return axis_length_; }
00094   void setAT(double axis_thick);
00095   double getAT() { return axis_thickness_; }
00096   void setCL(double cov_length);
00097   double getCL() { return cov_length_; }
00098   void setCT(double cov_thick);
00099   double getCT() { return cov_thickness_; }
00100 
00101   template<typename itT> std::list<std::pair<Ogre::SceneNode *, vision_msgs::partial_lo> >& displayJloSet(itT start, itT end, bool render);
00102 
00103   // Overrides from Display
00104   virtual void targetFrameChanged();
00105   virtual void fixedFrameChanged();
00106   virtual void createProperties();
00107   virtual void update(float wall_dt, float ros_dt);
00108   virtual void reset();
00109   virtual void processMessage()=0;
00110 
00111 protected:
00112   void clear();
00113 
00114   void popJloSet();
00115 
00116   // overrides from Display
00117   virtual void onEnable();
00118   virtual void onDisable();
00119 
00120   std::string topic_;
00121   uint32_t keep_;
00122   bool show_text_;
00123   bool show_axis_;
00124   bool show_cov_;
00125   double axis_length_;
00126   double axis_thickness_;
00127   double cov_length_;
00128   double cov_thickness_;
00129   bool m_binited;
00130   std::list< std::list< std::pair< Ogre::SceneNode *, vision_msgs::partial_lo> > > jlo_nodes_;
00131   Ogre::SceneNode* scene_node_;
00132 
00133   rviz::ROSTopicStringPropertyWPtr topic_property_;
00134   rviz::IntPropertyWPtr keep_property_;
00135   rviz::CategoryPropertyWPtr extended_props_p_;
00136   rviz::BoolPropertyWPtr show_cov_p_;
00137   rviz::BoolPropertyWPtr show_axis_p_;
00138   rviz::BoolPropertyWPtr show_text_p_;
00139   rviz::FloatPropertyWPtr axis_thickness_p_;
00140   rviz::FloatPropertyWPtr axis_length_p_;
00141   rviz::FloatPropertyWPtr cov_length_p_;
00142   rviz::FloatPropertyWPtr cov_thickness_p_;
00143 
00144   bool inited_jlo;
00145   ros::ServiceClient jlo_client;
00146   bool GetJlo(unsigned long id, unsigned long parent_id, vision_msgs::partial_lo &lo);
00147   unsigned  long NameQueryJlo(std::string name);
00148 
00149   template<typename T> void setSceneNodePose(Ogre::SceneNode* scene_node, T mat, Ogre::Quaternion &orientation);
00150 };
00151 
00152 
00153 template<typename itT>
00154 std::list<std::pair<Ogre::SceneNode *, vision_msgs::partial_lo> >& JloDisplayBase::displayJloSet(itT start, itT end, bool render=true)
00155 {
00156   if(m_binited)
00157   {
00158     Ogre::Quaternion orientation;
00159     const double matid[] = {1, 0, 0, 0,
00160                             0, 1, 0, 0,
00161                             0, 0, 1, 0,
00162                             0, 0, 0, 1};
00163 
00164     setSceneNodePose(scene_node_, matid, orientation);
00165 
00166     jlo_nodes_.push_back(std::list<std::pair<Ogre::SceneNode *, vision_msgs::partial_lo> >());
00167     while(jlo_nodes_.size() > keep_)
00168       popJloSet();
00169 
00170     for(itT it=start; it!=end; it++)
00171     {
00172       Ogre::SceneNode* node = scene_node_->createChildSceneNode();
00173 
00174       vision_msgs::partial_lo lo;
00175       vision_msgs::partial_lo::_pose_type mat;
00176       vision_msgs::partial_lo::_cov_type cov;
00177 
00178       if(it->pose == 1)
00179       {
00180         cov[ 0] = 0.0;
00181         cov[ 7] = 0.0;
00182         cov[14] = 0.0;
00183         cov[21] = 0.0;
00184         cov[28] = 0.0;
00185         cov[35] = 0.0;
00186         std::copy(&matid[0], &matid[16], mat.begin());
00187         lo.pose = mat;
00188       }
00189       else
00190       {
00191         unsigned long reference_frame_id = 1; 
00192         reference_frame_id = NameQueryJlo(fixed_frame_);
00193         printf("NAmeQuery %s resolved to id: %ld\n", fixed_frame_.c_str(), reference_frame_id);
00194         if(!GetJlo(it->pose, reference_frame_id, lo))
00195           continue;
00196         mat = lo.pose;
00197         cov = lo.cov;
00198       }
00199       jlo_nodes_.back().push_back(std::pair<Ogre::SceneNode *, vision_msgs::partial_lo>(node, lo));
00200       setSceneNodePose(node , mat, orientation);
00201       if(show_axis_)
00202       {
00203         ogre_tools::Axes* axes_ = new ogre_tools::Axes( scene_manager_, node, axis_length_, axis_thickness_);
00204         axes_->getSceneNode()->setVisible( true);
00205         axes_->setOrientation(orientation);
00206       }
00207       Ogre::Matrix3 rotMat;
00208       orientation.ToRotationMatrix(rotMat);
00209       if(show_cov_)
00210       {
00211         for(int j = 0; j< 12; j++)
00212         {
00213           ogre_tools::Axes* axes_cov = new ogre_tools::Axes( scene_manager_, node, cov_length_, cov_thickness_ );
00214           Ogre::SceneNode* axis_tmp = axes_cov->getSceneNode();
00215 
00216           Ogre::Vector3 vec(  cov[0] * (j%6 == 0 ? 1 : 0) * (j>5?-1:1),
00217                               cov[7] * (j%6 == 1 ? 1 : 0) * (j>5?-1:1),
00218                               cov[14] * (j%6 == 2 ? 1 : 0) * (j>5?-1:1));
00219 
00221            Ogre::Matrix3 rot (mat[0],mat[1],mat[2],
00222                               mat[4],mat[5],mat[6],
00223                               mat[8],mat[9],mat[10]);
00224           /*    normal -> ogre: x = -y, y = z, z = -x*/
00225           Ogre::Vector3 vec_trans = rot * vec;
00226           axis_tmp->setPosition(Ogre::Vector3(-vec_trans.y, vec_trans.z, -vec_trans.x));
00227           Ogre::Matrix3  temp;
00228           temp.FromEulerAnglesZYX(Ogre::Radian(cov[21] * (j%6 == 3 ? 1 : 0) * (j>5?-1:1)),
00229                                   Ogre::Radian(cov[28] * (j%6 == 4 ? 1 : 0) * (j>5?-1:1)),
00230                                   Ogre::Radian(cov[35] * (j%6 == 5 ? 1 : 0) * (j>5?-1:1)));
00231           Ogre::Quaternion temp_quat;
00232           temp = rotMat * temp;
00233           temp_quat.FromRotationMatrix(temp);
00234           axes_cov->setOrientation(temp_quat);
00235           axis_tmp->setVisible( true);
00236         }
00237       }
00238       if(show_text_)
00239       {
00240         ogre_tools::MovableText* text = new ogre_tools::MovableText(it->label, "Arial",0.1, Ogre::ColourValue::White);
00241         Ogre::SceneNode *text_node= node->createChildSceneNode();
00242         text_node->attachObject(text);
00243         text_node->setVisible( true);
00244       }
00245       node->setVisible(true);
00246     }
00247     if(render)
00248       causeRender();
00249   }
00250   return jlo_nodes_.back();
00251 }
00252 
00253 template<typename T>
00254 void JloDisplayBase::setSceneNodePose(Ogre::SceneNode* scene_node, T mat, Ogre::Quaternion &orientation)
00255 {
00256   if(m_binited)
00257   {
00258      std::string fxFrame = fixed_frame_;
00259      if(fixed_frame_.length() < 2)
00260        fxFrame = "/map";
00261      try
00262      {
00263        tf::Stamped<tf::Pose> pose_w(btTransform(btMatrix3x3(mat[0],mat[1],mat[2],
00264                                                          mat[4],mat[5],mat[6],
00265                                                          mat[8],mat[9],mat[10]),
00266                                   btVector3(mat[3], mat[7], mat[11])), ros::Time(),
00267                                   "/map");
00268 
00269       Ogre::Vector3 position;
00270       if(!vis_manager_->getFrameManager()->getTransform(fxFrame, ros::Time(), position, orientation, false))
00271       {
00272           ROS_ERROR("Error getting transforming frame '%s'",
00273             fxFrame.c_str());
00274             return;
00275       }
00276         /*vis_manager_->getTFClient()->transformPose(fixed_frame_, pose_w, pose_w);*/
00277       Ogre::Vector3 position_w = Ogre::Vector3(pose_w.getOrigin().x(),
00278         pose_w.getOrigin().y(), pose_w.getOrigin().z());
00279       rviz::robotToOgre(position_w);
00280       position_w.x += position.x;
00281       position_w.y += position.y;
00282       position_w.z += position.z;
00283       Ogre::Matrix3 rot, rot2;
00284       orientation.ToRotationMatrix(rot);
00285 
00286       rot2 = rot * Ogre::Matrix3(mat[0],mat[1],mat[2],mat[4],mat[5],mat[6],mat[8],mat[9],mat[10]);
00287       orientation.FromRotationMatrix(rot2);
00288       btScalar yaw_w, pitch_w, roll_w;
00289       pose_w.getBasis().getEulerYPR(yaw_w, pitch_w, roll_w);
00290 
00291       Ogre::Matrix3 orientation_w(1,0,0,0,1,0,0,0,1);
00292       scene_node->setPosition(position_w);
00293     }
00294     catch(...)
00295     {
00296       return;
00297     }
00298   }
00299 }
00300 
00301 } // namespace mapping_rviz_plugin
00302 
00303 
00304 #endif /* RVIZ_POLYGONAL_MAP_DISPLAY_H_ */
jlo_display_base.h
