From 86218737309a092b4dd37c60d2eede051ac34571 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 23 Apr 2024 12:46:11 +0200 Subject: [PATCH] Separate spawn --- .pre-commit-config.yaml | 12 +- panther_gazebo/README.md | 15 +- panther_gazebo/launch/simulation.launch.py | 209 ++++++--------- panther_gazebo/launch/spawn.launch.py | 297 +++++++++++++++++++++ 4 files changed, 390 insertions(+), 143 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/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..28c7361e2 --- /dev/null +++ b/panther_gazebo/launch/spawn.launch.py @@ -0,0 +1,297 @@ +#!/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), + ) + + 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), + ] + )