robot_link.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
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 #ifndef RVIZ_ROBOT_LINK_H
00031 #define RVIZ_ROBOT_LINK_H
00032 
00033 #include <string>
00034 #include <map>
00035 
00036 #include <QObject>
00037 
00038 #include <OGRE/OgreVector3.h>
00039 #include <OGRE/OgreQuaternion.h>
00040 #include <OGRE/OgreAny.h>
00041 #include <OGRE/OgreMaterial.h>
00042 
00043 #include "rviz/ogre_helpers/object.h"
00044 #include "rviz/selection/forwards.h"
00045 
00046 namespace Ogre
00047 {
00048 class SceneManager;
00049 class Entity;
00050 class SubEntity;
00051 class SceneNode;
00052 class Vector3;
00053 class Quaternion;
00054 class Any;
00055 class RibbonTrail;
00056 }
00057 
00058 namespace urdf
00059 {
00060 class ModelInterface;
00061 class Link;
00062 typedef boost::shared_ptr<const Link> LinkConstPtr;
00063 class Geometry;
00064 typedef boost::shared_ptr<const Geometry> GeometryConstPtr;
00065 class Pose;
00066 }
00067 
00068 namespace rviz
00069 {
00070 class Shape;
00071 class Axes;
00072 class DisplayContext;
00073 class FloatProperty;
00074 class Property;
00075 class BoolProperty;
00076 class QuaternionProperty;
00077 class Robot;
00078 class RobotLinkSelectionHandler;
00079 class VectorProperty;
00080 class RobotJoint;
00081 typedef boost::shared_ptr<RobotLinkSelectionHandler> RobotLinkSelectionHandlerPtr;
00082 
00083 
00088 class RobotLink: public QObject
00089 {
00090 Q_OBJECT
00091 public:
00092   RobotLink( Robot* robot,
00093              const urdf::LinkConstPtr& link,
00094              const std::string& parent_joint_name,
00095              bool visual,
00096              bool collision);
00097   virtual ~RobotLink();
00098 
00099   virtual void setRobotAlpha(float a);
00100 
00101   virtual void setTransforms(const Ogre::Vector3& visual_position, const Ogre::Quaternion& visual_orientation,
00102                      const Ogre::Vector3& collision_position, const Ogre::Quaternion& collision_orientation);
00103 
00104   // access
00105   const std::string& getName() const { return name_; }
00106   const std::string& getParentJointName() const { return parent_joint_name_; }
00107   const std::vector<std::string>& getChildJointNames() const { return child_joint_names_; }
00108   Property* getLinkProperty() const { return link_property_; }
00109   Ogre::SceneNode* getVisualNode() const { return visual_node_; }
00110   Ogre::SceneNode* getCollisionNode() const { return collision_node_; }
00111   Robot* getRobot() const { return robot_; }
00112 
00113   // Remove link_property_ from its old parent and add to new_parent.  If new_parent==NULL then leav unparented.
00114   void setParentProperty(Property* new_parent);
00115 
00116   // hide or show all sub properties (hide to make tree easier to see)
00117   virtual void hideSubProperties(bool hide);
00118 
00119   void setToErrorMaterial();
00120   void setToNormalMaterial();
00121 
00122   void setColor( float red, float green, float blue );
00123   void unsetColor();
00124 
00126   bool setSelectable( bool selectable );
00127   bool getSelectable();
00128 
00129   Ogre::Vector3 getPosition();
00130   Ogre::Quaternion getOrientation();
00131 
00132   bool hasGeometry() const;
00133 
00134   /* If set to true, the link will only render to the depth channel
00135    * and be in render group 0, so it is rendered before anything else.
00136    * Thus, it will occlude other objects without being visible.
00137    */
00138   void setOnlyRenderDepth( bool onlyRenderDepth );
00139   bool getOnlyRenderDepth() const { return only_render_depth_; }
00140 
00141   // place subproperties as children of details_ or joint_property_
00142   void useDetailProperty(bool use_detail);
00143 
00144   // expand all sub properties
00145   void expandDetails(bool expand);
00146 
00147 public Q_SLOTS:
00152   void updateVisibility();
00153 
00154 private Q_SLOTS:
00155   void updateAlpha();
00156   void updateTrail();
00157   void updateAxes();
00158 
00159 private:
00160   void setRenderQueueGroup( Ogre::uint8 group );
00161   bool getEnabled() const;
00162   void createEntityForGeometryElement( const urdf::LinkConstPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, Ogre::SceneNode* scene_node, Ogre::Entity*& entity );
00163 
00164   void createVisual( const urdf::LinkConstPtr& link);
00165   void createCollision( const urdf::LinkConstPtr& link);
00166   void createSelection();
00167   Ogre::MaterialPtr getMaterialForLink( const urdf::LinkConstPtr& link );
00168 
00169 
00170 protected:
00171   Robot* robot_;
00172   Ogre::SceneManager* scene_manager_;
00173   DisplayContext* context_;
00174 
00175   std::string name_;                          
00176   std::string parent_joint_name_;
00177   std::vector<std::string> child_joint_names_;
00178 
00179   
00180 
00181   // properties
00182   Property* link_property_;
00183   Property* details_;
00184   VectorProperty* position_property_;
00185   QuaternionProperty* orientation_property_;
00186   Property* trail_property_;
00187   Property* axes_property_;
00188   FloatProperty* alpha_property_;
00189 
00190 private:
00191   typedef std::map<Ogre::SubEntity*, Ogre::MaterialPtr> M_SubEntityToMaterial;
00192   M_SubEntityToMaterial materials_;
00193   Ogre::MaterialPtr default_material_;
00194   std::string default_material_name_;
00195 
00196   std::vector<Ogre::Entity*> visual_meshes_;    
00197   std::vector<Ogre::Entity*> collision_meshes_; 
00198 
00199   Ogre::SceneNode* visual_node_;              
00200   Ogre::SceneNode* collision_node_;           
00201 
00202   Ogre::RibbonTrail* trail_;
00203 
00204   Axes* axes_;
00205 
00206   float material_alpha_; 
00207   float robot_alpha_; 
00208 
00209   bool only_render_depth_;
00210   bool is_selectable_;
00211 
00212   // joint stuff
00213   std::string joint_name_;
00214 
00215   RobotLinkSelectionHandlerPtr selection_handler_;
00216 
00217   Ogre::MaterialPtr color_material_;
00218   bool using_color_;
00219 
00220   friend class RobotLinkSelectionHandler;
00221 };
00222 
00223 } // namespace rviz
00224 
00225 #endif // RVIZ_ROBOT_LINK_H


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Oct 6 2014 07:26:36