Addresses review comments

.github/docker/minimal-nav1-bringup/Dockerfile

@@ -4,7 +4,7 @@ FROM docker.io/ros:$ROS_DISTRO-ros-base
 RUN apt update && apt install -y curl ros-$ROS_DISTRO-turtlebot3-navigation ros-$ROS_DISTRO-dwa-local-planner
 
 RUN mkdir -p /tb3 && cd /tb3 \
-  && curl -sL https://github.com/ros-navigation/navigation2/archive/refs/heads/master.tar.gz -o navigation2.tar.gz \
+  && curl -sL https://github.com/ros-navigation/navigation2/archive/refs/tags/1.3.4.tar.gz -o navigation2.tar.gz \
   && mkdir -p /tb3/navigation2 && tar zxf navigation2.tar.gz -C /tb3/navigation2 --strip-components=1 && rm navigation2.tar.gz
 
 ENV TURTLEBOT3_MODEL=burger

free_fleet/free_fleet/ros2_types.py

@@ -23,36 +23,36 @@
 
 # https://github.com/ros2/rcl_interfaces/blob/rolling/builtin_interfaces/msg/Time.msg
 @dataclass
-class Time(IdlStruct, typename='Time'):
+class Time(IdlStruct):
     sec: pycdr2.types.int32
     nanosec: pycdr2.types.uint32
 
 
 # https://github.com/ros2/rcl_interfaces/blob/rolling/builtin_interfaces/msg/Duration.msg
 @dataclass
-class Duration(IdlStruct, typename='Duration'):
+class Duration(IdlStruct):
     sec: pycdr2.types.int32
     nanosec: pycdr2.types.uint32
 
 
 # https://github.com/ros2/common_interfaces/blob/rolling/std_msgs/msg/Header.msg
 @dataclass
-class Header(IdlStruct, typename='Header'):
+class Header(IdlStruct):
     stamp: Time
     frame_id: str
 
 
 # https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/Point.msg
 @dataclass
-class GeometryMsgs_Point(IdlStruct, typename='GeometryMsgs_Point'):
+class GeometryMsgs_Point(IdlStruct):
     x: pycdr2.types.float64
     y: pycdr2.types.float64
     z: pycdr2.types.float64
 
 
 # https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/Quaternion.msg
 @dataclass
-class GeometryMsgs_Quaternion(IdlStruct, typename='GeometryMsgs_Quaternion'):
+class GeometryMsgs_Quaternion(IdlStruct):
     x: pycdr2.types.float64 = 0
     y: pycdr2.types.float64 = 0
     z: pycdr2.types.float64 = 0
@@ -61,45 +61,36 @@ class GeometryMsgs_Quaternion(IdlStruct, typename='GeometryMsgs_Quaternion'):
 
 # https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/Pose.msg
 @dataclass
-class GeometryMsgs_Pose(IdlStruct, typename='GeometryMsgs_Pose'):
+class GeometryMsgs_Pose(IdlStruct):
     position: GeometryMsgs_Point
     orientation: GeometryMsgs_Quaternion
 
 
 # https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/PoseStamped.msg
 @dataclass
-class GeometryMsgs_PoseStamped(IdlStruct, typename='GeometryMsgs_PoseStamped'):
+class GeometryMsgs_PoseStamped(IdlStruct):
     header: Header
     pose: GeometryMsgs_Pose
 
 
 # https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/action/NavigateToPose.action
 @dataclass
-class NavigateToPose_SendGoal_Request(
-    IdlStruct,
-    typename='NavigateToPose_SendGoal_Request'
-):
+class NavigateToPose_SendGoal_Request(IdlStruct):
     goal_id: pycdr2.types.array[pycdr2.types.uint8, 16]
     pose: GeometryMsgs_PoseStamped
     behavior_tree: str
 
 
 # https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/action/NavigateToPose.action
 @dataclass
-class NavigateToPose_SendGoal_Response(
-    IdlStruct,
-    typename='NavigateToPose_SendGoal_Response'
-):
+class NavigateToPose_SendGoal_Response(IdlStruct):
     accepted: bool
     stamp: Time
 
 
 # https://design.ros2.org/articles/actions.html#get-result-service
 @dataclass
-class NavigateToPose_GetResult_Request(
-    IdlStruct,
-    typename='NavigateToPose_GetResult_Request'
-):
+class NavigateToPose_GetResult_Request(IdlStruct):
     goal_id: pycdr2.types.array[pycdr2.types.uint8, 16]
 
 
@@ -117,16 +108,13 @@ class GoalStatus(Enum):
 # https://design.ros2.org/articles/actions.html#get-result-service
 # https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/action/NavigateToPose.action
 @dataclass
-class NavigateToPose_GetResult_Response(
-    IdlStruct,
-    typename='NavigateToPose_GetResult_Response'
-):
+class NavigateToPose_GetResult_Response(IdlStruct):
     status: pycdr2.types.int8
 
 
 # https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/action/NavigateToPose.action
 @dataclass
-class NavigateToPose_Feedback(IdlStruct, typename='NavigateToPose_Feedback'):
+class NavigateToPose_Feedback(IdlStruct):
     goal_id: pycdr2.types.array[pycdr2.types.uint8, 16]
     current_pose: GeometryMsgs_PoseStamped
     navigation_time: Duration
@@ -137,71 +125,62 @@ class NavigateToPose_Feedback(IdlStruct, typename='NavigateToPose_Feedback'):
 
 # https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/Vector3.msg
 @dataclass
-class GeometryMsgs_Vector3(IdlStruct, typename='GeometryMsgs_Vector3'):
+class GeometryMsgs_Vector3(IdlStruct):
     x: pycdr2.types.float64
     y: pycdr2.types.float64
     z: pycdr2.types.float64
 
 
 # https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/Transform.msg
 @dataclass
-class GeometryMsgs_Transform(IdlStruct, typename='GeometryMsgs_Transform'):
+class GeometryMsgs_Transform(IdlStruct):
     translation: GeometryMsgs_Vector3
     rotation: GeometryMsgs_Quaternion
 
 
 # https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/TransformStamped.msg
 @dataclass
-class GeometryMsgs_TransformStamped(
-    IdlStruct,
-    typename='GeometryMsgs_TransformStamped'
-):
+class GeometryMsgs_TransformStamped(IdlStruct):
     header: Header
     child_frame_id: str
     transform: GeometryMsgs_Transform
 
 
 # https://github.com/ros2/geometry2/blob/rolling/tf2_msgs/msg/TFMessage.msg
 @dataclass
-class TFMessage(IdlStruct, typename='TFMessage'):
+class TFMessage(IdlStruct):
     transforms: pycdr2.types.sequence[GeometryMsgs_TransformStamped]
 
 
 # https://github.com/ros2/unique_identifier_msgs/blob/rolling/msg/UUID.msg
 @dataclass
-class UUID(IdlStruct, typename='UUID'):
+class UUID(IdlStruct):
     uuid: pycdr2.types.array[pycdr2.types.uint8, 16]
 
 
 # https://github.com/ros2/rcl_interfaces/blob/rolling/action_msgs/msg/GoalInfo.msg
 @dataclass
-class ActionMsgs_GoalInfo(IdlStruct, typename='ActionMsgs_GoalInfo'):
+class ActionMsgs_GoalInfo(IdlStruct):
     goal_id: UUID
     stamp: Time
 
 
 # https://github.com/ros2/rcl_interfaces/blob/rolling/action_msgs/srv/CancelGoal.srv
 @dataclass
-class ActionMsgs_CancelGoal_Request(
-    IdlStruct,
-    typename='ActionMsgs_CancelGoal_Request'
-):
+class ActionMsgs_CancelGoal_Request(IdlStruct):
     goal_info: ActionMsgs_GoalInfo
 
 
 # https://github.com/ros2/rcl_interfaces/blob/rolling/action_msgs/srv/CancelGoal.srv
 @dataclass
-class ActionMsgs_CancelGoal_Response(
-    IdlStruct,
-    typename='ActionMsgs_CancelGoal_Response'
-):
+class ActionMsgs_CancelGoal_Response(IdlStruct):
     return_code: pycdr2.types.int8
     goals_canceling: pycdr2.types.sequence[ActionMsgs_GoalInfo]
 
 
 # https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/BatteryState.msg
 @dataclass
-class SensorMsgs_BatteryState(IdlStruct, typename='SensorMsgs_BatteryState'):
+class SensorMsgs_BatteryState(IdlStruct):
     header: Header
     voltage: pycdr2.types.float32
     temperature: pycdr2.types.float32

free_fleet/free_fleet/utils.py

@@ -29,8 +29,7 @@ def namespacify(base_name: str, namespace: str, delimiter: str = '/') -> str:
     If no namespace is provided, returns the base_name. Otherwise, naively
     prefixes the base_name with namespace and delimiter, returning the result.
     """
-    return f'{namespace}{delimiter}{base_name}' if len(namespace) != 0 \
-        else base_name
+    return base_name if not namespace else f'{namespace}{delimiter}{base_name}'
 
 
 def make_nav2_cancel_all_goals_request() -> ActionMsgs_CancelGoal_Request:

free_fleet_adapter/free_fleet_adapter/nav1_robot_adapter.py

@@ -41,7 +41,7 @@
 from geometry_msgs.msg import TransformStamped
 import rclpy
 import rmf_adapter.easy_full_control as rmf_easy
-from rmf_adapter.robot_update_handle import ActivityIdentifier
+from rmf_adapter.robot_update_handle import ActivityIdentifier, Tier
 from tf_transformations import euler_from_quaternion, quaternion_from_euler
 
 import zenoh
@@ -220,8 +220,7 @@ def navigate_to_pose(
         self.move_base_simple_goal_pub.put(serialized_msg.tobytes())
         self.node.get_logger().info(
             'Sending move_base_simple/goal, over zenoh topic '
-            f'[{self.move_base_simple_goal_zenoh_topic}], '
-            f'msg: [{pose_stamped}]'
+            f'[{self.move_base_simple_goal_zenoh_topic}]'
         )
 
         self._tmp_active_goal_id = None
@@ -275,7 +274,7 @@ def stop_current_navigation(self):
         self.node.get_logger().info(
             'Sending move_base/cancel, over zenoh topic '
             f'[{self.move_base_cancel_goal_zenoh_topic}], '
-            f'msg: [{goal_id}]'
+            f'goal ID: [{goal_id}]'
         )
 
     def cancel_navigation(self, goal_id_str: str):
@@ -290,7 +289,7 @@ def cancel_navigation(self, goal_id_str: str):
         self.node.get_logger().info(
             'Sending move_base/cancel, over zenoh topic '
             f'[{self.move_base_cancel_goal_zenoh_topic}], '
-            f'msg: [{goal_id}]'
+            f'goal ID: [{goal_id}]'
         )
 
 
@@ -320,6 +319,7 @@ def __init__(
         self.battery_soc = 1.0
 
         self.replan_counts = 0
+        self.nav_issue_ticket = None
 
         self.tf_handler = Nav1TfHandler(
             self.name,
@@ -412,13 +412,26 @@ def _is_navigation_done(self) -> bool:
                 self.node.get_logger().info(
                     f'Navigation goal {nav_goal_id} reached'
                 )
+                if self.nav_issue_ticket is not None:
+                    msg = {}
+                    self.nav_issue_ticket.resolve(msg)
+                    self.nav_issue_ticket = None
+                    self.node.get_logger().info(
+                        'Navigation issue ticket has been resolved'
+                    )
                 return True
             case goal_status.PREEMPTED:
                 self.node.get_logger().info(
                     f'Navigation goal {nav_goal_id} was cancelled'
                 )
                 return True
             case _:
+                self.nav_issue_ticket = self.create_nav_issue_ticket(
+                    'navigation',
+                    f'Navigate to pose result status [{goal_status.status}]',
+                    self.nav_goal_id
+                )
+
                 # TODO(ac): test replanning behavior if goal status is
                 # neither executing or succeeded
                 self.replan_counts += 1
@@ -430,16 +443,35 @@ def _is_navigation_done(self) -> bool:
                 self.update_handle.more().replan()
                 return False
 
-    def _check_update_handle_initialization(self):
+    # TODO(ac): issue ticket can be more generic for execute actions too
+    def create_nav_issue_ticket(self, category, msg, nav_goal_id=None):
         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.'
+                'Failed to create navigation issue ticket for robot ' \
+                f'{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)
+            return None
+
+        tier = Tier.Error
+        detail = {
+            'nav_goal_id': f'{nav_goal_id}',
+            'message': msg
+        }
+        nav_issue_ticket = \
+            self.update_handle.more().create_issue(tier, category, detail)
+        self.node.get_logger().info(
+            f'Created [{category}] issue ticket for robot [{self.name}] with '
+            f'nav goal ID [{nav_goal_id}]')
+        return nav_issue_ticket
 
     def update(self, state: rmf_easy.RobotState):
-        self._check_update_handle_initialization()
+        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)
+            return
 
         activity_identifier = None
         if self.execution:
@@ -472,7 +504,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 +533,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