We now need run some additional nodes. First of all, we run Gazebo, and (if we want, I currently commented it out) RViz. Then we spawn robots. We create Localization nodes, as well as Path Planner nodes. And Coordinator node. And so on. So let's take a look at a new multi_simulation_launch.py:
# (c) robotics.snowcron.com
# Use: MIT license
from launch import LaunchDescription
import launch
import launch_ros.actions
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition, LaunchConfigurationNotEquals, LaunchConfigurationEquals
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution
from launch.actions import (GroupAction, SetEnvironmentVariable)
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.actions import ExecuteProcess, EmitEvent
from launch_ros.actions import PushRosNamespace
from launch.events import Shutdown
from nav2_common.launch import ReplaceString
from launch_ros.substitutions import FindPackageShare
import xacro
import sys
sys.path.append("src/nav25d_07/launch")
from globals import *
# ---
def generate_launch_description():
# For this demo, we either have one robot, in which case its namespace is "",
# or two robots: "robot1" and "robot2"
robots = [
# { 'type': 'dif4wd', 'name': '', 'x_pos': 120, 'y_pos': 125, 'z_pos': 4.0,
# "color_name" : "Blue", "color_rgb" : "0 0 1 1"},
{'type': 'dif4wd', 'name': 'robot1', 'x_pos': 120, 'y_pos': 125.0, 'z_pos': 4.0,
"color_name" : 'Yellow', "color_rgb" : "1 1 0 1"},
{'type': 'dif4wd', 'name': 'robot2', 'x_pos': 125, 'y_pos': 125.0, 'z_pos': 4.0,
"color_name" : 'Blue', "color_rgb" : "0 0 1 1"},
]
# I am going to fill this array, and then to feed nodes from it to ROS2: just a mater of
# convenience
arrNodes = []
world = LaunchConfiguration('world')
# Temporary commented, as I do not need RViz right now
#rviz_config_file = LaunchConfiguration('rviz_config_file')
# Temporary commented
#
# # def_rviz_path defined in globals.py
# declare_rviz_config_file_cmd = DeclareLaunchArgument(
# 'rviz_config_file',
# default_value=def_rviz_path,
# description='Full path to the RVIZ config file to use')
# arrNodes.append(declare_rviz_config_file_cmd)
# use_rviz = LaunchConfiguration('use_rviz')
# declare_use_rviz_cmd = DeclareLaunchArgument(
# 'use_rviz',
# default_value='True',
# description='Whether to start RVIZ')
# arrNodes.append(declare_use_rviz_cmd)
# Use simulated time, rather than clock time
use_sim_time = LaunchConfiguration('use_sim_time')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='True',
description='Use simulation (Gazebo) clock if true')
arrNodes.append(declare_use_sim_time_cmd)
# get world file name from command line
declare_world_cmd = DeclareLaunchArgument(
'world',
default_value=def_world_path,
description='Full path to world file to load')
arrNodes.append(declare_world_cmd)
# Create Gazebo node
gazebo_node = ExecuteProcess(
cmd=['gazebo', '--verbose', world,
'-s', 'libgazebo_ros_init.so',
'-s', 'libgazebo_ros_factory.so',
],
output='screen')
arrNodes.append(gazebo_node)
# Temporary commented
#
# rviz_node = Node(package ='rviz2',
# executable ='rviz2',
# name ='rviz2',
# output ='log',
# arguments =['-d', rviz_config_file])
# arrNodes.append(rviz_node)
# Create Coordinator node
coordinator_node = Node(
package='nav25d_07',
executable='coordinator',
name='coordinator_node',
parameters=[{'use_sim_time': use_sim_time}],
output='screen',
)
arrNodes.append(coordinator_node)
# In cycle, for all robots, add nodes such as spawning, Localization, Path planners...
for robot in robots:
namespace = "/" + robot['name'] if robot['name'] != "" else ""
# --- URDF
# I have two models, 2 wheels is more accurate when turning, 4 wheels can go uphill...
# Everything like in real life.
if(robot["type"] == "dif4wd"):
def_urdf = os.path.join(def_bringup_dir,'description','robot_4wd.urdf.xacro')
else:
def_urdf = os.path.join(def_bringup_dir,'description','robot_2wd.urdf.xacro')
robot_description_config = xacro.process_file(
def_urdf,
mappings={
"namespace" : namespace,
"robot_type": robot['type'],
"robot_name" : robot['name'] + "/" if robot['name'] != '' else '',
"robot_material_name": robot['color_name'],
"robot_material_color_rgb": robot['color_rgb'],
},
)
# Robot state publisher
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
start_robot_state_publisher_cmd = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
namespace=namespace,
output='screen',
#remappings=def_remappings,
parameters=[params]
)
arrNodes.append(start_robot_state_publisher_cmd)
# Spawning robots (showing them in Gazebo)
spawn_entity_cmd = Node(package='gazebo_ros',
executable='spawn_entity.py',
namespace=namespace,
arguments=[
"-topic", namespace + "/robot_description",
'-entity', robot['name'],
'-robot_namespace', robot['name'],
'-x', str(robot['x_pos']),
'-y', str(robot['y_pos']),
'-z', str(robot['z_pos']),
],
#parameters=[{'use_sim_time': use_sim_time}],
parameters=[params],
output='screen')
arrNodes.append(spawn_entity_cmd)
# Creating path planners. I want to use different ones in different tests, so
# I create both types
global_planner_straight_line_node = Node(
package='nav25d_07',
executable='global_planner_straight_line',
name='global_planner_straight_line_node',
namespace=namespace,
arguments=[
'robot_name', robot['name'], # No "-"
],
parameters=[{'use_sim_time': use_sim_time}],
output='screen',
)
arrNodes.append(global_planner_straight_line_node)
# ---
global_planner_a_star_grid_node = Node(
package='nav25d_07',
executable='global_planner_a_star_grid',
name='global_planner_a_star_grid_node',
namespace=namespace,
arguments=[
'robot_name', robot['name'], # No "-"
],
parameters=[{'use_sim_time': use_sim_time}],
output='screen',
)
arrNodes.append(global_planner_a_star_grid_node)
# --- Robot class
robot_node = Node(
package='nav25d_07',
executable='robot',
name='robot_node',
namespace=namespace,
arguments=[
'robot_name', robot['name'], # No "-"
],
parameters=[params],
output='screen'
)
arrNodes.append(robot_node)
# --- Localization class
localization_node = Node(
package='nav25d_07',
executable='localization',
name='localization_node',
namespace=namespace,
arguments=[
'robot_name', robot['name'], # No "-"
'-x', str(robot['x_pos']),
'-y', str(robot['y_pos']),
'-z', str(robot['z_pos']),
],
parameters=[params],
output='screen'
)
arrNodes.append(localization_node)
# --- Transformations. I have addressed it in one of the earlier sections
# TBD: should use namespace as above
node_tf_world_to_odom = launch_ros.actions.Node(
package='tf2_ros',
executable='static_transform_publisher',
arguments=[str(robot['x_pos']), str(robot['y_pos']), str(robot['z_pos']),
'0', '0', '0', '/world', robot['name'] + '/odom'],
parameters=[{'use_sim_time': use_sim_time}],
output='screen')
arrNodes.append(node_tf_world_to_odom)
# --- If Gazebo exits, close RViz and vice versa
gazebo_exit_event_handler = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=gazebo_node,
on_exit=EmitEvent(event=Shutdown(reason='gazebo exited'))))
arrNodes.append(gazebo_exit_event_handler)
# Temporary commented
#
# rviz_exit_event_handler = RegisterEventHandler(
# event_handler=OnProcessExit(
# target_action=rviz_node,
# on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
# arrNodes.append(rviz_exit_event_handler)
# Create the launch description and populate
ld = LaunchDescription()
for node in arrNodes:
ld.add_action(node)
return ld
Note that any software developer will immediately suggest to combine some (or all) robot-related classes initialization... Ideally, to move the "for all robots" cycle into the Coordinator's code. I might address the issue later.