Skip to content

Commit

Permalink
fix: Changed cloned_multi_tb3_simulation_launch.py file to conform wi…
Browse files Browse the repository at this point in the history 
…th common ROS 2 launch syntax (#4811)

* Changed cloned_multi_tb3_simulation_launch.py file to conform with common ROS 2 launch syntax and thereby allow for including it in other launch files as expected

Signed-off-by: Tanner, Gilbert <[email protected]>

* chore: Changed formatting to conform to flake8 linting rules

Signed-off-by: Tanner, Gilbert <[email protected]>

chore: Changed formatting to conform to flake8 linting rules II

Signed-off-by: Tanner, Gilbert <[email protected]>

chore: Changed formatting to conform to flake8 linting rules III

Signed-off-by: Tanner, Gilbert <[email protected]>

chore: Changed formatting to conform to flake8 linting rules IV

Signed-off-by: Tanner, Gilbert <[email protected]>

* Changed ParseMultiRobotPose to be a launch substitution class

Signed-off-by: Tanner, Gilbert <[email protected]>

* chore: Changed formatting to conform to flake8 linting rules II

Signed-off-by: Tanner, Gilbert <[email protected]>

* Changed formatting based on PR feedback

Signed-off-by: Tanner, Gilbert <[email protected]>

* Undo removal of position parsing options

Signed-off-by: Tanner, Gilbert <[email protected]>

---------

Signed-off-by: Tanner, Gilbert <[email protected]>
  • Loading branch information
@TannerGilbert
TannerGilbert authored Jan 7, 2025
1 parent 81ca3a1 commit 43d7f7e
Show file tree
Hide file tree
Showing 2 changed files with 111 additions and 110 deletions.
136 changes: 74 additions & 62 deletions nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
View file
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
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
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

0 comments on commit 43d7f7e

Please sign in to comment.