diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi index c22ee3fed8..0617fab139 100644 --- a/moveit_py/moveit/planning.pyi +++ b/moveit_py/moveit/planning.pyi @@ -60,6 +60,9 @@ class PlanningSceneMonitor: def stop_state_monitor(self, *args, **kwargs) -> Any: ... def update_frame_transforms(self, *args, **kwargs) -> Any: ... def wait_for_current_robot_state(self, *args, **kwargs) -> Any: ... + def request_planning_scene_state(self, *args, **kwargs) -> Any: ... + def process_collision_object(self, *args, **kwargs) -> Any: ... + def process_attached_collision_object(self, *args, **kwargs) -> Any: ... @property def name(self) -> Any: ... diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp index 9682fd6cf0..d72ec0358c 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp @@ -42,6 +42,22 @@ namespace bind_planning_scene_monitor { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_py.bind_planning_scene_monitor"); +bool processCollisionObject(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + moveit_msgs::msg::CollisionObject& collision_object_msg) +{ + moveit_msgs::msg::CollisionObject::ConstSharedPtr const_ptr = + std::make_shared(collision_object_msg); + return planning_scene_monitor->processCollisionObjectMsg(const_ptr); +} + +bool processAttachedCollisionObjectMsg(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + moveit_msgs::msg::AttachedCollisionObject& attached_collision_object_msg) +{ + moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr const_ptr = + std::make_shared(attached_collision_object_msg); + return planning_scene_monitor->processAttachedCollisionObjectMsg(const_ptr); +} + LockedPlanningSceneContextManagerRO readOnly(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) { @@ -119,6 +135,14 @@ void initPlanningSceneMonitor(py::module& m) R"( Stops the state monitor. )") + .def("request_planning_scene_state", &planning_scene_monitor::PlanningSceneMonitor::requestPlanningSceneState, + py::arg("service_name"), + R"( + Request the planning scene. + + Args: + service_name (str): The name of the service to call. + )") .def("wait_for_current_robot_state", &planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState, R"( @@ -129,6 +153,23 @@ void initPlanningSceneMonitor(py::module& m) R"( Clears the octomap. )") + .def("process_collision_object", &moveit_py::bind_planning_scene_monitor::processCollisionObject, + py::arg("collision_object_msg"), // py::arg("color_msg") = nullptr, + R"( + Apply a collision object to the planning scene. + + Args: + collision_object_msg (moveit_msgs.msg.CollisionObject): The collision object to apply to the planning scene. + )") + .def("process_attached_collision_object", + &moveit_py::bind_planning_scene_monitor::processAttachedCollisionObjectMsg, + py::arg("attached_collision_object_msg"), + R"( + Apply an attached collision object msg to the planning scene. + + Args: + attached_collision_object_msg (moveit_msgs.msg.AttachedCollisionObject): The attached collision object to apply to the planning scene. + )") .def("read_only", &moveit_py::bind_planning_scene_monitor::readOnly, R"( diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index fd5199a2e5..bf3a175cfe 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -392,6 +392,13 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor // Called to update the planning scene with a new message. bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene); + // Called to update a collision object in the planning scene. + bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg); + + // Called to update an attached collision object in the planning scene. + bool processAttachedCollisionObjectMsg( + const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& attached_collision_object_msg); + protected: /** @brief Initialize the planning scene monitor * @param scene The scene instance to fill with data (an instance is allocated if the one passed in is not allocated) @@ -418,18 +425,12 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor /** @brief Configure the default padding*/ void configureDefaultPadding(); - /** @brief Callback for a new collision object msg*/ - void collisionObjectCallback(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj); - /** @brief Callback for a new planning scene world*/ void newPlanningSceneWorldCallback(const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world); /** @brief Callback for octomap updates */ void octomapUpdateCallback(); - /** @brief Callback for a new attached object msg*/ - void attachObjectCallback(const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj); - /** @brief Callback for a change for an attached object of the current state of the planning scene */ void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached); diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 72ae9cf704..4921316479 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -780,47 +780,45 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann return result; } -void PlanningSceneMonitor::newPlanningSceneWorldCallback( - const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) +bool PlanningSceneMonitor::processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& object) { - if (scene_) + if (!scene_) + return false; + + updateFrameTransforms(); { - updateFrameTransforms(); - { - std::unique_lock ulock(scene_update_mutex_); - last_update_time_ = rclcpp::Clock().now(); - scene_->getWorldNonConst()->clearObjects(); - scene_->processPlanningSceneWorldMsg(*world); - if (octomap_monitor_) - { - if (world->octomap.octomap.data.empty()) - { - octomap_monitor_->getOcTreePtr()->lockWrite(); - octomap_monitor_->getOcTreePtr()->clear(); - octomap_monitor_->getOcTreePtr()->unlockWrite(); - } - } - } - triggerSceneUpdateEvent(UPDATE_SCENE); + std::unique_lock ulock(scene_update_mutex_); + last_update_time_ = rclcpp::Clock().now(); + if (!scene_->processCollisionObjectMsg(*object)) + return false; } + triggerSceneUpdateEvent(UPDATE_GEOMETRY); + RCLCPP_INFO(logger_, "Published update collision object"); + return true; } -void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) +bool PlanningSceneMonitor::processAttachedCollisionObjectMsg( + const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& object) { if (!scene_) - return; + { + return false; + } updateFrameTransforms(); { std::unique_lock ulock(scene_update_mutex_); last_update_time_ = rclcpp::Clock().now(); - if (!scene_->processCollisionObjectMsg(*obj)) - return; + if (!scene_->processAttachedCollisionObjectMsg(*object)) + return false; } triggerSceneUpdateEvent(UPDATE_GEOMETRY); + RCLCPP_INFO(logger_, "Published update attached"); + return true; } -void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) +void PlanningSceneMonitor::newPlanningSceneWorldCallback( + const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world) { if (scene_) { @@ -828,9 +826,19 @@ void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::msg::Attached { std::unique_lock ulock(scene_update_mutex_); last_update_time_ = rclcpp::Clock().now(); - scene_->processAttachedCollisionObjectMsg(*obj); + scene_->getWorldNonConst()->clearObjects(); + scene_->processPlanningSceneWorldMsg(*world); + if (octomap_monitor_) + { + if (world->octomap.octomap.data.empty()) + { + octomap_monitor_->getOcTreePtr()->lockWrite(); + octomap_monitor_->getOcTreePtr()->clear(); + octomap_monitor_->getOcTreePtr()->unlockWrite(); + } + } } - triggerSceneUpdateEvent(UPDATE_GEOMETRY); + triggerSceneUpdateEvent(UPDATE_SCENE); } } @@ -1256,7 +1264,7 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio { collision_object_subscriber_ = pnode_->create_subscription( collision_objects_topic, rclcpp::SystemDefaultsQoS(), - [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return collisionObjectCallback(obj); }); + [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { processCollisionObjectMsg(obj); }); RCLCPP_INFO(logger_, "Listening to '%s'", collision_objects_topic.c_str()); } @@ -1339,7 +1347,7 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top attached_collision_object_subscriber_ = pnode_->create_subscription( attached_objects_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) { - return attachObjectCallback(obj); + processAttachedCollisionObjectMsg(obj); }); RCLCPP_INFO(logger_, "Listening to '%s' for attached collision objects", attached_collision_object_subscriber_->get_topic_name());