Multimodal Visual Understanding + SLAM Navigation

1. Course Content

• Basic: Run the example program to perform a comprehensive task by combining the robot's visual understanding capability with SLAM navigation.

• Advanced: Master the key source code newly introduced in this lesson.

2. Preparation

• The navigation function requires building a grid map, a pose map, and an annotated road network file first. For details, refer to the tutorial 【LiDAR Course — Slam-Toolbox Mapping】 to complete the construction of the grid map and the pose map.

xxxxxxxxxx
ros2 launch yahboomcar_bringup dify_llm_agents.launch.py use_nav:=true use_camera:=true use_tts:=true

• If you need to view the camera image, select the camera topic from the Topic drop-down in the robot_view panel in the sidebar.

• Voice recognition node

xxxxxxxxxx
ros2 run multi_brains asr_detect

3. Recording Semantic Locations

• Move the car to the semantic location to be recorded (by any method such as keyboard, gamepad, Nav2 goal, or carrying).

• Choose either the RobotManager interface operation or the CLI operation below.

• Method 1: Fill in the symbol and semantic location, and click record location.

• A terminal will pop up in the VNC screen to execute the record location command; press Enter to close it when done.

• Method 2: Use the robot_control CLI tool to record the position of the car's base_link in the map (Raspberry Pi 5 users need to use the CLI command in the container terminal).

xxxxxxxxxx
robot_control call-tool RecordMapLocation --name ShippingArea --symbol A

• --name specifies the name of the record point; --symbol specifies the symbol of the record point for easy recognition and distinction.

• If the recording succeeds, the content shown in the figure above is displayed. If a symbol with the same name already exists in the map mapping file, the previous symbol content will be automatically overwritten.

4. Case Demonstration

The cases are for reference only. Just give commands according to your own needs. The virtual kitchen and master bedroom here can be recorded at any location on the map.

• Navigate to the kitchen and the master bedroom, remember the items you see, and finally return to the starting position and tell me what you saw in those two places.

• Wake up the robot and issue the command. The execution-layer large model will execute subtasks according to the task steps planned by the decision-layer model. For example, first place a pen in the simulated "master bedroom" and a pack of toilet paper in the simulated "kitchen", then issue the command: "Navigate to the 'master bedroom' and observe the items in the environment."

• The robot executes the following steps according to the output flow of the decision layer:

5. Source Code Analysis

• The path of the robot's action source code:

xxxxxxxxxx
~/yahboomcar_ros2_ws/yahboomcar_ws/src/multi_brains/multi_brains/action_service.py

navigation function code description:

• @llm_action: A decorator that registers the method as an action that can be called by the large model.

• navigation(self, point_name): Receives a location name (which can be a symbol or a semantic name) and returns [bool, str] indicating whether the navigation succeeded and the information.

• point_name = str(point_name).strip("'\""): Converts the location name passed in by the large model into a string and removes the quotation marks that may be carried at both ends.

• yaml.safe_load(file): Reads the map mapping YAML file and parses it into a dictionary, where the key is the symbol and the value is data containing the name, coordinates, and pose.

• available_points: Collects all recorded location names in the map, used to prompt the list of available locations when the target does not exist.

• symbol == point_name or data.get('name') == point_name: Supports matching the target location by symbol or by semantic name at the same time.

• PoseStamped(): Constructs a ROS2 pose message and sets frame_id="map" to indicate that the coordinate reference frame is the map coordinate frame.

• position.get('x/y/z') and orientation.get('x/y/z/w'): Reads the position and quaternion orientation from the YAML data; undefined fields use default values (w defaults to 1.0).

• navclient.wait_for_server(timeout_sec=10.0): Waits for the Nav2 NavigateToPose Action server to be ready; if it times out after 10 seconds, returns failure.

• NavigateToPose.Goal(): Constructs the navigation goal message and assigns the parsed target_pose to goal_msg.pose.

• send_goal_async(goal_msg): Asynchronously sends the navigation goal and loops waiting for the server response, checking for a wake-up interrupt every 0.2 seconds.

• goal_handle.accepted: Checks whether the navigation goal was accepted by Nav2; if rejected, returns an error.

• nav_timeout = 240.0: The navigation timeout is 240 seconds; after timeout, the navigation goal is automatically canceled and failure is returned.

• goal_handle.cancel_goal_async(): Asynchronously cancels the currently executing navigation goal when the navigation times out or is interrupted.

• self.stop(): Immediately stops the chassis motion on interruption to prevent inertial sliding.

• _precise_adjust(target_pose.pose, point_name): Calls the precise adjustment function after successful navigation to fine-tune the chassis pose to improve the final localization accuracy.

    @llm_action
    def navigation(self, point_name):
        '''Get the target pose from map_mapping_file and navigate to it'''
        point_name = str(point_name).strip("'\"")
        target_pose = None
        available_points = []
        try:
            with open(self.map_mapping_file, 'r', encoding='utf-8') as file:
                map_data = yaml.safe_load(file) or {}
            for symbol, data in map_data.items():
                if not isinstance(data, dict):
                    continue
                if 'name' in data:
                    available_points.append(f"{data['name']}({symbol})")
                if symbol == point_name or data.get('name') == point_name:
                    target_pose = PoseStamped()
                    target_pose.header.frame_id = "map"
                    target_pose.header.stamp = self.get_clock().now().to_msg()
                    position = data.get('position', {})
                    target_pose.pose.position.x = float(position.get('x', 0.0))
                    target_pose.pose.position.y = float(position.get('y', 0.0))
                    target_pose.pose.position.z = float(position.get('z', 0.0))
                    orientation = data.get('orientation', {})
                    target_pose.pose.orientation.x = float(orientation.get('x', 0.0))
                    target_pose.pose.orientation.y = float(orientation.get('y', 0.0))
                    target_pose.pose.orientation.z = float(orientation.get('z', 0.0))
                    target_pose.pose.orientation.w = float(orientation.get('w', 1.0))
        except Exception as e:
            self.get_logger().error(f"Failed to load map mapping file: {str(e)}")
            return [False, f"navigation({point_name}) failed: failed to load map mapping file: {str(e)}"]

        # self.navpose_dict
        if target_pose is None:
            points_str = ", ".join(available_points) if available_points else "none"
            msg = (f"target point '{point_name}' does not exist in the map mapping file, "
                   f"available points: {points_str}")
            self.get_logger().error(msg)
            return [False, f"navigation({point_name}) failed: {msg}"]

        if not self.navclient.wait_for_server(timeout_sec=10.0):
            self.get_logger().error("Nav2 action server not available")
            return [False, f"navigation({point_name}) failed: Nav2 action server not available"]

        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = target_pose
        send_goal_future = self.navclient.send_goal_async(goal_msg)
        while rclpy.ok() and not send_goal_future.done():
            if self.interrupt_event.is_set():
                return [False, f"navigation({point_name}) interrupted by wakeup"]
            time.sleep(0.2)

        if not send_goal_future.done():
            self.nav_goal_handle = None
            self.get_logger().error(f"navigation({point_name}) aborted: rclpy shutdown")
            return [False, f"navigation({point_name}) aborted: rclpy shutdown"]
        goal_handle = send_goal_future.result()
        if not goal_handle or not goal_handle.accepted:
            self.get_logger().error("Navigation goal was rejected!")
            return [False, f"navigation({point_name}) failed: navigation goal was rejected"]

        self.nav_goal_handle = goal_handle
        result_future = goal_handle.get_result_async()

        nav_timeout = 240.0
        deadline = time.time() + nav_timeout
        while rclpy.ok() and not result_future.done() and time.time() < deadline:
            if self.interrupt_event.is_set():
                self.nav_goal_handle = None
                try:
                    goal_handle.cancel_goal_async()
                except Exception:
                    pass
                self.stop()
                return [False, f"navigation({point_name}) interrupted by wakeup"]
            time.sleep(0.2)

        self.nav_goal_handle = None
        # P1
        if not result_future.done():
            self.get_logger().error(
                f"navigation({point_name}) timed out after {nav_timeout:.0f}s without result")
            try:
                goal_handle.cancel_goal_async()
            except Exception:
                pass
            return [False, f"navigation({point_name}) failed: timed out after {nav_timeout:.0f}s"]
        try:
            result = result_future.result()
        except Exception as e:
            self.get_logger().error(f"navigation({point_name}) failed to get result: {e}")
            return [False, f"navigation({point_name}) failed: {e}"]

        if result.status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info(f"navigation({point_name}) succeeded, start precise adjustment")
            time.sleep(1.0)
            if self.interrupt_event.is_set():
                return [False, f"navigation({point_name}) interrupted by wakeup"]
            adjust_result = self._precise_adjust(target_pose.pose, point_name)
            if not adjust_result[0]:
                self.get_logger().warn(Fore.YELLOW+f"navigation({point_name}) arrived but precise adjustment failed: {adjust_result[1]}"+Fore.RESET)
            return [True, f"navigation({point_name}) succeeded"]
        self.get_logger().error(f"navigation({point_name}) failed with status: {result.status}")
        return [False, f"navigation({point_name}) failed with status {result.status}"]