Multimodal Visual Understanding + SLAM Navigation

1. Course Content

• Basic: Run the example program to perform a comprehensive task using the robot's visual understanding capabilities combined with SLAM navigation.
• Advanced: Master the key source code introduced in this section.

2. Starting the Agent

Note: If it has already been started, there is no need to start it again.

Enter the command on the vehicle terminal:

start_agent

The terminal prints the following information, indicating a successful connection.

[!NOTE]

Note: To experience this course, you need to first build at least one grid map according to the LiDAR chapter.

2.1 Configuring the Map Mapping File

• Command to start navigation on the vehicle's infotainment system

For Raspberry Pi 5 users, if the ROS container is not running, you need to start it first.

xxxxxxxxxx
bringup_m3

Open a terminal inside the container.

xxxxxxxxxx
exec_m3

(ORIN mainboard)

xxxxxxxxxx
ros2 launch m3_bringup nav2.launch.py

(RDK & PI5 mainboard) Step-by-step execution, visualized on the accompanying virtual machine Rosmaster-M3-Ubuntu22.04, start it.

​x
#Virtual machine side
rviz2 -d /home/yahboom/yahboomM3_ws/user_ws/src/m3_bringup/rviz/nav2.rviz
​
#Host side
ros2 launch m3_bringup nav2.launch.py gui:=False

[!TIP]

• The AMCL positioning algorithm is used for navigation by default here, which is suitable for most applicable scenarios. If higher positioning accuracy is required, please refer to the product tutorial [02.Lidar Course]—[13.slam_toolbox localization and navigation] section to build a pose map and use the slam_toolbox pure positioning mode for navigation.

Next, the rviz2 visualization interface will open. Click on 2D Pose Estimate in the top toolbar to enter selection mode. Roughly mark the robot's location and orientation on the map.

The robot's model will be displayed on the map, as shown below:

We can name any precise point on the map. Here, we use "Master Bedroom" and "Kitchen" as examples.

As shown in the image below, we first click on Nav2 The Goal tool guides the vehicle's navigation system to the target location we need to mark.

Wait for the vehicle's navigation system to reach the target location to be marked.

• Run the following command in the terminal to obtain the robot's pose information in the map coordinate system:

xxxxxxxxxx
ros2 run tf2_ros tf2_echo map base_footprint

Open the map_mapping.yaml map mapping file (you can choose any method to open it, such as VNC, vscode, or command line):

• Here's an example of opening the map configuration file via command line:

xxxxxxxxxx
nano ~/yahboomM3_ws/user_ws/src/m3_bringup/config/multi_brains/map_mapping.yaml

• Modify the symbolic pose under the common_map_areas field, where name is the location name, and fill in the previously obtained poses into the position and orientation fields.

xxxxxxxxxx
#根据实际的场景环境，自定义地图中的区域，可以添加任意个区域，注意和大模型的地图映射保持一致即可
#According to the actual scene environment, customize the areas in the map. You can add any number of areas, just make sure they are consistent with the map mapping of the large model
#地图映射Map mapping
​
common_map_areas: #常规导航 common navigation
  A: 
  name: 'Master bedroom'
  position:
    x: 3.974
    y: -2.634
  orientation:
    x: 0.0
    y: 0.0
    z: -0.688
    w: 0.726
  B: 
    name: 'xxx'
    position:
      x: 1.488
      y: 0.661 
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.725
      w: 0.688  

• After configuring all the points that need to be marked, save the file (Ctrl+X) for immediate effect.

2.2 Configuring Map Mapping Variables in Dify

• After configuring the map mapping file above, we also need to ensure that the AI model knows in advance the relationships between locations and symbols in these maps.
• Start the Dify service (if it is already running, there is no need to start it again):

xxxxxxxxxx
bringup_dify

• Enter the vehicle's IP address directly into your browser's address bar to access the Dify management page, then click to select the corresponding AI application.

[!NOTE]

• Domestic users: multi_brains_zh
• International users: multi_brains_en

• Click to select Session Variables in the upper right corner, and then click the edit button for the map_mapping variable.

• In the pop-up Edit Session Variables window, edit the mapping relationships according to the settings in the previous map mapping file, and then click Save.

Finally, remember to click "Publish" -> "Publish Update" to save your changes.

3. Running Examples

3.1 Starting the Program

• Open the terminal on the vehicle's infotainment system and enter the command to start the AI intelligent agent system, or you can use the shortcut: multi_brains to start the AI large model function:

xxxxxxxxxx
ros2 launch m3_bringup llm_agent_control.launch.py

• Command to start navigation on the vehicle's infotainment system

(ORIN & PI5 mainboard)

xxxxxxxxxx
ros2 launch m3_bringup nav2.launch.py

(RDK mainboard) Run in steps, and visualize the process by starting the accompanying virtual machine Rosmaster-M3-Ubuntu22.04:

xxxxxxxxxx
#Virtual Machine
rviz2 -d /home/yahboom/yahboomM3_ws/user_ws/src/m3_bringup/rviz/nav2.rviz
#Host side
ros2 launch m3_bringup nav2.launch.py gui:=False

• Afterwards, follow the navigation activation process to initialize positioning. This will open the rviz2 visualization interface. Click on 2D Pose Estimate in the top toolbar to enter selection mode. Roughly mark the robot's location and orientation on the map. After initial positioning, the preparation is complete.

3.2 Test Cases

These test cases are for reference only. Provide instructions according to your own needs.

• Please remember your current location, then navigate to the kitchen and the master bedroom in that order, remembering the items you see in each room. Finally, return to your starting position and tell me what you saw in those two places.
• Wake up the robot and issue commands. The execution layer model will execute sub-tasks according to the task steps planned by the decision layer model. 1. Navigate to the "master bedroom" and observe the items in the environment. Place a pen in the simulated "master bedroom" and a pack of toilet paper in the simulated "kitchen":

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

4. Source Code Analysis

• Robot motion source code path:

xxxxxxxxxx
~/yahboomM3_ws/code_ws/src/multi_brains/multi_brains/action_service.py

action_service.py program:

Case 1 uses the seewhat, navigation, load_target_points, and get_current_pose methods from the CustomActionServer class. Seewhat has already been explained in the Multimodal Visual Understanding + Robotic Arm Grasping chapter. This lesson explains the newly introduced navigation, load_target_points, and get_current_pose functions.

init_ros_comunication initialization function:

• Creates a nav2 navigation client to request the ros2 navigation action server for subsequent navigation target point requests; creates a TF listener to monitor coordinate transformations between the map and basefootprint.

xxxxxxxxxx
# Create a navigation client and request the navigation action server.
self.navclient=ActionClient(self, NavigateToPose, 'navigate_to_pose')
# Create a TensorFlow listener to monitor coordinate transformations
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)

The load_target_points function:

• Loads the target point coordinates from the map mapping file map_mapping.yaml and creates a navigation dictionary to store the characters and their corresponding map coordinates, where each point coordinate is a PoseStamped type data.

xxxxxxxxxx
    def load_target_points(self):
        """
        加载地图映射文件 /Load map mapping file
        """
        self.navpose_dict = {}
        self.road_net_dict = {}
        with open(self.map_mapping_file, "r") as file:
            full_target_points = yaml.safe_load(file)
            common_target_points = full_target_points.get("common_map_areas", {})
            road_net_target_points = full_target_points.get("road_net_map_areas", {})
​
        for name, data in common_target_points.items():#加载常规导航点 / Load common navigation points
            pose = PoseStamped()
            pose.header.frame_id = "map"
            pose.pose.position.x = data["position"]["x"]
            pose.pose.position.y = data["position"]["y"]
            pose.pose.position.z = data["position"]["z"]
            pose.pose.orientation.x = data["orientation"]["x"]
            pose.pose.orientation.y = data["orientation"]["y"]
            pose.pose.orientation.z = data["orientation"]["z"]
            pose.pose.orientation.w = data["orientation"]["w"]
            self.navpose_dict[name] = pose
​

The __normal_navigation function:

• Receives a character parameter (corresponding to the character in the map mapping above), parses the coordinates of the corresponding character from the dictionary, and uses the self.navclient navigation client object to request the ROS2 navigation action server. When the navigation action server returns a value of 4, it indicates successful navigation; other values indicate failure (possibly due to obstacles, planning failure, etc.). After navigation is complete, the result of the action execution is fed back to the large model.

xxxxxxxxxx
    def __normal_navigation(self, point_name)->None:
        '''常规导航功能 / Normal navigation function '''
        self.navigation_finish_flag = False
        self.goal_handle = None
        self.result = None
        self.res = None
        point_name = point_name.strip("'\"")
        if point_name not in self.navpose_dict:
            self.get_logger().error(f"Target point '{point_name}' does not exist in the navigation dictionary." )
            return None
​
        if self.first_record:
            # 出发前记录当前在全局地图中的坐标(只有在每个任务周期的第一次执行时才会记录)/ before starting a new task, record the current pose in the global map
            transform = self.tf_buffer.lookup_transform(
                "map", "base_footprint", rclpy.time.Time()
            )
            pose = PoseStamped()
            pose.header.frame_id = "map"
            pose.pose.position.x = transform.transform.translation.x
            pose.pose.position.y = transform.transform.translation.y
            pose.pose.position.z = 0.0
            pose.pose.orientation = transform.transform.rotation
            self.navpose_dict["zero"] = pose
            self.road_net_dict["zero"] = pose
            self.first_record = False
​
        # 获取目标点坐标 /get_target_pose
        target_pose = self.navpose_dict.get(point_name)
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = target_pose
        send_goal_future = self.navclient.send_goal_async(goal_msg)
​
        def goal_response_callback(future):
            self.goal_handle = future.result()
            if not self.goal_handle or not self.goal_handle.accepted:
                self.get_logger().error("Goal was rejected!")
                return None
​
            get_result_future = self.goal_handle.get_result_async()
​
            def result_callback(future_result):
                self.result = future_result.result()
                self.navigation_finish_flag = True
                if self.result.status == 4:
                    self.get_logger().info("Navigation finished!")
                    self.res= True
                else:
                    self.get_logger().info(f"Navigation failed with status: {self.result.status}")
                    self.res= False
​
            get_result_future.add_done_callback(result_callback)
        send_goal_future.add_done_callback(goal_response_callback)
​
        while not self.navigation_finish_flag:
            if self.interrupt_event.is_set() :
                self.navclient._cancel_goal(self.goal_handle)
                return None
            time.sleep(0.1)
        self.stop()
        return self.res

The get_current_pose function retrieves the robot's current map coordinates in the global coordinate system and adds these coordinates to a dictionary for later retrieval.

xxxxxxxxxx
    def get_current_pose(self):
        """
        获取当前在全局地图坐标系下的位置 /Get the current position in the global map coordinate system
        """
        transform = self.tf_buffer.lookup_transform("map", "base_footprint", rclpy.time.Time())
        pose = PoseStamped()
        pose.header.frame_id = "map"
        pose.pose.position.x = transform.transform.translation.x
        pose.pose.position.y = transform.transform.translation.y
        pose.pose.position.z = 0.0
        pose.pose.orientation = transform.transform.rotation
        self.navpose_dict["zero"] = pose
        position = pose.pose.position
        orientation = pose.pose.orientation
        self.get_logger().info(f"Recorded Pose: Position: x={position.x}, y={position.y},z={position.z},Orientation: x={orientation.x}, y={orientation.y}, z={orientation.z}, w={orientation.w}")
        return True