def generate_launch_description():
entities = [
ExecuteProcess(
cmd=[
'gzserver',
'--verbose',
'-s',
'libgazebo_ros_init.so', # Publish /clock
'-s',
'libgazebo_ros_factory.so', # Provide injection endpoints
world_filename
],
output='screen'),
# Joystick driver, generates /namespace/joy messages
# Node(package='joy', node_executable='joy_node', output='screen'),
# Add forward-facing camera to the simulation
# Node(package='sim_fiducial', executable='inject_entity.py', output='screen',
# arguments=[forward_camera_sdf, '0', '0', '0', '0', '0', '0']),
Node(package='sim_fiducial',
executable='inject_entity.py',
output='screen',
arguments=[elemental_camera_sdf, '0', '0', '0', '0', '0', '0']),
Node(package='fiducial_vlam',
executable='vloc_main',
output='screen',
parameters=vloc_args,
namespace='forward_camera'),
Node(package='fiducial_vlam',
executable='vmap_main',
output='screen',
parameters=vmap_args),
]
return LaunchDescription(entities)
def generate_launch_description():
# launch paths
metrobot_description_dir = get_package_share_directory(
'metrobot_description')
metrobot_simulation_dir = get_package_share_directory(
'metrobot_simulation')
# Create the launch configuration variables
use_sim_time = LaunchConfiguration('use_sim_time', default='True')
launch_gazebo_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
get_package_share_directory('gazebo_ros'),
'/launch/gazebo.launch.py'
]))
set_use_sim_time_cmd = ExecuteProcess(
cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time],
output='screen')
launch_robot_description_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
metrobot_description_dir, '/launch/metrobot_description.launch.py'
]),
launch_arguments={'use_sim_time': use_sim_time}.items())
spawn_robot_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[metrobot_simulation_dir, '/launch/spawn_metrobot.launch.py']))
ld = LaunchDescription()
ld.add_action(launch_gazebo_cmd)
ld.add_action(set_use_sim_time_cmd)
ld.add_action(launch_robot_description_cmd)
ld.add_action(spawn_robot_cmd)
return ld
def generate_launch_description():
bringup_dir = get_package_share_directory('nav2_bringup')
# Simulation settings
world = LaunchConfiguration('world')
simulator = LaunchConfiguration('simulator')
# Declare the launch arguments
declare_world_cmd = DeclareLaunchArgument(
'world',
default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'),
description='Full path to world file to load')
declare_simulator_cmd = DeclareLaunchArgument(
'simulator',
default_value='gazebo',
description='The simulator to use (gazebo or gzserver)')
# Start Gazebo with plugin providing the robot spawing service
start_gazebo_cmd = ExecuteProcess(cmd=[
simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', '-s',
'libgazebo_ros_init.so', world
],
output='screen')
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add the actions to start gazebo, robots and simulations
ld.add_action(start_gazebo_cmd)
return ld
def generate_launch_description():
# launch paths
gazebo_worlds_path = get_package_share_directory('gazebo_worlds')
# export models path
os.environ['GAZEBO_MODEL_PATH'] = os.getenv('GAZEBO_MODEL_PATH') + ":" + gazebo_worlds_path + "/models"
world = LaunchConfiguration('world')
declare_world_cmd = DeclareLaunchArgument(
'world', default_value=os.path.join(gazebo_worlds_path, 'worlds', 'house.world'),
description='Specify world file name'
)
# Create the launch configuration variables
use_sim_time = LaunchConfiguration('use_sim_time', default='True')
launch_gzserver_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource([get_package_share_directory('gazebo_ros'), "/launch/gzserver.launch.py"]),
launch_arguments={'world': world, 'verbose': 'true'}.items()
)
launch_gzclient_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource([get_package_share_directory('gazebo_ros'), "/launch/gzclient.launch.py"])
)
set_use_sim_time_cmd = ExecuteProcess(
cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time],
output='screen')
ld = LaunchDescription()
ld.add_action(declare_world_cmd)
ld.add_action(launch_gzserver_cmd)
ld.add_action(launch_gzclient_cmd)
ld.add_action(set_use_sim_time_cmd)
return ld
def test_logging_long_messages():
# Set the output format to a "verbose" format that is expected by the executable output
os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \
'[{severity}] [{name}]: {message} ({function_name}() at {file_name}:{line_number})'
executable = os.path.join(os.getcwd(), 'test_logging_long_messages')
if os.name == 'nt':
executable += '.exe'
ld = LaunchDescription()
launch_test = LaunchTestService()
action = launch_test.add_fixture_action(
ld,
ExecuteProcess(cmd=[executable],
name='test_logging_long_messages',
output='screen'))
output_file = os.path.join(os.path.dirname(__file__),
'test_logging_long_messages')
launch_test.add_output_test(ld, action,
create_output_test_from_file(output_file))
launch_service = LaunchService()
launch_service.include_launch_description(ld)
return_code = launch_test.run(launch_service)
assert return_code == 0, 'Launch failed with exit code %r' % (
return_code, )
def generate_launch_description():
# Must match camera name in URDF file
camera_name = 'forward_camera'
camera_frame = 'forward_camera_frame'
sim_fiducial_path = get_package_share_directory('sim_fiducial')
world_path = os.path.join(sim_fiducial_path, 'worlds', 'one_marker.world')
map_path = os.path.join(sim_fiducial_path, 'worlds', 'one_marker_map.yaml')
sdf_path = os.path.join(sim_fiducial_path, 'sdf', 'forward_camera.sdf')
vmap_node_params = {
# Publish marker /tf
'psm_publish_tfs': 1,
# Load map
'map_load_filename': map_path,
# Don't save the map
'map_save_filename': '',
}
model_params = {
# Match orca.urdf (slight positive buoyancy):
'mdl_mass': 9.9,
'mdl_volume': 0.01,
'mdl_fluid_density': 997.0,
}
pose_filter_node_params = {
'four_dof': True, # Simplify calcs
'predict_accel':
False, # No control input, drag or buoyancy in this sim
'predict_accel_control': False,
'predict_accel_drag': False,
'predict_accel_buoyancy': False,
'filter_baro': False, # No depth in this sim
'filter_fcam': True,
'publish_tf': True, # Publish map=>base tf for rviz
'good_pose_dist': 5.0, # Marker is 2-3m away, so make this pretty far
'good_obs_dist': 10.0,
# Process noise, similar to /depth noise
'ukf_process_noise': 0.0004,
# Turn outlier detection off
'ukf_outlier_distance': -1.0,
}
pose_filter_node_params.update(model_params)
all_entities = [
# Launch Gazebo, loading the world
ExecuteProcess(
cmd=[
# 'gazebo',
'gzserver',
'--verbose',
'-s',
'libgazebo_ros_init.so', # Publish /clock
'-s',
'libgazebo_ros_factory.so', # Provide injection endpoints
world_path
],
output='screen'),
# Add the sdf to the simulation
Node(package='sim_fiducial',
node_executable='inject_entity.py',
output='screen',
arguments=[sdf_path, '0', '0', '0', '0', '0', '0']),
# Publish, and possibly build, a map
Node(package='fiducial_vlam',
node_executable='vmap_main',
output='screen',
node_name='vmap_node',
parameters=[vmap_node_params]),
# Localize against the map
Node(
package='fiducial_vlam',
node_executable='vloc_main',
output='screen',
node_name='vloc_forward',
node_namespace=camera_name,
parameters=[{
# Localize, don't calibrate
'loc_calibrate_not_loocalize': 0,
# 0: OpenCV, 1: GTSAM
'loc_camera_sam_not_cv': 1,
'psl_camera_frame_id': camera_frame,
'psl_publish_tfs': 0,
# 0: DICT_4x4_50 (default)
# 8: DICT_6X6_250
'loc_aruco_dictionary_id': 8,
# Gazebo publishes camera info best-effort
'psl_sub_camera_info_best_effort_not_reliable': 1,
# Publish the camera pose
'psl_publish_camera_pose': 1,
# Don't publish anything else
'psl_publish_base_pose': 0,
'psl_publish_camera_odom': 0,
'psl_publish_base_odom': 0,
# Keep the existing timestamps
'psl_stamp_msgs_with_current_time': 0,
}]),
# Combine poses and observations into fiducial poses
Node(package='orca_filter',
node_executable='fp_node',
output='screen',
node_name='fp_node',
node_namespace=camera_name),
# Run a pose filter (6dof, 4dof or 1dof)
Node(package='orca_filter',
node_executable='pose_filter_node',
output='screen',
node_name='pose_filter_node',
parameters=[pose_filter_node_params],
remappings=[
('fcam_fp', '/' + camera_name + '/fp'),
])
]
return LaunchDescription(all_entities)
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]), )
# NOTE: Load the processed xacro file directly
description_location = os.path.join(
get_package_share_directory('racecar_description'))
xacro_file = os.path.join(description_location, 'urdf', 'racecar.xacro')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
robot_description_value = doc.toxml()
params = {"robot_description": robot_description_value}
node_robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params])
spawn_entity = Node(package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-topic', 'robot_description', '-entity',
'racecar', '-x', '0', '-y', '0', '-z', '0.5'
],
output='screen')
# NOTE: THIS IS DEPRECATED VERSION OF LOADING
load_joint_state_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_start_controller', 'joint_state_broadcaster'
],
output='screen')
# NOTE: THIS IS DEPRECATED VERSION OF LOADING CONTROLLER
controllers = [
'left_rear_wheel_velocity_controller',
'right_rear_wheel_velocity_controller',
'left_front_wheel_velocity_controller',
'right_front_wheel_velocity_controller',
'left_steering_hinge_position_controller',
'right_steering_hinge_position_controller'
]
load_controllers = [
ExecuteProcess(cmd=['ros2', 'control', 'load_start_controller', state],
output='screen') for state in controllers
]
racecar_controller = os.path.join(
get_package_share_directory("racecar_control"),
"config",
"racecar_control.yaml",
)
return LaunchDescription([
RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=load_controllers,
)),
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def generate_launch_description():
# 1 or more drones:
drones = ['drone1']
# Starting locations:
starting_locations = [
# Face the wall of markers in fiducial.world
# ['-2.5', '1.5', '1', '0'],
# ['-1.5', '0.5', '2', '0.785'],
# ['-0.5', '1.5', '1', '0'],
# ['-1.5', '2.5', '.5', '-0.785']
['-1.5', '1.5', '1', '0'],
# Face all 4 directions in f2.world
# ['-2.5', '1.5', '1', '0'],
# ['-1.5', '0.5', '1', '1.57'],
# ['-0.5', '1.5', '1', '3.14'],
# ['-1.5', '2.5', '1', '-1.57']
]
fiducial_vlam_path = get_package_share_directory('fiducial_vlam')
tello_description_path = get_package_share_directory('tello_description')
map_path = os.path.join(fiducial_vlam_path, 'launch',
'fiducial_marker_locations_office.yaml')
rviz_config_path = os.path.join(fiducial_vlam_path, 'launch',
'launch_one.rviz')
# Global entities
entities = [
# Rviz
ExecuteProcess(cmd=['rviz2', '-d', rviz_config_path], output='screen'),
# # Launch Gazebo, loading tello.world
# ExecuteProcess(cmd=[
# 'gazebo',
# '--verbose',
# '-s', 'libgazebo_ros_init.so', # Publish /clock
# '-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node
# world_path
# ], output='screen'),
# Load and publish a known map
Node(
package='fiducial_vlam',
node_executable='vmap_node',
output='screen',
node_name='vmap_node',
parameters=[{
'use_sim_time': False, # Use /clock if available
'publish_tfs': 1, # Publish marker /tf
'marker_length': 0.1778, # Marker length
'marker_map_load_full_filename':
map_path, # Load a pre-built map from disk
'make_not_use_map': 0 # Don't save a map to disk
}]),
# Joystick driver, generates /namespace/joy messages
Node(
package='joy',
node_executable='joy_node',
output='screen',
node_name='joy_node',
parameters=[{
'use_sim_time': False, # Use /clock if available
}]),
# Flock controller (basically a joystick multiplexer, also starts/stops missions)
Node(
package='flock2',
node_executable='flock_base',
output='screen',
node_name='flock_base',
parameters=[{
'use_sim_time': False, # Use /clock if available
'drones': drones
}]),
# WIP: planner
Node(
package='flock2',
node_executable='planner_node',
output='screen',
node_name='planner_node',
parameters=[{
'use_sim_time': False, # Use /clock if available
'drones': drones,
'arena_x': -5.0,
'arena_y': -5.0,
'arena_z': 10.0,
}]),
]
# Per-drone entities
for idx, namespace in enumerate(drones):
suffix = '_' + str(idx + 1)
urdf_path = os.path.join(tello_description_path, 'urdf',
'tello' + suffix + '.urdf')
entities.extend([
# # Add a drone to the simulation
# Node(package='tello_gazebo', node_executable='inject_entity.py', output='screen',
# arguments=[urdf_path]+starting_locations[idx]),
# # Publish base_link to camera_link tf
# Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen',
# node_name=namespace+'_tf_pub', arguments=[urdf_path], parameters=[{
# 'use_sim_time': False, # Use /clock if available
# }]),
#
# Localize this drone against the map
# Future: need odometry for base_link, not camera_link
Node(
package='fiducial_vlam',
node_executable='vloc_node',
output='screen',
node_name='vloc_node',
node_namespace=namespace,
parameters=[{
'use_sim_time': False, # Use /clock if available
'publish_tfs': 1, # Publish drone and camera /tf
'stamp_msgs_with_current_time':
0, # Use incoming message time, not now()
'base_frame_id': 'base_link' + suffix,
'map_init_pose_z': -0.035,
'camera_frame_id': 'camera_link' + suffix,
'base_odometry_pub_topic': 'filtered_odom',
}]),
# Odometry filter takes camera pose, generates base_link odom, and publishes map to base_link tf
# Node(package='odom_filter', node_executable='filter_node', output='screen',
# node_name='filter_node', node_namespace=namespace, parameters=[{
# 'use_sim_time': True, # Use /clock if available
# 'map_frame': 'map',
# 'base_frame': 'base_link' + suffix
# }]),
# Driver
Node(package='tello_driver',
node_executable='tello_driver',
output='screen',
node_name='tello_driver',
node_namespace=namespace),
# Drone controller
Node(
package='flock2',
node_executable='drone_base',
output='screen',
node_name='drone_base',
node_namespace=namespace,
parameters=[{
'use_sim_time': False, # Use /clock if available
}]),
])
return LaunchDescription(entities)
def generate_launch_description():
# 1 or more drones:
# drones = ['drone1', 'drone2']
drones = ['drone1']
tello_gazebo_path = get_package_share_directory('tello_gazebo')
tello_description_path = get_package_share_directory('tello_description')
world_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial.world')
map_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial_map.yaml')
# Global entities
entities = [
# Launch Gazebo, loading tello.world
ExecuteProcess(
cmd=[
'gazebo',
'--verbose',
'-s',
'libgazebo_ros_init.so', # Publish /clock
'-s',
'libgazebo_ros_factory.so', # Provide gazebo_ros::Node
world_path
],
output='screen'),
# Load and publish a known map
Node(
package='fiducial_vlam',
node_executable='vmap_node',
output='screen',
node_name='vmap_node',
parameters=[{
'publish_tfs': 1, # Publish marker /tf
'marker_length': 0.1778, # Marker length
'marker_map_load_full_filename':
map_path, # Load a pre-built map from disk
'make_not_use_map': 0
}]), # Don't save a map to disk
# Joystick driver, generates /namespace/joy messages
# Only controls the first drone
Node(package='joy',
node_executable='joy_node',
output='screen',
node_namespace=drones[0]),
# Joystick controller, generates /namespace/cmd_vel messages
Node(package='tello_driver',
node_executable='tello_joy',
output='screen',
node_namespace=drones[0]),
]
# Per-drone entities
for idx, namespace in enumerate(drones):
suffix = '_' + str(idx + 1)
urdf_path = os.path.join(tello_description_path, 'urdf',
'tello' + suffix + '.urdf')
entities.extend([
# Add a drone to the simulation
Node(package='tello_gazebo',
node_executable='inject_entity.py',
output='screen',
arguments=[urdf_path, '0', str(idx), '1', '0']),
# Localize this drone against the map
Node(package='fiducial_vlam',
node_executable='vloc_node',
output='screen',
node_name='vloc_node',
node_namespace=namespace,
parameters=[{
'publish_tfs': 1,
'base_frame_id': 'base_link' + suffix,
't_camera_base_z': -0.035,
'camera_frame_id': 'camera_link' + suffix
}]),
])
return LaunchDescription(entities)
def test_pub_basic(self, launch_service, proc_info, proc_output):
params = [('/clitest/topic/pub_basic', False, True),
('/clitest/topic/pub_compatible_qos', True, True),
('/clitest/topic/pub_incompatible_qos', True, False)]
for topic, provide_qos, compatible_qos in params:
with self.subTest(topic=topic,
provide_qos=provide_qos,
compatible_qos=compatible_qos):
# Check for inconsistent arguments
assert provide_qos if not compatible_qos else True
received_message_count = 0
expected_minimum_message_count = 1
expected_maximum_message_count = 5
pub_extra_options = []
subscription_qos_profile = 10
if provide_qos:
if compatible_qos:
# For compatible test, put publisher at very high quality
# and subscription at low
pub_extra_options = [
'--qos-reliability', 'reliable',
'--qos-durability', 'transient_local'
]
subscription_qos_profile = QoSProfile(
depth=10,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE)
else:
# For an incompatible example, reverse the quality extremes
# and expect no messages to arrive
pub_extra_options = [
'--qos-reliability', 'best_effort',
'--qos-durability', 'volatile'
]
subscription_qos_profile = QoSProfile(
depth=10,
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
expected_maximum_message_count = 0
expected_minimum_message_count = 0
future = rclpy.task.Future()
def message_callback(msg):
"""If we receive one message, the test has succeeded."""
nonlocal received_message_count
received_message_count += 1
future.set_result(True)
subscription = self.node.create_subscription(
String, topic, message_callback, subscription_qos_profile)
assert subscription
try:
command_action = ExecuteProcess(
cmd=(['ros2', 'topic', 'pub'] + pub_extra_options +
[topic, 'std_msgs/String', 'data: hello']),
additional_env={'PYTHONUNBUFFERED': '1'},
output='screen')
with launch_testing.tools.launch_process(
launch_service,
command_action,
proc_info,
proc_output,
output_filter=launch_testing_ros.tools.
basic_output_filter(
filtered_rmw_implementation=
get_rmw_implementation_identifier(
))) as command:
self.executor.spin_until_future_complete(
future, timeout_sec=10)
command.wait_for_shutdown(timeout=10)
# Check results
assert (
received_message_count >= expected_minimum_message_count and
received_message_count <= expected_maximum_message_count), \
('Received {} messages from pub on {},'
'which is not in expected range {}-{}').format(
received_message_count, topic,
expected_minimum_message_count,
expected_maximum_message_count
)
finally:
# Cleanup
self.node.destroy_subscription(subscription)
def generate_launch_description():
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
))
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf")
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/kinematics.yaml")
robot_description_kinematics = {
"robot_description_kinematics": kinematics_yaml
}
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters":
"""default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
"moveit_resources_panda_moveit_config",
"config/panda_controllers.yaml")
moveit_controllers = {
"moveit_simple_controller_manager":
moveit_simple_controllers_yaml,
"moveit_controller_manager":
"moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
# Start the actual move_group node/action server
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
],
)
# RViz
rviz_config_file = (
get_package_share_directory("run_ompl_constrained_planning") +
"/launch/run_move_group.rviz")
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml,
],
)
# Static TF
static_tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=[
"0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"
],
)
# Publish TF
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_controller"]:
load_controllers += [
ExecuteProcess(
cmd=[
"ros2 run controller_manager spawner.py {}".format(
controller)
],
shell=True,
output="screen",
)
]
# Warehouse mongodb server
mongodb_server_node = Node(
package="warehouse_ros_mongo",
executable="mongo_wrapper_ros.py",
parameters=[
{
"warehouse_port": 33829
},
{
"warehouse_host": "localhost"
},
{
"warehouse_plugin":
"warehouse_ros_mongo::MongoDatabaseConnection"
},
],
output="screen",
)
return LaunchDescription([
rviz_node,
static_tf,
robot_state_publisher,
run_move_group_node,
ros2_control_node,
mongodb_server_node,
] + load_controllers)
def generate_launch_description():
package_name = 'ifm3d_ros2'
node_namespace = 'ifm3d'
node_name = 'camera'
node_exe = 'camera_standalone'
# os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \
# "[{%s}] {%s} [{%s}]: {%s}\n({%s}() at {%s}:{%s})" % \
# ("severity", "time", "name", "message",
# "function_name", "file_name", "line_number")
os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \
"[{%s}] {%s} [{%s}]: {%s}" % ("severity", "time", "name", "message")
# XXX: This is a hack, there does not seem to be a nice way (or at least
# finding the docs is not obvious) to do this with the ROS2 launch api
#
# Basically, we are trying to allow for passing through the command line
# args to the launch file through to the node executable itself (like ROS
# 1).
#
# My assumption is that either:
# 1. This stuff exists somewhere in ROS2 and I don't know about it yet
# 2. This stuff will exist in ROS2 soon, so, this will likely get factored
# out (hopefully soon)
#
parameters = []
remaps = []
for arg in sys.argv:
if ':=' in arg:
split_arg = arg.split(sep=':=', maxsplit=1)
assert len(split_arg) == 2
if arg.startswith("ns"):
node_namespace = split_arg[1]
elif arg.startswith("node"):
node_name = split_arg[1]
elif arg.startswith("params"):
parameters.append(tuple(split_arg)[1])
else:
remaps.append(tuple(split_arg))
def add_prefix(tup):
assert len(tup) == 2
if node_namespace.startswith("/"):
prefix = "%s/%s" % (node_namespace, node_name)
else:
prefix = "/%s/%s" % (node_namespace, node_name)
retval = [None, None]
if not tup[0].startswith(prefix):
retval[0] = prefix + '/' + tup[0]
else:
retval[0] = tup[0]
if not tup[1].startswith(prefix):
retval[1] = prefix + '/' + tup[1]
else:
retval[1] = tup[1]
return tuple(retval)
remaps = list(map(add_prefix, remaps))
#------------------------------------------------------------
# Nodes
#------------------------------------------------------------
#
# Coord frame transform from camera_optical_link to camera_link
#
tf_node = \
ExecuteProcess(
cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher',
'0', '0', '0', str(-pi/2.), '0', str(-pi/2.),
str(node_name + "_link"), str(node_name + "_optical_link")],
#output='screen',
log_cmd=True
)
#
# The camera component
#
camera_node = \
LifecycleNode(
package=package_name,
node_executable=node_exe,
node_namespace=node_namespace,
node_name=node_name,
output='screen',
parameters=parameters,
remappings=remaps,
log_cmd=True,
)
#------------------------------------------------------------
# Events we need to emit to induce state transitions
#------------------------------------------------------------
camera_configure_evt = \
EmitEvent(
event=ChangeState(
lifecycle_node_matcher = \
launch.events.matches_action(camera_node),
transition_id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE
)
)
camera_activate_evt = \
EmitEvent(
event=ChangeState(
lifecycle_node_matcher = \
launch.events.matches_action(camera_node),
transition_id = lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE
)
)
camera_cleanup_evt = \
EmitEvent(
event=ChangeState(
lifecycle_node_matcher = \
launch.events.matches_action(camera_node),
transition_id = lifecycle_msgs.msg.Transition.TRANSITION_CLEANUP
)
)
camera_shutdown_evt = EmitEvent(event=launch.events.Shutdown())
#------------------------------------------------------------
# These are the edges of the state machine graph we want to autonomously
# manage
#------------------------------------------------------------
#
# unconfigured -> configuring -> inactive
#
camera_node_unconfigured_to_inactive_handler = \
RegisterEventHandler(
OnStateTransition(
target_lifecycle_node = camera_node,
start_state = 'configuring',
goal_state = 'inactive',
entities = [
LogInfo(msg = "Emitting 'TRANSITION_ACTIVATE' event"),
camera_activate_evt,
],
)
)
#
# active -> deactivating -> inactive
#
camera_node_active_to_inactive_handler = \
RegisterEventHandler(
OnStateTransition(
target_lifecycle_node = camera_node,
start_state = 'deactivating',
goal_state = 'inactive',
entities = [
LogInfo(msg = "Emitting 'TRANSITION_CLEANUP' event"),
camera_cleanup_evt,
],
)
)
#
# inactive -> cleaningup -> unconfigured
#
camera_node_inactive_to_unconfigured_handler = \
RegisterEventHandler(
OnStateTransition(
target_lifecycle_node = camera_node,
start_state = 'cleaningup',
goal_state = 'unconfigured',
entities = [
LogInfo(msg = "Emitting 'TRANSITION_CONFIGURE' event"),
camera_configure_evt,
],
)
)
#
# * -> errorprocessing -> unconfigured
#
camera_node_errorprocessing_to_unconfigured_handler = \
RegisterEventHandler(
OnStateTransition(
target_lifecycle_node = camera_node,
start_state = 'errorprocessing',
goal_state = 'unconfigured',
entities = [
LogInfo(msg = "Emitting 'TRANSITION_CONFIGURE' event"),
camera_configure_evt,
],
)
)
#
# * -> shuttingdown -> finalized
#
camera_node_shuttingdown_to_finalized_handler = \
RegisterEventHandler(
OnStateTransition(
target_lifecycle_node = camera_node,
start_state = 'shuttingdown',
goal_state = 'finalized',
entities = [
LogInfo(msg = "Emitting 'SHUTDOWN' event"),
camera_shutdown_evt,
],
)
)
#------------------------------------------------------------
# Now, add all the actions to a launch description and return it
#------------------------------------------------------------
ld = LaunchDescription()
ld.add_action(camera_node_unconfigured_to_inactive_handler)
ld.add_action(camera_node_active_to_inactive_handler)
ld.add_action(camera_node_inactive_to_unconfigured_handler)
ld.add_action(camera_node_errorprocessing_to_unconfigured_handler)
ld.add_action(camera_node_shuttingdown_to_finalized_handler)
ld.add_action(tf_node)
ld.add_action(camera_node)
ld.add_action(camera_configure_evt)
return ld
def generate_test_description():
launch_description = LaunchDescription()
launch_description.add_action(
ExecuteProcess(cmd=[
os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')
],
name='test_dump_params'))
launch_description.add_action(
ExecuteProcess(cmd=[
os.path.join(os.path.dirname(__file__),
'test_dump_params_node.py'), 'test_dump_params_copy'
],
name='test_dump_params_copy'))
processes_to_test = [
ExecuteProcess(cmd=[os.getenv('TEST_EXECUTABLE'), '-h'],
name='test_dump_params_help',
output='screen'),
ExecuteProcess(cmd=[os.getenv('TEST_EXECUTABLE')],
name='test_dump_params_default',
output='screen'),
ExecuteProcess(
cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params'],
name='test_dump_params_yaml',
output='screen'),
ExecuteProcess(cmd=[
os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params'
],
name='test_dump_params_markdown',
output='screen'),
ExecuteProcess(
cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params', '-v'],
name='test_dump_params_yaml_verbose',
output='screen'),
ExecuteProcess(cmd=[
os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params',
'-v'
],
name='test_dump_params_markdown_verbose',
output='screen'),
ExecuteProcess(cmd=[
os.getenv('TEST_EXECUTABLE'), '-f', 'error', '-n',
'test_dump_params'
],
name='test_dump_params_bad_format',
output='screen'),
ExecuteProcess(cmd=[
os.getenv('TEST_EXECUTABLE'), '-n',
'test_dump_params,test_dump_params_copy'
],
name='test_dump_params_multiple',
output='screen'),
ExecuteProcess(
cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params_error'],
name='test_dump_params_error',
output='screen')
]
for process in processes_to_test:
launch_description.add_action(process)
launch_description.add_action(launch_testing.actions.ReadyToTest())
return launch_description, {'processes_to_test': processes_to_test}
def generate_launch_description():
description = LaunchDescription()
module_toggles = {
'sensors': {
'arducam': False,
'msp_bridge': False,
'vectornav': True
},
'autonomy': {},
'utilities': {
'image_viewer': False
}
}
description.add_action(
LogInfo(msg=["===================================="]))
for modules in module_toggles.values():
for module_name, enabled in modules.items():
description.add_action(
LogInfo(msg=[
module_name, ': ', "Enabled" if enabled else "Disabled"
]))
description.add_action(
LogInfo(msg=["===================================="]))
description.add_action(
DeclareLaunchArgument(name='enable_rosbag_logging',
default_value='True',
description='Enable rosbag2 logging'))
description.add_action(
DeclareLaunchArgument(name='enable_ros1_bridge',
default_value='True',
description='Enable ros1 bridge'))
description.add_action(
DeclareLaunchArgument(name='enable_rviz',
default_value='True',
description='Enable rviz2 gui'))
# Rosbag2 Logging
# Add more topics here
topics = ['/acrobat/imu', '/acrobat/camera', '/acrobat/fc_imu']
description.add_action(
ExecuteProcess(cmd=['ros2', 'bag', 'record', '--no-discovery'] +
topics,
name='rosbag2',
cwd='/home/martin/rosbag',
output='screen',
emulate_tty=True,
condition=IfCondition(get_toggle('rosbag_logging'))))
# Launch ROS1 Bridge
description.add_action(
Node(package='ros1_bridge',
node_executable='dynamic_bridge',
node_name='ros1_bridge',
output='screen',
emulate_tty=True,
condition=IfCondition(get_toggle('ros1_bridge'))))
# Launch RViz2
description.add_action(
Node(package='rviz2',
node_executable='rviz2',
node_name='rviz',
output='screen',
emulate_tty=True,
condition=IfCondition(get_toggle('rviz'))))
sensor_nodes = get_acrobat_sensor_nodes(module_toggles['sensors'])
autonomy_nodes = get_acrobat_autonomy_nodes(module_toggles['autonomy'])
utility_nodes = get_acrobat_utility_nodes(module_toggles['utilities'])
acrobat_node_container = ComposableNodeContainer(
node_name='acrobat_container',
node_namespace='',
package='rclcpp_components',
node_executable='component_container',
composable_node_descriptions=sensor_nodes + autonomy_nodes +
utility_nodes,
output='screen',
emulate_tty=True,
)
description.add_action(acrobat_node_container)
return description
def generate_move_group_test_description(*args,
gtest_name: SomeSubstitutionsType):
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
))
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf")
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/kinematics.yaml")
robot_description_kinematics = {
"robot_description_kinematics": kinematics_yaml
}
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters":
"""default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/ompl_planning.yaml")
ompl_planning_yaml["panda_arm"]["enforce_constrained_state_space"] = True
ompl_planning_yaml["panda_arm"][
"projection_evaluator"] = "joints(panda_joint1,panda_joint2)"
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
"moveit_resources_panda_moveit_config",
"config/panda_controllers.yaml")
moveit_controllers = {
"moveit_simple_controller_manager":
moveit_simple_controllers_yaml,
"moveit_controller_manager":
"moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.5,
"trajectory_execution.allowed_goal_duration_margin": 1.0,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
# Start the actual move_group node/action server
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
],
)
# Static TF
static_tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=[
"0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"
],
)
# Publish TF
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_controller"]:
load_controllers += [
ExecuteProcess(
cmd=[
"ros2 run controller_manager spawner.py {}".format(
controller)
],
shell=True,
output="screen",
)
]
# test executable
ompl_constraint_test = launch_ros.actions.Node(
executable=PathJoinSubstitution(
[LaunchConfiguration("test_binary_dir"), gtest_name]),
parameters=[
robot_description, robot_description_semantic, kinematics_yaml
],
output="screen",
)
return launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(
name="test_binary_dir",
description="Binary directory of package "
"containing test executables",
),
run_move_group_node,
static_tf,
robot_state_publisher,
ros2_control_node,
TimerAction(period=2.0, actions=[ompl_constraint_test]),
launch_testing.actions.ReadyToTest(),
] + load_controllers), {
"run_move_group_node": run_move_group_node,
"static_tf": static_tf,
"robot_state_publisher": robot_state_publisher,
"ros2_control_node": ros2_control_node,
"ompl_constraint_test": ompl_constraint_test,
}
def generate_launch_description():
# moveit_cpp.yaml is passed by filename for now since it's node specific
moveit_cpp_yaml_file_name = (
get_package_share_directory("run_moveit_cpp") + "/config/moveit_cpp.yaml"
)
# Component yaml files are grouped in separate namespaces
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
)
)
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
moveit_simple_controllers_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/panda_controllers.yaml"
)
moveit_controllers = {
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
ompl_planning_pipeline_config = {
"planning_pipelines": ["ompl"],
"ompl": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
},
}
ompl_planning_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/ompl_planning.yaml"
)
ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml)
# MoveItCpp demo executable
run_moveit_cpp_node = Node(
name="run_moveit_cpp",
package="run_moveit_cpp",
# TODO(henningkayser): add debug argument
# prefix='xterm -e gdb --args',
executable="run_moveit_cpp",
output="screen",
parameters=[
moveit_cpp_yaml_file_name,
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
moveit_controllers,
],
)
# RViz
rviz_config_file = (
get_package_share_directory("run_moveit_cpp") + "/launch/run_moveit_cpp.rviz"
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[robot_description, robot_description_semantic],
)
# Static TF
static_tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
)
# Publish TF
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in [
"panda_arm_controller",
"panda_hand_controller",
"joint_state_controller",
]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
shell=True,
output="screen",
)
]
return LaunchDescription(
[
static_tf,
robot_state_publisher,
rviz_node,
run_moveit_cpp_node,
ros2_control_node,
]
+ load_controllers
)
def generate_launch_description():
map_yaml_file = os.getenv('TEST_MAP')
filter_mask_file = os.getenv('TEST_MASK')
world = os.getenv('TEST_WORLD')
bt_navigator_xml = os.path.join(
get_package_share_directory('nav2_bt_navigator'), 'behavior_trees',
os.getenv('BT_NAVIGATOR_XML'))
bringup_dir = get_package_share_directory('nav2_bringup')
script_dir = os.path.dirname(os.path.realpath(__file__))
params_file = os.path.join(script_dir, 'keepout_params.yaml')
# Replace the `use_astar` setting on the params file
param_substitutions = {
'planner_server.ros__parameters.GridBased.use_astar':
os.getenv('ASTAR'),
'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file,
'yaml_filename': filter_mask_file
}
configured_params = RewrittenYaml(source_file=params_file,
root_key='',
param_rewrites=param_substitutions,
convert_types=True)
return LaunchDescription([
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
# Launch gazebo server for simulation
ExecuteProcess(cmd=[
'gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world
],
output='screen'),
# TODO(orduno) Launch the robot state publisher instead
# using a local copy of TB3 urdf file
Node(package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=[
'0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'
]),
Node(
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'base_link',
'base_scan']),
Node(package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_filters',
output='screen',
parameters=[{
'node_names':
['filter_mask_server', 'costmap_filter_info_server']
}, {
'autostart': True
}]),
# Nodes required for Costmap Filters configuration
Node(package='nav2_map_server',
executable='map_server',
name='filter_mask_server',
output='screen',
parameters=[configured_params]),
Node(package='nav2_map_server',
executable='costmap_filter_info_server',
name='costmap_filter_info_server',
output='screen',
parameters=[configured_params]),
IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
launch_arguments={
'namespace': '',
'use_namespace': 'False',
'map': map_yaml_file,
'use_sim_time': 'True',
'params_file': configured_params,
'bt_xml_file': bt_navigator_xml,
'autostart': 'True'
}.items()),
])
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
)
gazebo_ros2_control_demos_path = os.path.join(
get_package_share_directory('gazebo_ros2_control_demos'))
xacro_file = os.path.join(gazebo_ros2_control_demos_path,
'urdf',
'test_cart_effort.xacro.urdf')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
params = {'robot_description': doc.toxml()}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params]
)
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'cartpole'],
output='screen')
load_joint_state_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
'joint_state_broadcaster'],
output='screen'
)
load_joint_trajectory_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'effort_controllers'],
output='screen'
)
return LaunchDescription([
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_broadcaster],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
<<<<<<< HEAD
target_action=load_joint_state_broadcaster,
on_exit=[load_effort_controller],
=======
target_action=load_joint_state_controller,
on_exit=[load_joint_trajectory_controller],
>>>>>>> 895ade63dc26f2cbecef0396d82eeaf4d1493d08
)
),
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def test_echo_basic(self, launch_service, proc_info, proc_output):
params = [
('/clitest/topic/echo_basic', False, True, False),
('/clitest/topic/echo_compatible_qos', True, True, False),
('/clitest/topic/echo_incompatible_qos', True, False, False),
('/clitest/topic/echo_message_lost', False, True, True),
]
for topic, provide_qos, compatible_qos, message_lost in params:
with self.subTest(topic=topic,
provide_qos=provide_qos,
compatible_qos=compatible_qos):
# Check for inconsistent arguments
assert provide_qos if not compatible_qos else True
echo_extra_options = []
publisher_qos_profile = 10
if provide_qos:
if compatible_qos:
# For compatible test, put publisher at very high quality
# and subscription at low
echo_extra_options = [
'--qos-reliability', 'best_effort',
'--qos-durability', 'volatile'
]
publisher_qos_profile = QoSProfile(
depth=10,
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
else:
# For an incompatible example, reverse the quality extremes
# and expect no messages to arrive
echo_extra_options = [
'--qos-reliability', 'reliable',
'--qos-durability', 'transient_local'
]
publisher_qos_profile = QoSProfile(
depth=10,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE)
if message_lost:
echo_extra_options.append('--lost-messages')
publisher = self.node.create_publisher(String, topic,
publisher_qos_profile)
assert publisher
def publish_message():
publisher.publish(String(data='hello'))
publish_timer = self.node.create_timer(0.5, publish_message)
try:
command_action = ExecuteProcess(
cmd=(['ros2', 'topic', 'echo'] + echo_extra_options +
[topic, 'std_msgs/String']),
additional_env={'PYTHONUNBUFFERED': '1'},
output='screen')
with launch_testing.tools.launch_process(
launch_service,
command_action,
proc_info,
proc_output,
output_filter=launch_testing_ros.tools.
basic_output_filter(
filtered_rmw_implementation=
get_rmw_implementation_identifier(
))) as command:
# The future won't complete - we will hit the timeout
self.executor.spin_until_future_complete(
rclpy.task.Future(), timeout_sec=5)
command.wait_for_shutdown(timeout=10)
# Check results
if compatible_qos:
# TODO(ivanpauno): remove special case when FastRTPS implements the feature
# https://github.com/ros2/rmw_fastrtps/issues/395
assert command.output, 'Echo CLI printed no output'
if message_lost and 'rmw_fastrtps' in get_rmw_implementation_identifier(
):
assert 'does not support reporting lost messages' in command.output
assert get_rmw_implementation_identifier(
) in command.output
return
assert 'data: hello' in command.output.splitlines(), (
'Echo CLI did not print expected message')
else:
# TODO(mm318): remove special case for FastRTPS when
# https://github.com/ros2/rmw_fastrtps/issues/356 is resolved
if 'rmw_fastrtps' in get_rmw_implementation_identifier(
):
assert not command.output, (
'Echo CLI should not have received anything with incompatible QoS'
)
else:
assert command.output, (
'Echo CLI did not print incompatible QoS warning'
)
assert (
'New publisher discovered on this topic, offering incompatible'
' QoS.' in command.output
), ('Echo CLI did not print expected incompatible QoS warning'
)
finally:
# Cleanup
self.node.destroy_timer(publish_timer)
self.node.destroy_publisher(publisher)
def generate_launch_description():
urdf1 = os.path.join(get_package_share_directory('tello_description'),
'urdf', 'tello_1.urdf')
urdf2 = os.path.join(get_package_share_directory('tello_description'),
'urdf', 'tello_2.urdf')
dr1_ns = 'drone1'
dr2_ns = 'drone2'
dr1_params = [{
'drone_ip': '192.168.86.206',
'command_port': 11001,
'drone_port': 12001,
'data_port': 13001,
'video_port': 14001
}]
dr2_params = [{
'drone_ip': '192.168.86.212',
'command_port': 11002,
'drone_port': 12002,
'data_port': 13002,
'video_port': 14002
}]
base_params = [{'drones': [dr1_ns, dr2_ns]}]
filter1_params = [{'map_frame': 'map', 'base_frame': 'base1'}]
filter2_params = [{'map_frame': 'map', 'base_frame': 'base2'}]
return LaunchDescription([
# Rviz
ExecuteProcess(
cmd=['rviz2', '-d', 'install/flock2/share/flock2/launch/two.rviz'],
output='screen'),
# Publish N sets of static transforms
Node(package='robot_state_publisher',
node_executable='robot_state_publisher',
output='screen',
arguments=[urdf1]),
Node(package='robot_state_publisher',
node_executable='robot_state_publisher',
output='screen',
arguments=[urdf2]),
# N drivers
Node(package='tello_driver',
node_executable='tello_driver_main',
output='screen',
node_name='driver1',
node_namespace=dr1_ns,
parameters=dr1_params),
Node(package='tello_driver',
node_executable='tello_driver_main',
output='screen',
node_name='driver2',
node_namespace=dr2_ns,
parameters=dr2_params),
# Joystick
Node(package='joy', node_executable='joy_node', output='screen'),
# Flock controller
Node(package='flock2',
node_executable='flock_base',
output='screen',
node_name='flock_base',
parameters=base_params),
# N drone controllers
Node(package='flock2',
node_executable='drone_base',
output='screen',
node_name='base1',
node_namespace=dr1_ns),
Node(package='flock2',
node_executable='drone_base',
output='screen',
node_name='base2',
node_namespace=dr2_ns),
# Mapper
Node(package='fiducial_vlam',
node_executable='vmap_main',
output='screen'),
# N visual localizers
Node(package='fiducial_vlam',
node_executable='vloc_main',
output='screen',
node_name='vloc1',
node_namespace=dr1_ns),
Node(package='fiducial_vlam',
node_executable='vloc_main',
output='screen',
node_name='vloc2',
node_namespace=dr2_ns),
])
def generate_launch_description():
# Get parameters for the Servo node
servo_yaml = load_yaml("moveit_servo",
"config/panda_simulated_config.yaml")
servo_params = {"moveit_servo": servo_yaml}
# Get URDF and SRDF
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
))
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf")
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
# RViz
rviz_config_file = (get_package_share_directory("moveit2_tutorials") +
"/config/demo_rviz_config.rviz")
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[robot_description, robot_description_semantic],
)
# ros2_control using FakeSystem as hardware
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[
robot_description,
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
),
],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_broadcaster"]:
load_controllers += [
ExecuteProcess(
cmd=[
"ros2 run controller_manager spawner {}".format(controller)
],
shell=True,
output="screen",
)
]
# Launch as much as possible in components
container = ComposableNodeContainer(
name="moveit_servo_demo_container",
namespace="/",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
ComposableNode(
package="robot_state_publisher",
plugin="robot_state_publisher::RobotStatePublisher",
name="robot_state_publisher",
parameters=[robot_description],
),
ComposableNode(
package="tf2_ros",
plugin="tf2_ros::StaticTransformBroadcasterNode",
name="static_tf2_broadcaster",
parameters=[{
"/child_frame_id": "panda_link0",
"/frame_id": "world"
}],
),
ComposableNode(
package="moveit_servo",
plugin="moveit_servo::ServoServer",
name="servo_server",
parameters=[
servo_params,
robot_description,
robot_description_semantic,
],
extra_arguments=[{
"use_intra_process_comms": True
}],
),
],
output="screen",
)
return LaunchDescription([rviz_node, ros2_control_node, container] +
load_controllers)
def generate_launch_description():
map_yaml_file = os.getenv('TEST_MAP')
world = os.getenv('TEST_WORLD')
urdf = os.getenv('TEST_URDF')
sdf = os.getenv('TEST_SDF')
bt_xml_file = os.path.join(
get_package_share_directory('nav2_bt_navigator'), 'behavior_trees',
os.getenv('BT_NAVIGATOR_XML'))
bringup_dir = get_package_share_directory('nav2_bringup')
robot1_params_file = os.path.join(
bringup_dir, # noqa: F841
'params/nav2_multirobot_params_1.yaml')
robot2_params_file = os.path.join(
bringup_dir, # noqa: F841
'params/nav2_multirobot_params_2.yaml')
# Names and poses of the robots
robots = [{
'name': 'robot1',
'x_pose': 0.0,
'y_pose': 0.5,
'z_pose': 0.01
}, {
'name': 'robot2',
'x_pose': 0.0,
'y_pose': -0.5,
'z_pose': 0.01
}]
# Launch Gazebo server for simulation
start_gazebo_cmd = ExecuteProcess(cmd=[
'gzserver', '-s', 'libgazebo_ros_factory.so', '--minimal_comms', world
],
output='screen')
# Define commands for spawing the robots into Gazebo
spawn_robots_cmds = []
for robot in robots:
spawn_robots_cmds.append(
Node(package='nav2_gazebo_spawner',
executable='nav2_gazebo_spawner',
output='screen',
arguments=[
'--robot_name',
TextSubstitution(text=robot['name']), '--robot_namespace',
TextSubstitution(text=robot['name']), '--sdf',
TextSubstitution(text=sdf), '-x',
TextSubstitution(text=str(robot['x_pose'])), '-y',
TextSubstitution(text=str(robot['y_pose'])), '-z',
TextSubstitution(text=str(robot['z_pose']))
]))
# Define commands for launching the robot state publishers
robot_state_pubs_cmds = []
for robot in robots:
robot_state_pubs_cmds.append(
Node(package='robot_state_publisher',
executable='robot_state_publisher',
namespace=TextSubstitution(text=robot['name']),
output='screen',
parameters=[{
'use_sim_time': 'True'
}],
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
arguments=[urdf]))
# Define commands for launching the navigation instances
nav_instances_cmds = []
for robot in robots:
params_file = eval(f"{robot['name']}_params_file")
group = GroupAction([
# Instances use the robot's name for namespace
PushRosNamespace(robot['name']),
IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
launch_arguments={
'namespace': robot['name'],
'map': map_yaml_file,
'use_sim_time': 'True',
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'autostart': 'True',
'use_remappings': 'True'
}.items())
])
nav_instances_cmds.append(group)
ld = LaunchDescription()
ld.add_action(
SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), )
ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), )
ld.add_action(start_gazebo_cmd)
for spawn_robot in spawn_robots_cmds:
ld.add_action(spawn_robot)
for state_pub in robot_state_pubs_cmds:
ld.add_action(state_pub)
for nav_instance in nav_instances_cmds:
ld.add_action(nav_instance)
return ld
def generate_launch_description():
# Get the launch directory
social_bringup_dir = get_package_share_directory('social_nav2_bringup')
nav_bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(nav_bringup_dir, 'launch')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
autostart = LaunchConfiguration('autostart')
use_remappings = LaunchConfiguration('use_remappings')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_simulator = LaunchConfiguration('use_simulator')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')
headless = LaunchConfiguration('headless')
world = LaunchConfiguration('world')
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56
remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace', default_value='', description='Top-level namespace')
declare_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(social_bringup_dir, 'maps',
'turtlebot3_house.yaml'),
description='Full path to map file to load')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_use_remappings_cmd = DeclareLaunchArgument(
'use_remappings',
default_value='false',
description='Arguments to pass to all nodes launched by the file')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(social_bringup_dir, 'params',
'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launched nodes')
declare_bt_xml_cmd = DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(
get_package_share_directory('social_nav2_bringup'),
'behavior_trees', 'follow_point.xml'),
description='Full path to the behavior tree xml file to use')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart',
default_value='true',
description='Automatically startup the nav2 stack')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join(social_bringup_dir, 'rviz',
'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use')
declare_use_simulator_cmd = DeclareLaunchArgument(
'use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
'use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ')
declare_simulator_cmd = DeclareLaunchArgument(
'headless',
default_value='False',
description='Whether to execute gzclient)')
declare_world_cmd = DeclareLaunchArgument(
'world',
# TODO(orduno) Switch back once ROS argument passing has been fixed upstream
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
# default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
# 'worlds/turtlebot3_worlds/waffle.model'),
default_value=os.path.join(social_bringup_dir, 'worlds',
'waffle_house.model'),
description='Full path to world model file to load')
# Specify the actions
start_gazebo_server_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=[
'gzserver', '-s', 'libgazebo_ros_init.so', '-s',
'libgazebo_ros_factory.so', world
],
cwd=[launch_dir],
output='screen')
start_gazebo_client_cmd = ExecuteProcess(condition=IfCondition(
PythonExpression([use_simulator, ' and not ', headless])),
cmd=['gzclient'],
cwd=[launch_dir],
output='screen')
urdf = os.path.join(social_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
namespace=namespace,
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}],
remappings=remappings,
arguments=[urdf])
start_rviz_cmd = Node(
condition=IfCondition(use_rviz),
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_file],
output='screen',
)
exit_event_handler = RegisterEventHandler(
event_handler=OnProcessExit(target_action=start_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(
reason='rviz exited'))))
agent_spawner_cmd = Node(package='pedsim_gazebo_plugin',
executable='spawn_pedsim_agents.py',
name='spawn_pedsim_agents',
output='screen')
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_use_remappings_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any conditioned actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
ld.add_action(agent_spawner_cmd)
ld.add_action(start_rviz_cmd)
ld.add_action(exit_event_handler)
# Add the actions to launch all of the navigation nodes
ld.add_action(start_robot_state_publisher_cmd)
return ld
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(bringup_dir, 'launch')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
autostart = LaunchConfiguration('autostart')
use_remappings = LaunchConfiguration('use_remappings')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_simulator = LaunchConfiguration('use_simulator')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')
simulator = LaunchConfiguration('simulator')
world = LaunchConfiguration('world')
# TODO(orduno) Remove once `PushNodeRemapping` is resolved
# https://github.com/ros2/launch_ros/issues/56
remappings = [((namespace, '/tf'), '/tf'),
((namespace, '/tf_static'), '/tf_static'), ('/scan', 'scan'),
('/tf', 'tf'), ('/tf_static', 'tf_static'),
('/cmd_vel', 'cmd_vel'), ('/map', 'map')]
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace', default_value='', description='Top-level namespace')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(bringup_dir, 'maps',
'turtlebot3_world.yaml'),
description='Full path to map file to load')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launched nodes')
declare_bt_xml_cmd = DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(
get_package_share_directory('nav2_bt_navigator'), 'behavior_trees',
'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart',
default_value='false',
description='Automatically startup the nav2 stack')
declare_use_remappings_cmd = DeclareLaunchArgument(
'use_remappings',
default_value='false',
description='Arguments to pass to all nodes launched by the file')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join(bringup_dir, 'rviz',
'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use')
declare_use_simulator_cmd = DeclareLaunchArgument(
'use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
'use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ')
declare_simulator_cmd = DeclareLaunchArgument(
'simulator',
default_value='gazebo',
description='The simulator to use (gazebo or gzserver)')
declare_world_cmd = DeclareLaunchArgument(
'world',
# TODO(orduno) Switch back once ROS argument passing has been fixed upstream
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
# default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
# 'worlds/turtlebot3_worlds/waffle.model'),
default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'),
description='Full path to world model file to load')
# Specify the actions
start_gazebo_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=[simulator, '-s', 'libgazebo_ros_init.so', world],
cwd=[launch_dir],
output='screen')
urdf = os.path.join(get_package_share_directory('turtlebot3_description'),
'urdf', 'turtlebot3_waffle.urdf')
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
node_executable='robot_state_publisher',
node_name='robot_state_publisher',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}],
use_remappings=IfCondition(use_remappings),
remappings=remappings,
arguments=[urdf])
start_rviz_cmd = Node(condition=IfCondition(use_rviz),
package='rviz2',
node_executable='rviz2',
node_name='rviz2',
arguments=['-d', rviz_config_file],
output='screen',
use_remappings=IfCondition(use_remappings),
remappings=[('/tf', 'tf'),
('/tf_static', 'tf_static'),
('goal_pose', 'goal_pose'),
('/clicked_point', 'clicked_point'),
('/initialpose', 'initialpose')])
exit_event_handler = RegisterEventHandler(
event_handler=OnProcessExit(target_action=start_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(
reason='rviz exited'))))
bringup_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'nav2_bringup_launch.py')),
launch_arguments={
'namespace': namespace,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'autostart': autostart,
'use_remappings': use_remappings
}.items())
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_use_remappings_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any conditioned actions
ld.add_action(start_gazebo_cmd)
ld.add_action(start_rviz_cmd)
# Add other nodes and processes we need
ld.add_action(exit_event_handler)
# Add the actions to launch all of the navigation nodes
ld.add_action(start_robot_state_publisher_cmd)
ld.add_action(bringup_cmd)
return ld
def generate_test_description(rmw_implementation):
path_to_fixtures = os.path.join(os.path.dirname(__file__), 'fixtures')
additional_env = {
'RMW_IMPLEMENTATION': rmw_implementation, 'PYTHONUNBUFFERED': '1'
}
path_to_talker_node_script = os.path.join(path_to_fixtures, 'talker_node.py')
path_to_listener_node_script = os.path.join(path_to_fixtures, 'listener_node.py')
hidden_talker_node_action = Node(
executable=sys.executable,
arguments=[path_to_talker_node_script],
remappings=[('chatter', '_hidden_chatter')],
additional_env=additional_env
)
talker_node_action = Node(
executable=sys.executable,
arguments=[path_to_talker_node_script],
additional_env=additional_env
)
listener_node_action = Node(
executable=sys.executable,
arguments=[path_to_listener_node_script],
remappings=[('chatter', 'chit_chatter')],
additional_env=additional_env
)
path_to_repeater_node_script = os.path.join(path_to_fixtures, 'repeater_node.py')
array_repeater_node_action = Node(
executable=sys.executable,
arguments=[path_to_repeater_node_script, 'test_msgs/msg/Arrays'],
name='array_repeater',
remappings=[('/array_repeater/output', '/arrays')],
output='screen',
additional_env=additional_env
)
defaults_repeater_node_action = Node(
executable=sys.executable,
arguments=[path_to_repeater_node_script, 'test_msgs/msg/Defaults'],
name='defaults_repeater',
remappings=[('/defaults_repeater/output', '/defaults')],
additional_env=additional_env,
)
bounded_sequences_repeater_node_action = Node(
executable=sys.executable,
arguments=[
path_to_repeater_node_script, 'test_msgs/msg/BoundedSequences'
],
name='bounded_sequences_repeater',
remappings=[('/bounded_sequences_repeater/output', '/bounded_sequences')],
additional_env=additional_env
)
unbounded_sequences_repeater_node_action = Node(
executable=sys.executable,
arguments=[
path_to_repeater_node_script, 'test_msgs/msg/UnboundedSequences'
],
name='unbounded_sequences_repeater',
remappings=[('/unbounded_sequences_repeater/output', '/unbounded_sequences')],
additional_env=additional_env
)
path_to_controller_node_script = os.path.join(path_to_fixtures, 'controller_node.py')
cmd_vel_controller_node_action = Node(
executable=sys.executable,
arguments=[path_to_controller_node_script],
additional_env=additional_env
)
return LaunchDescription([
# Always restart daemon to isolate tests.
ExecuteProcess(
cmd=['ros2', 'daemon', 'stop'],
name='daemon-stop',
on_exit=[
ExecuteProcess(
cmd=['ros2', 'daemon', 'start'],
name='daemon-start',
on_exit=[
# Add talker/listener pair.
talker_node_action,
listener_node_action,
# Add hidden talker.
hidden_talker_node_action,
# Add topic repeaters.
array_repeater_node_action,
defaults_repeater_node_action,
bounded_sequences_repeater_node_action,
unbounded_sequences_repeater_node_action,
# Add stamped data publisher.
cmd_vel_controller_node_action,
launch_testing.actions.ReadyToTest()
],
additional_env=additional_env
)
]
),
]), locals()
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
# Names and poses of the robots
robots = [{
'name': 'robot1',
'x_pose': 0.0,
'y_pose': 0.5,
'z_pose': 0.01
}, {
'name': 'robot2',
'x_pose': 0.0,
'y_pose': -0.5,
'z_pose': 0.01
}]
# Simulation settings
world = LaunchConfiguration('world')
simulator = LaunchConfiguration('simulator')
# On this example all robots are launched with the same settings
map_yaml_file = LaunchConfiguration('map')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
rviz_config_file = LaunchConfiguration('rviz_config')
log_settings = LaunchConfiguration('log_settings', default='true')
# Declare the launch arguments
declare_world_cmd = DeclareLaunchArgument(
'world',
default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'),
description='Full path to world file to load')
declare_simulator_cmd = DeclareLaunchArgument(
'simulator',
default_value='gazebo',
description='The simulator to use (gazebo or gzserver)')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(bringup_dir, 'maps',
'turtlebot3_world.yaml'),
description='Full path to map file to load')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launched nodes')
declare_bt_xml_cmd = DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(
get_package_share_directory('nav2_bt_navigator'), 'behavior_trees',
'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config',
default_value=os.path.join(bringup_dir, 'rviz',
'nav2_namespaced_view.rviz'),
description='Full path to the RVIZ config file to use')
# Start Gazebo with plugin providing the robot spawing service
start_gazebo_cmd = ExecuteProcess(
cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world],
output='screen')
# Define commands for spawing the robots into Gazebo
spawn_robots_cmds = []
for robot in robots:
spawn_robots_cmds.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch',
'spawn_robot_launch.py')),
launch_arguments={
'x_pose': TextSubstitution(text=str(robot['x_pose'])),
'y_pose': TextSubstitution(text=str(robot['y_pose'])),
'z_pose': TextSubstitution(text=str(robot['z_pose'])),
'robot_name': robot['name'],
'turtlebot_type': TextSubstitution(text='waffle')
}.items()))
# Define commands for launching the navigation instances
nav_instances_cmds = []
for robot in robots:
namespaced_rviz_config_file = ReplaceString(
source_file=rviz_config_file,
replacements={'<robot_namespace>': ('/' + robot['name'])})
group = GroupAction([
# TODO(orduno)
# Each `action.Node` within the `localization` and `navigation` launch
# files has two versions, one with the required remaps and another without.
# The `use_remappings` flag specifies which runs.
# A better mechanism would be to have a PushNodeRemapping() action:
# https://github.com/ros2/launch_ros/issues/56
# For more on why we're remapping topics, see the note below
# PushNodeRemapping(remappings)
# Instances use the robot's name for namespace
PushRosNamespace(robot['name']),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch',
'nav2_tb3_simulation_launch.py')),
launch_arguments={
# TODO(orduno) might not be necessary to pass the robot name
'namespace': robot['name'],
'map_yaml_file': map_yaml_file,
'use_sim_time': 'True',
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'autostart': 'False',
'use_remappings': 'True',
'rviz_config_file': namespaced_rviz_config_file,
'use_simulator': 'False'
}.items()),
LogInfo(condition=IfCondition(log_settings),
msg=['Launching ', robot['name']]),
LogInfo(condition=IfCondition(log_settings),
msg=[robot['name'], ' map yaml: ', map_yaml_file]),
LogInfo(condition=IfCondition(log_settings),
msg=[robot['name'], ' params yaml: ', params_file]),
LogInfo(condition=IfCondition(log_settings),
msg=[robot['name'], ' behavior tree xml: ', bt_xml_file]),
LogInfo(condition=IfCondition(log_settings),
msg=[
robot['name'], ' rviz config file: ',
namespaced_rviz_config_file
])
])
nav_instances_cmds.append(group)
# A note on the `remappings` variable defined above and the fact it's passed as a node arg.
# A few topics have fully qualified names (have a leading '/'), these need to be remapped
# to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# for multi-robot transforms:
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_rviz_config_file_cmd)
# Add the actions to start gazebo, robots and simulations
ld.add_action(start_gazebo_cmd)
for spawn_robot_cmd in spawn_robots_cmds:
ld.add_action(spawn_robot_cmd)
for simulation_instance_cmd in nav_instances_cmds:
ld.add_action(simulation_instance_cmd)
return ld
def generate_launch_description():
model, plugin, media = GazeboRosPaths.get_paths()
if 'GAZEBO_MODEL_PATH' in environ:
model += ':' + environ['GAZEBO_MODEL_PATH']
if 'GAZEBO_PLUGIN_PATH' in environ:
plugin += ':' + environ['GAZEBO_PLUGIN_PATH']
if 'GAZEBO_RESOURCE_PATH' in environ:
media += ':' + environ['GAZEBO_RESOURCE_PATH']
env = {
'GAZEBO_MODEL_PATH': model,
'GAZEBO_PLUGIN_PATH': plugin,
'GAZEBO_RESOURCE_PATH': media
}
return LaunchDescription([
DeclareLaunchArgument('world',
default_value='',
description='Specify world file name'),
DeclareLaunchArgument(
'version',
default_value='false',
description='Set "true" to output version information.'),
DeclareLaunchArgument(
'verbose',
default_value='false',
description='Set "true" to increase messages written to terminal.'
),
DeclareLaunchArgument(
'help',
default_value='false',
description='Set "true" to produce gzserver help message.'),
DeclareLaunchArgument(
'pause',
default_value='false',
description='Set "true" to start the server in a paused state.'),
DeclareLaunchArgument(
'physics',
default_value='',
description='Specify a physics engine (ode|bullet|dart|simbody).'),
DeclareLaunchArgument('play',
default_value='',
description='Play the specified log file.'),
DeclareLaunchArgument('record',
default_value='false',
description='Set "true" to record state data.'),
DeclareLaunchArgument(
'record_encoding',
default_value='',
description='Specify compression encoding format for log data '
'(zlib|bz2|txt).'),
DeclareLaunchArgument(
'record_path',
default_value='',
description='Absolute path in which to store state data.'),
DeclareLaunchArgument(
'record_period',
default_value='',
description='Specify recording period (seconds).'),
DeclareLaunchArgument('record_filter',
default_value='',
description='Specify recording filter '
'(supports wildcard and regular expression).'),
DeclareLaunchArgument(
'seed',
default_value='',
description='Start with a given a random number seed.'),
DeclareLaunchArgument(
'iters',
default_value='',
description='Specify number of iterations to simulate.'),
DeclareLaunchArgument(
'minimal_comms',
default_value='false',
description='Set "true" to reduce TCP/IP traffic output.'),
DeclareLaunchArgument(
'profile',
default_value='',
description='Specify physics preset profile name from the options '
'in the world file.'),
DeclareLaunchArgument(
'extra_gazebo_args',
default_value='',
description='Extra arguments to be passed to Gazebo'),
# Specific to gazebo_ros
DeclareLaunchArgument(
'gdb',
default_value='false',
description='Set "true" to run gzserver with gdb'),
DeclareLaunchArgument(
'valgrind',
default_value='false',
description='Set "true" to run gzserver with valgrind'),
DeclareLaunchArgument(
'init',
default_value='true',
description='Set "false" not to load "libgazebo_ros_init.so"'),
DeclareLaunchArgument(
'factory',
default_value='true',
description='Set "false" not to load "libgazebo_ros_factory.so"'),
# Wait for (https://github.com/ros-simulation/gazebo_ros_pkgs/pull/941)
# DeclareLaunchArgument('force_system', default_value='true',
# description='Set "false" not to load \
# "libgazebo_ros_force_system.so"'),
ExecuteProcess(
cmd=[
'gzserver',
# Pass through arguments to gzserver
LaunchConfiguration('world'),
' ',
_boolean_command('version'),
' ',
_boolean_command('verbose'),
' ',
_boolean_command('help'),
' ',
_boolean_command('pause'),
' ',
_arg_command('physics'),
' ',
LaunchConfiguration('physics'),
' ',
_arg_command('play'),
' ',
LaunchConfiguration('play'),
' ',
_boolean_command('record'),
' ',
_arg_command('record_encoding'),
' ',
LaunchConfiguration('record_encoding'),
' ',
_arg_command('record_path'),
' ',
LaunchConfiguration('record_path'),
' ',
_arg_command('record_period'),
' ',
LaunchConfiguration('record_period'),
' ',
_arg_command('record_filter'),
' ',
LaunchConfiguration('record_filter'),
' ',
_arg_command('seed'),
' ',
LaunchConfiguration('seed'),
' ',
_arg_command('iters'),
' ',
LaunchConfiguration('iters'),
' ',
_boolean_command('minimal_comms'),
_plugin_command('init'),
' ',
_plugin_command('factory'),
' ',
# Wait for (https://github.com/ros-simulation/gazebo_ros_pkgs/pull/941)
# _plugin_command('force_system'), ' ',
_arg_command('profile'),
' ',
LaunchConfiguration('profile'),
LaunchConfiguration('extra_gazebo_args'),
],
output='screen',
additional_env=env,
shell=True,
prefix=PythonExpression([
'"gdb -ex run --args" if "true" == "',
LaunchConfiguration('gdb'), '" else "valgrind" if "true" == "',
LaunchConfiguration('valgrind'), '" else ""'
]),
)
])
def generate_servo_test_description(
*args,
gtest_name: SomeSubstitutionsType,
start_position_path: SomeSubstitutionsType = ""):
# Get parameters using the demo config file
servo_yaml = load_yaml("moveit_servo",
"config/panda_simulated_config.yaml")
servo_params = {"moveit_servo": servo_yaml}
# Get URDF and SRDF
if start_position_path:
initial_positions_file = os.path.join(os.path.dirname(__file__),
start_position_path)
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
),
mappings={"initial_positions_file": initial_positions_file},
)
else:
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
))
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf")
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_controller"]:
load_controllers += [
ExecuteProcess(
cmd=[
"ros2 run controller_manager spawner.py {}".format(
controller)
],
shell=True,
output="screen",
)
]
# Component nodes for tf and Servo
test_container = ComposableNodeContainer(
name="test_servo_integration_container",
namespace="/",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
ComposableNode(
package="robot_state_publisher",
plugin="robot_state_publisher::RobotStatePublisher",
name="robot_state_publisher",
parameters=[robot_description],
),
ComposableNode(
package="tf2_ros",
plugin="tf2_ros::StaticTransformBroadcasterNode",
name="static_tf2_broadcaster",
parameters=[{
"/child_frame_id": "panda_link0",
"/frame_id": "world"
}],
),
ComposableNode(
package="moveit_servo",
plugin="moveit_servo::ServoServer",
name="servo_server",
parameters=[
servo_params,
robot_description,
robot_description_semantic,
],
extra_arguments=[{
"use_intra_process_comm": True
}],
),
],
output="screen",
)
# Unknown how to set timeout
# https://github.com/ros2/launch/issues/466
servo_gtest = launch_ros.actions.Node(
executable=PathJoinSubstitution(
[LaunchConfiguration("test_binary_dir"), gtest_name]),
parameters=[servo_params],
output="screen",
)
return launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(
name="test_binary_dir",
description="Binary directory of package "
"containing test executables",
),
ros2_control_node,
test_container,
TimerAction(period=2.0, actions=[servo_gtest]),
launch_testing.actions.ReadyToTest(),
] + load_controllers), {
"test_container": test_container,
"servo_gtest": servo_gtest,
"ros2_control_node": ros2_control_node,
}
def generate_launch_description():
model, plugin, media = GazeboRosPaths.get_paths()
if 'GAZEBO_MODEL_PATH' in environ:
model += ':' + environ['GAZEBO_MODEL_PATH']
if 'GAZEBO_PLUGIN_PATH' in environ:
plugin += ':' + environ['GAZEBO_PLUGIN_PATH']
if 'GAZEBO_RESOURCE_PATH' in environ:
media += ':' + environ['GAZEBO_RESOURCE_PATH']
env = {
'GAZEBO_MODEL_PATH': model,
'GAZEBO_PLUGIN_PATH': plugin,
'GAZEBO_RESOURCE_PATH': media
}
return LaunchDescription([
DeclareLaunchArgument(
'version',
default_value='false',
description='Set "true" to output version information'),
DeclareLaunchArgument(
'verbose',
default_value='false',
description='Set "true" to increase messages written to terminal'),
DeclareLaunchArgument(
'help',
default_value='false',
description='Set "true" to produce gzclient help message'),
DeclareLaunchArgument(
'extra_gazebo_args',
default_value='',
description='Extra arguments to be passed to Gazebo'),
# Specific to gazebo_ros
DeclareLaunchArgument(
'gdb',
default_value='false',
description='Set "true" to run gzserver with gdb'),
DeclareLaunchArgument(
'valgrind',
default_value='false',
description='Set "true" to run gzserver with valgrind'),
# Execute
ExecuteProcess(
cmd=[[
'gzclient',
_boolean_command('version'),
' ',
_boolean_command('verbose'),
' ',
_boolean_command('help'),
' ',
LaunchConfiguration('extra_gazebo_args'),
]],
output='screen',
additional_env=env,
shell=True,
prefix=PythonExpression([
'"gdb -ex run --args" if "true" == "',
LaunchConfiguration('gdb'), '"else "valgrind" if "true" == "',
LaunchConfiguration('valgrind'), '"else ""'
]),
)
])
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(bringup_dir, 'launch')
# Create the launch configuration variables
slam = LaunchConfiguration('slam')
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
autostart = LaunchConfiguration('autostart')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_simulator = LaunchConfiguration('use_simulator')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')
headless = LaunchConfiguration('headless')
world = LaunchConfiguration('world')
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56
remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')]
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace',
default_value='',
description='Top-level namespace')
declare_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack')
declare_slam_cmd = DeclareLaunchArgument(
'slam',
default_value='False',
description='Whether run a SLAM')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(
bringup_dir, 'maps', 'turtlebot3_world.yaml'),
description='Full path to map file to load')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart', default_value='true',
description='Automatically startup the nav2 stack')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join(
bringup_dir, 'rviz', 'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use')
declare_use_simulator_cmd = DeclareLaunchArgument(
'use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
'use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz',
default_value='True',
description='Whether to start RVIZ')
declare_simulator_cmd = DeclareLaunchArgument(
'headless',
default_value='False',
description='Whether to execute gzclient)')
declare_world_cmd = DeclareLaunchArgument(
'world',
# TODO(orduno) Switch back once ROS argument passing has been fixed upstream
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
# default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
# worlds/turtlebot3_worlds/waffle.model')
default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'),
description='Full path to world model file to load')
# Specify the actions
start_gazebo_server_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world],
cwd=[launch_dir], output='screen')
start_gazebo_client_cmd = ExecuteProcess(
condition=IfCondition(PythonExpression(
[use_simulator, ' and not ', headless])),
cmd=['gzclient'],
cwd=[launch_dir], output='screen')
urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
namespace=namespace,
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
remappings=remappings,
arguments=[urdf])
rviz_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'rviz_launch.py')),
condition=IfCondition(use_rviz),
launch_arguments={'namespace': '',
'use_namespace': 'False',
'rviz_config': rviz_config_file}.items())
bringup_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'bringup_launch.py')),
launch_arguments={'namespace': namespace,
'use_namespace': use_namespace,
'slam': slam,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'params_file': params_file,
'autostart': autostart}.items())
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_slam_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any conditioned actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(start_robot_state_publisher_cmd)
ld.add_action(rviz_cmd)
ld.add_action(bringup_cmd)
return ld
