diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 92799537c55..d610d4b43cc 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -217,6 +217,9 @@ class World bool moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& shape_pose); + /** \brief Update the pose of all shapes in an object. Shape size is verified. Returns true on success. */ + bool moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses); + /** \brief Move the object pose (thus moving all shapes and subframes in the object) * according to the given transform specified in world frame. * The transform is relative to and changes the object pose. It does not replace it. diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index eae07cf01cc..ce7c6006fc6 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -259,6 +259,26 @@ bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeC return false; } +bool World::moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses) +{ + auto it = objects_.find(object_id); + if (it != objects_.end()) + { + if (shape_poses.size() == it->second->shapes_.size()) + { + for (std::size_t i = 0; i < shape_poses.size(); ++i) + { + ASSERT_ISOMETRY(shape_poses[i]) // unsanitized input, could contain a non-isometry + it->second->shape_poses_[i] = shape_poses[i]; + it->second->global_shape_poses_[i] = it->second->pose_ * shape_poses[i]; + } + notify(it->second, MOVE_SHAPE); + return true; + } + } + return false; +} + bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& transform) { const auto it = objects_.find(object_id); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 479f3426ce1..9dbcab0addd 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1857,6 +1857,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision { if (world_->hasObject(object.id)) { + // update object pose if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty()) { RCLCPP_WARN(getLogger(), "Move operation for object '%s' ignores the geometry specified in the message.", @@ -1870,6 +1871,45 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform; world_->setObjectPose(object.id, object_frame_transform); + + // update shape poses + if (!object.primitive_poses.empty() || !object.mesh_poses.empty() || !object.plane_poses.empty()) + { + auto world_object = world_->getObject(object.id); // object exists, checked earlier + + std::size_t shape_size = object.primitive_poses.size() + object.mesh_poses.size() + object.plane_poses.size(); + if (shape_size != world_object->shape_poses_.size()) + { + ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' must have same number of geometry poses. Cannot move.", + object.id.c_str()); + return false; + } + + // order matters -> primitive, mesh and plane + EigenSTL::vector_Isometry3d shape_poses; + for (const auto& shape_pose : object.primitive_poses) + { + shape_poses.emplace_back(); + PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + } + for (const auto& shape_pose : object.mesh_poses) + { + shape_poses.emplace_back(); + PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + } + for (const auto& shape_pose : object.plane_poses) + { + shape_poses.emplace_back(); + PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + } + + if (!world_->moveShapesInObject(object.id, shape_poses)) + { + ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' internal world error. Cannot move.", object.id.c_str()); + return false; + } + } + return true; }