Fix getTransform() (moveit#2113)

Replace: - PlanningScene::getTransforms().getTransform() -> PlanningScene::getFrameTransform() - PlanningScene::getTransforms().canTransform() -> PlanningScene::knowsFrameTransform() getTransforms() essentially yields the SceneTransforms instance of the parent scene only. Accessing the corresponding frame transforms obviously yields the wrong results.
davetcoleman · Jun 2, 2020 · d20c634 · d20c634
1 parent a7d233e
commit d20c634
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 9 deletions.
diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
@@ -1022,7 +1022,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
   moveit::core::AttachedBodyCallback current_state_attached_body_callback_;
 
   // This variable is not necessarily used by child planning scenes
-  // This Transforms class is actually a SceneTransform class
+  // This Transforms class is actually a SceneTransforms class
   moveit::core::TransformsPtr scene_transforms_;  // if NULL use parent's
 
   collision_detection::WorldPtr world_;             // never NULL, never shared with parent/child

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -1285,7 +1285,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::Octomap& map)
   std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map)));
   if (!map.header.frame_id.empty())
   {
-    const Eigen::Isometry3d& t = getTransforms().getTransform(map.header.frame_id);
+    const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id);
     world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t);
   }
   else
@@ -1321,7 +1321,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map)
   }
 
   std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map.octomap)));
-  const Eigen::Isometry3d& t = getTransforms().getTransform(map.header.frame_id);
+  const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id);
   Eigen::Isometry3d p;
   tf2::fromMsg(map.origin, p);
   p = t * p;
@@ -1493,7 +1493,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
         if (object.object.header.frame_id != object.link_name)
         {
           const Eigen::Isometry3d& transform = robot_state_->getGlobalLinkTransform(link_model).inverse() *
-                                               getTransforms().getTransform(object.object.header.frame_id);
+                                               getFrameTransform(object.object.header.frame_id);
           for (Eigen::Isometry3d& pose : poses)
             pose = transform * pose;
         }
@@ -1533,7 +1533,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
         if (object.object.header.frame_id != object.link_name)
         {
           const Eigen::Isometry3d& transform = robot_state_->getGlobalLinkTransform(link_model).inverse() *
-                                               getTransforms().getTransform(object.object.header.frame_id);
+                                               getFrameTransform(object.object.header.frame_id);
           for (auto& subframe : subframe_poses)
             subframe.second = transform * subframe.second;
         }
@@ -1727,7 +1727,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject
     return false;
   }
 
-  if (!getTransforms().canTransform(object.header.frame_id))
+  if (!knowsFrameTransform(object.header.frame_id))
   {
     ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown frame: " << object.header.frame_id);
     return false;
@@ -1737,7 +1737,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject
   if (object.operation == moveit_msgs::CollisionObject::ADD && world_->hasObject(object.id))
     world_->removeObject(object.id);
 
-  const Eigen::Isometry3d& object_frame_transform = getTransforms().getTransform(object.header.frame_id);
+  const Eigen::Isometry3d& object_frame_transform = getFrameTransform(object.header.frame_id);
 
   for (std::size_t i = 0; i < object.primitives.size(); ++i)
   {
@@ -1808,7 +1808,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObjec
       ROS_WARN_NAMED(LOGNAME, "Move operation for object '%s' ignores the geometry specified in the message.",
                      object.id.c_str());
 
-    const Eigen::Isometry3d& t = getTransforms().getTransform(object.header.frame_id);
+    const Eigen::Isometry3d& t = getFrameTransform(object.header.frame_id);
     EigenSTL::vector_Isometry3d new_poses;
     for (const geometry_msgs::Pose& primitive_pose : object.primitive_poses)
     {

diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp
@@ -485,7 +485,7 @@ void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray& tab
     ROS_INFO_STREAM_NAMED(LOGNAME, "Original pose: " << table.pose.position.x << "," << table.pose.position.y << ","
                                                      << table.pose.position.z);
     std::string error_text;
-    const Eigen::Isometry3d& original_transform = planning_scene_->getTransforms().getTransform(original_frame);
+    const Eigen::Isometry3d& original_transform = planning_scene_->getFrameTransform(original_frame);
     Eigen::Isometry3d original_pose;
     tf2::fromMsg(table.pose, original_pose);
     original_pose = original_transform * original_pose;