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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:28