Add visual geometry mesh url to CollisionObject

moveit_commander/src/moveit_commander/move_group.py

@@ -632,13 +632,15 @@ def plan(self, joints=None):
         error_code = MoveItErrorCodes()
         error_code.deserialize(error_code_msg)
         plan = RobotTrajectory()
-        return (error_code.val == MoveItErrorCodes.SUCCESS,
-                plan.deserialize(trajectory_msg),
-                planning_time,
-                error_code)
-
+        return (
+            error_code.val == MoveItErrorCodes.SUCCESS,
+            plan.deserialize(trajectory_msg),
+            planning_time,
+            error_code,
+        )
+
     def construct_motion_plan_request(self):
-        """ Returns a MotionPlanRequest filled with the current goals of the move_group_interface"""
+        """Returns a MotionPlanRequest filled with the current goals of the move_group_interface"""
         mpr = MotionPlanRequest()
         return mpr.deserialize(self._g.construct_motion_plan_request())
 

moveit_commander/src/moveit_commander/planning_scene_interface.py

@@ -197,6 +197,24 @@ def remove_attached_object(self, link=None, name=None):
             aco.object.id = name
         self.__submit(aco, attach=True)
 
+    def allow_collisions(self, link_1, link_2=""):
+        """
+        Allow collisions between link_1(one or many) and link_2.
+        """
+        return self._psi.allow_collisions(link_1, link_2)
+
+    def disallow_collisions(self, link_1, link_2=""):
+        """
+        Disallow collisions between link_1(one or many) and link_2.
+        """
+        return self._psi.disallow_collisions(link_1, link_2)
+
+    def set_collisions(self, set_to_allow, link_1, link_2):
+        """
+        Set collisions between link_1 and link_2.
+        """
+        return self._psi.set_collisions(set_to_allow, link_1, link_2)
+
     def get_known_object_names(self, with_type=False):
         """
         Get the names of all known objects in the world. If with_type is set to true, only return objects that have a known type.
@@ -250,6 +268,14 @@ def get_attached_objects(self, object_ids=[]):
             aobjs[key] = msg
         return aobjs
 
+    def apply_collision_object(self, collision_object_message):
+        """
+        Applies the collision object message to the scene.
+        """
+        return self._psi.apply_collision_object(
+            conversions.msg_to_string(collision_object_message)
+        )
+
     def apply_planning_scene(self, planning_scene_message):
         """
         Applies the planning scene message.

moveit_core/collision_detection/include/moveit/collision_detection/world.h

@@ -91,6 +91,13 @@ class World
      *  This frame is returned when getTransform() is called with the object's name. */
     Eigen::Isometry3d pose_;
 
+    /** \brief (Optional) The URL of the mesh used for the visual representation of this object.
+     * Only used for display in Rviz, not for collision checking. */
+    std::string visual_geometry_mesh_url_;
+
+    /** \brief The pose of the visual geometry (mesh). */
+    Eigen::Isometry3d visual_geometry_pose_;
+
     /** \brief All the shapes making up this object.
      *
      * The pose of each Shape is stored in the corresponding element of the shape_poses_ array. */
@@ -244,6 +251,10 @@ class World
   /** \brief Set subframes on an object. The frames are relative to the object pose. */
   bool setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses);
 
+  /** \brief Set the visual geometry of an object. The pose is relative to the object pose. */
+  bool setObjectVisualGeometry(const std::string& object_id, const std::string& mesh_url,
+                               const Eigen::Isometry3d& visual_geometry_pose);
+
   /** \brief Clear all objects.
    * If there are no other pointers to corresponding instances of Objects,
    * the memory is freed. */

moveit_core/collision_detection/src/world.cpp

@@ -87,6 +87,8 @@ void World::addToObject(const std::string& object_id, const Eigen::Isometry3d& p
     obj = std::make_shared<Object>(object_id);
     action |= CREATE;
     obj->pose_ = pose;
+    obj->visual_geometry_mesh_url_ = "";
+    obj->visual_geometry_pose_ = Eigen::Isometry3d::Identity();
   }
   else
     ensureUnique(obj);
@@ -337,6 +339,24 @@ bool World::setSubframesOfObject(const std::string& object_id, const moveit::cor
   return true;
 }
 
+bool World::setObjectVisualGeometry(const std::string& object_id, const std::string& mesh_url,
+                                    const Eigen::Isometry3d& visual_geometry_pose)
+{
+  ASSERT_ISOMETRY(visual_geometry_pose);  // unsanitized input, could contain a non-isometry
+  ObjectPtr& obj = objects_[object_id];
+  int action = 0;
+  if (!obj)
+  {
+    obj.reset(new Object(object_id));
+    action |= CREATE;
+  }
+  ensureUnique(obj);
+  obj->visual_geometry_mesh_url_ = mesh_url;
+  obj->visual_geometry_pose_ = visual_geometry_pose;
+  notify(obj, Action(action));
+  return true;
+}
+
 void World::updateGlobalPosesInternal(ObjectPtr& obj, bool update_shape_poses, bool update_subframe_poses)
 {
   // Update global shape poses

moveit_core/planning_scene/src/planning_scene.cpp

@@ -454,6 +454,7 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
           scene->setObjectType(it.first, getObjectType(it.first));
 
         scene->world_->setSubframesOfObject(obj.id_, obj.subframe_poses_);
+        scene->world_->setObjectVisualGeometry(obj.id_, obj.visual_geometry_mesh_url_, obj.visual_geometry_pose_);
       }
     }
   }
@@ -801,6 +802,8 @@ bool PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collisio
     p = tf2::toMsg(frame_pair.second);
     collision_obj.subframe_poses.push_back(p);
   }
+  collision_obj.visual_geometry_mesh_url = obj->visual_geometry_mesh_url_;
+  collision_obj.visual_geometry_pose = tf2::toMsg(obj->visual_geometry_pose_);
 
   return true;
 }
@@ -1001,6 +1004,8 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const
           out << pose_pair.first << std::endl;     // Subframe name
           writePoseToText(out, pose_pair.second);  // Subframe pose
         }
+
+        // TODO (felixvd): Write visual geometry?
       }
     }
   out << "." << std::endl;
@@ -1509,6 +1514,8 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
     {
       // items to build the attached object from (filled from existing world object or message)
       Eigen::Isometry3d object_pose_in_link;
+      std::string visual_geometry_mesh_url;
+      Eigen::Isometry3d visual_geometry_pose;
       std::vector<shapes::ShapeConstPtr> shapes;
       EigenSTL::vector_Isometry3d shape_poses;
       moveit::core::FixedTransformsMap subframe_poses;
@@ -1528,6 +1535,8 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
           shapes = obj_in_world->shapes_;
           shape_poses = obj_in_world->shape_poses_;
           subframe_poses = obj_in_world->subframe_poses_;
+          visual_geometry_mesh_url = obj_in_world->visual_geometry_mesh_url_;
+          visual_geometry_pose = obj_in_world->visual_geometry_pose_;
         }
         else
         {
@@ -1555,6 +1564,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
           std::string name = object.object.subframe_names[i];
           subframe_poses[name] = subframe_pose;
         }
+
+        visual_geometry_mesh_url = object.object.visual_geometry_mesh_url;
+        PlanningScene::poseMsgToEigen(object.object.visual_geometry_pose, visual_geometry_pose);
       }
 
       if (shapes.empty())
@@ -1591,7 +1603,8 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
                           object.object.id.c_str(), object.link_name.c_str());
 
         robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, object.touch_links,
-                                 object.link_name, object.detach_posture, subframe_poses);
+                                 object.link_name, object.detach_posture, subframe_poses, visual_geometry_mesh_url,
+                                 visual_geometry_pose);
         ROS_DEBUG_NAMED(LOGNAME, "Attached object '%s' to link '%s'", object.object.id.c_str(),
                         object.link_name.c_str());
       }
@@ -1613,9 +1626,21 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
         touch_links.insert(std::make_move_iterator(object.touch_links.begin()),
                            std::make_move_iterator(object.touch_links.end()));
 
+        if (!object.object.visual_geometry_mesh_url.empty())  // If visual geometry is defined
+        {
+          visual_geometry_mesh_url = object.object.visual_geometry_mesh_url;
+          PlanningScene::poseMsgToEigen(object.object.visual_geometry_pose, visual_geometry_pose);
+        }
+        else
+        {
+          visual_geometry_mesh_url = "";
+          visual_geometry_pose = Eigen::Isometry3d::Identity();
+        }
+
         robot_state_->clearAttachedBody(object.object.id);
         robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, touch_links,
-                                 object.link_name, detach_posture, subframe_poses);
+                                 object.link_name, detach_posture, subframe_poses, visual_geometry_mesh_url,
+                                 visual_geometry_pose);
         ROS_DEBUG_NAMED(LOGNAME, "Appended things to object '%s' attached to link '%s'", object.object.id.c_str(),
                         object.link_name.c_str());
       }
@@ -1668,6 +1693,8 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
         const Eigen::Isometry3d& pose = attached_body->getGlobalPose();
         world_->addToObject(name, pose, attached_body->getShapes(), attached_body->getShapePoses());
         world_->setSubframesOfObject(name, attached_body->getSubframes());
+        world_->setObjectVisualGeometry(name, attached_body->getVisualGeometryUrl(),
+                                        attached_body->getVisualGeometryPose());
         ROS_DEBUG_NAMED(LOGNAME, "Detached object '%s' from link '%s' and added it back in the collision world",
                         name.c_str(), object.link_name.c_str());
       }
@@ -1854,6 +1881,15 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::CollisionObject
     subframes[name] = subframe_pose;
   }
   world_->setSubframesOfObject(object.id, subframes);
+
+  ROS_WARN_STREAM_NAMED(LOGNAME, "Got object with url " << object.visual_geometry_mesh_url);
+  if (!object.visual_geometry_mesh_url.empty())
+  {
+    ROS_WARN_NAMED(LOGNAME, "DEBUG 1");
+    Eigen::Isometry3d visual_geometry_pose;
+    PlanningScene::poseMsgToEigen(object.visual_geometry_pose, visual_geometry_pose);
+    world_->setObjectVisualGeometry(object.id, object.visual_geometry_mesh_url, visual_geometry_pose);
+  }
   return true;
 }
 

moveit_core/robot_state/include/moveit/robot_state/attached_body.h

@@ -65,8 +65,10 @@ class AttachedBody
    * The shape and subframe poses are relative to the \e pose, and \e pose is relative to the parent link. */
   AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose,
                const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
-               const std::set<std::string>& touch_links, const trajectory_msgs::JointTrajectory& detach_posture,
-               const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap());
+               const std::set<std::string>& touch_links, const trajectory_msgs::JointTrajectory& attach_posture,
+               const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap(),
+               const std::string& visual_geometry_mesh_url = "",
+               const Eigen::Isometry3d& visual_geometry_pose = Eigen::Isometry3d::Identity());
 
   ~AttachedBody();
 
@@ -200,6 +202,18 @@ class AttachedBody
   /** \brief Set the scale for the shapes of this attached object */
   void setScale(double scale);
 
+  /** \brief Get the visual geometry url of this attached object (only used for display) */
+  const std::string& getVisualGeometryUrl() const
+  {
+    return visual_geometry_mesh_url_;
+  }
+
+  /** \brief Get the visual geometry's pose (relative to the object pose) */
+  const Eigen::Isometry3d& getVisualGeometryPose() const
+  {
+    return visual_geometry_pose_;
+  }
+
   /** \brief Recompute global_collision_body_transform given the transform of the parent link */
   void computeTransform(const Eigen::Isometry3d& parent_link_global_transform);
 
@@ -240,6 +254,13 @@ class AttachedBody
 
   /** \brief Transforms to subframes on the object, relative to the model frame. */
   moveit::core::FixedTransformsMap global_subframe_poses_;
+
+  /** \brief (Optional) The URL of the mesh used for the visual representation of this object.
+   * Only used for display in Rviz, not for collision checking. */
+  std::string visual_geometry_mesh_url_;
+
+  /** \brief The pose of the visual geometry (mesh). */
+  Eigen::Isometry3d visual_geometry_pose_;
 };
 }  // namespace core
 }  // namespace moveit

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -1672,6 +1672,8 @@ class RobotState
    * @param link_name The link to attach to
    * @param detach_posture The posture of the gripper when placing the object
    * @param subframe_poses Transforms to points of interest on the object (can be used as end effector link)
+   * @param visual_geometry_mesh_url The URL of the visual representation of the object
+   * @param visual_geometry_pose The pose of the object's visual relative to the object pose
    *
    * This only adds the given body to this RobotState
    * instance.  It does not change anything about other
@@ -1684,7 +1686,9 @@ class RobotState
                   const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
                   const std::set<std::string>& touch_links, const std::string& link_name,
                   const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory(),
-                  const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap());
+                  const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap(),
+                  const std::string& visual_geometry_mesh_url = "",
+                  const Eigen::Isometry3d& visual_geometry_pose = Eigen::Isometry3d::Identity());
 
   /** @brief Add an attached body to a link
    * @param id The string id associated with the attached body
@@ -1695,6 +1699,8 @@ class RobotState
    * @param link_name The link to attach to
    * @param detach_posture The posture of the gripper when placing the object
    * @param subframe_poses Transforms to points of interest on the object (can be used as end effector link)
+   * @param visual_geometry_mesh_url The URL of the visual representation of the object
+   * @param visual_geometry_pose The pose of the object's visual relative to the object pose
    *
    * This only adds the given body to this RobotState
    * instance.  It does not change anything about other
@@ -1707,10 +1713,13 @@ class RobotState
                   const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
                   const std::vector<std::string>& touch_links, const std::string& link_name,
                   const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory(),
-                  const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap())
+                  const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap(),
+                  const std::string& visual_geometry_mesh_url = "",
+                  const Eigen::Isometry3d& visual_geometry_pose = Eigen::Isometry3d::Identity())
   {
     std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
-    attachBody(id, pose, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses);
+    attachBody(id, pose, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses,
+               visual_geometry_mesh_url, visual_geometry_pose);
   }
 
   /** \brief Get all bodies attached to the model corresponding to this state */

moveit_core/robot_state/src/attached_body.cpp

@@ -47,7 +47,8 @@ AttachedBody::AttachedBody(const LinkModel* parent, const std::string& id, const
                            const std::vector<shapes::ShapeConstPtr>& shapes,
                            const EigenSTL::vector_Isometry3d& shape_poses, const std::set<std::string>& touch_links,
                            const trajectory_msgs::JointTrajectory& detach_posture,
-                           const FixedTransformsMap& subframe_poses)
+                           const FixedTransformsMap& subframe_poses, const std::string& visual_geometry_mesh_url,
+                           const Eigen::Isometry3d& visual_geometry_pose)
   : parent_link_model_(parent)
   , id_(id)
   , pose_(pose)
@@ -57,6 +58,8 @@ AttachedBody::AttachedBody(const LinkModel* parent, const std::string& id, const
   , detach_posture_(detach_posture)
   , subframe_poses_(subframe_poses)
   , global_subframe_poses_(subframe_poses)
+  , visual_geometry_mesh_url_(visual_geometry_mesh_url)
+  , visual_geometry_pose_(visual_geometry_pose)
 {
   ASSERT_ISOMETRY(pose)  // unsanitized input, could contain a non-isometry
   for (const auto& t : shape_poses_)

moveit_core/robot_state/src/conversions.cpp

@@ -227,6 +227,8 @@ static void _attachedBodyToMsg(const AttachedBody& attached_body, moveit_msgs::A
     pose = tf2::toMsg(frame_pair.second);
     aco.object.subframe_poses.push_back(pose);
   }
+  aco.object.visual_geometry_mesh_url = attached_body.getVisualGeometryUrl();
+  aco.object.visual_geometry_pose = tf2::toMsg(attached_body.getVisualGeometryPose());
 }
 
 static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::AttachedCollisionObject& aco, RobotState& state)
@@ -324,13 +326,16 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::Attached
                           aco.link_name.c_str(), aco.object.id.c_str());
         else
         {
+          Eigen::Isometry3d visual_geometry_pose;
+          tf2::fromMsg(aco.object.visual_geometry_pose, visual_geometry_pose);
           if (state.clearAttachedBody(aco.object.id))
             ROS_DEBUG_NAMED(LOGNAME,
                             "The robot state already had an object named '%s' attached to link '%s'. "
                             "The object was replaced.",
                             aco.object.id.c_str(), aco.link_name.c_str());
           state.attachBody(aco.object.id, object_pose, shapes, shape_poses, aco.touch_links, aco.link_name,
-                           aco.detach_posture, subframe_poses);
+                           aco.detach_posture, subframe_poses, aco.object.visual_geometry_mesh_url,
+                           visual_geometry_pose);
           ROS_DEBUG_NAMED(LOGNAME, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str());
         }
       }

moveit_core/robot_state/src/robot_state.cpp

@@ -1030,10 +1030,12 @@ void RobotState::attachBody(const std::string& id, const Eigen::Isometry3d& pose
                             const std::vector<shapes::ShapeConstPtr>& shapes,
                             const EigenSTL::vector_Isometry3d& shape_poses, const std::set<std::string>& touch_links,
                             const std::string& link, const trajectory_msgs::JointTrajectory& detach_posture,
-                            const moveit::core::FixedTransformsMap& subframe_poses)
+                            const moveit::core::FixedTransformsMap& subframe_poses,
+                            const std::string& visual_geometry_mesh_url, const Eigen::Isometry3d& visual_geometry_pose)
 {
-  attachBody(std::make_unique<AttachedBody>(robot_model_->getLinkModel(link), id, pose, shapes, shape_poses,
-                                            touch_links, detach_posture, subframe_poses));
+  const LinkModel* l = robot_model_->getLinkModel(link);
+  attachBody(new AttachedBody(l, id, pose, shapes, shape_poses, touch_links, detach_posture, subframe_poses,
+                              visual_geometry_mesh_url, visual_geometry_pose));
 }
 
 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const