Skip to content

Commit

Permalink
Separate spawn
Browse files Browse the repository at this point in the history
  • Loading branch information
@rafal-gorecki
rafal-gorecki committed Apr 23, 2024
1 parent 0dad65d commit 8621873
Show file tree
Hide file tree
Showing 4 changed files with 390 additions and 143 deletions.
12 changes: 6 additions & 6 deletions .pre-commit-config.yaml
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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"]
Expand Down Expand Up @@ -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:
Expand Down
15 changes: 10 additions & 5 deletions panther_gazebo/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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/<wheel_type arg>_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/<wheel_type arg>.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 <husarion_office_gz share directory>/worlds/husarion_world.sdf**]: path to Gazebo world file used for simulation.
Expand Down
209 changes: 77 additions & 132 deletions panther_gazebo/launch/simulation.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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():
Expand All @@ -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(
Expand All @@ -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(
Expand All @@ -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(
Expand All @@ -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(
Expand All @@ -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=[
Expand All @@ -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",
Expand All @@ -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=[],
Expand All @@ -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,
Expand All @@ -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,
]
)
Loading

0 comments on commit 8621873

Please sign in to comment.