Normalize quaternions when adding new or moving collision objects (mo…

…veit#1420)
ubi-agni · May 11, 2019 · 482ff84 · 482ff84
1 parent 30e764f
commit 482ff84
Show file tree

Hide file tree

Showing 2 changed files with 20 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
@@ -983,6 +983,9 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
   bool processCollisionObjectRemove(const moveit_msgs::CollisionObject& object);
   bool processCollisionObjectMove(const moveit_msgs::CollisionObject& object);
 
+  /** convert Pose msg to Eigen::Isometry, normalizing the quaternion part if necessary. */
+  static void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out);
+
   MOVEIT_STRUCT_FORWARD(CollisionDetector);
 
   /* \brief A set of compatible collision detectors */

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -1713,6 +1713,14 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject
   return false;
 }
 
+void PlanningScene::poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out)
+{
+  Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z);
+  Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z);
+  quaternion.normalize();
+  out = translation * quaternion;
+}
+
 bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject& object)
 {
   if (object.primitives.empty() && object.meshes.empty() && object.planes.empty())
@@ -1752,7 +1760,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject
     if (s)
     {
       Eigen::Isometry3d object_pose;
-      tf2::fromMsg(object.primitive_poses[i], object_pose);
+      PlanningScene::poseMsgToEigen(object.primitive_poses[i], object_pose);
       world_->addToObject(object.id, shapes::ShapeConstPtr(s), object_frame_transform * object_pose);
     }
   }
@@ -1762,7 +1770,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject
     if (s)
     {
       Eigen::Isometry3d object_pose;
-      tf2::fromMsg(object.mesh_poses[i], object_pose);
+      PlanningScene::poseMsgToEigen(object.mesh_poses[i], object_pose);
       world_->addToObject(object.id, shapes::ShapeConstPtr(s), object_frame_transform * object_pose);
     }
   }
@@ -1772,7 +1780,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject
     if (s)
     {
       Eigen::Isometry3d object_pose;
-      tf2::fromMsg(object.plane_poses[i], object_pose);
+      PlanningScene::poseMsgToEigen(object.plane_poses[i], object_pose);
       world_->addToObject(object.id, shapes::ShapeConstPtr(s), object_frame_transform * object_pose);
     }
   }
@@ -1806,22 +1814,22 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObjec
 
     const Eigen::Isometry3d& t = getTransforms().getTransform(object.header.frame_id);
     EigenSTL::vector_Isometry3d new_poses;
-    for (std::size_t i = 0; i < object.primitive_poses.size(); ++i)
+    for (const geometry_msgs::Pose& primitive_pose : object.primitive_poses)
     {
       Eigen::Isometry3d object_pose;
-      tf2::fromMsg(object.primitive_poses[i], object_pose);
+      PlanningScene::poseMsgToEigen(primitive_pose, object_pose);
       new_poses.push_back(t * object_pose);
     }
-    for (std::size_t i = 0; i < object.mesh_poses.size(); ++i)
+    for (const geometry_msgs::Pose& mesh_pose : object.mesh_poses)
     {
       Eigen::Isometry3d object_pose;
-      tf2::fromMsg(object.mesh_poses[i], object_pose);
+      PlanningScene::poseMsgToEigen(mesh_pose, object_pose);
       new_poses.push_back(t * object_pose);
     }
-    for (std::size_t i = 0; i < object.plane_poses.size(); ++i)
+    for (const geometry_msgs::Pose& plane_pose : object.plane_poses)
     {
       Eigen::Isometry3d object_pose;
-      tf2::fromMsg(object.plane_poses[i], object_pose);
+      PlanningScene::poseMsgToEigen(plane_pose, object_pose);
       new_poses.push_back(t * object_pose);
     }