robot_model_display: arrange scene nodes of links in URDF's tree structure in order to avoid jittering of movable links due to tf latency.

AndreasR30 · AndreasR30 · commit c7229506e858 · 2021-12-29T20:48:30.000+01:00
diff --git a/src/rviz/default_plugin/robot_model_display.cpp b/src/rviz/default_plugin/robot_model_display.cpp
@@ -229,6 +229,25 @@ void RobotModelDisplay::update(float wall_dt, float /*ros_dt*/)
 
   if (has_new_transforms_ || update)
   {
+    Ogre::Vector3 position;
+    Ogre::Quaternion orientation;
+    if (context_->getFrameManager()->getTransform(robot_->getRootLink()->getName(), ros::Time(),
+                                                  position, orientation))
+    {
+      robot_->setPosition(position);
+      robot_->setOrientation(orientation);
+      linkUpdaterStatusFunction(StatusProperty::Ok, robot_->getRootLink()->getName(), "Transform OK",
+                                this);
+    }
+    else
+    {
+      std::stringstream ss;
+      ss << "No transform from [" << robot_->getRootLink()->getName() << "] to ["
+         << fixed_frame_.toStdString() << "]";
+      linkUpdaterStatusFunction(StatusProperty::Error, robot_->getRootLink()->getName(), ss.str(), this);
+    }
+
+
     robot_->update(TFLinkUpdater(context_->getFrameManager(),
                                  boost::bind(linkUpdaterStatusFunction, _1, _2, _3, this),
                                  tf_prefix_property_->getStdString()));
diff --git a/src/rviz/robot/link_updater.h b/src/rviz/robot/link_updater.h
@@ -40,7 +40,8 @@ namespace rviz
 class LinkUpdater
 {
 public:
-  virtual bool getLinkTransforms(const std::string& link_name,
+  virtual bool getLinkTransforms(const std::string& parent_link_name,
+                                 const std::string& link_name,
                                  Ogre::Vector3& visual_position,
                                  Ogre::Quaternion& visual_orientation,
                                  Ogre::Vector3& collision_position,
diff --git a/src/rviz/robot/robot.cpp b/src/rviz/robot/robot.cpp
@@ -218,11 +218,14 @@ void Robot::clear()
 
 RobotLink* Robot::LinkFactory::createLink(Robot* robot,
                                           const urdf::LinkConstSharedPtr& link,
+                                          Ogre::SceneNode* parent_visual_node,
+                                          Ogre::SceneNode* parent_collision_node,
                                           const std::string& parent_joint_name,
                                           bool visual,
                                           bool collision)
 {
-  return new RobotLink(robot, link, parent_joint_name, visual, collision);
+  return new RobotLink(robot, link, parent_visual_node, parent_collision_node, parent_joint_name, visual,
+                       collision);
 }
 
 RobotJoint* Robot::LinkFactory::createJoint(Robot* robot, const urdf::JointConstSharedPtr& joint)
@@ -244,20 +247,26 @@ void Robot::load(const urdf::ModelInterface& urdf, bool visual, bool collision)
   // Create properties for each link.
   // Properties are not added to display until changedLinkTreeStyle() is called (below).
   {
-    typedef std::map<std::string, urdf::LinkSharedPtr> M_NameToUrdfLink;
-    M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
-    M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
-    for (; link_it != link_end; ++link_it)
+    // traverse URDF tree in depth first order and copy tree structure for links' scene nodes
+    std::vector<std::tuple<urdf::LinkConstSharedPtr, Ogre::SceneNode*, Ogre::SceneNode*>> link_stack_;
+    link_stack_.emplace_back(urdf.getRoot(), root_visual_node_, root_collision_node_);
+    while (!link_stack_.empty())
     {
-      const urdf::LinkConstSharedPtr& urdf_link = link_it->second;
+      urdf::LinkConstSharedPtr urdf_link = std::get<0>(link_stack_.back());
+      Ogre::SceneNode* parent_visual_node = std::get<1>(link_stack_.back());
+      Ogre::SceneNode* parent_collision_node = std::get<2>(link_stack_.back());
+      link_stack_.pop_back();
+
       std::string parent_joint_name;
 
       if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
       {
         parent_joint_name = urdf_link->parent_joint->name;
       }
 
-      RobotLink* link = link_factory_->createLink(this, urdf_link, parent_joint_name, visual, collision);
+      RobotLink* link =
+          link_factory_->createLink(this, urdf_link, parent_visual_node, parent_collision_node,
+                                    parent_joint_name, visual, collision);
 
       if (urdf_link == urdf.getRoot())
       {
@@ -267,6 +276,9 @@ void Robot::load(const urdf::ModelInterface& urdf, bool visual, bool collision)
       links_[urdf_link->name] = link;
 
       link->setRobotAlpha(alpha_);
+
+      for (const auto& c : urdf_link->child_links)
+        link_stack_.emplace_back(c, link->getVisualTreeNode(), link->getCollisionTreeNode());
     }
   }
 
@@ -709,8 +721,8 @@ void Robot::update(const LinkUpdater& updater)
 
     Ogre::Vector3 visual_position, collision_position;
     Ogre::Quaternion visual_orientation, collision_orientation;
-    if (updater.getLinkTransforms(link->getName(), visual_position, visual_orientation,
-                                  collision_position, collision_orientation))
+    if (updater.getLinkTransforms(link->getParentLinkName(), link->getName(), visual_position,
+                                  visual_orientation, collision_position, collision_orientation))
     {
       // Check if visual_orientation, visual_position, collision_orientation, and collision_position are
       // NaN.
diff --git a/src/rviz/robot/robot.h b/src/rviz/robot/robot.h
@@ -199,6 +199,8 @@ class Robot : public QObject
 
     virtual RobotLink* createLink(Robot* robot,
                                   const urdf::LinkConstSharedPtr& link,
+                                  Ogre::SceneNode* parent_visual_node,
+                                  Ogre::SceneNode* parent_collision_node,
                                   const std::string& parent_joint_name,
                                   bool visual,
                                   bool collision);
diff --git a/src/rviz/robot/robot_link.cpp b/src/rviz/robot/robot_link.cpp
@@ -151,13 +151,16 @@ static std::map<const RobotLink*, std::string> errors;
 
 RobotLink::RobotLink(Robot* robot,
                      const urdf::LinkConstSharedPtr& link,
+                     Ogre::SceneNode* parent_visual_node,
+                     Ogre::SceneNode* parent_collision_node,
                      const std::string& parent_joint_name,
                      bool visual,
                      bool collision)
   : robot_(robot)
   , scene_manager_(robot->getDisplayContext()->getSceneManager())
   , context_(robot->getDisplayContext())
   , name_(link->name)
+  , parent_link_name_(link->getParent() ? link->getParent()->name : "")
   , parent_joint_name_(parent_joint_name)
   , visual_node_(nullptr)
   , collision_node_(nullptr)
@@ -198,8 +201,12 @@ RobotLink::RobotLink(Robot* robot,
 
   link_property_->collapse();
 
-  visual_node_ = robot_->getVisualNode()->createChildSceneNode();
-  collision_node_ = robot_->getCollisionNode()->createChildSceneNode();
+  // create scene nodes in a tree structure analog to URDF tree
+  // we use visual_node_ and collision_node_ as leafs in order not to hide child links when set to invisible
+  visual_tree_node_ = parent_visual_node->createChildSceneNode();
+  visual_node_ = visual_tree_node_->createChildSceneNode();
+  collision_tree_node_ = parent_collision_node->createChildSceneNode();
+  collision_node_ = collision_tree_node_->createChildSceneNode();
 
   // create material for coloring links
   color_material_ = Ogre::MaterialPtr(new Ogre::Material(
@@ -306,6 +313,8 @@ RobotLink::~RobotLink()
 
   scene_manager_->destroySceneNode(visual_node_);
   scene_manager_->destroySceneNode(collision_node_);
+  scene_manager_->destroySceneNode(visual_tree_node_);
+  scene_manager_->destroySceneNode(collision_tree_node_);
 
   if (trail_)
   {
@@ -872,16 +881,16 @@ void RobotLink::setTransforms(const Ogre::Vector3& visual_position,
                               const Ogre::Vector3& collision_position,
                               const Ogre::Quaternion& collision_orientation)
 {
-  if (visual_node_)
+  if (visual_tree_node_)
   {
-    visual_node_->setPosition(visual_position);
-    visual_node_->setOrientation(visual_orientation);
+    visual_tree_node_->setPosition(visual_position);
+    visual_tree_node_->setOrientation(visual_orientation);
   }
 
-  if (collision_node_)
+  if (collision_tree_node_)
   {
-    collision_node_->setPosition(collision_position);
-    collision_node_->setOrientation(collision_orientation);
+    collision_tree_node_->setPosition(collision_position);
+    collision_tree_node_->setOrientation(collision_orientation);
   }
 
   position_property_->setVector(visual_position);
diff --git a/src/rviz/robot/robot_link.h b/src/rviz/robot/robot_link.h
@@ -82,6 +82,8 @@ class RobotLink : public QObject
 public:
   RobotLink(Robot* robot,
             const urdf::LinkConstSharedPtr& link,
+            Ogre::SceneNode* parent_visual_node,
+            Ogre::SceneNode* parent_collision_node,
             const std::string& parent_joint_name,
             bool visual,
             bool collision);
@@ -99,6 +101,10 @@ class RobotLink : public QObject
   {
     return name_;
   }
+  const std::string& getParentLinkName() const
+  {
+    return parent_link_name_;
+  }
   const std::string& getParentJointName() const
   {
     return parent_joint_name_;
@@ -119,6 +125,14 @@ class RobotLink : public QObject
   {
     return collision_node_;
   }
+  Ogre::SceneNode* getVisualTreeNode() const
+  {
+    return visual_tree_node_;
+  }
+  Ogre::SceneNode* getCollisionTreeNode() const
+  {
+    return collision_tree_node_;
+  }
   Robot* getRobot() const
   {
     return robot_;
@@ -200,6 +214,7 @@ private Q_SLOTS:
   DisplayContext* context_;
 
   std::string name_; ///< Name of this link
+  std::string parent_link_name_;
   std::string parent_joint_name_;
   std::vector<std::string> child_joint_names_;
 
@@ -226,6 +241,8 @@ private Q_SLOTS:
 
   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_tree_node_;    ///< The scene node above visual_node is attached to
+  Ogre::SceneNode* collision_tree_node_; ///< The scene node above collision_node is attached to
 
   Ogre::RibbonTrail* trail_;
 
diff --git a/src/rviz/robot/tf_link_updater.cpp b/src/rviz/robot/tf_link_updater.cpp
@@ -43,24 +43,47 @@ TFLinkUpdater::TFLinkUpdater(FrameManager* frame_manager,
 {
 }
 
-bool TFLinkUpdater::getLinkTransforms(const std::string& _link_name,
+bool TFLinkUpdater::getLinkTransforms(const std::string& _parent_link_name,
+                                      const std::string& _link_name,
                                       Ogre::Vector3& visual_position,
                                       Ogre::Quaternion& visual_orientation,
                                       Ogre::Vector3& collision_position,
                                       Ogre::Quaternion& collision_orientation) const
 {
+  if (_parent_link_name.empty())
+  {
+    visual_position = Ogre::Vector3::ZERO;
+    visual_orientation = Ogre::Quaternion::IDENTITY;
+    collision_position = Ogre::Vector3::ZERO;
+    collision_orientation = Ogre::Quaternion::IDENTITY;
+    return true;
+  }
+  std::string parent_link_name = concat(tf_prefix_, _parent_link_name);
   std::string link_name = concat(tf_prefix_, _link_name);
 
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
-  if (!frame_manager_->getTransform(link_name, ros::Time(), position, orientation))
+
+  geometry_msgs::TransformStamped tf;
+  try
+  {
+    tf = frame_manager_->getTF2BufferPtr()->lookupTransform(parent_link_name, link_name, ros::Time());
+  }
+  catch (std::runtime_error& e)
   {
     std::stringstream ss;
     ss << "No transform from [" << link_name << "] to [" << frame_manager_->getFixedFrame() << "]";
     setLinkStatus(StatusProperty::Error, link_name, ss.str());
+    ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", link_name.c_str(),
+              parent_link_name.c_str(), e.what());
     return false;
   }
 
+  position =
+      Ogre::Vector3(tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z);
+  orientation = Ogre::Quaternion(tf.transform.rotation.w, tf.transform.rotation.x,
+                                 tf.transform.rotation.y, tf.transform.rotation.z);
+
   setLinkStatus(StatusProperty::Ok, link_name, "Transform OK");
 
   // Collision/visual transforms are the same in this case
diff --git a/src/rviz/robot/tf_link_updater.h b/src/rviz/robot/tf_link_updater.h
@@ -52,7 +52,8 @@ class TFLinkUpdater : public LinkUpdater
   TFLinkUpdater(FrameManager* frame_manager,
                 const StatusCallback& status_cb = StatusCallback(),
                 const std::string& tf_prefix = std::string());
-  bool getLinkTransforms(const std::string& link_name,
+  bool getLinkTransforms(const std::string& parent_link_name,
+                         const std::string& link_name,
                          Ogre::Vector3& visual_position,
                          Ogre::Quaternion& visual_orientation,
                          Ogre::Vector3& collision_position,
