Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[PSM] Add proccess Collision Object to PSM and request planning scene to moveit py to allow syncing of mutliple PSM #2536

Merged
merged 7 commits into from
Nov 27, 2023
Merged

[PSM] Add proccess Collision Object to PSM and request planning scene to moveit py to allow syncing of mutliple PSM #2536

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
3428fe2
PlanningSceneMonitor and request planning scene to moveit py to allow…
JensVanhooydonck Nov 20, 2023
c2639ef
pre-commit fixes
JensVanhooydonck Nov 21, 2023
386674c
Merge branch 'main' into feature/multiple_planning_scene_monitor_syncing
sjahr Nov 27, 2023
eb10d2b
Update moveit_ros/planning/planning_scene_monitor/src/planning_scene_…
JensVanhooydonck Nov 27, 2023
6c0161b
Removed unneeded callback functions
JensVanhooydonck Nov 27, 2023
a71b56c
Merge branch 'feature/multiple_planning_scene_monitor_syncing' of htt…
JensVanhooydonck Nov 27, 2023
a2be672
Merge branch 'main' into feature/multiple_planning_scene_monitor_syncing
sjahr Nov 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions moveit_py/moveit/planning.pyi
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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: ...

Expand Down
41 changes: 41 additions & 0 deletions moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<const moveit_msgs::msg::CollisionObject>(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<const moveit_msgs::msg::AttachedCollisionObject>(attached_collision_object_msg);
return planning_scene_monitor->processAttachedCollisionObjectMsg(const_ptr);
}

LockedPlanningSceneContextManagerRO
readOnly(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
{
Expand Down Expand Up @@ -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"(
Expand All @@ -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"(
Expand Down
7 changes: 7 additions & 0 deletions ...ing/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
59 changes: 38 additions & 21 deletions moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -780,6 +780,42 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann
return result;
}

bool PlanningSceneMonitor::processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& object)
{
if (!scene_)
return false;

updateFrameTransforms();
{
std::unique_lock<std::shared_mutex> 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;
}

bool PlanningSceneMonitor::processAttachedCollisionObjectMsg(
const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& object)
{
if (scene_)
{
updateFrameTransforms();
{
std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
last_update_time_ = rclcpp::Clock().now();
if (!scene_->processAttachedCollisionObjectMsg(*object))
return false;
}
triggerSceneUpdateEvent(UPDATE_GEOMETRY);
RCLCPP_INFO(logger_, "Published update attached");
return true;
}
return false;
}

void PlanningSceneMonitor::newPlanningSceneWorldCallback(
const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world)
{
Expand Down Expand Up @@ -807,31 +843,12 @@ void PlanningSceneMonitor::newPlanningSceneWorldCallback(

void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj)
{
if (!scene_)
return;

updateFrameTransforms();
{
std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
last_update_time_ = rclcpp::Clock().now();
if (!scene_->processCollisionObjectMsg(*obj))
return;
}
triggerSceneUpdateEvent(UPDATE_GEOMETRY);
processCollisionObjectMsg(obj);
}

void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj)
{
if (scene_)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it would be even nicer to completely get rid of the callback member functions and just call the new function with a lambda when the services are initialized

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Shall I just remove the callback functions and change the call of these functions from

  collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::CollisionObject>(
        collision_objects_topic, rclcpp::SystemDefaultsQoS(),
        [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return collisionObjectCallback(obj); });
    RCLCPP_INFO(logger_, "Listening to '%s'", collision_objects_topic.c_str());

to

  collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::CollisionObject>(
        collision_objects_topic, rclcpp::SystemDefaultsQoS(),
        [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return processCollisionObjectMsg(obj); });
    RCLCPP_INFO(logger_, "Listening to '%s'", collision_objects_topic.c_str());

And the same for the AttachedCollisionObjects?

These functions do not seem to be used somewhere else. I've added this to make sure there would not be any breaking changes.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes 👍 They aren't used anywhere else and if they only call another function we don't need them. I think the lambda needs be

-   [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return processCollisionObjectMsg(obj); }
+   [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { processCollisionObjectMsg(obj); }

because the create_subscription expects a function argument that returns void but just use whatever compiles

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changed it

{
updateFrameTransforms();
{
std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
last_update_time_ = rclcpp::Clock().now();
scene_->processAttachedCollisionObjectMsg(*obj);
}
triggerSceneUpdateEvent(UPDATE_GEOMETRY);
}
processAttachedCollisionObjectMsg(obj);
}

void PlanningSceneMonitor::excludeRobotLinksFromOctree()
Expand Down