Go to the documentation of this file.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_H_
00031 #define RVIZ_ROBOT_H_
00032
00033 #include "link_updater.h"
00034
00035 #include <string>
00036 #include <map>
00037
00038 #include <OgreVector3.h>
00039 #include <OgreQuaternion.h>
00040 #include <OgreAny.h>
00041
00042 namespace Ogre
00043 {
00044 class SceneManager;
00045 class Entity;
00046 class SceneNode;
00047 class Vector3;
00048 class Quaternion;
00049 class Any;
00050 class RibbonTrail;
00051 class SceneNode;
00052 }
00053
00054 namespace rviz
00055 {
00056 class Object;
00057 class Axes;
00058 }
00059
00060 namespace tf
00061 {
00062 class TransformListener;
00063 }
00064
00065 namespace urdf
00066 {
00067 class ModelInterface;
00068 class Link;
00069 class Joint;
00070 }
00071
00072 namespace rviz
00073 {
00074
00075 class Property;
00076 class EnumProperty;
00077 class BoolProperty;
00078 class Robot;
00079 class RobotLink;
00080 class RobotJoint;
00081 class DisplayContext;
00082
00089 class Robot : public QObject
00090 {
00091 Q_OBJECT
00092 public:
00093 Robot( Ogre::SceneNode* root_node, DisplayContext* context, const std::string& name, Property* parent_property );
00094 virtual ~Robot();
00095
00103 virtual void load( const urdf::ModelInterface &urdf, bool visual = true, bool collision = true );
00104
00108 virtual void clear();
00109
00110 virtual void update(const LinkUpdater& updater);
00111
00116 virtual void setVisible( bool visible );
00117
00122 void setVisualVisible( bool visible );
00123
00128 void setCollisionVisible( bool visible );
00129
00133 bool isVisible();
00138 bool isVisualVisible();
00143 bool isCollisionVisible();
00144
00145 void setAlpha(float a);
00146 float getAlpha() { return alpha_; }
00147
00148 RobotLink* getRootLink() { return root_link_; }
00149 RobotLink* getLink( const std::string& name );
00150 RobotJoint* getJoint( const std::string& name );
00151
00152 typedef std::map< std::string, RobotLink* > M_NameToLink;
00153 typedef std::map< std::string, RobotJoint* > M_NameToJoint;
00154 const M_NameToLink& getLinks() const { return links_; }
00155 const M_NameToJoint& getJoints() const { return joints_; }
00156
00157 const std::string& getName() { return name_; }
00158
00159 Ogre::SceneNode* getVisualNode() { return root_visual_node_; }
00160 Ogre::SceneNode* getCollisionNode() { return root_collision_node_; }
00161 Ogre::SceneNode* getOtherNode() { return root_other_node_; }
00162 Ogre::SceneManager* getSceneManager() { return scene_manager_; }
00163 DisplayContext* getDisplayContext() { return context_; }
00164
00165 virtual void setPosition( const Ogre::Vector3& position );
00166 virtual void setOrientation( const Ogre::Quaternion& orientation );
00167 virtual void setScale( const Ogre::Vector3& scale );
00168 virtual const Ogre::Vector3& getPosition();
00169 virtual const Ogre::Quaternion& getOrientation();
00170
00172 class LinkFactory
00173 {
00174 public:
00175 virtual RobotLink* createLink( Robot* robot,
00176 const boost::shared_ptr<const urdf::Link>& link,
00177 const std::string& parent_joint_name,
00178 bool visual,
00179 bool collision);
00180 virtual RobotJoint* createJoint( Robot* robot,
00181 const boost::shared_ptr<const urdf::Joint>& joint);
00182 };
00183
00193 void setLinkFactory(LinkFactory *link_factory);
00194
00195
00196 enum LinkTreeStyle {
00197 STYLE_LINK_LIST,
00198 STYLE_DEFAULT = STYLE_LINK_LIST,
00199 STYLE_JOINT_LIST,
00200 STYLE_LINK_TREE,
00201 STYLE_JOINT_LINK_TREE
00202 };
00203
00205 void setLinkTreeStyle(LinkTreeStyle style);
00206
00208 Property *getLinkTreeProperty()
00209 {
00210 return link_tree_;
00211 }
00212
00213
00214 void calculateJointCheckboxes();
00215
00216 private Q_SLOTS:
00217 void changedLinkTreeStyle();
00218 void changedExpandTree();
00219 void changedHideSubProperties();
00220 void changedEnableAllLinks();
00221 void changedExpandLinkDetails();
00222 void changedExpandJointDetails();
00223
00224 protected:
00226 void updateLinkVisibilities();
00227
00230 void unparentLinkProperties();
00231
00232
00233 void useDetailProperty(bool use_detail);
00234
00236 void addLinkToLinkTree(LinkTreeStyle style, Property *parent, RobotLink *link);
00237 void addJointToLinkTree(LinkTreeStyle style, Property *parent, RobotJoint *joint);
00238
00239
00240 void setEnableAllLinksCheckbox(QVariant val);
00241
00243 void initLinkTreeStyle();
00244 static bool styleShowLink(LinkTreeStyle style);
00245 static bool styleShowJoint(LinkTreeStyle style);
00246 static bool styleIsTree(LinkTreeStyle style);
00247
00248 Ogre::SceneManager* scene_manager_;
00249
00250 M_NameToLink links_;
00251 M_NameToJoint joints_;
00252 RobotLink *root_link_;
00253
00254 LinkFactory *link_factory_;
00255
00256 Ogre::SceneNode* root_visual_node_;
00257 Ogre::SceneNode* root_collision_node_;
00258 Ogre::SceneNode* root_other_node_;
00259
00260 bool visible_;
00261 bool visual_visible_;
00262 bool collision_visible_;
00263
00264 DisplayContext* context_;
00265 Property* link_tree_;
00266 EnumProperty* link_tree_style_;
00267 BoolProperty* expand_tree_;
00268 BoolProperty* expand_link_details_;
00269 BoolProperty* expand_joint_details_;
00270 BoolProperty* enable_all_links_;
00271 std::map<LinkTreeStyle, std::string> style_name_map_;
00272
00273 bool doing_set_checkbox_;
00274 bool robot_loaded_;
00275
00276
00277
00278 bool inChangedEnableAllLinks;
00279
00280 std::string name_;
00281 float alpha_;
00282 };
00283
00284 }
00285
00286 #endif