From c60478a7adb6fc986197403b49b203532987929f Mon Sep 17 00:00:00 2001 From: Shubham Gawande Date: Wed, 28 Feb 2024 12:06:29 +0100 Subject: [PATCH] added multi robot spawing functinality --- launch/robile_mrs.launch.py | 106 ++++++++++++++++++++++++++++ robile_safety/multi_robot_system.py | 0 setup.py | 1 + urdf/4_wheel_config.urdf.xacro | 76 ++++++++++++++++++++ 4 files changed, 183 insertions(+) create mode 100644 launch/robile_mrs.launch.py create mode 100644 robile_safety/multi_robot_system.py create mode 100644 urdf/4_wheel_config.urdf.xacro diff --git a/launch/robile_mrs.launch.py b/launch/robile_mrs.launch.py new file mode 100644 index 0000000..38cea7f --- /dev/null +++ b/launch/robile_mrs.launch.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +# Authors: Deebul Nair + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import TextSubstitution +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterValue + +def generate_robot_nodes(robot_name, x_pose, y_pose): + pkg_share = FindPackageShare("robile_safety").find("robile_safety") + urdf_file = PathJoinSubstitution([pkg_share, "urdf", "4_wheel_config.urdf.xacro"]) + + xacro_command = [ + FindExecutable(name="xacro"), + TextSubstitution(text=" "), + urdf_file, + TextSubstitution(text=" "), + TextSubstitution(text="robot_name:=" + robot_name) + ] + + urdf_content = Command(xacro_command) + + # Wrap the URDF content in ParameterValue to explicitly define it as a string + robot_description = {'robot_description': ParameterValue(urdf_content, value_type=str)} + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description], + namespace=robot_name + ) + + spawn_entity = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-entity', robot_name, '-topic', 'robot_description', '-x', x_pose, '-y', y_pose], + output='screen', + namespace=robot_name + ) + + return GroupAction([robot_state_publisher, spawn_entity]) + +def generate_launch_description(): + # Set the number of robots + number_of_robots = 3 + + # Define the world + world = os.path.join(get_package_share_directory('robile_gazebo'), 'worlds', 'closed_walls.world') + + # Common Gazebo nodes + gzserver = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gzserver.launch.py')), + launch_arguments={'world': world}.items() + ) + + gzclient = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gzclient.launch.py')) + ) + + # Generate robot nodes + robot_nodes = [] + for i in range(number_of_robots): + robot_name = f'robot_{i}' + x_pose = str(1.0 * i) + y_pose = '-3.5' + robot_nodes.append(generate_robot_nodes(robot_name, x_pose, y_pose)) + + return LaunchDescription([ + gzserver, + gzclient, + *robot_nodes + ]) + + # urdf_path = os.path.join( + # get_package_share_directory('robile_description'), + # 'robots', + # urdf_file_name) + + # with open(urdf_path, 'r') as infp: + # robot_desc = infp.read() + + # return LaunchDescription([ + # DeclareLaunchArgument( + # 'use_sim_time', + # default_value='false', + # description='Use simulation (Gazebo) clock if true'), + + # Node( + # package='robot_state_publisher', + # executable='robot_state_publisher', + # name='robot_state_publisher', + # output='screen', + # parameters=[{ + # 'use_sim_time': use_sim_time, + # 'robot_description': robot_desc + # }], + # ), + # ]) \ No newline at end of file diff --git a/robile_safety/multi_robot_system.py b/robile_safety/multi_robot_system.py new file mode 100644 index 0000000..e69de29 diff --git a/setup.py b/setup.py index 95894f6..5b30689 100644 --- a/setup.py +++ b/setup.py @@ -13,6 +13,7 @@ ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share/', package_name, 'launch'), glob('launch/*.py')), + (os.path.join('share/', package_name, 'urdf'), glob('urdf/*.xacro')), ], install_requires=['setuptools', 'py_trees', 'rclpy'], zip_safe=True, diff --git a/urdf/4_wheel_config.urdf.xacro b/urdf/4_wheel_config.urdf.xacro new file mode 100644 index 0000000..7092d3b --- /dev/null +++ b/urdf/4_wheel_config.urdf.xacro @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +