From c671fda7ae84a30109d4340440a653863c522868 Mon Sep 17 00:00:00 2001 From: Amal Nanavati <amaln@uw.edu> Date: Fri, 8 Sep 2023 13:52:29 -0700 Subject: [PATCH] Added 3 workspace walls to the planning scene --- ada_feeding/config/ada_planning_scene.yaml | 37 ++++++++-- ada_feeding/scripts/ada_planning_scene.py | 79 +++++++++++++++++++--- 2 files changed, 100 insertions(+), 16 deletions(-) diff --git a/ada_feeding/config/ada_planning_scene.yaml b/ada_feeding/config/ada_planning_scene.yaml index a355379c..b8d51221 100644 --- a/ada_feeding/config/ada_planning_scene.yaml +++ b/ada_feeding/config/ada_planning_scene.yaml @@ -6,28 +6,53 @@ ada_planning_scene: - wheelchair_collision - table - head - # For each object, specify the filename of the mesh, and the position and - # orientation relative to the robot's base frame. + - workspace_wall_front + - workspace_wall_left + - workspace_wall_top + # For each object, specify: + # - Shape: EITHER `filepath` (for a mesh) OR `primitive_type` and + # `primitive_dims`(for a primitive -- see shape_msgs/SolidPrimitive.msg) + # - Pose: `position` and `quat_xyzw` (see geometry_msgs/Pose.msg) + # - Frame ID: `frame_id` (the frame_id of the object that the pose is + # relative to) wheelchair: # the wheelchair mesh filename: wheelchair.stl position: [0.02, -0.02, -0.05] quat_xyzw: [0.0, 0.0, 0.0, 1.0] - frame_id: root # the frame_id of the wheelchair meshthat the pose is relative to + frame_id: root # the frame_id that the pose is relative to # an expanded mesh around the wheelchair to account for a user sitting in it wheelchair_collision: filename: wheelchair_collision.stl position: [0.02, -0.02, -0.05] # should match the wheelchair position quat_xyzw: [0.0, 0.0, 0.0, 1.0] # should match the wheelchair orientation - frame_id: root # the frame_id of the wheelchair meshthat the pose is relative to + frame_id: root # the frame_id that the pose is relative to table: # the table mesh filename: table.stl position: [0.08, -0.5, -0.45] quat_xyzw: [0.0, 0.0, 0.0, 1.0] - frame_id: root # the frame_id of the wheelchair meshthat the pose is relative to + frame_id: root # the frame_id that the pose is relative to head: # the head mesh filename: tom.stl # This is an initial guess of head position; it will be updated as the # robot perceives the face. position: [0.29, 0.35, 0.85] quat_xyzw: [-0.0616284, -0.0616284, -0.704416, 0.704416] - frame_id: root # the frame_id of the wheelchair meshthat the pose is relative to \ No newline at end of file + frame_id: root # the frame_id that the pose is relative to + workspace_wall_front: + primitive_type: 1 # Box=1. See shape_msgs/SolidPrimitive.msg + primitive_dims: [1.59, 0.01, 0.8] # Box has 3 dims: [x, y, z] + position: [-0.05, -0.58, 0.65] + quat_xyzw: [0.0, 0.0, 0.0, 1.0] + frame_id: root # the frame_id that the pose is relative to + workspace_wall_left: + primitive_type: 1 # Box=1. See shape_msgs/SolidPrimitive.msg + primitive_dims: [0.01, 1.5, 1.60] # Box has 3 dims: [x, y, z] + position: [0.75, 0.17, 0.25] + quat_xyzw: [0.0, 0.0, 0.0, 1.0] + frame_id: root # the frame_id that the pose is relative to + workspace_wall_top: + primitive_type: 1 # Box=1. See shape_msgs/SolidPrimitive.msg + primitive_dims: [1.59, 1.5, 0.01] # Box has 3 dims: [x, y, z] + position: [-0.05, 0.17, 1.05] + quat_xyzw: [0.0, 0.0, 0.0, 1.0] + frame_id: root # the frame_id that the pose is relative to \ No newline at end of file diff --git a/ada_feeding/scripts/ada_planning_scene.py b/ada_feeding/scripts/ada_planning_scene.py index e0055827..71839312 100755 --- a/ada_feeding/scripts/ada_planning_scene.py +++ b/ada_feeding/scripts/ada_planning_scene.py @@ -23,7 +23,15 @@ from rclpy.node import Node CollisionMeshParams = namedtuple( - "CollisionMeshParams", ["filepath", "position", "quat_xyzw", "frame_id"] + "CollisionMeshParams", + [ + "filepath", + "primitive_type", + "primitive_dims", + "position", + "quat_xyzw", + "frame_id", + ], ) @@ -108,15 +116,47 @@ def load_parameters(self) -> None: for object_id in object_ids.value: filename = self.declare_parameter( f"{object_id}.filename", + None, descriptor=ParameterDescriptor( name="filename", type=ParameterType.PARAMETER_STRING, - description=f"The filename of the mesh for the '{object_id}' object.", + description=( + f"The filename of the mesh for the '{object_id}' object. " + "Either this or `primitive_type` and `primitive_dims` must " + "be specified." + ), + read_only=True, + ), + ) + primitive_type = self.declare_parameter( + f"{object_id}.primitive_type", + None, + descriptor=ParameterDescriptor( + name="primitive_type", + type=ParameterType.PARAMETER_INTEGER, + description=( + f"The primitive type of the '{object_id}' object. " + "Either this and `primitive_dims` must be defined, or `filename`." + ), + read_only=True, + ), + ) + primitive_dims = self.declare_parameter( + f"{object_id}.primitive_dims", + None, + descriptor=ParameterDescriptor( + name="primitive_dims", + type=ParameterType.PARAMETER_DOUBLE_ARRAY, + description=( + f"The dimensions of the '{object_id}' object. " + "Either this and `primitive_type` must be defined, or `filename`." + ), read_only=True, ), ) position = self.declare_parameter( f"{object_id}.position", + None, descriptor=ParameterDescriptor( name="position", type=ParameterType.PARAMETER_DOUBLE_ARRAY, @@ -126,6 +166,7 @@ def load_parameters(self) -> None: ) quat_xyzw = self.declare_parameter( f"{object_id}.quat_xyzw", + None, descriptor=ParameterDescriptor( name="quat_xyzw", type=ParameterType.PARAMETER_DOUBLE_ARRAY, @@ -138,6 +179,7 @@ def load_parameters(self) -> None: ) frame_id = self.declare_parameter( f"{object_id}.frame_id", + None, descriptor=ParameterDescriptor( name="frame_id", type=ParameterType.PARAMETER_STRING, @@ -147,8 +189,15 @@ def load_parameters(self) -> None: ) # Add the object to the list of objects + filepath = ( + None + if filename.value is None + else path.join(assets_dir.value, filename.value) + ) self.objects[object_id] = CollisionMeshParams( - filepath=path.join(assets_dir.value, filename.value), + filepath=filepath, + primitive_type=primitive_type.value, + primitive_dims=primitive_dims.value, position=position.value, quat_xyzw=quat_xyzw.value, frame_id=frame_id.value, @@ -176,13 +225,23 @@ def initialize_planning_scene(self) -> None: """ # Add each object to the planning scene for object_id, params in self.objects.items(): - self.moveit2.add_collision_mesh( - id=object_id, - filepath=params.filepath, - position=params.position, - quat_xyzw=params.quat_xyzw, - frame_id=params.frame_id, - ) + if params.primitive_type is None: + self.moveit2.add_collision_mesh( + id=object_id, + filepath=params.filepath, + position=params.position, + quat_xyzw=params.quat_xyzw, + frame_id=params.frame_id, + ) + else: + self.moveit2.add_collision_primitive( + id=object_id, + prim_type=params.primitive_type, + dims=params.primitive_dims, + position=params.position, + quat_xyzw=params.quat_xyzw, + frame_id=params.frame_id, + ) def main(args: List = None) -> None: