Add links' Center Of Mass visualisation for RobotModel display type

src/rviz/default_plugin/robot_model_display.cpp

@@ -88,6 +88,19 @@ RobotModelDisplay::RobotModelDisplay()
                                             "Robot Model normally assumes the link name is the same as the tf frame name. "
                                             " This option allows you to set a prefix.  Mainly useful for multi-robot situations.",
                                             this, SLOT( updateTfPrefix() ));
+
+  // Links' center of mass display
+  center_of_mass_property_ = new Property("Center of mass", true,
+                                          "Whether to display centers of mass of each part. Set link alpha less then 1.0",
+                                          this, SLOT(update_center_of_mass_visible()), this);
+
+  center_of_mass_marker_scale_property_ = new FloatProperty("Marker scale", 0.03,
+                                                            "Center of mass marker scale",
+                                                            center_of_mass_property_,
+                                                            SLOT(update_center_of_mass_marker_scale()), this);
+
+
+  center_of_mass_property_->collapse();
 }
 
 RobotModelDisplay::~RobotModelDisplay()
@@ -105,6 +118,8 @@ void RobotModelDisplay::onInitialize()
   updateVisualVisible();
   updateCollisionVisible();
   updateAlpha();
+  update_center_of_mass_visible();
+  update_center_of_mass_marker_scale();
 }
 
 void RobotModelDisplay::updateAlpha()
@@ -134,6 +149,7 @@ void RobotModelDisplay::updateCollisionVisible()
   context_->queueRender();
 }
 
+
 void RobotModelDisplay::updateTfPrefix()
 {
   clearStatuses();
@@ -246,6 +262,20 @@ void RobotModelDisplay::reset()
   has_new_transforms_ = true;
 }
 
+
+
+void RobotModelDisplay::update_center_of_mass_visible()
+{
+  robot_->setCentersOfMassVisible( center_of_mass_property_->getValue().toBool() );
+  context_->queueRender();
+}
+
+void RobotModelDisplay::update_center_of_mass_marker_scale()
+{
+  robot_->setCenterOfMassMarkerScale(center_of_mass_marker_scale_property_->getFloat());
+  context_->queueRender();
+}
+
 } // namespace rviz
 
 #include <pluginlib/class_list_macros.h>

src/rviz/default_plugin/robot_model_display.h

@@ -81,6 +81,9 @@ private Q_SLOTS:
   void updateAlpha();
   void updateRobotDescription();
 
+  void update_center_of_mass_visible();
+  void update_center_of_mass_marker_scale();
+
 protected:
   /** @brief Loads a URDF from the ros-param named by our
    * "Robot Description" property, iterates through the links, and
@@ -105,6 +108,9 @@ private Q_SLOTS:
   StringProperty* robot_description_property_;
   FloatProperty* alpha_property_;
   StringProperty* tf_prefix_property_;
+
+  Property* center_of_mass_property_;                         /// < Sets visibility of links' CoMs
+  FloatProperty* center_of_mass_marker_scale_property_;       /// < Sets scale of CoMs' markers
 };
 
 } // namespace rviz

src/rviz/robot/robot.cpp

@@ -164,6 +164,7 @@ void Robot::setCollisionVisible( bool visible )
   updateLinkVisibilities();
 }
 
+
 void Robot::updateLinkVisibilities()
 {
   M_NameToLink::iterator it = links_.begin();
@@ -175,6 +176,7 @@ void Robot::updateLinkVisibilities()
   }
 }
 
+
 bool Robot::isVisible()
 {
   return visible_;
@@ -190,6 +192,7 @@ bool Robot::isCollisionVisible()
   return collision_visible_;
 }
 
+
 void Robot::setAlpha(float a)
 {
   alpha_ = a;
@@ -204,6 +207,7 @@ void Robot::setAlpha(float a)
   }
 }
 
+
 void Robot::clear()
 {
   // unparent all link and joint properties so they can be deleted in arbitrary
@@ -239,9 +243,10 @@ RobotLink* Robot::LinkFactory::createLink(
     const urdf::LinkConstSharedPtr& link,
     const std::string& parent_joint_name,
     bool visual,
-    bool collision)
+    bool collision,
+    bool center_of_mass)
 {
-  return new RobotLink(robot, link, parent_joint_name, visual, collision);
+  return new RobotLink(robot, link, parent_joint_name, visual, collision, center_of_mass);
 }
 
 RobotJoint* Robot::LinkFactory::createJoint(
@@ -251,7 +256,7 @@ RobotJoint* Robot::LinkFactory::createJoint(
   return new RobotJoint(robot, joint);
 }
 
-void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision )
+void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision, bool center_of_mass )
 {
   link_tree_->hide(); // hide until loaded
   robot_loaded_ = false;
@@ -282,7 +287,8 @@ void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision
                                                    urdf_link,
                                                    parent_joint_name,
                                                    visual,
-                                                   collision );
+                                                   collision,
+                                                   center_of_mass);
 
       if (urdf_link == urdf.getRoot())
       {
@@ -325,6 +331,8 @@ void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision
 
   setVisualVisible( isVisualVisible() );
   setCollisionVisible( isCollisionVisible() );
+  setCentersOfMassVisible( isCentersOfMassVisible() );
+  setCenterOfMassMarkerScale(getCenterOfMassMarkerScale());
 }
 
 void Robot::unparentLinkProperties()
@@ -736,6 +744,7 @@ void Robot::update(const LinkUpdater& updater)
     link->setToNormalMaterial();
 
     Ogre::Vector3 visual_position, collision_position;
+    Ogre::Vector3 com_position;
     Ogre::Quaternion visual_orientation, collision_orientation;
     if( updater.getLinkTransforms( link->getName(),
                                    visual_position, visual_orientation,
@@ -827,4 +836,35 @@ const Ogre::Quaternion& Robot::getOrientation()
   return root_visual_node_->getOrientation();
 }
 
+
+void Robot::setCentersOfMassVisible( bool visible)
+{
+  centers_of_mass_visible_ = visible;
+  updateLinkVisibilities();
+}
+
+void Robot::setCenterOfMassMarkerScale(float scale)
+{
+  center_of_mass_marker_scale_ = scale;
+
+  M_NameToLink::iterator it = links_.begin();
+  M_NameToLink::iterator end = links_.end();
+  for ( ; it != end; ++it )
+  {
+    RobotLink* link = it->second;
+
+    link->setCenterOfMassMarkerScale(center_of_mass_marker_scale_);
+  }
+}
+
+float Robot::getCenterOfMassMarkerScale()
+{
+  return center_of_mass_marker_scale_;
+}
+
+bool Robot::isCentersOfMassVisible()
+{
+  return centers_of_mass_visible_;
+}
+
 } // namespace rviz

src/rviz/robot/robot.h

@@ -94,8 +94,12 @@ Q_OBJECT
    * @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 center_of_mass Whether or not to load the links' centers of mass
    */
-  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 center_of_mass = true );
 
   /**
    * \brief Clears all data loaded from a URDF
@@ -122,6 +126,7 @@ Q_OBJECT
    */
   void setCollisionVisible( bool visible );
 
+
   /**
    * \brief Returns whether anything is visible
    */
@@ -140,6 +145,34 @@ Q_OBJECT
   void setAlpha(float a);
   float getAlpha() { return alpha_; }
 
+
+
+  /**
+   * \brief Set wheter the centers of mass of each part is visible
+   * @param visible Wheter the centers of mass of each link is visible
+   */
+  void setCentersOfMassVisible(bool visible);
+
+  /**
+   * \brief Set center of mass marker scale
+   * @param scale center of mass marker scale
+   */
+  void setCenterOfMassMarkerScale(float scale);
+
+  /**
+   * \brief return center of mass marker scale
+   */
+  float getCenterOfMassMarkerScale();
+
+  /**
+   * \brief Returns whether or not centers of mass of each link is visible.
+   * To be visible this and isVisible() must both be true
+   */
+  bool isCentersOfMassVisible();
+
+
+
+
   RobotLink* getRootLink() { return root_link_; }
   RobotLink* getLink( const std::string& name );
   RobotJoint* getJoint( const std::string& name );
@@ -171,7 +204,8 @@ Q_OBJECT
                                    const urdf::LinkConstSharedPtr& link,
                                    const std::string& parent_joint_name,
                                    bool visual,
-                                   bool collision);
+                                   bool collision,
+                                   bool center_of_mass);
     virtual RobotJoint* createJoint( Robot* robot,
                                      const urdf::JointConstSharedPtr& joint);
   };
@@ -256,6 +290,10 @@ private Q_SLOTS:
   bool visual_visible_;                         ///< Should we show the visual representation?
   bool collision_visible_;                      ///< Should we show the collision representation?
 
+
+  bool centers_of_mass_visible_;                ///< Should we show centers of mass of each link?
+  float center_of_mass_marker_scale_;           ///< Scale of the center of mass markers
+
   DisplayContext* context_;
   Property* link_tree_;
   EnumProperty* link_tree_style_;