WIP: Refactor world frame retrieval

benchmark_suite/include/moveit_benchmark_suite/scene.h

@@ -46,10 +46,7 @@ class CollisionPluginLoader
   std::map<std::string, collision_detection::CollisionPluginPtr> plugins_;  ///< Loaded plugins.
 };
 
-void getTransformsFromTf(std::vector<geometry_msgs::TransformStamped>& transforms,
-                         const robot_model::RobotModelConstPtr& rm);
-
-void addTransformsToSceneMsg(const std::vector<geometry_msgs::TransformStamped>& transforms,
-                             moveit_msgs::PlanningScene& scene_msg);
+void getVirtualModelTransform(std::vector<geometry_msgs::TransformStamped>& transforms,
+                              const robot_model::RobotModelConstPtr& robot, double timeout);
 
 }  // namespace moveit_benchmark_suite

benchmark_suite/src/benchmarks/motion_planning.cpp

@@ -65,9 +65,9 @@ int main(int argc, char** argv)
   auto robot = std::make_shared<Robot>("robot", "robot_description");
   robot->initialize();
 
-  // Get transforms from tf listener
+  // Get virtual transforms from robot model or tf listener
   std::vector<geometry_msgs::TransformStamped> transforms;
-  getTransformsFromTf(transforms, robot->getModelConst());
+  getVirtualModelTransform(transforms, robot->getModelConst(), 1.0);
 
   // Prepare query setup
   QuerySetup query_setup;
@@ -100,8 +100,9 @@ int main(int argc, char** argv)
     scene_msgs.back().is_diff = true;
     parser.getCollisionObjects(scene_msgs.back().world.collision_objects);
 
-    // If tf add it to the planning scene
-    addTransformsToSceneMsg(transforms, scene_msgs.back());
+    // Add virtual transform
+    for (const auto& transform : transforms)
+      scene_msgs.back().fixed_frame_transforms.push_back(transform);
 
     query_setup.addQuery("scene", scene.first, "");
   }

benchmark_suite/src/scene.cpp

@@ -61,43 +61,73 @@ bool CollisionPluginLoader::activate(const std::string& name, const planning_sce
   return false;
 }
 
-void moveit_benchmark_suite::getTransformsFromTf(std::vector<geometry_msgs::TransformStamped>& transforms,
-                                                 const robot_model::RobotModelConstPtr& rm)
+void moveit_benchmark_suite::getVirtualModelTransform(std::vector<geometry_msgs::TransformStamped>& transforms,
+                                                      const robot_model::RobotModelConstPtr& robot, double timeout)
 {
-  const std::string& target = rm->getModelFrame();
+  auto tf_buffer = std::make_shared<tf2_ros::Buffer>();
+  auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
+  planning_scene::PlanningScene scene(robot);
 
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tfListener(tf_buffer_);
+  const std::string& root_link = robot->getRootLinkName();
+  std::string virtual_frame;
+  geometry_msgs::TransformStamped transform;
 
-  ros::Duration(1.0).sleep();
+  // SRDF has virtual joints
+  if (robot->getSRDF() && !robot->getSRDF()->getVirtualJoints().empty())
+  {
+    // Must parse virtual joints for the case where the virtual joint type is `fixed`
+    for (const auto& virtual_joint : robot->getSRDF()->getVirtualJoints())
+    {
+      if (virtual_joint.child_link_.compare(root_link) == 0)
+      {
+        virtual_frame = virtual_joint.parent_frame_;
+        break;
+      }
+    }
+
+    // Prioritize virtual joint from srdf, then check tf listener. A fixed joint type will
+    // never find a transform from the scene.
+    if (!scene.knowsFrameTransform(virtual_frame) &&
+        !tf_buffer->canTransform(virtual_frame, root_link, ros::Time{ 0 }, ros::Duration{ timeout }))
+    {
+      ROS_FATAL("can't transform to model frame");
+      return;
+    }
+
+    try
+    {
+      transform = tf_buffer->lookupTransform(virtual_frame, root_link, ros::Time(0), ros::Duration{ timeout });
+      transforms.push_back(transform);
+    }
+    catch (tf2::TransformException& ex)
+    {
+      ROS_WARN("%s", ex.what());
+    }
+    return;
+  }
+
+  // No virtual joints, check all tf listener names (that are not part of the robot) against root link
+  ros::Duration(timeout).sleep();  // Needed because tf names must be available
 
   std::vector<std::string> all_frame_names;
-  tf_buffer_._getFrameStrings(all_frame_names);
-  for (const std::string& all_frame_name : all_frame_names)
+  tf_buffer->_getFrameStrings(all_frame_names);
+
+  for (const std::string& tf_frame_name : all_frame_names)
   {
-    if (all_frame_name == target || rm->hasLinkModel(all_frame_name))
+    if (robot->hasLinkModel(tf_frame_name))
       continue;
 
-    geometry_msgs::TransformStamped f;
     try
     {
-      f = tf_buffer_.lookupTransform(target, all_frame_name, ros::Time(0));
+      transform = tf_buffer->lookupTransform(tf_frame_name, root_link, ros::Time(0));
+      transforms.push_back(transform);
+      return;
     }
     catch (tf2::TransformException& ex)
     {
-      ROS_WARN_STREAM("Unable to transform object from frame '" << all_frame_name << "' to planning frame '" << target
+      ROS_WARN_STREAM("Unable to transform object from frame '" << tf_frame_name << "' to planning frame '" << root_link
                                                                 << "' (" << ex.what() << ")");
       continue;
     }
-    f.header.frame_id = all_frame_name;
-    f.child_frame_id = target;
-    transforms.push_back(f);
   }
 }
-
-void moveit_benchmark_suite::addTransformsToSceneMsg(const std::vector<geometry_msgs::TransformStamped>& transforms,
-                                                     moveit_msgs::PlanningScene& scene_msg)
-{
-  for (const auto& transform : transforms)
-    scene_msg.fixed_frame_transforms.push_back(transform);
-}