Using all statuses

Signed-off-by: Aaron Chong <[email protected]>
open-rmf · Nov 15, 2024 · fdff8bc · fdff8bc
1 parent 60d0f5d
commit fdff8bc
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 17 deletions.
diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py
@@ -189,22 +189,32 @@ def _is_navigation_done(self) -> bool:
                     reply.ok.payload.to_bytes()
                 )
                 self.node.get_logger().debug(f'Result: {rep.status}')
-                if rep.status == GoalStatus.STATUS_EXECUTING.value:
-                    return False
-                elif rep.status == GoalStatus.STATUS_SUCCEEDED.value:
-                    self.node.get_logger().info(
-                        f'Navigation goal {self.nav_goal_id} reached'
-                    )
-                    return True
-                else:
-                    # TODO(ac): test replanning behavior if goal status is
-                    # neither executing or succeeded
-                    self.replan_counts += 1
-                    self.node.get_logger().error(
-                        f'Navigation goal {self.nav_goal_id} status '
-                        f'{rep.status}, replan count [{self.replan_counts}]')
-                    self.update_handle.more().replan()
-                    return False
+                match rep.status:
+                    case GoalStatus.STATUS_EXECUTING.value | \
+                         GoalStatus.STATUS_ACCEPTED.value | \
+                         GoalStatus.STATUS_CANCELING.value:
+                        return False
+                    case GoalStatus.STATUS_SUCCEEDED.value:
+                        self.node.get_logger().info(
+                            f'Navigation goal {self.nav_goal_id} reached'
+                        )
+                        return True
+                    case GoalStatus.STATUS_CANCELED.value:
+                        self.node.get_logger().info(
+                            f'Navigation goal {self.nav_goal_id} was cancelled'
+                        )
+                        return True
+                    case _:
+                        # TODO(ac): test replanning behavior if goal status is
+                        # neither executing or succeeded
+                        self.replan_counts += 1
+                        self.node.get_logger().error(
+                            f'Navigation goal {self.nav_goal_id} status '
+                            f'{rep.status}, replan count '
+                            f'[{self.replan_counts}]'
+                        )
+                        self.update_handle.more().replan()
+                        return False
             except Exception as e:
                 self.node.get_logger().debug(
                     f'Received (ERROR: "{reply.err.payload.to_string()}"): '

diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py
@@ -229,7 +229,6 @@ def test_robot_handle_navigate_to_pose(self):
         except RuntimeError:
             able_to_handle_navigate = False
         assert able_to_handle_navigate
-        assert not robot_adapter._is_navigation_done()
         time.sleep(5)
         assert robot_adapter._is_navigation_done()