Skip to content

Allowed Collision Matrix not updating using apply_planning_scene service. #3527

New issue
New issue
Closed
#3534
Closed
Allowed Collision Matrix not updating using apply_planning_scene service.#3527
#3534
Labels
bug
@BrendanWooAdmetal

Description

@BrendanWooAdmetal
BrendanWooAdmetal
opened on Nov 14, 2023

Description

I am attempting to modify the Allowed Collision Matrix (ACM) to prevent collision checking with an object added to the scene.
I have followed the steps for updating the ACM recommended here: Updating allowed collision matrix used for motion plans

I retrieve the ACM using the get_planning_scene service, modify the ACM, and send it to the apply_planning_scene service.
However, checking get_planning_scene after shows the update has not been applied.
Notably I am able to successfully add new objects to the scene using apply_planning_scene, but the ACM is not changed.
The same is true if I use the planning_scene topic.

Your environment

  • ROS Distro: Noetic
  • OS Version: Ubuntu 20.04
  • Binary
  • 1.1.13-2focal.20231014.010446

Steps to reproduce

The following code is a minimal test intended to replicate the issue.
Run it alongside MoveIt.

import rospy
import moveit_commander
from moveit_msgs.srv import GetPlanningScene, ApplyPlanningSceneRequest
from moveit_msgs.msg import PlanningScene, AllowedCollisionEntry


def allow_collisions_with_object(obj_name, scene):
    """ Updates the MoveIt PlanningScene using the AllowedCollisionMatrix to ignore collisions for an object """
    # Set up service to get the current planning scene
    service_timeout = 5.0
    _get_planning_scene = rospy.ServiceProxy(
        "get_planning_scene", GetPlanningScene
    )
    _get_planning_scene.wait_for_service(service_timeout)

    request = GetPlanningScene()
    request.components = 0    # Get just the Allowed Collision Matrix
    planning_scene = _get_planning_scene.call(request)
    print(f"\n\n\n--- allowed_collision_matrix before update:{planning_scene.scene.allowed_collision_matrix} ---\n\n\n")

    # Set this object to ignore collisions with all objects. The entry values are not updated
    planning_scene.scene.allowed_collision_matrix.entry_names.append(obj_name)
    for entry in planning_scene.scene.allowed_collision_matrix.entry_values:
        entry.enabled.append(False)
    enabled = [False for i in range(len(planning_scene.scene.allowed_collision_matrix.entry_names))]
    entry = AllowedCollisionEntry(enabled=enabled)  # Test kwarg in constructor
    planning_scene.scene.allowed_collision_matrix.entry_values.append(entry)

    # Set the default entries. They are also not updated
    planning_scene.scene.allowed_collision_matrix.default_entry_names = [obj_name]
    planning_scene.scene.allowed_collision_matrix.default_entry_values = [False]
    planning_scene.scene.is_diff = True # Mark this as a diff message to force an update of the allowed collision matrix
    planning_scene.scene.robot_state.is_diff = True

    planning_scene_diff_req = ApplyPlanningSceneRequest()
    planning_scene_diff_req.scene = planning_scene.scene

    # Updating the Allowed Collision Matrix through the apply_planning_scene service shows no effect.
    # However, adding objects to the planning scene works fine.
    # scene._apply_planning_scene_diff.call(planning_scene_diff_req)
    scene.apply_planning_scene(planning_scene.scene)

    # Attempting to use the planning_scene topic for asynchronous updates also does not work
    # planning_scene_pub = rospy.Publisher("planning_scene", PlanningScene, queue_size=5)
    # planning_scene_pub.publish(planning_scene.scene)

    print(f"\n--- Sent message:{planning_scene.scene.allowed_collision_matrix} ---\n")

    # The planning scene retrieved after the update should have taken place shows the Allowed Collision Matrix is the same as before
    request = GetPlanningScene()
    request.components = 0    # Get just the Allowed Collision Matrix
    planning_scene = _get_planning_scene.call(request)
    print(f"\n--- allowed_collision_matrix after update:{planning_scene.scene.allowed_collision_matrix} ---\n")


if __name__ == "__main__":
    # Moveit scene variable
    scene = moveit_commander.PlanningSceneInterface()
    allow_collisions_with_object("temp_obj_name", scene)

Expected behaviour

After the update, the ACM is retrieved. It should now show the changes in the ACM requested in the message.

Actual behaviour

After the update, the ACM is the same as it was before, with none of our changes to the default entries or entries displayed.

Backtrace or Console output

--- allowed_collision_matrix before update:entry_names: 
  - Gripper_cam_link
  - Gripper_link
  - Link1
  - Link2
  - Link3
  - Link4
  - Link5
  - Link6
  - Mount1
  - base_link
  - robot_body_link
entry_values: 
  - 
    enabled: [False, True, False, False, True, True, True, True, True, False, False]
  - 
    enabled: [True, False, False, False, True, True, True, True, True, False, False]
  - 
    enabled: [False, False, False, True, True, True, False, False, False, True, True]
  - 
    enabled: [False, False, True, False, True, True, False, False, False, True, True]
  - 
    enabled: [True, True, True, True, False, True, True, True, True, True, False]
  - 
    enabled: [True, True, True, True, True, False, True, True, True, False, False]
  - 
    enabled: [True, True, False, False, True, True, False, True, True, False, False]
  - 
    enabled: [True, True, False, False, True, True, True, False, True, False, False]
  - 
    enabled: [True, True, False, False, True, True, True, True, False, False, False]
  - 
    enabled: [False, False, True, True, True, False, False, False, False, False, True]
  - 
    enabled: [False, False, True, True, False, False, False, False, False, True, False]
default_entry_names: []
default_entry_values: [] ---


--- Sent message:entry_names: 
  - Gripper_cam_link
  - Gripper_link
  - Link1
  - Link2
  - Link3
  - Link4
  - Link5
  - Link6
  - Mount1
  - base_link
  - robot_body_link
  - temp_obj_name
entry_values: 
  - 
    enabled: [False, True, False, False, True, True, True, True, True, False, False, False]
  - 
    enabled: [True, False, False, False, True, True, True, True, True, False, False, False]
  - 
    enabled: [False, False, False, True, True, True, False, False, False, True, True, False]
  - 
    enabled: [False, False, True, False, True, True, False, False, False, True, True, False]
  - 
    enabled: [True, True, True, True, False, True, True, True, True, True, False, False]
  - 
    enabled: [True, True, True, True, True, False, True, True, True, False, False, False]
  - 
    enabled: [True, True, False, False, True, True, False, True, True, False, False, False]
  - 
    enabled: [True, True, False, False, True, True, True, False, True, False, False, False]
  - 
    enabled: [True, True, False, False, True, True, True, True, False, False, False, False]
  - 
    enabled: [False, False, True, True, True, False, False, False, False, False, True, False]
  - 
    enabled: [False, False, True, True, False, False, False, False, False, True, False, False]
  - 
    enabled: [False, False, False, False, False, False, False, False, False, False, False, False]
default_entry_names: 
  - temp_obj_name
default_entry_values: [False] ---


--- allowed_collision_matrix after update:entry_names: 
  - Gripper_cam_link
  - Gripper_link
  - Link1
  - Link2
  - Link3
  - Link4
  - Link5
  - Link6
  - Mount1
  - base_link
  - robot_body_link
entry_values: 
  - 
    enabled: [False, True, False, False, True, True, True, True, True, False, False]
  - 
    enabled: [True, False, False, False, True, True, True, True, True, False, False]
  - 
    enabled: [False, False, False, True, True, True, False, False, False, True, True]
  - 
    enabled: [False, False, True, False, True, True, False, False, False, True, True]
  - 
    enabled: [True, True, True, True, False, True, True, True, True, True, False]
  - 
    enabled: [True, True, True, True, True, False, True, True, True, False, False]
  - 
    enabled: [True, True, False, False, True, True, False, True, True, False, False]
  - 
    enabled: [True, True, False, False, True, True, True, False, True, False, False]
  - 
    enabled: [True, True, False, False, True, True, True, True, False, False, False]
  - 
    enabled: [False, False, True, True, True, False, False, False, False, False, True]
  - 
    enabled: [False, False, True, True, False, False, False, False, False, True, False]
default_entry_names: []
default_entry_values: [] ---

Metadata

Metadata

Assignees

No one assigned

    Labels

    bug

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions