Made the frame ID be root instead of the robot base so the entire col…

…lision scene doesn't rotate with the robot

ada_feeding/config/ada_planning_scene.yaml

@@ -12,18 +12,22 @@ ada_planning_scene:
       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
     # 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
     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
     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]
+      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

ada_feeding/scripts/ada_planning_scene.py

@@ -23,7 +23,7 @@
 from rclpy.node import Node
 
 CollisionMeshParams = namedtuple(
-    "CollisionMeshParams", ["filepath", "position", "quat_xyzw"]
+    "CollisionMeshParams", ["filepath", "position", "quat_xyzw", "frame_id"]
 )
 
 
@@ -136,12 +136,22 @@ def load_parameters(self) -> None:
                     read_only=True,
                 ),
             )
+            frame_id = self.declare_parameter(
+                f"{object_id}.frame_id",
+                descriptor=ParameterDescriptor(
+                    name="frame_id",
+                    type=ParameterType.PARAMETER_STRING,
+                    description=("The frame ID that the pose is in."),
+                    read_only=True,
+                ),
+            )
 
             # Add the object to the list of objects
             self.objects[object_id] = CollisionMeshParams(
                 filepath=path.join(assets_dir.value, filename.value),
                 position=position.value,
                 quat_xyzw=quat_xyzw.value,
+                frame_id=frame_id.value,
             )
 
     def wait_for_moveit(self) -> None:
@@ -171,6 +181,7 @@ def initialize_planning_scene(self) -> None:
                 filepath=params.filepath,
                 position=params.position,
                 quat_xyzw=params.quat_xyzw,
+                frame_id=params.frame_id,
             )