Welcome to MicroROS Self-balancing Robot Car repository

Multi-car Navigation2 Navigation

Note: The virtual machine needs to be in the same LAN as the car, and the ROS_DOMAIN_ID needs to be the same. You can check [Must-read before use] to set the IP and ROS_DOMAIN_ID on the board.

1. Program function description

After the program is started, you can give the target points of the two cars in rviz. After receiving the command, the two cars will calculate the path based on their own posture and move to their destinations.

xxxxxxxxxx
Note: Before running the program, the car needs to be restarted in a standing position to ensure that all sensors are reset

2. Basic settings for multi-machine functions

Take two cars as an example. It is recommended to use two computers with matching virtual machines, change the config_robot.py files respectively, set robot.set_ros_namespace() to robot1 and robot2 respectively; set robot.set_udp_config() to the IP addresses of the two virtual machines respectively, and the ROS_DOMAIN_ID of the two cars and the ROS_DOMAIN_ID of the virtual machine need to be set to the same. Then open the terminal in the /home/yahboom directory, enter sudo python3 config_Balance_Car.py to run this program (the rest of the programs other than running multiple cars need to be changed back and re-run this program).

3. Start and connect the agent

Take the matching virtual machine as an example. In the two virtual machines, enter the following command to start the agent of each car.

xxxxxxxxxx
sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble udp4 --port 8899 -v4

Then, turn on the switches of the two cars and wait for the two cars to connect to their respective agents. If the connection is successful, the terminal display is as shown in the figure below.

Check the currently started node. In the two virtual machines, randomly select one and open the terminal to enter,

xxxxxxxxxx
ros2 node list

As shown in the figure above, the nodes of both cars have been started. Query the current topic information, enter in the terminal,

xxxxxxxxxx
ros2 topic list

4. Start rviz and load the map program

4.1. Start rviz display

Select one of the two virtual machines at random, open the terminal and enter,

xxxxxxxxxx
ros2 launch yahboomcar_nav multi_nav_display_launch.py

The functions of the symbols in Figures 1-4 above are as follows:

[1]: robot1 calibrates the initial position

[2]: robot1 gives the target point

[3]: robot2 calibrates the initial position

[4]: robot2 gives the target point

4.2. Load the map

Select one of the two virtual machines at random, open the terminal and enter,

xxxxxxxxxx
ros2 launch yahboomcar_multi map_server_launch.py

Note: The map may fail to load here. If the map is not loaded, press ctrl c to close it and rerun the program.

Take the virtual machine as an example. The map loaded here is,

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.yaml

If you need to modify the default loading of other maps, copy the map's yaml file and pgm file to the directory

/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/, and then modify Modify the map_server_launch.py ​​program, which is located in /home/yahboom/yahboomcar_ws/src/yahboomcar_multi/launch, and modify the following places,

Replace the red box with the name of your own map, save and exit, and then enter the following command to compile,

xxxxxxxxxx
cd ~/yahboomcar_ws
colcon build

Then enter the following command to re-source the environment variable,

xxxxxxxxxx
source ~/.bashrc

5. Start the car's underlying data processing program

In the virtual machine terminal that starts robot1, enter,

xxxxxxxxxx
ros2 launch yahboomcar_multi yahboomcar_bringup_multi.launch.xml robot_name:=robot1

In the virtual machine terminal that starts robot2, enter,

xxxxxxxxxx
ros2 launch yahboomcar_multi yahboomcar_bringup_multi.launch.xml robot_name:=robot2

yahboomcar_bringup_multi.launch.xml source code path (take the matching virtual machine as an example),

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_multi/launch/yahboomcar_bringup_multi.launch.xml

xxxxxxxxxx
<launch>
    <arg name="robot_name" default="robot1"/>
    <group>
        <push-ros-namespace namespace="$(var robot_name)"/>
        <!--base_node-->
        <node name="base" pkg="yahboomcar_bringup" exec="cmdvel2bl" output="screen">
            <param name="mode" value="nav"/>
            <remap from="/cmd_vel" to="cmd_vel"/>
            <remap from="/cmd_vel_bl" to="cmd_vel_bl"/>
        </node>
        <!--imu_filter_node-->
        <node name="imu_filter" pkg="imu_filter_madgwick" exec="imu_filter_madgwick_node" output="screen">
            <param name="fixed_frame" value="$(var robot_name)/base_link"/>
            <param name="use_mag" value="false"/>
            <param name="publish_tf" value="false"/>
            <param name="world_frame" value="$(var robot_name)/enu"/>
            <param name="orientation_stddev" value="0.00"/>
            <remap from="imu/data_raw" to="imu"/>
        </node>
        <!--ekf_node-->
        <node name="ekf_filter_node" pkg="robot_localization" exec="ekf_node">
            <param from="$(find-pkg-share yahboomcar_multi)/param/ekf_$(var robot_name).yaml"/>
            <remap from="odometry/filtered" to="odom"/>
            <remap from="/odom_raw" to="odom_raw"/>
        </node>
        <node pkg="tf2_ros" exec="static_transform_publisher" name="base_link_to_base_imu"
          args="0 0.016325 0.080691 0 0 0  $(var robot_name)/base_link $(var robot_name)/imu_frame " />
    </group>
    <include file="$(find-pkg-share yahboomcar_description)/launch/description_multi_$(var robot_name).launch.py"/>
    
</launch>

Here, a pair of tags are used. The command space of all programs within this tag will be robot_name, which is the robot1 or robot2 we defined. Among them, there are also some parameter files or topic names, which are also automatically selected and loaded through this robot_name. You can view the content in the code for details.

6. Start the AMCL positioning program of the car

In the virtual machine terminal that starts robot1, enter,

xxxxxxxxxx
ros2 launch yahboomcar_multi robot1_amcl_launch.py

In the virtual machine terminal that starts robot2, enter,

xxxxxxxxxx
ros2 launch yahboomcar_multi robot2_amcl_launch.py

As shown in the figure above, "Please set the initial pose..." appears, and the corresponding [2D Pose] tool can be used to give the initial poses to the two cars respectively. According to the position of the car in the actual environment, click and drag with the mouse in rviz, and the car model moves to the position we set. As shown in the figure below, if the area scanned by the radar roughly coincides with the actual obstacle, it means that the posture is accurate.

Red indicates robot1 and blue indicates robot2.

Note: If the terminal cannot print "Please set the initial pose...", it may be a problem of data timestamp asynchrony. Press the reset button of the car to let the car reconnect to the proxy to ensure that the data timestamp is correct. Repeat several times until "Please set the initial pose..." appears.

Take the virtual machine as an example, the source code location: /home/yahboom/yahboomcar_ws/src/yahboomcar_multi/launch

robot1_amcl_launch.py

xxxxxxxxxx
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    #package_path = get_package_share_directory('yahboomcar_multi')
    #nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    lifecycle_nodes = ['map_server']
    param_file = os.path.join(get_package_share_directory('yahboomcar_multi'),'param','robot1_amcl_params.yaml')
    amcl_node = Node(
        name="robot1_amcl",
        package='nav2_amcl',
        executable='amcl',
        parameters=[os.path.join(get_package_share_directory('yahboomcar_multi'),'param','robot1_amcl_params.yaml')],
        remappings=[('/initialpose', '/robot1/initialpose')],
        output = "screen"
    )
    
    life_node = Node(
        name="robot1_amcl_lifecycle_manager",
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        output='screen',
        parameters=[{'use_sim_time': False},{'autostart': True},{'node_names': ['robot1_amcl']}]
        )
        
    base_link_to_laser_tf_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='base_link_to_base_laser',
        namespace = 'robot1',
        arguments=['0', '0' , '0.138','0','0','0','robot1/base_link','robot1/laser_frame']
    )
    
    return LaunchDescription([
        #lifecycle_nodes,
        #use_sim_time,\
        amcl_node,
        life_node,
        base_link_to_laser_tf_node
    ])

amcl_node: Start the amcl node program, used to estimate the pose and realize positioning

life_node: amcl node life cycle manager

base_link_to_laser_tf_node: static transformation of chassis and radar data

7. Start the car navigation program

In the virtual machine terminal that starts robot1, enter,

xxxxxxxxxx
ros2 launch yahboomcar_multi robot1_navigation_dwb_launch.py

In the virtual machine terminal that starts robot2, enter,

xxxxxxxxxx
ros2 launch yahboomcar_multi robot2_navigation_dwb_launch.py

As shown in the figure above, the "Creating bond timer..." appears, indicating that the program has been loaded, then you can use the corresponding [2D Goal Pose】Given the target points of the two cars, the cars will generate a path based on their respective positions and surrounding obstacles and autonomously navigate to their respective destinations.

The yellow route is the route planned by robot1, and the green line is the route planned by robot2.

8. Multi-car navigation expansion

The tutorial takes two cars as an example. If you want to add other cars, you need to make the following modifications.

8.1. Add a new car URDF model and add a urdf model loader

Note: The path is,

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_description/launch/description_multi_robot1.launch.py
/home/yahboom/yahboomcar_ws/src/yahboomcar_description/urdf/STM32-V2-V1_robot1.urdf

• Add a new car model

For reference, /home/yahboom/yahboomcar_ws/src/yahboomcar_description/urdf/STM32-V2-V1_robot1.urdf

Change the name and robot1 in the urdf file to other car names, such as robot3.

• Added urdf model loader

For reference, /home/yahboom/yahboomcar_ws/src/yahboomcar_description/launch/description_multi_robot1.launch.py Change the name and robot1 in the launch file to other car names. The name needs to be consistent with the newly added urdf.

8.2, Added car ekf parameter table

Note: The path is,

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_multi/param/ekf_robot1.yaml

You can refer to /home/yahboom/yahboomcar_ws/src/yahboomcar_multi/param/ekf_robot1.yaml, change the name and robot1 that appears in the file to other car names, and the name needs to be consistent with the newly added urdf.

8.3. Add a car amcl parameter table and launch file for amcl

Note: The path is:

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_multi/param/robot1_amcl_params.yaml
/home/yahboom/yahboomcar_ws/src/yahboomcar_multi/launch/robot1_amcl_launch.py

• Add a car amcl parameter table

Refer to /home/yahboom/yahboomcar_ws/src/yahboomcar_multi/param/robot1_amcl_params.yaml, and change the name and robot1 that appears in the file to the name of other cars. The name needs to be consistent with the newly added urdf.

• Added launch file of amcl

Refer to /home/yahboom/yahboomcar_ws/src/yahboomcar_multi/launch/robot1_amcl_launch.py, change the name and robot1 in the file to the name of other cars, and the name needs to be consistent with the newly added urdf.

8.4, Added car nav2 parameter table and launch file for starting nav2

Note: The path is:

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_multi/param/robot1_nav_params.yaml
/home/yahboom/yahboomcar_ws/src/yahboomcar_multi/launch/robot1_navigation_dwb_launch.py

• Added car nav2 parameter table

Refer to /home/yahboom/yahboomcar_ws/src/yahboomcar_multi/param/robot1_nav_params.yaml, change the name and robot1 in the file to the name of other cars, and the name needs to be consistent with the newly added urdf.

• Added launch file for nav2

Refer to /home/yahboom/yahboomcar_ws/src/yahboomcar_multi/launch/robot1_navigation_dwb_launch.py, change the name and robot1 in the file to the name of other cars, and the name needs to be consistent with the newly added urdf.

8.5, Added [2D Pose Estimate] and [2D Goal Pose] in the rviz toolbar

Note: The path is:

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/rviz

Modify the multi_nav.rviz file, the directory of the file is /home/yahboom/yahboomcar_ws/src/yahboomcar_nav/rviz/multi_nav.rviz, find the following part,

- Class: rviz_default_plugins/SetInitialPose
  Covariance x: 0.25
  Covariance y: 0.25
  Covariance yaw: 0.06853891909122467
  Topic:
    Depth: 5
    Durability Policy: Volatile
    History Policy: Keep Last
    Reliability Policy: Reliable
    Value: /robot1/initialpose
- Class: rviz_default_plugins/SetGoal
  Topic:
    Depth: 5
    Durability Policy: Volatile
    History Policy: Keep Last
    Reliability Policy: Reliable
    Value: /robot1/goal_pose
...

The above are two tools for robot1. You can copy one and put it behind. Change the robot1 that appears to the name of other cars. The name needs to be consistent with the newly added urdf.

After completing the above 5 steps, return to the yahboomcar_ws workspace, compile it using colcon build, and then run the test according to the tutorial. After successful operation, you can add the car model and radar data to display in rviz.