feat: add launch files

Adde launch files for slam, nav2, localization and organized existing launch files

Author: anurye

drl_agent/CMakeLists.txt

@@ -30,6 +30,7 @@ install(
  DIRECTORY
  launch
  scripts
+ config
  DESTINATION
  share/${PROJECT_NAME}/
 )

drl_agent/launch/localization.launch.py

@@ -0,0 +1,55 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import PushRosNamespace
+from ament_index_python.packages import get_package_share_directory
+
+
+ARGUMENTS = [
+ DeclareLaunchArgument('use_sim_time', default_value='true',
+ choices=['true', 'false'],
+ description='Use sim time'),
+ DeclareLaunchArgument('namespace', default_value='',
+ description='Robot namespace')
+]
+
+
+def generate_launch_description():
+ drl_agent_pkg = get_package_share_directory('drl_agent')
+ pkg_nav2_bringup = get_package_share_directory('nav2_bringup')
+
+ localization_params_arg = DeclareLaunchArgument(
+ 'params',
+ default_value=PathJoinSubstitution(
+ [drl_agent_pkg, 'config', 'localization.yaml']),
+ description='Localization parameters')
+
+ map_arg = DeclareLaunchArgument(
+ 'map',
+ default_value=PathJoinSubstitution(
+ [drl_agent_pkg, 'maps', 'warehouse.yaml']), # TODO: add default map here
+ description='Full path to map yaml file to load')
+
+ namespace = LaunchConfiguration('namespace')
+ use_sim_time = LaunchConfiguration('use_sim_time')
+
+ localization = GroupAction([
+ PushRosNamespace(namespace),
+
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ PathJoinSubstitution(
+ [pkg_nav2_bringup, 'launch', 'localization_launch.py'])),
+ launch_arguments={'namespace': namespace,
+ 'map': LaunchConfiguration('map'),
+ 'use_sim_time': use_sim_time,
+ 'params_file': LaunchConfiguration('params')}.items()),
+ ])
+
+ ld = LaunchDescription(ARGUMENTS)
+ ld.add_action(localization_params_arg)
+ ld.add_action(map_arg)
+ ld.add_action(localization)
+ return ld

drl_agent/launch/nav2.launch.py

@@ -0,0 +1,68 @@
+#!/usr/bin/python3
+
+from launch import LaunchDescription
+from launch.actions import (
+ DeclareLaunchArgument,
+ GroupAction,
+ IncludeLaunchDescription,
+ OpaqueFunction
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import PushRosNamespace, SetRemap
+from ament_index_python.packages import get_package_share_directory
+
+
+ARGUMENTS = [
+ DeclareLaunchArgument('use_sim_time', default_value='true',
+ choices=['true', 'false'],
+ description='Use sim time'),
+ DeclareLaunchArgument('params_file',
+ default_value=PathJoinSubstitution([
+ get_package_share_directory('drl_agent'),
+ 'config',
+ 'nav2.yaml'
+ ]),
+ description='Nav2 parameters'),
+ DeclareLaunchArgument('namespace', default_value='',
+ description='Robot namespace')
+]
+
+
+def launch_setup(context, *args, **kwargs):
+ pkg_nav2_bringup = get_package_share_directory('nav2_bringup')
+
+ nav2_params = LaunchConfiguration('params_file')
+ namespace = LaunchConfiguration('namespace')
+ use_sim_time = LaunchConfiguration('use_sim_time')
+
+ namespace_str = namespace.perform(context)
+ if (namespace_str and not namespace_str.startswith('/')):
+ namespace_str = '/' + namespace_str
+
+ launch_nav2 = PathJoinSubstitution(
+ [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
+
+ nav2 = GroupAction([
+ PushRosNamespace(namespace),
+ SetRemap(namespace_str + '/global_costmap/scan', namespace_str + '/scan'),
+ SetRemap(namespace_str + '/local_costmap/scan', namespace_str + '/scan'),
+
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(launch_nav2),
+ launch_arguments=[
+ ('use_sim_time', use_sim_time),
+ ('params_file', nav2_params.perform(context)),
+ ('use_composition', 'False'),
+ ('namespace', namespace_str)
+ ]
+ ),
+ ])
+
+ return [nav2]
+
+
+def generate_launch_description():
+ ld = LaunchDescription(ARGUMENTS)
+ ld.add_action(OpaqueFunction(function=launch_setup))
+ return ld

drl_agent/launch/slam.launch.py

@@ -0,0 +1,80 @@
+#!/usr/bin/python3
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node, PushRosNamespace
+from nav2_common.launch import RewrittenYaml
+from ament_index_python.packages import get_package_share_directory
+
+
+ARGUMENTS = [
+ DeclareLaunchArgument('use_sim_time', default_value='true',
+ choices=['true', 'false'],
+ description='Use sim time'),
+ DeclareLaunchArgument('sync', default_value='false',
+ choices=['true', 'false'],
+ description='Use synchronous SLAM'),
+ DeclareLaunchArgument('namespace', default_value='',
+ description='Robot namespace')
+]
+
+
+def generate_launch_description():
+ drl_agent_pkg = get_package_share_directory('drl_agent')
+
+ namespace = LaunchConfiguration('namespace')
+ sync = LaunchConfiguration('sync')
+
+ slam_params_arg = DeclareLaunchArgument(
+ 'params',
+ default_value=PathJoinSubstitution(
+ [drl_agent_pkg, 'config', 'slam.yaml']),
+ description='Robot namespace')
+
+ slam_params = RewrittenYaml(
+ source_file=LaunchConfiguration('params'),
+ root_key=namespace,
+ param_rewrites={},
+ convert_types=True
+ )
+
+ remappings = [
+ ('/tf', 'tf'),
+ ('/tf_static', 'tf_static'),
+ ('/scan', 'scan'),
+ ('/map', 'map'),
+ ('/map_metadata', 'map_metadata'),
+ ]
+
+ slam = GroupAction([
+ PushRosNamespace(namespace),
+
+ Node(package='slam_toolbox',
+ executable='sync_slam_toolbox_node',
+ name='slam_toolbox',
+ output='screen',
+ parameters=[
+ slam_params,
+ {'use_sim_time': LaunchConfiguration('use_sim_time')}
+ ],
+ remappings=remappings,
+ condition=IfCondition(sync)),
+
+ Node(package='slam_toolbox',
+ executable='async_slam_toolbox_node',
+ name='slam_toolbox',
+ output='screen',
+ parameters=[
+ slam_params,
+ {'use_sim_time': LaunchConfiguration('use_sim_time')}
+ ],
+ remappings=remappings,
+ condition=UnlessCondition(sync))
+ ])
+
+ ld = LaunchDescription(ARGUMENTS)
+ ld.add_action(slam_params_arg)
+ ld.add_action(slam)
+ return ld

drl_agent/launch/test_drl_agent.launch.py

@@ -3,7 +3,13 @@
 import rclpy
 from rclpy.node import Node
 import numpy as np
-from drl_agent_interfaces.srv import Step, Reset, Seed, GetDimensions, SampleActionSpace
+from drl_agent_interfaces.srv import (
+ Step,
+ Reset,
+ Seed,
+ GetDimensions,
+ SampleActionSpace
+ )
 
 
 class EnvInterface(Node):
@@ -80,7 +86,7 @@ def set_env_seed(self, seed):
  rclpy.spin_until_future_complete(self, future)
  except Exception as e:
  self.get_logger().error(f"Service call /seed failed: {e}")
- self.get_logger().info(f"\nEnvironment seed set to: {seed}, Success: {future.result().success}\n")
+ self.get_logger().info(f"Environment seed set to: {seed}, Success: {future.result().success}\n")
 
 
 def main(args=None):

drl_agent/launch/train_drl_agent.launch.py

@@ -33,9 +33,9 @@ def __init__(self, args, env):
 self.rl_agent = Agent(state_dim=state_dim, action_dim=action_dim, max_action=max_action)
 try:
 self.rl_agent.load(self.pytorch_models_dir, self.args.file_name)
-self.get_logger().info("\n > Model parameters loaded successfuly\n")
+self.get_logger().info(f'{"Model parameters loaded successfuly":-^50}')
 except Exception as e:
-self.get_logger().error("\n > Could not load network parameters :(\n")
+self.get_logger().error(f'{"Could not load network parameters :(":-^50}')
 sys.exit(-1)
 
 # Set seed for the test environment

drl_agent/scripts/environment/environment_interface.py

@@ -30,6 +30,7 @@ install(
  urdf
  worlds
  rviz
+ config
  DESTINATION
  share/${PROJECT_NAME}/
 )

drl_agent/scripts/policies/test_td7_agent.py

@@ -0,0 +1,3 @@
+gazebo:
+ ros__parameters:
+ publish_rate: 62.0