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 #ifndef RVIZ_ROBOT_LINK_H
00031 #define RVIZ_ROBOT_LINK_H
00032
00033 #include "properties/forwards.h"
00034 #include "selection/forwards.h"
00035
00036 #include <ogre_tools/object.h>
00037
00038 #include <string>
00039 #include <map>
00040
00041 #include <OGRE/OgreVector3.h>
00042 #include <OGRE/OgreQuaternion.h>
00043 #include <OGRE/OgreAny.h>
00044 #include <OGRE/OgreMaterial.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 ogre_tools
00059 {
00060 class Shape;
00061 class Axes;
00062 }
00063
00064 namespace urdf
00065 {
00066 class Model;
00067 class Link;
00068 typedef boost::shared_ptr<const Link> LinkConstPtr;
00069 class Geometry;
00070 typedef boost::shared_ptr<const Geometry> GeometryConstPtr;
00071 class Pose;
00072 }
00073
00074 class TiXmlElement;
00075
00076 namespace rviz
00077 {
00078
00079 class PropertyManager;
00080 class Robot;
00081 class VisualizationManager;
00082 class RobotLinkSelectionHandler;
00083 typedef boost::shared_ptr<RobotLinkSelectionHandler> RobotLinkSelectionHandlerPtr;
00084
00089 class RobotLink
00090 {
00091 public:
00092 RobotLink(Robot* parent, VisualizationManager* manager);
00093 ~RobotLink();
00094
00095 void load(TiXmlElement* root_element, urdf::Model& descr, const urdf::LinkConstPtr& link, bool visual, bool collision);
00096
00097 void setAlpha(float a);
00098
00099 bool getShowTrail();
00100 void setShowTrail( bool show );
00101
00102 bool getShowAxes();
00103 void setShowAxes( bool show );
00104
00105 void setTransforms(const Ogre::Vector3& visual_position, const Ogre::Quaternion& visual_orientation,
00106 const Ogre::Vector3& collision_position, const Ogre::Quaternion& collision_orientation, bool applyOffsetTransforms);
00107
00108 Ogre::Vector3 getPositionInRobotFrame();
00109 Ogre::Quaternion getOrientationInRobotFrame();
00110
00111 const std::string& getName() { return name_; }
00112
00113 void setToErrorMaterial();
00114 void setToNormalMaterial();
00115
00116 void setPropertyManager(PropertyManager* property_manager);
00117 void createProperties();
00118
00119 bool isValid();
00120
00121 protected:
00122
00123 void createEntityForGeometryElement(TiXmlElement* root_element, const urdf::LinkConstPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, Ogre::SceneNode* parent_node, Ogre::Entity*& entity, Ogre::SceneNode*& scene_node, Ogre::SceneNode*& offset_node);
00124
00125 void createVisual(TiXmlElement* root_element, const urdf::LinkConstPtr& link);
00126 void createCollision(TiXmlElement* root_element, const urdf::LinkConstPtr& link);
00127 void createSelection(const urdf::Model& descr, const urdf::LinkConstPtr& link);
00128
00129
00130 Robot* parent_;
00131 Ogre::SceneManager* scene_manager_;
00132 PropertyManager* property_manager_;
00133 VisualizationManager* vis_manager_;
00134
00135 std::string name_;
00136
00137 typedef std::map<Ogre::SubEntity*, Ogre::MaterialPtr> M_SubEntityToMaterial;
00138 M_SubEntityToMaterial materials_;
00139 Ogre::MaterialPtr default_material_;
00140 std::string default_material_name_;
00141
00142 Ogre::Entity* visual_mesh_;
00143 Ogre::Entity* collision_mesh_;
00144
00145 Ogre::SceneNode* visual_node_;
00146 Ogre::SceneNode* visual_offset_node_;
00147 Ogre::SceneNode* collision_node_;
00148 Ogre::SceneNode* collision_offset_node_;
00149
00150 Ogre::Vector3 position_;
00151 Ogre::Quaternion orientation_;
00152
00153 Ogre::RibbonTrail* trail_;
00154
00155 ogre_tools::Axes* axes_;
00156
00157
00158 std::string joint_name_;
00159
00160 CollObjectHandle selection_object_;
00161 RobotLinkSelectionHandlerPtr selection_handler_;
00162
00163
00164 Vector3PropertyWPtr position_property_;
00165 QuaternionPropertyWPtr orientation_property_;
00166 BoolPropertyWPtr trail_property_;
00167 BoolPropertyWPtr axes_property_;
00168
00169 friend class RobotLinkSelectionHandler;
00170 };
00171
00172 }
00173
00174 #endif // RVIZ_ROBOT_LINK_H