From ec58ca856541e30a7d85b26dba7fef0adfe17434 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 26 Mar 2024 14:45:06 +0100 Subject: [PATCH] Multi robot spawn working --- README.md | 16 +- panther_battery/launch/battery.launch.py | 9 + panther_bringup/launch/bringup.launch.py | 20 +- .../launch/controller.launch.py | 7 + panther_gazebo/launch/simulation.launch.py | 196 +++++++++++------- panther_gazebo/package.xml | 1 + panther_lights/launch/lights.launch.py | 10 + panther_manager/launch/manager_bt.launch.py | 9 + 8 files changed, 172 insertions(+), 96 deletions(-) diff --git a/README.md b/README.md index b05e654d7..e9297b36a 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,14 @@ ROS 2 packages for Panther autonomous mobile robot ## Quick start +### Create workspace + +```bash +mkdir ~/husarion_ws +cd ~/husarion_ws +git clone https://github.com/husarion/panther_ros.git src/panther_ros +``` + ### Configure environment The repository is used to run the code both on the real robot and in the simulation. Specify `HUSARION_ROS_BUILD_TYPE` the variable according to your needs. @@ -22,14 +30,6 @@ Simulation: export HUSARION_ROS_BUILD_TYPE=simulation ``` -### Create workspace - -```bash -mkdir ~/husarion_ws -cd ~/husarion_ws -git clone https://github.com/husarion/panther_ros.git src/panther_ros -``` - ### Build ``` bash diff --git a/panther_battery/launch/battery.launch.py b/panther_battery/launch/battery.launch.py index fa4698a56..e0d83b031 100644 --- a/panther_battery/launch/battery.launch.py +++ b/panther_battery/launch/battery.launch.py @@ -21,6 +21,13 @@ def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all Panther topics", + ) + panther_version = LaunchConfiguration("panther_version") declare_panther_version_arg = DeclareLaunchArgument("panther_version") @@ -29,9 +36,11 @@ def generate_launch_description(): executable="battery_node", name="battery_node", parameters=[{"panther_version": panther_version}], + namespace=namespace, ) actions = [ + declare_namespace_arg, declare_panther_version_arg, battery_node, ] diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index 419aa73d6..aa37cc776 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -33,7 +33,7 @@ PathJoinSubstitution, PythonExpression, ) -from launch_ros.actions import Node, PushRosNamespace, SetParameter +from launch_ros.actions import Node, SetParameter from launch_ros.substitutions import FindPackageShare @@ -294,6 +294,7 @@ def generate_launch_description(): ("/tf", "tf"), ("/tf_static", "tf_static"), ], + namespace=namespace, condition=IfCondition(use_ekf), ) @@ -309,13 +310,14 @@ def generate_launch_description(): ), condition=UnlessCondition(use_sim), launch_arguments={ + "namespace": namespace, "panther_version": panther_version, "shutdown_hosts_config_path": shutdown_hosts_config_path, }.items(), ) other_action_timer = TimerAction( - period=10.0, + period=7.0, actions=[ battery_launch, imu_launch, @@ -325,18 +327,6 @@ def generate_launch_description(): ], ) - waiting_msg = TimerAction( - period=7.0, - actions=[ - LogInfo( - msg=( - "We're working on ensuring everything functions properly... Please wait a few" - " seconds more!" - ) - ) - ], - ) - actions = [ declare_namespace_arg, declare_use_sim_arg, @@ -351,11 +341,9 @@ def generate_launch_description(): declare_use_ekf_arg, declare_ekf_config_path_arg, declare_shutdown_hosts_config_path_arg, - PushRosNamespace(namespace), SetParameter(name="use_sim_time", value=use_sim), welcome_msg, controller_launch, - waiting_msg, other_action_timer, ] diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py index 051228480..047d767e2 100644 --- a/panther_controller/launch/controller.launch.py +++ b/panther_controller/launch/controller.launch.py @@ -151,6 +151,7 @@ def generate_launch_description(): ("/tf", "tf"), ("/tf_static", "tf_static"), ], + namespace=namespace, condition=UnlessCondition(use_sim), ) @@ -161,6 +162,7 @@ def generate_launch_description(): parameters=[robot_description], remappings=[("/tf", "tf"), ("/tf_static", "tf_static")], condition=IfCondition(publish_robot_state), + namespace=namespace, ) robot_controller_spawner = Node( @@ -175,6 +177,7 @@ def generate_launch_description(): "--namespace", namespace, ], + namespace=namespace, ) joint_state_broadcaster_spawner = Node( @@ -189,6 +192,7 @@ def generate_launch_description(): "--namespace", namespace, ], + namespace=namespace, ) # Delay start of robot_controller after joint_state_broadcaster @@ -208,7 +212,10 @@ def generate_launch_description(): "controller_manager", "--controller-manager-timeout", "10", + "--namespace", + namespace, ], + namespace=namespace, ) # Delay start of imu_broadcaster after robot_controller diff --git a/panther_gazebo/launch/simulation.launch.py b/panther_gazebo/launch/simulation.launch.py index cca236dc1..18d89f2ac 100644 --- a/panther_gazebo/launch/simulation.launch.py +++ b/panther_gazebo/launch/simulation.launch.py @@ -15,16 +15,23 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + LogInfo, + TimerAction, +) 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.substitutions import FindPackageShare +from nav2_common.launch import ParseMultiRobotPose def generate_launch_description(): @@ -120,30 +127,40 @@ def generate_launch_description(): description="SDF world file", ) - pose_x = LaunchConfiguration("pose_x") - declare_pose_x_arg = DeclareLaunchArgument( - "pose_x", + x = LaunchConfiguration("x") + declare_x_arg = DeclareLaunchArgument( + "x", default_value=["5.0"], description="Initial robot position in the global 'x' axis.", ) - pose_y = LaunchConfiguration("pose_y") - declare_pose_y_arg = DeclareLaunchArgument( - "pose_y", + y = LaunchConfiguration("y") + declare_y_arg = DeclareLaunchArgument( + "y", default_value=["-5.0"], description="Initial robot position in the global 'y' axis.", ) - pose_z = LaunchConfiguration("pose_z") - declare_pose_z_arg = DeclareLaunchArgument( - "pose_z", + z = LaunchConfiguration("z") + declare_z_arg = DeclareLaunchArgument( + "z", default_value=["0.2"], description="Initial robot position in the global 'z' axis.", ) - rot_yaw = LaunchConfiguration("rot_yaw") - declare_rot_yaw_arg = DeclareLaunchArgument( - "rot_yaw", default_value=["0.0"], description="Initial robot orientation." + roll = LaunchConfiguration("roll") + declare_roll_arg = DeclareLaunchArgument( + "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." + ) + + yaw = LaunchConfiguration("yaw") + declare_yaw_arg = DeclareLaunchArgument( + "yaw", default_value=["0.0"], description="Initial robot orientation." ) publish_robot_state = LaunchConfiguration("publish_robot_state") @@ -163,6 +180,15 @@ def generate_launch_description(): description="Namespace for all Panther topics", ) + 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}'" + ), + ) + gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -176,68 +202,95 @@ def generate_launch_description(): launch_arguments={"gz_args": world_cfg}.items(), ) - gz_spawn_entity = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", - "panther", - "-allow_renaming", - "true", - "-topic", - "robot_description", - "-x", - pose_x, - "-y", - pose_y, - "-z", - pose_z, - "-Y", - rot_yaw, - ], - output="screen", - namespace=namespace, - ) + 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} + } - gz_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="gz_bridge", - parameters=[{"config_file": gz_bridge_config_path}], - remappings=[("/tf", "tf"), ("/tf_static", "tf_static")], - namespace=namespace, - output="screen", - ) + spawn_group = [] + for idx, robot_name in enumerate(robots_list): + init_pose = robots_list[robot_name] - 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": namespace, - }.items(), - ) + 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", + "panther", + "-allow_renaming", + "true", + "-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}], + remappings=[("/tf", "tf"), ("/tf_static", "tf_static")], + 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=5.0 * idx, + actions=[ + spawn_log, + spawn_robot, + gz_bridge, + bringup_launch, + ], + ) + spawn_group.append(group) return LaunchDescription( [ declare_world_arg, - declare_pose_x_arg, - declare_pose_y_arg, - declare_pose_z_arg, - declare_rot_yaw_arg, + 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, @@ -245,11 +298,10 @@ def generate_launch_description(): declare_gz_bridge_config_path_arg, declare_publish_robot_state_arg, declare_namespace_arg, + declare_robots_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), gz_sim, - gz_bridge, - gz_spawn_entity, - bringup_launch, + *spawn_group, ] ) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index 96b706c55..3af509214 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -21,6 +21,7 @@ ign_ros2_control launch launch_ros + nav2_common panther_bringup robot_state_publisher ros_gz_bridge diff --git a/panther_lights/launch/lights.launch.py b/panther_lights/launch/lights.launch.py index 10e7fbd15..a8f005dc5 100644 --- a/panther_lights/launch/lights.launch.py +++ b/panther_lights/launch/lights.launch.py @@ -28,6 +28,13 @@ def generate_launch_description(): description="Path to a YAML file with a description of led configuration", ) + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all Panther topics", + ) + user_led_animations_file = LaunchConfiguration("user_led_animations_file") declare_user_led_animations_file_arg = DeclareLaunchArgument( "user_led_animations_file", @@ -40,6 +47,7 @@ def generate_launch_description(): executable="driver_node", name="lights_driver_node", on_exit=Shutdown(), + namespace=namespace, ) lights_controller_node = Node( @@ -51,10 +59,12 @@ def generate_launch_description(): {"user_led_animations_file": user_led_animations_file}, ], on_exit=Shutdown(), + namespace=namespace, ) actions = [ declare_led_config_file_arg, + declare_namespace_arg, declare_user_led_animations_file_arg, lights_driver_node, lights_controller_node, diff --git a/panther_manager/launch/manager_bt.launch.py b/panther_manager/launch/manager_bt.launch.py index 74584c374..c29c493c2 100644 --- a/panther_manager/launch/manager_bt.launch.py +++ b/panther_manager/launch/manager_bt.launch.py @@ -52,6 +52,13 @@ def launch_setup(context): description="Path to BehaviorTree project file.", ) + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all Panther topics", + ) + shutdown_hosts_config_path = LaunchConfiguration("shutdown_hosts_config_path") declare_shutdown_hosts_config_path_arg = DeclareLaunchArgument( "shutdown_hosts_config_path", @@ -70,11 +77,13 @@ def launch_setup(context): "shutdown_hosts_path": shutdown_hosts_config_path, }, ], + namespace=namespace, ) return [ declare_panther_version_arg, declare_bt_project_path_arg, + declare_namespace_arg, declare_shutdown_hosts_config_path_arg, manager_bt_node, ]