Added workspace walls to the planning scene (#82)

Added 3 workspace walls to the planning scene

Author: amalnanavati

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
+      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

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: