TiCare Navigation FSM

This document outlines the high-level logic and state transitions for the nav_manager node. The system follows a “Black Box” integration model using ROS 2 topics for inter-package communication.

State Definitions

1. IDLE

• Description: System is initialized and waiting for a mission.

• Actions: Load the environment map (CAR model) via /map_server/load_map.

• Transition: Move to LOCALIZING upon receiving the "start_nav" message on the /com2nav topic.

2. LOCALIZING

• Description: Checks if a recovery rotation is needed to improve AMCL localization before proceeding.

• Actions: Check the recovery_rotation_active flag.

  • If False: Immediate transition to SAVING_START_POSE.

  • If True: Publish a rotation command to /cmd_vel for a short duration (e.g., 2-3 seconds). Once finished, set recovery_rotation_active to False.

• Transition: Move to SAVING_START_POSE after the optional rotation.

3. SAVING_START_POSE

• Description: Records the starting position for the “Return Home” phase.

• Actions: Send an asynchronous request to the pose_recorder service with the label "start_point". Once confirmed, publish "start_vis" to /nav2vis to activate the camera.

• Transition A (Success): If confirmed, publish "start_vis" to /nav2vis and move to SEARCHING.

• Transition B (Failure): If the service indicates localization is not ready, set recovery_rotation_active = True and return to LOCALIZING.

4. SEARCHING

• Description: Executes the search pattern in the target room.

• Actions: Start the /follow_waypoints action using points from coverage_points.yaml.

• Transition A: If "object_detected" is received from /vis2nav, cancel navigation and move to SAVING_OBJECT_POSE.

• Transition B: If the search timer expires (Timeout), publish "stop_vis"to /nav2visto stop the camera and move to RETURNING_HOME.

5. SAVING_OBJECT_POSE

• Description: Records the exact location of the identified object.

• Actions: Request the pose_recorder service to save the current pose as "object_point". Publish "object_point" to /nav2com to notify the Communication module.

• Transition: Move to RETURNING_HOME.

6. RETURNING_HOME

• Description: Robot returns to the mission starting point.

• Actions: Execute the /navigate_to_pose action toward the stored "start_point". Upon arrival, publish "home" to /nav2com.

• Transition A: If an "object_point" was successfully saved during this mission, move to AWAITING_RETURN.

• Transition B: If no object was found (Search Timeout), reset mission data and return to IDLE.

7. AWAITING_RETURN

• Description: Passive state waiting for user confirmation to guide them to the object.

• Actions: Wait for the "return" message on the /com2nav topic.

• Transition: Upon receiving "return", move to NAV_TO_OBJECT.

8. NAV_TO_OBJECT

• Description: Robot guides the user to the detected object’s location.

• Actions: Execute /navigate_to_pose using the stored "object_point".

• Transition: Once the goal is reached, reset mission context and return to IDLE.

TO-DO / Future Improvements

• [ ] Error/Recovery State: Implement a dedicated state to handle failures in linear flow (e.g., navigation goal unreachable, service timeout, or localization loss).

• [x] Dynamic Re-localization: Add a recovery behavior in the LOCALIZING state if the robot remains static with high covariance for too long.

• [ ] Manual State Override: Implement a subscriber (e.g., /nav_manager/debug_set_state) to manually force transitions between states for testing purposes without needing inputs from Vision or Communication packages.