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),
+ ]
+ )