00001 /* 00002 * Copyright (c) 2013, 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_JOINT_H 00031 #define RVIZ_ROBOT_JOINT_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 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 class Joint; 00063 class Geometry; 00064 class Pose; 00065 } 00066 00067 namespace rviz 00068 { 00069 class Shape; 00070 class Axes; 00071 class DisplayContext; 00072 class FloatProperty; 00073 class Property; 00074 class BoolProperty; 00075 class QuaternionProperty; 00076 class Robot; 00077 class RobotLinkSelectionHandler; 00078 class VectorProperty; 00079 class RobotJoint; 00080 00081 00086 class RobotJoint: public QObject 00087 { 00088 Q_OBJECT 00089 public: 00090 RobotJoint( Robot* robot, const boost::shared_ptr<const urdf::Joint>& joint ); 00091 virtual ~RobotJoint(); 00092 00093 00094 void setTransforms(const Ogre::Vector3& parent_link_position, 00095 const Ogre::Quaternion& parent_link_orientation); 00096 00097 const std::string& getName() const { return name_; } 00098 const std::string& getParentLinkName() const { return parent_link_name_; } 00099 const std::string& getChildLinkName() const { return child_link_name_; } 00100 const Property* getJointProperty() const { return joint_property_; } 00101 Property* getJointProperty() { return joint_property_; } 00102 RobotJoint* getParentJoint(); 00103 void hideSubProperties(bool hide); 00104 00105 // Remove joint_property_ from its old parent and add to new_parent. If new_parent==NULL then leav unparented. 00106 void setParentProperty(Property* new_parent); 00107 00108 Ogre::Vector3 getPosition(); 00109 Ogre::Quaternion getOrientation(); 00110 00111 void setRobotAlpha(float a) {} 00112 00113 bool hasDescendentLinksWithGeometry() const { return has_decendent_links_with_geometry_; } 00114 00115 // place subproperties as children of details_ or joint_property_ 00116 void useDetailProperty(bool use_detail); 00117 00118 // expand all sub properties 00119 void expandDetails(bool expand); 00120 00121 // Set the description for the joint. 00122 // Also sets the checkbox. 00123 // Also sets has_decendent_links_with_geometry_. 00124 // Called when the link_tree style changes. 00125 void setJointPropertyDescription(); 00126 00127 // set checkboxes based on state of descendent link enables 00128 // Should only be called by Robot::calculateJointCheckboxes() 00129 void calculateJointCheckboxesRecursive( 00130 int& links_with_geom, // returns # of children with geometry 00131 int& links_with_geom_checked, // returns # of enabled children with geometry 00132 int& links_with_geom_unchecked); // returns # of disabled children with geometry 00133 00134 00135 private Q_SLOTS: 00136 void updateAxes(); 00137 void updateChildVisibility(); 00138 00139 private: 00140 bool getEnabled() const; 00141 00142 // true if displaying in a tree style. False if list style. 00143 bool styleIsTree() const; 00144 00145 // determine the state of child link(s) 00146 void getChildLinkState( 00147 int& links_with_geom, // returns # of children with geometry 00148 int& links_with_geom_checked, // returns # of enabled children with geometry 00149 int& links_with_geom_unchecked, // returns # of disabled children with geometry 00150 bool recursive) const; // True: all descendant links. False: just single child link. 00151 00152 // set the value of the enable checkbox without touching child joints/links 00153 void setJointCheckbox(QVariant val); 00154 00155 00156 protected: 00157 Robot* robot_; 00158 std::string name_; 00159 std::string parent_link_name_; 00160 std::string child_link_name_; 00161 00162 // properties 00163 Property* joint_property_; 00164 Property* details_; 00165 VectorProperty* position_property_; 00166 QuaternionProperty* orientation_property_; 00167 Property* axes_property_; 00168 00169 private: 00170 Ogre::Vector3 joint_origin_pos_; 00171 Ogre::Quaternion joint_origin_rot_; 00172 bool has_decendent_links_with_geometry_; 00173 00174 bool doing_set_checkbox_; // prevents updateChildVisibility() from touching children 00175 00176 Axes* axes_; 00177 }; 00178 00179 } // namespace rviz 00180 00181 #endif // RVIZ_ROBOT_LINK_H