From 9d8ef9436af2a81e01778f74d8da20276bc3a045 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 23 Apr 2024 12:54:59 +0200 Subject: [PATCH] Separate spawn.launch.py --- .pre-commit-config.yaml | 12 +- README.md | 5 +- panther/panther_hardware.repos | 6 +- panther/panther_simulation.repos | 20 -- .../config/WH01_controller.yaml | 2 - .../config/WH02_controller.yaml | 2 - .../config/WH04_controller.yaml | 2 - .../launch/controller.launch.py | 6 +- panther_description/urdf/body.urdf.xacro | 53 ++-- .../urdf/panther_macro.urdf.xacro | 7 +- panther_description/urdf/wheel.urdf.xacro | 19 +- panther_gazebo/README.md | 15 +- panther_gazebo/launch/simulation.launch.py | 209 +++++------- panther_gazebo/launch/spawn.launch.py | 298 ++++++++++++++++++ 14 files changed, 448 insertions(+), 208 deletions(-) create mode 100644 panther_gazebo/launch/spawn.launch.py diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 82dcb2a4c..62403bb76 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.5.0 + rev: v4.6.0 hooks: - id: check-added-large-files # mesh files has to be taken into account @@ -23,7 +23,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/PyCQA/isort - rev: 5.12.0 + rev: 5.13.2 hooks: - id: isort args: ["--profile", "black"] @@ -56,19 +56,19 @@ repos: types: [text] - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt - rev: 0.2.1 + rev: 0.2.3 hooks: - id: yamlfmt files: ^.github|./\.yaml - repo: https://github.com/psf/black - rev: 23.10.1 + rev: 24.4.0 hooks: - id: black - args: ["--line-length=99", "--experimental-string-processing"] + args: ["--line-length=99"] - repo: https://github.com/PyCQA/flake8 - rev: 6.1.0 + rev: 7.0.0 hooks: - id: flake8 args: diff --git a/README.md b/README.md index bee010306..7ebd06a71 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,10 @@ export HUSARION_ROS_BUILD_TYPE=simulation ### Build ``` bash -vcs import src < src/panther_ros/panther/panther_$HUSARION_ROS_BUILD_TYPE.repos +vcs import src < src/panther_ros/panther/panther_hardware.repos +if [ "$HUSARION_ROS_BUILD_TYPE" == "simulation" ]; then + vcs import src < src/panther_ros/panther/panther_simulation.repos +fi cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index 11dace30f..1d1790618 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -19,7 +19,7 @@ repositories: type: git url: https://github.com/husarion/ros_components_description.git version: ros2 - ros2_controllers: + ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git - url: https://github.com/delihus/ros2_controllers - version: 60919d60fb02eb920b0bf42e4d86853db23233cc + url: https://github.com/rafal-gorecki/ros2_controllers.git + version: humble diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index 3cbc3a910..05dad7358 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -1,25 +1,5 @@ repositories: - husarion_controllers: - type: git - url: https://github.com/husarion/husarion_controllers - version: main husarion_office_gz: type: git url: https://github.com/husarion/husarion_office_gz.git version: main - panther_msgs: - type: git - url: https://github.com/husarion/panther_msgs.git - version: ros2 - robot_localization: # Caused by this error https://github.com/cra-ros-pkg/robot_localization/pull/876, it can be removed after the next synchronization of apt packages. - type: git - url: https://github.com/cra-ros-pkg/robot_localization.git - version: humble-devel - ros_components_description: - type: git - url: https://github.com/husarion/ros_components_description.git - version: ros2 - ros2_controllers: - type: git - url: https://github.com/delihus/ros2_controllers - version: 60919d60fb02eb920b0bf42e4d86853db23233cc diff --git a/panther_controller/config/WH01_controller.yaml b/panther_controller/config/WH01_controller.yaml index 519cd9907..d1ca2783c 100644 --- a/panther_controller/config/WH01_controller.yaml +++ b/panther_controller/config/WH01_controller.yaml @@ -14,7 +14,6 @@ imu_broadcaster: ros__parameters: use_namespace_as_sensor_name_prefix: true - tf_frame_prefix_enable: false sensor_name: imu frame_id: imu_link @@ -29,7 +28,6 @@ ros__parameters: left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] - tf_frame_prefix_enable: false wheel_separation: 0.697 wheel_radius: 0.1825 diff --git a/panther_controller/config/WH02_controller.yaml b/panther_controller/config/WH02_controller.yaml index 8b86ac421..9f3e11a6e 100644 --- a/panther_controller/config/WH02_controller.yaml +++ b/panther_controller/config/WH02_controller.yaml @@ -14,7 +14,6 @@ imu_broadcaster: ros__parameters: use_namespace_as_sensor_name_prefix: true - tf_frame_prefix_enable: false sensor_name: imu frame_id: imu_link @@ -31,7 +30,6 @@ front_right_wheel_name: fr_wheel_joint rear_left_wheel_name: rl_wheel_joint rear_right_wheel_name: rr_wheel_joint - tf_frame_prefix_enable: false wheel_separation_x: 0.44 wheel_separation_y: 0.6785 diff --git a/panther_controller/config/WH04_controller.yaml b/panther_controller/config/WH04_controller.yaml index ad30d720c..10881ab8d 100644 --- a/panther_controller/config/WH04_controller.yaml +++ b/panther_controller/config/WH04_controller.yaml @@ -14,7 +14,6 @@ imu_broadcaster: ros__parameters: use_namespace_as_sensor_name_prefix: true - tf_frame_prefix_enable: false sensor_name: imu frame_id: imu_link @@ -29,7 +28,6 @@ ros__parameters: left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] - tf_frame_prefix_enable: false wheel_separation: 0.616 wheel_radius: 0.1016 diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py index 291516b08..93d99681c 100644 --- a/panther_controller/launch/controller.launch.py +++ b/panther_controller/launch/controller.launch.py @@ -27,7 +27,6 @@ FindExecutable, LaunchConfiguration, PathJoinSubstitution, - PythonExpression, ) from launch_ros.actions import Node, SetParameter from launch_ros.substitutions import FindPackageShare @@ -157,13 +156,11 @@ def generate_launch_description(): ], condition=UnlessCondition(use_sim), ) - namespace_ext = PythonExpression(['"', namespace, '"+ "/" if "', namespace, '" else ""']) - robot_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", output="both", - parameters=[robot_description, {"frame_prefix": namespace_ext}], + parameters=[robot_description], namespace=namespace, condition=IfCondition(publish_robot_state), ) @@ -228,7 +225,6 @@ def generate_launch_description(): target_action=robot_controller_spawner, on_exit=[imu_broadcaster_spawner], ), - condition=IfCondition(use_sim), ) actions = [ diff --git a/panther_description/urdf/body.urdf.xacro b/panther_description/urdf/body.urdf.xacro index e12f9e2e6..0581cb234 100644 --- a/panther_description/urdf/body.urdf.xacro +++ b/panther_description/urdf/body.urdf.xacro @@ -4,19 +4,26 @@ - + + + + + + + + - - + + - + @@ -43,51 +50,51 @@ - - + + - + - - + + - + - - + + - + - - + + - + - - + + - + - - + + - + diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro index b2b116164..c5a65b342 100644 --- a/panther_description/urdf/panther_macro.urdf.xacro +++ b/panther_description/urdf/panther_macro.urdf.xacro @@ -34,6 +34,7 @@ @@ -192,7 +197,7 @@ - + diff --git a/panther_description/urdf/wheel.urdf.xacro b/panther_description/urdf/wheel.urdf.xacro index 6e980d6f8..8affe2c51 100644 --- a/panther_description/urdf/wheel.urdf.xacro +++ b/panther_description/urdf/wheel.urdf.xacro @@ -2,7 +2,14 @@ - + + + + + + + + @@ -31,15 +38,15 @@ - - + + - + @@ -69,14 +76,14 @@ - + 1.0 0.0 - ${fdir} + ${fdir} diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md index 66bf24a67..85f201198 100644 --- a/panther_gazebo/README.md +++ b/panther_gazebo/README.md @@ -4,18 +4,23 @@ A package containing the launch files and dependencies needed to run the simulat ## Usage -The recommended method for launching the simulation is by utilizing the [simulation.launch.py](https://github.com/husarion/panther_ros/panther_gazebo/launch/simulation.launch.py) file. Below, you will find launch arguments that enable simulation configuration. +The recommended method for launching the simulation is by utilizing the [simulation.launch.py](https://github.com/husarion/panther_ros/panther_gazebo/launch/simulation.launch.py) file. Below, you will find launch arguments that enable simulation configuration. You can also launch more robots using `spawn.launch.py` ​​after the system has been started. ### Launch Arguments +- `add_map_transform` [*bool*, default: **False**]: Adds a frame map that connects the tf trees of individual robots (useful when running multiple robots +). - `battery_config_path` [*string*, default: **panther_gazebo/config/battery_plugin_config.yaml**]: Path to the Ignition `LinearBatteryPlugin` configuration file. This configuration is intended for use in simulations only. For more information on how to configure this plugin, please refer to the [Linear Battery Plugin](#linear-battery-plugin) section. - `controller_config_path` [*string*, default: **panther_controller/config/_controller.yaml**]: Path to the controller configuration file. If you want to use a custom configuration, you can specify the path to your custom controller configuration file here. - `gz_bridge_config_path` [*string*, default: **panther_gazebo/config/gz_bridge.yaml**]: Path to the `parameter_bridge` configuration file. For detailed information on configuring the `parameter_bridge`, please refer to this [example](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge#example-5-configuring-the-bridge-via-yaml). -- `pos_x` [*float*, default: **5.0**]: spawn position **[m]** of the robot in the world in **X** direction. -- `pos_y` [*float*, default: **-5.0**]: spawn position **[m]** of the robot in the world in **Y** direction. -- `pos_z` [*float*, default: **0.2**]: spawn position **[m]** of the robot in the world in **Z** direction. -- `rot_yaw` [*float*, default: **0.0**]: spawn yaw angle **[rad]** of the robot in the world. +- `x` [*float*, default: **5.0**]: spawn position **[m]** of the robot in the world in **X** direction. +- `y` [*float*, default: **-5.0**]: spawn position **[m]** of the robot in the world in **Y** direction. +- `z` [*float*, default: **0.2**]: spawn position **[m]** of the robot in the world in **Z** direction. +- `roll` [*float*, default: **0.0**]: spawn roll angle **[rad]** of the robot in the world. +- `pitch` [*float*, default: **0.0**]: spawn pitch angle **[rad]** of the robot in the world. +- `yaw` [*float*, default: **0.0**]: spawn yaw angle **[rad]** of the robot in the world. - `publish_robot_state` [*bool*, default: **true**]: Whether to launch the robot_state_publisher node. When set to `false`, users should publish their own robot description. +- `robots` [*custom*, default: **""**]: The list of the robots spawned in the simulation e.g. robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}'" - `wheel_config_path` [*string*, default: **panther_description/config/.yaml**]: Path to the wheel configuration file. If you want to use a custom configuration, you can specify the path to your custom wheel configuration file here. Please refer to the `wheel_type` parameter description for more information. - `wheel_type` [*string*, default: **WH01**]: Specify the type of wheel. If you select a value from the provided options (`WH01`, `WH02`, `WH04`), you can disregard the `wheel_config_path` and `controller_config_path` parameters. If you have custom wheels, set this parameter to `CUSTOM` and provide the necessary configurations. - `world` [*string*, default: **-r /worlds/husarion_world.sdf**]: path to Gazebo world file used for simulation. diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py index 1ce6dc4b0..959cf89c3 100644 --- a/panther_gazebo/launch/simulation.launch.py +++ b/panther_gazebo/launch/simulation.launch.py @@ -15,133 +15,16 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - LogInfo, - OpaqueFunction, - TimerAction, -) +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution, PythonExpression, - TextSubstitution, ) -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import SetParameter from launch_ros.substitutions import FindPackageShare -from nav2_common.launch import ParseMultiRobotPose - - -def launch_setup(context): - wheel_type = LaunchConfiguration("wheel_type").perform(context) - wheel_config_path = LaunchConfiguration("wheel_config_path").perform(context) - controller_config_path = LaunchConfiguration("controller_config_path").perform(context) - battery_config_path = LaunchConfiguration("battery_config_path").perform(context) - gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path").perform(context) - world_cfg = LaunchConfiguration("world").perform(context) - x = LaunchConfiguration("x").perform(context) - y = LaunchConfiguration("y").perform(context) - z = LaunchConfiguration("z").perform(context) - roll = LaunchConfiguration("roll").perform(context) - pitch = LaunchConfiguration("pitch").perform(context) - yaw = LaunchConfiguration("yaw").perform(context) - publish_robot_state = LaunchConfiguration("publish_robot_state").perform(context) - namespace = LaunchConfiguration("namespace").perform(context) - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] - ) - ), - launch_arguments={"gz_args": world_cfg}.items(), - ) - - robots_list = ParseMultiRobotPose("robots").value() - if len(robots_list) == 0: - robots_list = { - namespace: {"x": x, "y": y, "z": z, "roll": roll, "pitch": pitch, "yaw": yaw} - } - - spawn_group = [] - for idx, robot_name in enumerate(robots_list): - init_pose = robots_list[robot_name] - - spawn_log = LogInfo( - msg=[f"Launching namespace={robot_name} with init_pose= {str(init_pose)}"] - ) - - spawn_robot = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", - robot_name, - "-topic", - "robot_description", - "-x", - TextSubstitution(text=str(init_pose["x"])), - "-y", - TextSubstitution(text=str(init_pose["y"])), - "-z", - TextSubstitution(text=str(init_pose["z"])), - "-Y", - TextSubstitution(text=str(init_pose["yaw"])), - ], - namespace=robot_name, - output="screen", - ) - - gz_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="gz_bridge", - parameters=[{"config_file": gz_bridge_config_path}], - namespace=robot_name, - output="screen", - ) - - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("panther_bringup"), - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "wheel_type": wheel_type, - "wheel_config_path": wheel_config_path, - "controller_config_path": controller_config_path, - "battery_config_path": battery_config_path, - "publish_robot_state": publish_robot_state, - "use_sim": "True", - "simulation_engine": "ignition-gazebo", - "namespace": robot_name, - }.items(), - ) - - group = TimerAction( - period=10.0 * idx, - actions=[ - spawn_log, - spawn_robot, - gz_bridge, - bringup_launch, - ], - ) - spawn_group.append(group) - - return [gz_sim, *spawn_group] def generate_launch_description(): @@ -158,6 +41,7 @@ def generate_launch_description(): choices=["WH01", "WH02", "WH04", "CUSTOM"], ) + wheel_config_path = LaunchConfiguration("wheel_config_path") declare_wheel_config_path_arg = DeclareLaunchArgument( "wheel_config_path", default_value=PathJoinSubstitution( @@ -174,6 +58,7 @@ def generate_launch_description(): ), ) + controller_config_path = LaunchConfiguration("controller_config_path") declare_controller_config_path_arg = DeclareLaunchArgument( "controller_config_path", default_value=PathJoinSubstitution( @@ -190,6 +75,7 @@ def generate_launch_description(): ), ) + battery_config_path = LaunchConfiguration("battery_config_path") declare_battery_config_path_arg = DeclareLaunchArgument( "battery_config_path", default_value=PathJoinSubstitution( @@ -205,6 +91,7 @@ def generate_launch_description(): ), ) + gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path") declare_gz_bridge_config_path_arg = DeclareLaunchArgument( "gz_bridge_config_path", default_value=PathJoinSubstitution( @@ -217,6 +104,7 @@ def generate_launch_description(): description="Path to the parameter_bridge configuration file", ) + world_cfg = LaunchConfiguration("world") declare_world_arg = DeclareLaunchArgument( "world", default_value=[ @@ -232,36 +120,37 @@ def generate_launch_description(): description="SDF world file", ) + x = LaunchConfiguration("x") declare_x_arg = DeclareLaunchArgument( - "x", - default_value=["5.0"], - description="Initial robot position in the global 'x' axis.", + "x", default_value="5.0", description="Initial robot position in the global 'x' axis." ) + y = LaunchConfiguration("y") declare_y_arg = DeclareLaunchArgument( - "y", - default_value=["-5.0"], - description="Initial robot position in the global 'y' axis.", + "y", default_value="-5.0", description="Initial robot position in the global 'y' axis." ) + z = LaunchConfiguration("z") declare_z_arg = DeclareLaunchArgument( - "z", - default_value=["0.2"], - description="Initial robot position in the global 'z' axis.", + "z", default_value="0.2", description="Initial robot position in the global 'z' axis." ) + roll = LaunchConfiguration("roll") declare_roll_arg = DeclareLaunchArgument( - "roll", default_value=["0.0"], description="Initial robot 'roll' orientation." + "roll", default_value="0.0", description="Initial robot 'roll' orientation." ) + pitch = LaunchConfiguration("pitch") declare_pitch_arg = DeclareLaunchArgument( - "pitch", default_value=["0.0"], description="Initial robot orientation." + "pitch", default_value="0.0", description="Initial robot orientation." ) + yaw = LaunchConfiguration("yaw") declare_yaw_arg = DeclareLaunchArgument( - "yaw", default_value=["0.0"], description="Initial robot orientation." + "yaw", default_value="0.0", description="Initial robot orientation." ) + publish_robot_state = LaunchConfiguration("publish_robot_state") declare_publish_robot_state_arg = DeclareLaunchArgument( "publish_robot_state", default_value="True", @@ -271,12 +160,14 @@ def generate_launch_description(): ), ) + namespace = LaunchConfiguration("namespace") declare_namespace_arg = DeclareLaunchArgument( "namespace", default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), description="Add namespace to all launched nodes", ) + robots = LaunchConfiguration("robots") declare_robots_arg = DeclareLaunchArgument( "robots", default_value=[], @@ -286,6 +177,58 @@ def generate_launch_description(): ), ) + add_map_transform = LaunchConfiguration("add_map_transform") + declare_add_map_transform_arg = DeclareLaunchArgument( + "add_map_transform", + default_value="False", + description=( + "Adds a frame map that connects the tf trees of individual robots (useful when running" + " multiple robots)." + ), + ) + + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ), + launch_arguments={"gz_args": world_cfg}.items(), + ) + + spawn_robots_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("panther_gazebo"), + "launch", + "spawn.launch.py", + ] + ) + ), + launch_arguments={ + "wheel_type": wheel_type, + "wheel_config_path": wheel_config_path, + "controller_config_path": controller_config_path, + "battery_config_path": battery_config_path, + "gz_bridge_config_path": gz_bridge_config_path, + "x": x, + "y": y, + "z": z, + "roll": roll, + "pitch": pitch, + "yaw": yaw, + "publish_robot_state": publish_robot_state, + "namespace": namespace, + "robots": robots, + "add_map_transform": add_map_transform, + }.items(), + ) + return LaunchDescription( [ declare_world_arg, @@ -303,8 +246,10 @@ def generate_launch_description(): declare_publish_robot_state_arg, declare_namespace_arg, declare_robots_arg, + declare_add_map_transform_arg, # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo) SetParameter(name="use_sim_time", value=True), - OpaqueFunction(function=launch_setup), + gz_sim, + spawn_robots_launch, ] ) diff --git a/panther_gazebo/launch/spawn.launch.py b/panther_gazebo/launch/spawn.launch.py new file mode 100644 index 000000000..985014604 --- /dev/null +++ b/panther_gazebo/launch/spawn.launch.py @@ -0,0 +1,298 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Husarion sp. z o.o. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + LogInfo, + OpaqueFunction, + TimerAction, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ParseMultiRobotPose + + +def launch_setup(context): + wheel_type = LaunchConfiguration("wheel_type").perform(context) + wheel_config_path = LaunchConfiguration("wheel_config_path").perform(context) + controller_config_path = LaunchConfiguration("controller_config_path").perform(context) + battery_config_path = LaunchConfiguration("battery_config_path").perform(context) + gz_bridge_config_path = LaunchConfiguration("gz_bridge_config_path").perform(context) + x = LaunchConfiguration("x").perform(context) + y = LaunchConfiguration("y").perform(context) + z = LaunchConfiguration("z").perform(context) + roll = LaunchConfiguration("roll").perform(context) + pitch = LaunchConfiguration("pitch").perform(context) + yaw = LaunchConfiguration("yaw").perform(context) + publish_robot_state = LaunchConfiguration("publish_robot_state").perform(context) + namespace = LaunchConfiguration("namespace").perform(context) + add_map_transform = LaunchConfiguration("add_map_transform").perform(context) + + robots_list = ParseMultiRobotPose("robots").value() + if len(robots_list) == 0: + robots_list = { + namespace: {"x": x, "y": y, "z": z, "roll": roll, "pitch": pitch, "yaw": yaw} + } + + spawn_group = [] + for idx, robot_name in enumerate(robots_list): + init_pose = robots_list[robot_name] + x, y, z, roll, pitch, yaw = [str(value) for value in init_pose.values()] + + spawn_log = LogInfo(msg=[f"Launching namespace={robot_name} with init_pose={init_pose}"]) + + spawn_robot = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + robot_name, + "-topic", + "robot_description", + "-x", + x, + "-y", + y, + "-z", + z, + "-Y", + yaw, + ], + namespace=robot_name, + output="screen", + ) + + gz_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="gz_bridge", + parameters=[{"config_file": gz_bridge_config_path}], + namespace=robot_name, + output="screen", + ) + + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("panther_bringup"), + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "wheel_type": wheel_type, + "wheel_config_path": wheel_config_path, + "controller_config_path": controller_config_path, + "battery_config_path": battery_config_path, + "publish_robot_state": publish_robot_state, + "use_sim": "True", + "simulation_engine": "ignition-gazebo", + "namespace": robot_name, + }.items(), + ) + + ns_prefix = robot_name + "/" if robot_name else robot_name + + map_transform = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_tf_publisher", + namespace=robot_name, + output="screen", + arguments=[x, y, z, roll, pitch, yaw, "map", ns_prefix + "odom"], + condition=IfCondition(add_map_transform), + ) + + # bringup.launch.py has a timerAction in it. If the timerAction in simulation.launch.py ​​is smaller than bringup.launhc.py, the namespace will be overwritten, resulting creating nodes with the same namespace. + group = TimerAction( + period=10.0 * idx, + actions=[ + spawn_log, + spawn_robot, + gz_bridge, + bringup_launch, + map_transform, + ], + ) + spawn_group.append(group) + + return spawn_group + + +def generate_launch_description(): + wheel_type = LaunchConfiguration("wheel_type") + declare_wheel_type_arg = DeclareLaunchArgument( + "wheel_type", + default_value="WH01", + description=( + "Specify the type of wheel. If you select a value from the provided options ('WH01'," + " 'WH02', 'WH04'), you can disregard the 'wheel_config_path' and" + " 'controller_config_path' parameters. If you have custom wheels, set this parameter" + " to 'CUSTOM' and provide the necessary configurations." + ), + choices=["WH01", "WH02", "WH04", "CUSTOM"], + ) + + declare_wheel_config_path_arg = DeclareLaunchArgument( + "wheel_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("panther_description"), + "config", + PythonExpression(["'", wheel_type, ".yaml'"]), + ] + ), + description=( + "Path to wheel configuration file. By default, it is located in " + "'panther_description/config/.yaml'. You can also specify the path " + "to your custom wheel configuration file here. " + ), + ) + + declare_controller_config_path_arg = DeclareLaunchArgument( + "controller_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("panther_controller"), + "config", + PythonExpression(["'", wheel_type, "_controller.yaml'"]), + ] + ), + description=( + "Path to controller configuration file. By default, it is located in" + " 'panther_controller/config/_controller.yaml'. You can also specify" + " the path to your custom controller configuration file here. " + ), + ) + + declare_battery_config_path_arg = DeclareLaunchArgument( + "battery_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("panther_gazebo"), + "config", + "battery_plugin_config.yaml", + ] + ), + description=( + "Path to the Ignition LinearBatteryPlugin configuration file. " + "This configuration is intended for use in simulations only." + ), + ) + + declare_gz_bridge_config_path_arg = DeclareLaunchArgument( + "gz_bridge_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("panther_gazebo"), + "config", + "gz_bridge.yaml", + ] + ), + description="Path to the parameter_bridge configuration file", + ) + + declare_x_arg = DeclareLaunchArgument( + "x", default_value="5.0", description="Initial robot position in the global 'x' axis." + ) + + declare_y_arg = DeclareLaunchArgument( + "y", default_value="-5.0", description="Initial robot position in the global 'y' axis." + ) + + declare_z_arg = DeclareLaunchArgument( + "z", default_value="0.2", description="Initial robot position in the global 'z' axis." + ) + + declare_roll_arg = DeclareLaunchArgument( + "roll", default_value="0.0", description="Initial robot 'roll' orientation." + ) + + declare_pitch_arg = DeclareLaunchArgument( + "pitch", default_value="0.0", description="Initial robot orientation." + ) + + declare_yaw_arg = DeclareLaunchArgument( + "yaw", default_value="0.0", description="Initial robot orientation." + ) + + declare_publish_robot_state_arg = DeclareLaunchArgument( + "publish_robot_state", + default_value="True", + description=( + "Whether to launch the robot_state_publisher node." + "When set to False, users should publish their own robot description." + ), + ) + + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Add namespace to all launched nodes", + ) + + declare_robots_arg = DeclareLaunchArgument( + "robots", + default_value=[], + description=( + "The list of the robots spawned in the simulation e. g. robots:='robot1={x: 0.0, y:" + " -1.0}; robot2={x: 1.0, y: -1.0}'" + ), + ) + + declare_add_map_transform_arg = DeclareLaunchArgument( + "add_map_transform", + default_value="False", + description=( + "Adds a frame map that connects the tf trees of individual robots (useful when running" + " multiple robots)." + ), + ) + + return LaunchDescription( + [ + declare_x_arg, + declare_y_arg, + declare_z_arg, + declare_roll_arg, + declare_pitch_arg, + declare_yaw_arg, + declare_wheel_type_arg, + declare_wheel_config_path_arg, + declare_controller_config_path_arg, + declare_battery_config_path_arg, + declare_gz_bridge_config_path_arg, + declare_publish_robot_state_arg, + declare_namespace_arg, + declare_robots_arg, + declare_add_map_transform_arg, + # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo) + SetParameter(name="use_sim_time", value=True), + OpaqueFunction(function=launch_setup), + ] + )