Address comments, fix battery_state and navigate_to_pose result parsi…

…ng, add additional check as temporary fix for race condition

Signed-off-by: Aaron Chong <[email protected]>

README.md

@@ -32,9 +32,6 @@ We recommend setting up `zenoh-bridge-ros2dds` with the released standalone bina
 
 Most of the tests have been performed using `rmw-cyclonedds-cpp`, while other RMW implementations have shown varying results. Support and testing with other RMW implementations will be set up down the road.
 
-> [!WARNING]
-> This has so far only been tested in simulation, and will undergo updates and changes as more testing is performed. Use at your own risk. For the legacy implementation, check out the [`legacy`](https://github.com/open-rmf/free_fleet/tree/legacy) branch.
-
 ## Dependency installation, source build and setup
 
 System dependencies,
@@ -294,7 +291,6 @@ ros2 run rmf_demos_tasks dispatch_patrol \
 
 ## TODOs
 
-* hardware testing
 * attempt to optimize tf messages (not all are needed)
 * ROS 1 nav support
 * custom actions to be abstracted

free_fleet/free_fleet/types.py

@@ -122,9 +122,6 @@ class NavigateToPose_GetResult_Response(
     typename='NavigateToPose_GetResult_Response'
 ):
     status: pycdr2.types.int8
-    NONE: pycdr2.types.uint16
-    error_code: pycdr2.types.uint16
-    error_msg: str
 
 
 # https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/action/NavigateToPose.action
@@ -205,6 +202,7 @@ class ActionMsgs_CancelGoal_Response(
 # https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg
 @dataclass
 class SensorMsgs_BatteryState(IdlStruct, typename='SensorMsgs_BatteryState'):
+    header: Header
     voltage: pycdr2.types.float32
     temperature: pycdr2.types.float32
     current: pycdr2.types.float32

free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py

@@ -237,8 +237,12 @@ def update(self, state: rmf_easy.RobotState):
             # TODO(ac): use an enum to record what type of execution it is,
             # whether navigation or custom executions
             if self.nav_goal_id is not None and self._is_navigation_done():
-                self.execution.finished()
-                self.execution = None
+                # TODO(ac): Refactor this check as as self._is_navigation_done
+                # takes a while and the execution may have become None due to
+                # task cancellation.
+                if self.execution is not None:
+                    self.execution.finished()
+                    self.execution = None
                 self.nav_goal_id = None
                 self.replan_counts = 0
             else: