Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix: Changed cloned_multi_tb3_simulation_launch.py file to conform with common ROS 2 launch syntax #4811

Merged
merged 7 commits into from
Jan 7, 2025
Merged

fix: Changed cloned_multi_tb3_simulation_launch.py file to conform with common ROS 2 launch syntax #4811

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
f0e884f
Changed cloned_multi_tb3_simulation_launch.py file to conform with co…
TannerGilbert Dec 20, 2024
ce0bff5
chore: Changed formatting to conform to flake8 linting rules
TannerGilbert Dec 20, 2024
51102d7
Changed ParseMultiRobotPose to be a launch substitution class
TannerGilbert Jan 5, 2025
1183a07
chore: Changed formatting to conform to flake8 linting rules II
TannerGilbert Jan 5, 2025
9f4ef45
Changed formatting based on PR feedback
TannerGilbert Jan 7, 2025
4ef40b9
Merge remote-tracking branch 'upstream/main' into fix/robots_launch_a…
TannerGilbert Jan 7, 2025
fb5b39d
Undo removal of position parsing options
TannerGilbert Jan 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
136 changes: 74 additions & 62 deletions nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,69 @@
from nav2_common.launch import ParseMultiRobotPose


def generate_robot_actions(context, *args, **kwargs):
bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(bringup_dir, 'launch')
use_rviz = LaunchConfiguration('use_rviz')
params_file = LaunchConfiguration('params_file')
autostart = LaunchConfiguration('autostart')
rviz_config_file = LaunchConfiguration('rviz_config')
map_yaml_file = LaunchConfiguration('map')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')

robots_substitution = ParseMultiRobotPose(LaunchConfiguration('robots'))
robots_list = robots_substitution.perform(context)

# Define commands for launching the navigation instances
bringup_cmd_group = []
for robot_name in robots_list:
init_pose = robots_list[robot_name]
group = GroupAction(
[
LogInfo(
msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose),]
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'rviz_launch.py')
),
condition=IfCondition(use_rviz),
launch_arguments={
'namespace': TextSubstitution(text=robot_name),
'rviz_config': rviz_config_file,
}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')
),
launch_arguments={
'namespace': robot_name,
'map': map_yaml_file,
'use_sim_time': 'True',
'params_file': params_file,
'autostart': autostart,
'use_rviz': 'False',
'use_simulator': 'False',
'headless': 'False',
'use_robot_state_pub': use_robot_state_pub,
'x_pose': TextSubstitution(text=str(init_pose['x'])),
'y_pose': TextSubstitution(text=str(init_pose['y'])),
'z_pose': TextSubstitution(text=str(init_pose['z'])),
'roll': TextSubstitution(text=str(init_pose['roll'])),
'pitch': TextSubstitution(text=str(init_pose['pitch'])),
'yaw': TextSubstitution(text=str(init_pose['yaw'])),
'robot_name': TextSubstitution(text=robot_name),
}.items(),
),
]
)

bringup_cmd_group.append(group)
bringup_cmd_group.append(LogInfo(msg=['number_of_robots=', str(len(robots_list))]))
return bringup_cmd_group


def generate_launch_description():
"""
Bring up the multi-robots with given launch arguments.
Expand All @@ -49,7 +112,6 @@ def generate_launch_description():
"""
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(bringup_dir, 'launch')
sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')

# Simulation settings
Expand All @@ -61,7 +123,6 @@ def generate_launch_description():
autostart = LaunchConfiguration('autostart')
rviz_config_file = LaunchConfiguration('rviz_config')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')
log_settings = LaunchConfiguration('log_settings', default='true')

# Declare the launch arguments
Expand All @@ -71,6 +132,13 @@ def generate_launch_description():
description='Full path to world file to load',
)

declare_robots_cmd = DeclareLaunchArgument(
'robots',
default_value="""robot1={x: 0.5, y: 0.5, yaw: 0};
robot2={x: -0.5, y: -0.5, z: 0, roll: 0, pitch: 0, yaw: 1.5707}""",
description='Robots and their initialization poses in YAML format',
)

declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'),
Expand All @@ -93,7 +161,8 @@ def generate_launch_description():

declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config',
default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
default_value=os.path.join(
bringup_dir, 'rviz', 'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use.',
)

Expand Down Expand Up @@ -121,60 +190,6 @@ def generate_launch_description():
OpaqueFunction(function=lambda _: os.remove(world_sdf))
]))

robots_list = ParseMultiRobotPose('robots').value()

# Define commands for launching the navigation instances
bringup_cmd_group = []
for robot_name in robots_list:
init_pose = robots_list[robot_name]
group = GroupAction(
[
LogInfo(
msg=[
'Launching namespace=',
robot_name,
' init_pose=',
str(init_pose),
]
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'rviz_launch.py')
),
condition=IfCondition(use_rviz),
launch_arguments={
'namespace': TextSubstitution(text=robot_name),
'rviz_config': rviz_config_file,
}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')
),
launch_arguments={
'namespace': robot_name,
'map': map_yaml_file,
'use_sim_time': 'True',
'params_file': params_file,
'autostart': autostart,
'use_rviz': 'False',
'use_simulator': 'False',
'headless': 'False',
'use_robot_state_pub': use_robot_state_pub,
'x_pose': TextSubstitution(text=str(init_pose['x'])),
'y_pose': TextSubstitution(text=str(init_pose['y'])),
'z_pose': TextSubstitution(text=str(init_pose['z'])),
'roll': TextSubstitution(text=str(init_pose['roll'])),
'pitch': TextSubstitution(text=str(init_pose['pitch'])),
'yaw': TextSubstitution(text=str(init_pose['yaw'])),
'robot_name': TextSubstitution(text=robot_name),
}.items(),
),
]
)

bringup_cmd_group.append(group)

set_env_vars_resources = AppendEnvironmentVariable(
'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models'))
set_env_vars_resources2 = AppendEnvironmentVariable(
Expand All @@ -188,6 +203,7 @@ def generate_launch_description():

# Declare the launch options
ld.add_action(declare_world_cmd)
ld.add_action(declare_robots_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_use_rviz_cmd)
Expand All @@ -200,8 +216,6 @@ def generate_launch_description():
ld.add_action(start_gazebo_cmd)
ld.add_action(remove_temp_sdf_file)

ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))]))

ld.add_action(
LogInfo(condition=IfCondition(log_settings), msg=['map yaml: ', map_yaml_file])
)
Expand All @@ -223,8 +237,6 @@ def generate_launch_description():
ld.add_action(
LogInfo(condition=IfCondition(log_settings), msg=['autostart: ', autostart])
)

for cmd in bringup_cmd_group:
ld.add_action(cmd)
ld.add_action(OpaqueFunction(function=generate_robot_actions))

return ld
85 changes: 37 additions & 48 deletions nav2_common/nav2_common/launch/parse_multirobot_pose.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,66 +12,55 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import sys
from typing import Dict, Text

import launch
from launch.launch_context import LaunchContext
import yaml


class ParseMultiRobotPose:
"""Parsing argument using sys module."""
class ParseMultiRobotPose(launch.Substitution):
"""
A custom substitution to parse the robots argument for multi-robot poses.

def __init__(self, target_argument: Text):
"""
Parse arguments for multi-robot's pose.
Expects input in the format:
robots:="robot1={x: 1.0, y: 1.0, yaw: 0.0};
robot2={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}"`

for example,
`ros2 launch nav2_bringup bringup_multirobot_launch.py
TannerGilbert marked this conversation as resolved.
Show resolved Hide resolved
robots:="robot1={x: 1.0, y: 1.0, yaw: 0.0};
robot2={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}"`
The individual robots are separated by a `;` and each robot consists of a name and pose object.
The name corresponds to the namespace of the robot and name of the Gazebo object.
The pose consists of X, Y, Z, Roll, Pitch, Yaw each of which can be omitted in which case it is
inferred as 0.
"""

`target_argument` shall be 'robots'.
Then, this will parse a string value for `robots` argument.
def __init__(self, robots_argument: launch.SomeSubstitutionsType) -> None:
super().__init__()
self.__robots_argument = robots_argument

Each robot name which corresponds to namespace and pose of it will be separated by `;`.
The pose consists of x, y and yaw with YAML format.

:param: target argument name to parse
"""
self.__args: Text = self.__parse_argument(target_argument)

def __parse_argument(self, target_argument: Text) -> Text:
"""Get value of target argument."""
if len(sys.argv) > 4:
argv = sys.argv[4:]
for arg in argv:
if arg.startswith(target_argument + ':='):
return arg.replace(target_argument + ':=', '')
def describe(self) -> Text:
"""Return a description of this substitution as a string."""
return ''

def value(self) -> Dict:
"""Get value of target argument."""
args = self.__args
parsed_args = [] if len(args) == 0 else args.split(';')
def perform(self, context: LaunchContext) -> Dict:
"""Resolve and parse the robots argument string into a dictionary."""
robots_str = self.__robots_argument.perform(context)
if not robots_str:
return {}

multirobots = {}
for arg in parsed_args:
key_val = arg.strip().split('=')
for robot_entry in robots_str.split(';'):
key_val = robot_entry.strip().split('=')
if len(key_val) != 2:
continue
key = key_val[0].strip()
val = key_val[1].strip()
robot_pose = yaml.safe_load(val)
if 'x' not in robot_pose:
robot_pose['x'] = 0.0
if 'y' not in robot_pose:
robot_pose['y'] = 0.0
if 'z' not in robot_pose:
robot_pose['z'] = 0.0
if 'roll' not in robot_pose:
robot_pose['roll'] = 0.0
if 'pitch' not in robot_pose:
robot_pose['pitch'] = 0.0
if 'yaw' not in robot_pose:
robot_pose['yaw'] = 0.0
multirobots[key] = robot_pose

robot_name, pose_str = key_val[0].strip(), key_val[1].strip()
robot_pose = yaml.safe_load(pose_str)
# Set default values if not provided
robot_pose.setdefault('x', 0.0)
robot_pose.setdefault('y', 0.0)
robot_pose.setdefault('z', 0.0)
robot_pose.setdefault('roll', 0.0)
robot_pose.setdefault('pitch', 0.0)
robot_pose.setdefault('yaw', 0.0)
multirobots[robot_name] = robot_pose
return multirobots
Loading