Remove update handle check, and just check individually

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

free_fleet_adapter/free_fleet_adapter/nav1_robot_adapter.py

@@ -430,16 +430,13 @@ def _is_navigation_done(self) -> bool:
                 self.update_handle.more().replan()
                 return False
 
-    def _check_update_handle_initialization(self):
+    def update(self, state: rmf_easy.RobotState):
         if self.update_handle is None:
             error_message = \
                 f'Failed to update robot {self.name}, robot adapter has ' \
                 'not yet been initialized with a fleet update handle.'
             self.node.get_logger().error(error_message)
-            raise RuntimeError(error_message)
-
-    def update(self, state: rmf_easy.RobotState):
-        self._check_update_handle_initialization()
+            return
 
         activity_identifier = None
         if self.execution:
@@ -472,7 +469,12 @@ def _handle_navigate_to_pose(
                 f'[{self.replan_counts}]'
             )
 
-            self._check_update_handle_initialization()
+            if self.update_handle is None:
+                error_message = \
+                    f'Failed to replan for robot {self.name}, robot adapter ' \
+                    'has not yet been initialized with a fleet update handle.'
+                self.node.get_logger().error(error_message)
+                return
             self.update_handle.more().replan()
             return
 
@@ -496,7 +498,12 @@ def _handle_navigate_to_pose(
             f'Navigation goal {nav_goal_id} was rejected, replan '
             f'count [{self.replan_counts}]'
         )
-        self._check_update_handle_initialization()
+        if self.update_handle is None:
+            error_message = \
+                f'Failed to replan for robot {self.name}, robot adapter has ' \
+                'not yet been initialized with a fleet update handle.'
+            self.node.get_logger().error(error_message)
+            return
         self.update_handle.more().replan()
         self.nav_goal_id = None
 

free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py

@@ -220,16 +220,13 @@ def _is_navigation_done(self) -> bool:
                     f'{type(e)}: {e}')
                 continue
 
-    def _check_update_handle_initialization(self):
+    def update(self, state: rmf_easy.RobotState):
         if self.update_handle is None:
             error_message = \
                 f'Failed to update robot {self.name}, robot adapter has not ' \
                 'yet been initialized with a fleet update handle.'
             self.node.get_logger().error(error_message)
-            raise RuntimeError(error_message)
-
-    def update(self, state: rmf_easy.RobotState):
-        self._check_update_handle_initialization()
+            return
 
         activity_identifier = None
         if self.execution:
@@ -266,7 +263,12 @@ def _handle_navigate_to_pose(
                 f'[{self.replan_counts}]'
             )
 
-            self._check_update_handle_initialization()
+            if self.update_handle is None:
+                error_message = \
+                    f'Failed to replan for robot {self.name}, robot adapter ' \
+                    'has not yet been initialized with a fleet update handle.'
+                self.node.get_logger().error(error_message)
+                return
             self.update_handle.more().replan()
             return
 
@@ -314,7 +316,13 @@ def _handle_navigate_to_pose(
                     f'Navigation goal {nav_goal_id} was rejected, replan '
                     f'count [{self.replan_counts}]'
                 )
-                self._check_update_handle_initialization()
+                if self.update_handle is None:
+                    error_message = \
+                        f'Failed to replan for robot {self.name}, robot ' \
+                        'adapter has not yet been initialized with a fleet ' \
+                        'update handle.'
+                    self.node.get_logger().error(error_message)
+                    return
                 self.update_handle.more().replan()
                 self.nav_goal_id = None
                 return