Fix/mid level planner (#36)

* removed occupancy frame

* fixed identity planner

* deleted comments

---------

Co-authored-by: Aryan-Naveen <[email protected]>
Co-authored-by: Robot Account <[email protected]>

Author: 3 people

robot_executor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

@@ -504,9 +504,9 @@ def plan_path(self, high_level_path_metric):
         Input: high level path in <robot>/odom frame, Nx2 numpy array
         Output: (bool, path) -> (success, path in odom frame)
         """
-        output = {
-            "target_point_metric": None,
-            "path_shapely": shapely.LineString(high_level_path_metric[:, :2]),
-            "path_waypoints_metric": None,
-        }
+        output = MidLevelPlannerOutput(
+            target_point_metric=None,
+            path_shapely=shapely.LineString(high_level_path_metric[:, :2]),
+            path_waypoints_metric=None,
+        )
         return True, output

spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py

@@ -252,10 +252,6 @@ def __init__(self):
         bdai_password = self.get_parameter("bosdyn_client_password").value
         assert bdai_password != ""
 
-        self.declare_parameter("occupancy_frame", "")
-        self.occupancy_frame = self.get_parameter("occupancy_frame").value
-        self.get_logger().info(f"{self.occupancy_frame=}")
-
         # Follow Skill
         self.declare_parameter("follower_lookahead", 0.0)
         follower_lookahead = self.get_parameter("follower_lookahead").value