mass property visualization (#714)

* Add mass properties to robot model display, robot, and robot link.
Visual representation is based on gazebo,
i.e. visualize mass as equivalent lead ball and inertia as box.

* revert mock_view_picker

* fix whitespace

* fix whitespace

* fix whitespace

* add definition for M_PI on windows

Author: briansoe66

rviz_default_plugins/include/rviz_default_plugins/displays/robot_model/robot_model_display.hpp

@@ -107,6 +107,8 @@ private Q_SLOTS:
   void updateAlpha();
   void updatePropertyVisibility();
   void updateRobotDescription();
+  void updateMassVisible();
+  void updateInertiaVisible();
 
   void updateTopic() override;
 
@@ -143,6 +145,10 @@ private Q_SLOTS:
   rviz_common::properties::FloatProperty * alpha_property_;
   rviz_common::properties::StringProperty * tf_prefix_property_;
 
+  rviz_common::properties::Property * mass_properties_;
+  rviz_common::properties::Property * mass_enabled_property_;
+  rviz_common::properties::Property * inertia_enabled_property_;
+
   std::unique_ptr<rviz_default_plugins::transformation::TransformerGuard<
       rviz_default_plugins::transformation::TFFrameTransformer>> transformer_guard_;
 };

rviz_default_plugins/include/rviz_default_plugins/robot/robot.hpp

@@ -110,8 +110,15 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC Robot : public QObject
    * @param urdf The robot description to read from
    * @param visual Whether or not to load the visual representation
    * @param collision Whether or not to load the collision representation
+   * @param mass Whether or not to load the mass representation
+   * @param inertia Whether or not to load the inertia representation
    */
-  virtual void load(const urdf::ModelInterface & urdf, bool visual = true, bool collision = true);
+  virtual void load(
+    const urdf::ModelInterface & urdf,
+    bool visual = true,
+    bool collision = true,
+    bool mass = true,
+    bool inertia = true);
 
   /**
    * \brief Clears all data loaded from a URDF
@@ -138,6 +145,18 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC Robot : public QObject
    */
   void setCollisionVisible(bool visible);
 
+  /**
+   * \brief Set whether the mass of each part is visible
+   * @param visible Whether the mass of each link is visible
+   */
+  void setMassVisible(bool visible);
+
+  /**
+   * \brief Set whether the inertia of each part is visible
+   * @param visible Whether the inertia of each link is visible
+   */
+  void setInertiaVisible(bool visible);
+
   /**
    * \brief Returns whether anything is visible
    */
@@ -153,6 +172,18 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC Robot : public QObject
    */
   bool isCollisionVisible();
 
+  /**
+   * \brief Returns whether or not mass of each link is visible.
+   * To be visible this and isVisible() must both be true
+   */
+  bool isMassVisible();
+
+  /**
+   * \brief Returns whether or not inertia of each link is visible.
+   * To be visible this and isVisible() must both be true
+   */
+  bool isInertiaVisible();
+
   void setAlpha(float a);
   float getAlpha() {return alpha_;}
 
@@ -190,7 +221,9 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC Robot : public QObject
       const urdf::LinkConstSharedPtr & link,
       const std::string & parent_joint_name,
       bool visual,
-      bool collision);
+      bool collision,
+      bool mass,
+      bool inertia);
     virtual RobotJoint * createJoint(
       Robot * robot,
       const urdf::JointConstSharedPtr & joint);
@@ -280,6 +313,9 @@ private Q_SLOTS:
   bool visual_visible_;                         ///< Should we show the visual representation?
   bool collision_visible_;                      ///< Should we show the collision representation?
 
+  bool mass_visible_;                           ///< Should we show mass of each link?
+  bool inertia_visible_;                        ///< Should we show inertia of each link?
+
   rviz_common::DisplayContext * context_;
   rviz_common::properties::Property * link_tree_;
   rviz_common::properties::EnumProperty * link_tree_style_;
@@ -300,7 +336,12 @@ private Q_SLOTS:
   float alpha_;
 
 private:
-  void createLinkProperties(const urdf::ModelInterface & urdf, bool visual, bool collision);
+  void createLinkProperties(
+    const urdf::ModelInterface & urdf,
+    bool visual,
+    bool collision,
+    bool mass,
+    bool inertia);
   void createJointProperties(const urdf::ModelInterface & urdf);
   void log_error(
     const RobotLink * link,

rviz_default_plugins/include/rviz_default_plugins/robot/robot_link.hpp

@@ -112,7 +112,9 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC RobotLink : public RobotElementBaseClass
     const urdf::LinkConstSharedPtr & link,
     const std::string & parent_joint_name,
     bool visual,
-    bool collision);
+    bool collision,
+    bool mass,
+    bool inertia);
   ~RobotLink() override;
 
   virtual void setRobotAlpha(float a);
@@ -188,6 +190,8 @@ private Q_SLOTS:
 
   void createCollision(const urdf::LinkConstSharedPtr & link);
   void createVisual(const urdf::LinkConstSharedPtr & link);
+  void createMass(const urdf::LinkConstSharedPtr & link);
+  void createInertia(const urdf::LinkConstSharedPtr & link);
   void createSelection();
 
   template<typename T>
@@ -243,10 +247,12 @@ private Q_SLOTS:
   std::vector<Ogre::Entity *> collision_meshes_;  ///< The entities representing the
 ///< collision mesh of this link (if they exist)
 
-  Ogre::SceneNode * visual_node_;              ///< The scene node the visual meshes
-///< are attached to
-  Ogre::SceneNode * collision_node_;           ///< The scene node the collision meshes
-///< are attached to
+  Ogre::SceneNode * visual_node_;          ///< The scene node the visual meshes are attached to
+  Ogre::SceneNode * collision_node_;       ///< The scene node the collision meshes are attached to
+  Ogre::SceneNode * mass_node_;            ///< The scene node the visual meshes are attached to
+  Ogre::SceneNode * inertia_node_;         ///< The scene node the collision meshes are attached to
+  rviz_rendering::Shape * mass_shape_;     ///< The shape representing the mass
+  rviz_rendering::Shape * inertia_shape_;  ///< The shape representing the inertia
 
   Ogre::RibbonTrail * trail_;
 

rviz_default_plugins/src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp

@@ -94,6 +94,17 @@ RobotModelDisplay::RobotModelDisplay()
     "Whether to display the collision representation of the robot.",
     this, SLOT(updateCollisionVisible()));
 
+  mass_properties_ = new Property("Mass Properties", QVariant(), "", this);
+  mass_enabled_property_ = new Property(
+    "Mass", false,
+    "Whether to display the visual representation of the mass of each link.",
+    mass_properties_, SLOT(updateMassVisible()), this);
+  inertia_enabled_property_ = new Property(
+    "Inertia", false,
+    "Whether to display the visual representation of the inertia of each link.",
+    mass_properties_, SLOT(updateInertiaVisible()), this);
+  mass_properties_->collapse();
+
   update_rate_property_ = new FloatProperty(
     "Update Interval", 0,
     "Interval at which to update the links, in seconds. "
@@ -202,6 +213,18 @@ void RobotModelDisplay::updateTfPrefix()
   context_->queueRender();
 }
 
+void RobotModelDisplay::updateMassVisible()
+{
+  robot_->setMassVisible(mass_enabled_property_->getValue().toBool());
+  context_->queueRender();
+}
+
+void RobotModelDisplay::updateInertiaVisible()
+{
+  robot_->setInertiaVisible(inertia_enabled_property_->getValue().toBool());
+  context_->queueRender();
+}
+
 void RobotModelDisplay::load_urdf()
 {
   if (!transformer_guard_->checkTransformer()) {

rviz_default_plugins/src/rviz_default_plugins/robot/robot.cpp

@@ -69,6 +69,8 @@ Robot::Robot(
   visible_(true),
   visual_visible_(true),
   collision_visible_(false),
+  mass_visible_(false),
+  inertia_visible_(false),
   context_(context),
   doing_set_checkbox_(false),
   robot_loaded_(false),
@@ -83,6 +85,8 @@ Robot::Robot(
 
   setVisualVisible(visual_visible_);
   setCollisionVisible(collision_visible_);
+  setMassVisible(mass_visible_);
+  setInertiaVisible(inertia_visible_);
   setAlpha(1.0f);
 
   link_tree_ = new Property("Links", QVariant(), "", parent_property);
@@ -136,7 +140,12 @@ Robot::~Robot()
   delete link_factory_;
 }
 
-void Robot::load(const urdf::ModelInterface & urdf, bool visual, bool collision)
+void Robot::load(
+  const urdf::ModelInterface & urdf,
+  bool visual,
+  bool collision,
+  bool mass,
+  bool inertia)
 {
   link_tree_->hide();  // hide until loaded
   robot_loaded_ = false;
@@ -148,7 +157,7 @@ void Robot::load(const urdf::ModelInterface & urdf, bool visual, bool collision)
   root_link_ = nullptr;
 
   // Properties are not added to display until changedLinkTreeStyle() is called (below).
-  createLinkProperties(urdf, visual, collision);
+  createLinkProperties(urdf, visual, collision, mass, inertia);
   createJointProperties(urdf);
 
   // robot is now loaded
@@ -164,6 +173,8 @@ void Robot::load(const urdf::ModelInterface & urdf, bool visual, bool collision)
 
   setVisualVisible(isVisualVisible() );
   setCollisionVisible(isCollisionVisible() );
+  setMassVisible(isMassVisible());
+  setInertiaVisible(isInertiaVisible());
 }
 
 void Robot::clear()
@@ -279,6 +290,18 @@ void Robot::setCollisionVisible(bool visible)
   updateLinkVisibilities();
 }
 
+void Robot::setMassVisible(bool visible)
+{
+  mass_visible_ = visible;
+  updateLinkVisibilities();
+}
+
+void Robot::setInertiaVisible(bool visible)
+{
+  inertia_visible_ = visible;
+  updateLinkVisibilities();
+}
+
 void Robot::updateLinkVisibilities()
 {
   for (auto & link_map_entry : links_) {
@@ -302,6 +325,16 @@ bool Robot::isCollisionVisible()
   return collision_visible_;
 }
 
+bool Robot::isMassVisible()
+{
+  return mass_visible_;
+}
+
+bool Robot::isInertiaVisible()
+{
+  return inertia_visible_;
+}
+
 void Robot::setAlpha(float a)
 {
   alpha_ = a;
@@ -410,9 +443,11 @@ RobotLink * Robot::LinkFactory::createLink(
   const urdf::LinkConstSharedPtr & link,
   const std::string & parent_joint_name,
   bool visual,
-  bool collision)
+  bool collision,
+  bool mass,
+  bool inertia)
 {
-  return new RobotLink(robot, link, parent_joint_name, visual, collision);
+  return new RobotLink(robot, link, parent_joint_name, visual, collision, mass, inertia);
 }
 
 RobotJoint * Robot::LinkFactory::createJoint(
@@ -422,7 +457,12 @@ RobotJoint * Robot::LinkFactory::createJoint(
   return new RobotJoint(robot, joint);
 }
 
-void Robot::createLinkProperties(const urdf::ModelInterface & urdf, bool visual, bool collision)
+void Robot::createLinkProperties(
+  const urdf::ModelInterface & urdf,
+  bool visual,
+  bool collision,
+  bool mass,
+  bool inertia)
 {
   for (const auto & link_entry : urdf.links_) {
     const urdf::LinkConstSharedPtr & urdf_link = link_entry.second;
@@ -433,7 +473,7 @@ void Robot::createLinkProperties(const urdf::ModelInterface & urdf, bool visual,
     }
 
     RobotLink * link = link_factory_->createLink(
-      this, urdf_link, parent_joint_name, visual, collision);
+      this, urdf_link, parent_joint_name, visual, collision, mass, inertia);
 
     if (urdf_link == urdf.getRoot()) {
       root_link_ = link;