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.