Fix errors while orientation covariance visualization in CovarianceVi…

CMakeLists.txt

@@ -137,6 +137,7 @@ find_package(catkin REQUIRED
   std_srvs
   tf2_ros
   tf2_geometry_msgs
+  tf2_eigen
   urdf
   visualization_msgs
 )

package.xml

@@ -53,6 +53,7 @@
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_eigen</depend>
   <depend>tinyxml2</depend>
   <depend>urdf</depend>
   <depend>visualization_msgs</depend>

src/rviz/default_plugin/odometry_display.cpp

@@ -27,12 +27,15 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <tf2_eigen/tf2_eigen.h>
+
 #include <rviz/ogre_helpers/arrow.h>
 #include <rviz/ogre_helpers/axes.h>
 #include <rviz/properties/enum_property.h>
 #include <rviz/properties/color_property.h>
 #include <rviz/properties/float_property.h>
 #include <rviz/properties/int_property.h>
+#include <rviz/properties/bool_property.h>
 #include <rviz/validate_floats.h>
 #include <rviz/validate_quaternions.h>
 
@@ -47,6 +50,12 @@ namespace rviz
 {
 OdometryDisplay::OdometryDisplay()
 {
+  continuous_transform_property_ =
+      new BoolProperty("Continuous Transform", false,
+                       "Retransform into fixed frame every timestep. This is particularly useful for "
+                       "messages whose frame moves w.r.t. fixed frame.",
+                       this);
+
   position_tolerance_property_ = new FloatProperty("Position Tolerance", .1,
                                                    "Distance, in meters from the last arrow dropped, "
                                                    "that will cause a new arrow to drop.",
@@ -153,6 +162,10 @@ void OdometryDisplay::clear()
   {
     last_used_message_.reset();
   }
+  if (ref_pose_)
+  {
+    ref_pose_.reset();
+  }
 }
 
 void OdometryDisplay::updateColorAndAlpha()
@@ -278,21 +291,16 @@ void OdometryDisplay::processMessage(const nav_msgs::Odometry::ConstPtr& message
 
   if (last_used_message_)
   {
-    Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x,
-                                last_used_message_->pose.pose.position.y,
-                                last_used_message_->pose.pose.position.z);
-    Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y,
-                                   message->pose.pose.position.z);
-    Eigen::Quaternionf last_orientation(last_used_message_->pose.pose.orientation.w,
-                                        last_used_message_->pose.pose.orientation.x,
-                                        last_used_message_->pose.pose.orientation.y,
-                                        last_used_message_->pose.pose.orientation.z);
-    Eigen::Quaternionf current_orientation(message->pose.pose.orientation.w,
-                                           message->pose.pose.orientation.x,
-                                           message->pose.pose.orientation.y,
-                                           message->pose.pose.orientation.z);
-
-    if ((last_position - current_position).length() < position_tolerance_property_->getFloat() &&
+    // use double precision in case the positions are very large, like for example with UTM coords
+    Eigen::Vector3d last_position, current_position;
+    Eigen::Quaterniond last_orientation, current_orientation;
+
+    tf2::fromMsg(message->pose.pose.position, current_position);
+    tf2::fromMsg(message->pose.pose.orientation, current_orientation);
+    tf2::fromMsg(last_used_message_->pose.pose.position, last_position);
+    tf2::fromMsg(last_used_message_->pose.pose.orientation, last_orientation);
+
+    if ((last_position - current_position).norm() < position_tolerance_property_->getFloat() &&
         last_orientation.angularDistance(current_orientation) < angle_tolerance_property_->getFloat())
     {
       return;
@@ -301,14 +309,37 @@ void OdometryDisplay::processMessage(const nav_msgs::Odometry::ConstPtr& message
 
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
-  if (!context_->getFrameManager()->transform(message->header, message->pose.pose, position, orientation))
+  if (!continuous_transform_property_->getBool())
   {
-    ROS_ERROR("Error transforming odometry '%s' from frame '%s' to frame '%s'", qPrintable(getName()),
-              message->header.frame_id.c_str(), qPrintable(fixed_frame_));
-    return;
+    if (!context_->getFrameManager()->transform(message->header, message->pose.pose, position,
+                                                orientation))
+    {
+      ROS_ERROR("Error transforming odometry '%s' from frame '%s' to frame '%s'", qPrintable(getName()),
+                message->header.frame_id.c_str(), qPrintable(fixed_frame_));
+      return;
+    }
+
+    // If we arrive here, we're good. Continue...
   }
+  else
+  {
+    // put the visuals at the pose specified by the message and transform their scene node continuously
 
-  // If we arrive here, we're good. Continue...
+    // Use ref pose for very large transforms where float (default in OGRE) has not enough precision.
+    // Such large transforms can occur when converting GPS coordinates to a euclidean coordinate system,
+    // e.g. UTM. The ref pose is set to the position of the first incoming odometry message and is axis
+    // aligned to the parent frame of the odometry message.
+    if (!ref_pose_)
+    {
+      ref_pose_.reset(new geometry_msgs::Pose());
+      ref_pose_->position = message->pose.pose.position;
+    }
+
+    const geometry_msgs::Pose& p = message->pose.pose;
+    position = Ogre::Vector3(p.position.x - ref_pose_->position.x, p.position.y - ref_pose_->position.y,
+                             p.position.z - ref_pose_->position.z);
+    orientation = Ogre::Quaternion(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
+  }
 
   // Create a scene node, and attach the arrow and the covariance to it
   Axes* axes = new Axes(scene_manager_, scene_node_, axes_length_property_->getFloat(),
@@ -372,6 +403,22 @@ void OdometryDisplay::update(float /*wall_dt*/, float /*ros_dt*/)
 
   assert(arrows_.size() == axes_.size());
   assert(axes_.size() == covariance_property_->sizeVisual());
+
+  // continuously set the scene node's position to ref pose
+  if (!continuous_transform_property_->getBool() || !last_used_message_ || !ref_pose_)
+    return;
+
+  Ogre::Vector3 position;
+  Ogre::Quaternion orientation;
+  if (!context_->getFrameManager()->transform(last_used_message_->header.frame_id, ros::Time(),
+                                              *ref_pose_, position, orientation))
+  {
+    // the error output with setStatus is already generated by MessageFilterDisplay
+    return;
+  }
+
+  scene_node_->setPosition(position);
+  scene_node_->setOrientation(orientation);
 }
 
 void OdometryDisplay::reset()

src/rviz/default_plugin/odometry_display.h

@@ -51,6 +51,7 @@ class ColorProperty;
 class FloatProperty;
 class IntProperty;
 class EnumProperty;
+class BoolProperty;
 
 class CovarianceProperty;
 
@@ -102,6 +103,9 @@ private Q_SLOTS:
   D_Axes axes_;
 
   nav_msgs::Odometry::ConstPtr last_used_message_;
+  geometry_msgs::Pose::Ptr ref_pose_;
+
+  rviz::BoolProperty* continuous_transform_property_;
 
   rviz::EnumProperty* shape_property_;
 

src/rviz/default_plugin/point_cloud_common.cpp

@@ -313,6 +313,12 @@ PointCloudCommon::PointCloudCommon(Display* display)
   , transformer_class_loader_(nullptr)
   , display_(display)
 {
+  continuous_transform_property_ =
+      new BoolProperty("Continuous Transform", false,
+                       "Retransform into fixed frame every timestep. This is particularly useful for "
+                       "messages whose frame moves w.r.t. fixed frame.",
+                       display_);
+
   selectable_property_ =
       new BoolProperty("Selectable", true,
                        "Whether or not the points in this point cloud are selectable.", display_,
@@ -637,6 +643,26 @@ void PointCloudCommon::update(float /*wall_dt*/, float /*ros_dt*/)
     new_color_transformer_ = false;
   }
 
+  if (continuous_transform_property_->getBool())
+  {
+    for (CloudInfoPtr& cloud_info : cloud_infos_)
+    {
+      if (!context_->getFrameManager()->getTransform(cloud_info->message_->header.frame_id, ros::Time(),
+                                                     cloud_info->position_, cloud_info->orientation_))
+      {
+        std::stringstream ss;
+        ss << "Failed to transform from frame [" << cloud_info->message_->header.frame_id
+           << "] to frame [" << context_->getFrameManager()->getFixedFrame() << "]";
+        display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
+      }
+      else
+      {
+        cloud_info->scene_node_->setPosition(cloud_info->position_);
+        cloud_info->scene_node_->setOrientation(cloud_info->orientation_);
+      }
+    }
+  }
+
   updateStatus();
 }
 

src/rviz/default_plugin/point_cloud_common.h

@@ -134,6 +134,7 @@ class PointCloudCommon : public QObject
 
   bool auto_size_;
 
+  BoolProperty* continuous_transform_property_;
   BoolProperty* selectable_property_;
   FloatProperty* point_world_size_property_;
   FloatProperty* point_pixel_size_property_;

src/rviz/default_plugin/robot_model_display.cpp

@@ -227,8 +227,27 @@ void RobotModelDisplay::update(float wall_dt, float /*ros_dt*/)
   float rate = update_rate_property_->getFloat();
   bool update = rate < 0.0001f || time_since_last_transform_ >= rate;
 
-  if (has_new_transforms_ || update)
+  if (robot_->getRootLink() && (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()));

src/rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.cpp

@@ -49,6 +49,7 @@ FixedOrientationOrthoViewController::FixedOrientationOrthoViewController() : dra
 {
   scale_property_ =
       new FloatProperty("Scale", 10, "How much to scale up the size of things in the scene.", this);
+  scale_property_->setMin(1e-14);
   angle_property_ = new FloatProperty("Angle", 0, "Angle around the Z axis to rotate.", this);
   x_property_ = new FloatProperty("X", 0, "X component of camera position.", this);
   y_property_ = new FloatProperty("Y", 0, "Y component of camera position.", this);

src/rviz/frame_manager.cpp

@@ -47,7 +47,7 @@ FrameManager::FrameManager(std::shared_ptr<tf2_ros::Buffer> tf_buffer,
       tf_buffer ? std::move(tf_buffer) : std::make_shared<tf2_ros::Buffer>(ros::Duration(10 * 60));
   tf_listener_ = tf_listener ?
                      std::move(tf_listener) :
-                     std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, ros::NodeHandle(), true);
+                     std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, ros::NodeHandle(), false);
 
   setSyncMode(SyncOff);
   setPause(false);

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,

src/rviz/robot/robot.cpp

@@ -66,6 +66,8 @@ Robot::Robot(Ogre::SceneNode* root_node,
   , robot_loaded_(false)
   , inChangedEnableAllLinks(false)
   , name_(name)
+  , root_link_(nullptr)
+  , alpha_(1.f)
 {
   root_visual_node_ = root_node->createChildSceneNode();
   root_collision_node_ = root_node->createChildSceneNode();
@@ -218,11 +220,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 +249,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 +278,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 +723,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.

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);