un-namespaced odom frame in tf tree

50 Views Asked by At

I am trying to simulate multiple turtlebot3s in ROS2 Humble, however I am having trouble with the tf frames. I want each robot to access the same map from the map topic, so i imagined the map frame links to each robots namespaced odom, tb3_1/odom, tb3_2/odom etc which link to their respective footprint and links. I currently have an odom frame in my tf tree which i am not sure where its coming from and the namespaced odom frames arent included, nor map. I am able to run it without namespacing the urdf file and hence the link frames but then the robots flicker between each other in rviz.

this is my launch file:

#!/usr/bin/env python3

import os

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

def generate_launch_description():
    TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
    # Get the path to the package
    pkg_path = get_package_share_directory('multiple_robots')
    # Construct the full path to the YAML file
    amcl_params_file = os.path.join(pkg_path, 'params', 'nav2_params.yaml')
    launch_file_dir = os.path.join(pkg_path, 'launch')
    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')

    rbt_n = os.environ['NUMBER_OF_ROBOTS']

    use_sim_time = LaunchConfiguration('use_sim_time', default='true')

    urdf_file_name = 'turtlebot3_' + TURTLEBOT3_MODEL + '.urdf'

    print('urdf_file_name : {}'.format(urdf_file_name))

    urdf_path = os.path.join(
        get_package_share_directory('turtlebot3_gazebo'),
        'urdf',
        urdf_file_name)

    with open(urdf_path, 'r') as infp:
        robot_desc = infp.read()
    
    world = os.path.join(
        pkg_path,
        'worlds',
        'factory.world'
    )
    
    gzserver_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
        ),
        launch_arguments={'world': world}.items()
    )

    gzclient_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
        )
    )

    spawn_turtlebot_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_file_dir, 'spawn_multiple.launch.py')
        )
    )


    #give each robot a navigation node
    nav_nodes = []
    state_nodes = []
    amcl_nodes = []
    for i in range(int(rbt_n)):
        prefix = 'tb3_'+str(i+1)
        my_node = Node(
            package='multiple_robots',
            executable='camera_display_node.py',
            name='camera_display_node',
            namespace=prefix,
            output='screen',
            parameters=[{'robot_id': i+1}] 
        )
        nav_nodes.append(my_node)

        with open(urdf_path, 'r') as infp:
            robot_desc = infp.read()

        

        # Modify the URDF content by adding the prefix to link and joint names
        robot_desc = robot_desc.replace('<link name="', '<link name="' + prefix + '/')
        robot_desc = robot_desc.replace('<joint name="', '<joint name="' + prefix + '/')
        robot_desc = robot_desc.replace('<parent link="', '<parent link="' + prefix + '/')
        robot_desc = robot_desc.replace('<child link="', '<child link="' + prefix + '/')


        rbt_state = Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            namespace=prefix,
            parameters=[{
                'use_sim_time': use_sim_time,
                'robot_description': robot_desc
            }]
        )
        state_nodes.append(rbt_state)

        # AMCL Node
        amcl = Node(
                package='nav2_amcl',
                executable='amcl',
                parameters=[amcl_params_file, {'base_frame_id': prefix + '/base_footprint',
                                   'odom_frame_id': prefix + '/odom'}],
                namespace=prefix,
                remappings=[
                ('map', '/map'),
            ],
                output='screen'
        )
        amcl_nodes.append(amcl)

    amcl_states = Node(
        package='multiple_robots',
        executable='set_amcl_states.py',
        arguments=[str(int(rbt_n))],
        output='screen'
    )
    
    # Map Server Node
    map_server = Node(
            package='multiple_robots',
            executable='map_publisher_node.py',
            output='screen'
        )

    
    
    # Launch RViz with a custom configuration
    rviz = Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            output='log',
            arguments=['-d', '/home/liam/ros2_ws/src/multiple_robots/rviz/multi.rviz']
        )
    
    
   
    ld = LaunchDescription()

    # Add the commands to the launch description
    ld.add_action(map_server)
    ld.add_action(rviz)   
    ld.add_action(gzserver_cmd)
    ld.add_action(gzclient_cmd)
    ld.add_action(spawn_turtlebot_cmd)
    #ld.add_action(rbt_state)


    #ld.add_action(amcl)
    for node in state_nodes:
        ld.add_action(node)
       
    for node in amcl_nodes:
        ld.add_action(node)

    ld.add_action(amcl_states)

    for node in nav_nodes:
        ld.add_action(node)
    
    #ld.add_action(my_node)
    return ld

here is the tf tree: enter image description here

here is my amcl params:

amcl:
  ros__parameters:
    use_sim_time: True
    use_map_topic: true
    first_map_only: false
    save_pose_rate: 0.5
    laser_min_range: 0.2
    laser_max_range: 3.5  # Typical range for Turtlebot3 Waffle's LDS
    max_beams: 60
    min_particles: 100
    max_particles: 2000
    kld_err: 0.05
    kld_z: 0.99
    odom_alpha1: 0.2  # Noise in rotation as function of rotation
    odom_alpha2: 0.2  # Noise in rotation as function of translation
    odom_alpha3: 0.2  # Noise in translation as function of translation
    odom_alpha4: 0.2  # Noise in translation as function of rotation
    global_frame_id: "map"
    laser_topic: "scan"
    resample_interval: 1
    transform_tolerance: 0.2
    recovery_alpha_slow: 0.001
    recovery_alpha_fast: 0.1
    update_min_d: 0.2
    update_min_a: 0.5
    update_interval: 3.0
    gui_publish_rate: 10.0
    laser_z_hit: 0.5
    laser_z_short: 0.05
    laser_z_max: 0.05
    laser_z_rand: 0.5
    laser_sigma_hit: 0.2
    laser_lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_model_type: "likelihood_field"
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    initial_pose_a: 0

I have tried changing the amcl params, using namespaced tf's via a transform and trying to use a global odometry instead.

0

There are 0 best solutions below