From eced87d4bb4db2d7e46db02ccc075ff597873f23 Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Fri, 3 May 2024 16:01:49 -0300 Subject: [PATCH] Fixes format. Signed-off-by: Franco Cipollone --- TODO | 9 -- andino_gz/launch/andino_gz.launch.py | 150 +++++++++--------- .../launch/include/gz_ros_bridge.launch.py | 9 +- .../launch/include/spawn_robot.launch.py | 3 +- 4 files changed, 85 insertions(+), 86 deletions(-) delete mode 100644 TODO diff --git a/TODO b/TODO deleted file mode 100644 index 4d9578f..0000000 --- a/TODO +++ /dev/null @@ -1,9 +0,0 @@ -☐ andino_gz.launch.py should launch bridge node only for the clock. (if ros_bridge is used.) -☐ spawn_robot launch file should launch its own ros bridge everytime it is launched. -☐ ros_bridge yaml should have some placeholders instead of andino so it is more generic and replaced by each spawn_robot call -☐ a ros namespace should be pushed to each ros bridge node, this should change the topic from the ros side(to verify) - - - - -ros2 launch andino_gz andino_gz.launch.py robots:="andino={x: 2., y: 0., z: 0.1, yaw: 0.};andino2={x: 0., y: 0., z: 0.1, yaw: 0.};" ros_bridge:=true \ No newline at end of file diff --git a/andino_gz/launch/andino_gz.launch.py b/andino_gz/launch/andino_gz.launch.py index 7a4b9da..e6b77b9 100644 --- a/andino_gz/launch/andino_gz.launch.py +++ b/andino_gz/launch/andino_gz.launch.py @@ -16,10 +16,13 @@ def generate_launch_description(): pkg_andino_gz = get_package_share_directory('andino_gz') ros_bridge_arg = DeclareLaunchArgument( - 'ros_bridge', default_value='true', description = 'Run ROS bridge node.') + 'ros_bridge', default_value='true', description='Run ROS bridge node.') rviz_arg = DeclareLaunchArgument('rviz', default_value='true', description='Start RViz.') - world_name_arg = DeclareLaunchArgument('world_name', default_value='depot.sdf', description='Name of the world to load.') - robots_arg = DeclareLaunchArgument('robots', default_value="andino={x: 0., y: 0., z: 0.1, yaw: 0.};", description='Robots to spawn, multiple robots can be stated separated by a ; ') + world_name_arg = DeclareLaunchArgument( + 'world_name', default_value='depot.sdf', description='Name of the world to load.') + robots_arg = DeclareLaunchArgument( + 'robots', default_value="andino={x: 0., y: 0., z: 0.1, yaw: 0.};", + description='Robots to spawn, multiple robots can be stated separated by a ; ') # Variables of launch file. rviz = LaunchConfiguration('rviz') @@ -30,87 +33,92 @@ def generate_launch_description(): world_path = PathJoinSubstitution([pkg_andino_gz, 'worlds', world_name]) base_group = GroupAction( - scoped=True, forwarding=False, - launch_configurations={ - 'ros_bridge': ros_bridge, - 'world_name': world_name - }, - actions=[ - # Gazebo Sim - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py') + scoped=True, forwarding=False, + launch_configurations={ + 'ros_bridge': ros_bridge, + 'world_name': world_name + }, + actions=[ + # Gazebo Sim + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': world_path}.items(), ), - launch_arguments={'gz_args': world_path}.items(), - ), - # ROS Bridge for generic Gazebo stuff - Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], - output='screen', - namespace='andino_gz_sim', - condition=IfCondition(ros_bridge), - ), - ]) + # ROS Bridge for generic Gazebo stuff + Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + output='screen', + namespace='andino_gz_sim', + condition=IfCondition(ros_bridge), + ), + ] + ) robots_list = ParseMultiRobotPose('robots').value() # When no robots are specified, spawn a single robot at the origin. # The default value isn't getting parsed correctly, so we need to check for an empty dictionary. if (robots_list == {}): - robots_list = {"andino": { "x": 0., "y": 0., "z": 0.1, "yaw": 0. } } + robots_list = {"andino": {"x": 0., "y": 0., "z": 0.1, "yaw": 0.}} log_number_robots = LogInfo(msg="Robots to spawn: " + str(robots_list)) spawn_robots_group = [] for robot_name in robots_list: init_pose = robots_list[robot_name] # As it is scoped and not forwarding, the launch configuration in this context gets cleared. group = GroupAction( - scoped=True, forwarding=False, - launch_configurations={ - 'rviz': rviz, - 'ros_bridge': ros_bridge - }, - actions=[ - LogInfo(msg="Group for robot: " + robot_name), - PushRosNamespace(condition=IfCondition(PythonExpression([TextSubstitution(text=str(len(robots_list.keys()))), ' > 1'])), namespace=robot_name), - # Spawn the robot and the Robot State Publisher node. - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_andino_gz, 'launch', 'include', 'spawn_robot.launch.py') - ), - launch_arguments={ - 'entity': robot_name, - 'initial_pose_x': str(init_pose['x']), - 'initial_pose_y': str(init_pose['y']), - 'initial_pose_z': str(init_pose['z']), - 'initial_pose_yaw': str(init_pose['yaw']), - 'robot_description_topic': 'robot_description', - 'use_sim_time': 'true', - }.items(), - ), - # RViz - Node( - condition=IfCondition(rviz), - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(pkg_andino_gz, 'rviz', 'andino_gz.rviz')], - parameters=[{'use_sim_time': True}], - remappings=[ - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ], - ), - # Run ros_gz bridge - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_andino_gz, 'launch', 'include', 'gz_ros_bridge.launch.py') + scoped=True, forwarding=False, + launch_configurations={ + 'rviz': rviz, + 'ros_bridge': ros_bridge + }, + actions=[ + LogInfo(msg="Group for robot: " + robot_name), + PushRosNamespace( + condition=IfCondition( + PythonExpression([TextSubstitution(text=str(len(robots_list.keys()))), ' > 1'])), + namespace=robot_name), + # Spawn the robot and the Robot State Publisher node. + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_andino_gz, 'launch', 'include', 'spawn_robot.launch.py') + ), + launch_arguments={ + 'entity': robot_name, + 'initial_pose_x': str(init_pose['x']), + 'initial_pose_y': str(init_pose['y']), + 'initial_pose_z': str(init_pose['z']), + 'initial_pose_yaw': str(init_pose['yaw']), + 'robot_description_topic': 'robot_description', + 'use_sim_time': 'true', + }.items(), + ), + # RViz + Node( + condition=IfCondition(rviz), + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(pkg_andino_gz, 'rviz', 'andino_gz.rviz')], + parameters=[{'use_sim_time': True}], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ], ), - launch_arguments={ - 'entity': robot_name, - }.items(), - condition=IfCondition(LaunchConfiguration('ros_bridge')), - ) - ]) + # Run ros_gz bridge + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_andino_gz, 'launch', 'include', 'gz_ros_bridge.launch.py') + ), + launch_arguments={ + 'entity': robot_name, + }.items(), + condition=IfCondition(LaunchConfiguration('ros_bridge')), + ) + ] + ) spawn_robots_group.append(group) ld = LaunchDescription() diff --git a/andino_gz/launch/include/gz_ros_bridge.launch.py b/andino_gz/launch/include/gz_ros_bridge.launch.py index 8014cb4..97fa3ae 100644 --- a/andino_gz/launch/include/gz_ros_bridge.launch.py +++ b/andino_gz/launch/include/gz_ros_bridge.launch.py @@ -7,7 +7,8 @@ from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration -from nav2_common.launch import ReplaceString, RewrittenYaml +from nav2_common.launch import ReplaceString + def generate_launch_description(): pkg_andino_gz = get_package_share_directory('andino_gz') @@ -27,11 +28,11 @@ def generate_launch_description(): executable='parameter_bridge', output='screen', parameters=[{ - 'config_file': bridge_config + 'config_file': bridge_config }], ) return LaunchDescription([ - entity_arg, - bridge_node, + entity_arg, + bridge_node, ]) diff --git a/andino_gz/launch/include/spawn_robot.launch.py b/andino_gz/launch/include/spawn_robot.launch.py index fe5c686..01b7829 100644 --- a/andino_gz/launch/include/spawn_robot.launch.py +++ b/andino_gz/launch/include/spawn_robot.launch.py @@ -5,15 +5,14 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression from launch_ros.actions import Node from xacro import process_file PKG_ANDINO_DESCRIPTION = get_package_share_directory('andino_description') PKG_ANDINO_GZ = get_package_share_directory('andino_gz') + def get_robot_description() -> str: """ Obtain the URDF from the xacro file.