Merge pull request #4 from ShubhamGawande191/unittest

added multi robot spawing functinality

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

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,

urdf/4_wheel_config.urdf.xacro

@@ -0,0 +1,76 @@
+<?xml version='1.0'?>
+
+<!--
+Copyright (c) 2021
+KELO Robotics GmbH
+
+Author:
+Sushant Chavan
+
+
+This software is published under a dual-license: GNU Lesser General Public
+License LGPL 2.1 and BSD license. The dual-license implies that users of this
+code may choose which terms they prefer.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright
+notice, this list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above copyright
+notice, this list of conditions and the following disclaimer in the
+documentation and/or other materials provided with the distribution.
+* Neither the name of Locomotec nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License LGPL as
+published by the Free Software Foundation, either version 2.1 of the
+License, or (at your option) any later version or the BSD license.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License LGPL and the BSD license for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License LGPL and BSD license along with this program.
+-->
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="4_wheel_config" >
+
+    <xacro:arg name="movable_joints" default="true"/>
+
+    <!-- Include desired robile bricks -->
+    <xacro:include filename="$(find robile_description)/urdf/robile_bricks/robile_active_wheel_brick.urdf.xacro" />
+    <xacro:include filename="$(find robile_description)/urdf/robile_bricks/robile_cpu_brick.urdf.xacro" />
+    <xacro:include filename="$(find robile_description)/urdf/robile_bricks/robile_master_battery_brick.urdf.xacro" />
+
+    <link name="base_link"/>
+
+    <!-- Build platform using robile bricks -->
+    <xacro:robile_active_wheel_brick name="robile_1" parent="base_link" movable_joints="$(arg movable_joints)">
+        <origin xyz="0.233 0.1165 0.05" rpy="0.0 0.0 0.0"/>
+    </xacro:robile_active_wheel_brick>
+
+    <xacro:robile_active_wheel_brick name="robile_2" parent="base_link" movable_joints="$(arg movable_joints)">
+        <origin xyz="0.233 -0.1165 0.05" rpy="0.0 0.0 0.0"/>
+    </xacro:robile_active_wheel_brick>
+
+    <xacro:robile_cpu_brick name="robile_3" parent="base_link">
+        <origin xyz="0.0 0.1165 0.05" rpy="0.0 0.0 0.0"/>
+    </xacro:robile_cpu_brick>
+
+    <xacro:robile_master_battery_brick name="robile_4" parent="base_link">
+        <origin xyz="0.0 -0.1165 0.05" rpy="0.0 0.0 0.0"/>
+    </xacro:robile_master_battery_brick>
+
+    <xacro:robile_active_wheel_brick name="robile_5" parent="base_link" movable_joints="$(arg movable_joints)">
+        <origin xyz="-0.233 0.1165 0.05" rpy="0.0 0.0 0.0"/>
+    </xacro:robile_active_wheel_brick>
+
+    <xacro:robile_active_wheel_brick name="robile_6" parent="base_link" movable_joints="$(arg movable_joints)">
+        <origin xyz="-0.233 -0.1165 0.05" rpy="0.0 0.0 0.0"/>
+    </xacro:robile_active_wheel_brick>
+</robot>