Added Couzin swarm model using turtlebot3_burger, model performance i…

swarm/launch/main.launch.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+
+
+def generate_launch_description():
+	use_sim_time = LaunchConfiguration('use_sim_time', default='True')
+
+	launch_file_dir = get_package_share_directory('swarm')
+	pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+	gzserver = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+	os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),)
+
+	gzclient = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+	os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')),)
+
+	execute_pro = ExecuteProcess(cmd=['ros2', 'param', 'set', '/gazebo', 
+	'use_sim_time', use_sim_time],output='screen')
+
+	multi_spawn = IncludeLaunchDescription(PythonLaunchDescriptionSource(
+	os.path.join(launch_file_dir,'multi_spawn.launch.py')),)
+
+
+	return LaunchDescription([gzserver,
+	gzclient,
+	execute_pro,
+	multi_spawn,])

swarm/launch/multi_spawn.launch.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+
+import sys
+import os
+import numpy as np
+from ament_index_python.packages import get_package_share_directory, get_package_prefix
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+
+def agents_list(num_of_agents):
+
+    agents = []
+
+    for i in range(num_of_agents):
+        agent_name = "tb3_"+str(i)
+        x = np.random.uniform(0,50)
+        y = np.random.uniform(0,50)                         
+        w = np.random.uniform(0,360)
+        agents.append({'name': agent_name, 'x_pose': x, 'y_pose': y, 'z_pose': 0.01,'w_pose':w})
+
+    return agents 
+
+def generate_launch_description():
+
+    TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    urdf_file_name = 'turtlebot3_' + TURTLEBOT3_MODEL + '.urdf'
+    sdf_file = 'turtlebot3_' + TURTLEBOT3_MODEL +'/model.sdf'
+    print('urdf_file_name : {}'.format(urdf_file_name))
+
+    urdf = os.path.join(
+        get_package_share_directory('turtlebot3_description'),
+        'urdf',
+        urdf_file_name)
+    sdf = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'models',
+        sdf_file)
+    # Names and poses of the robots
+    robots = agents_list(50)
+
+    # We create the list of spawn robots commands
+    spawn_robots_cmds = []
+    for robot in robots:
+
+        spawn_robots_cmds.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                os.path.join(get_package_share_directory('swarm'),
+                                                           'spawn_tb3.launch.py')),
+                launch_arguments={
+                                  'robot_urdf':sdf,
+                                  'x': TextSubstitution(text=str(robot['x_pose'])),
+                                  'y': TextSubstitution(text=str(robot['y_pose'])),
+                                  'z': TextSubstitution(text=str(robot['z_pose'])),
+                                  'w': TextSubstitution(text=str(robot['w_pose'])),
+                                  'robot_name': robot['name'],
+                                  'robot_namespace': robot['name']
+                                  }.items()))
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    for spawn_robot_cmd in spawn_robots_cmds:
+        ld.add_action(spawn_robot_cmd)
+
+    return ld

swarm/launch/spawn_tb3.launch.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+
+from launch import LaunchDescription
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+import launch.actions
+import launch_ros.actions
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='True')
+    world_file_name = 'empty_worlds/' + TURTLEBOT3_MODEL + '.model'
+    world = os.path.join(get_package_share_directory('turtlebot3_gazebo'),
+                         'worlds', world_file_name)
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    return LaunchDescription([
+        Node(
+            package='swarm',
+            executable='spawn_tb3',
+            output='screen',
+            arguments=[
+                '--robot_urdf', launch.substitutions.LaunchConfiguration('robot_urdf'),
+                '--robot_name', launch.substitutions.LaunchConfiguration('robot_name'),
+                '--robot_namespace', launch.substitutions.LaunchConfiguration('robot_namespace'),
+                '-x', launch.substitutions.LaunchConfiguration('x'),
+                '-y', launch.substitutions.LaunchConfiguration('y'),
+                '-z', launch.substitutions.LaunchConfiguration('z'),
+                '-w', launch.substitutions.LaunchConfiguration('w')]),
+
+    ])
+

swarm/launch/spawn_tb3.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python3
+
+"""Script used to spawn a tb3 in a generic position."""
+
+import argparse
+import os
+import xml.etree.ElementTree as ET
+
+from ament_index_python.packages import get_package_share_directory
+from gazebo_msgs.srv import SpawnEntity
+import rclpy
+
+
+def main():
+    # Get input arguments from user
+    parser = argparse.ArgumentParser(description='Spawn Robot into Gazebo with navigation2')
+    parser.add_argument('-urdf', '--robot_urdf', type=str, default='dummy.urdf',
+                        help='Name of the robot to spawn')
+    parser.add_argument('-n', '--robot_name', type=str, default='dummy_robot',
+                        help='Name of the robot to spawn')
+    parser.add_argument('-ns', '--robot_namespace', type=str, default='dummy_robot_ns',
+                        help='ROS namespace to apply to the tf and plugins')
+    parser.add_argument('-namespace', '--namespace', type=bool, default=True,
+                        help='Whether to enable namespacing')
+    parser.add_argument('-x', type=float, default=0,
+                        help='the x component of the initial position [meters]')
+    parser.add_argument('-y', type=float, default=0,
+                        help='the y component of the initial position [meters]')
+    parser.add_argument('-z', type=float, default=0,
+                        help='the z component of the initial position [meters]')
+
+    args, unknown = parser.parse_known_args()
+
+    # Start node
+    rclpy.init()
+    node = rclpy.create_node('entity_spawner')
+
+    node.get_logger().info(
+        'Creating Service client to connect to `/spawn_entity`')
+    client = node.create_client(SpawnEntity, '/spawn_entity')
+
+    node.get_logger().info('Connecting to `/spawn_entity` service...')
+    if not client.service_is_ready():
+        client.wait_for_service()
+        node.get_logger().info('...connected!')
+
+    # sdf_file_path = os.path.join(
+    #     get_package_share_directory('amazon_robot_gazebo'), 'models',
+    #     'amazon_robot2', 'model.sdf')
+    urdf_file_path = args.robot_urdf
+    print(urdf_file_path)
+
+#    # We need to remap the transform (/tf) topic so each robot has its own.
+#    # We do this by adding `ROS argument entries` to the urdf file for
+#    # each plugin broadcasting a transform. These argument entries provide the
+#    # remapping rule, i.e. /tf -> /<robot_id>/tf
+#    tree = ET.parse(urdf_file_path)
+#    root = tree.getroot()
+#    imu_plugin = None
+#    diff_drive_plugin = None 
+#    for plugin in root.iter('plugin'):
+#        if 'differential_drive_controller' in plugin.attrib.values():
+#            diff_drive_plugin = plugin
+#        elif 'box_bot_imu_plugin' in plugin.attrib.values():
+#            imu_plugin = plugin
+
+#    # We change the namespace to the robots corresponding one
+#    tag_diff_drive_ros_params = diff_drive_plugin.find('ros')
+#    tag_diff_drive_ns = ET.SubElement(tag_diff_drive_ros_params, 'namespace')
+#    tag_diff_drive_ns.text = '/' + args.robot_namespace
+#    ros_tf_remap = ET.SubElement(tag_diff_drive_ros_params, 'remapping')
+#    ros_tf_remap.text = '/tf:=/' + args.robot_namespace + '/tf'
+
+#    # if imu_plugin is not None:
+#    #     tag_imu_ros_params = imu_plugin.find('ros')
+#    #     tag_imu_ns = ET.SubElement(tag_imu_ros_params, 'namespace')
+#    #     tag_imu_ns.text = '/' + args.robot_namespace + '/imu'
+#    # else:
+#    #     print("ERROR>>>>>>>>>>>>>>>>>>>>> IMU NOT FOUND")
+
+
+    # Set data for request
+    request = SpawnEntity.Request()
+    request.name = args.robot_name
+    request.xml = ET.tostring(root, encoding='unicode')
+    request.initial_pose.position.x = float(args.x)
+    request.initial_pose.position.y = float(args.y)
+    request.initial_pose.position.z = float(args.z)
+
+
+    if args.namespace is True:
+        node.get_logger().info('spawning `{}` on namespace `{}` at {}, {}, {}'.format(
+            args.robot_name, args.robot_namespace, args.x, args.y, args.z))
+
+        request.robot_namespace = args.robot_namespace
+        print(args.robot_namespace)
+
+    else:
+        node.get_logger().info('spawning `{}` at {}, {}, {}'.format(
+            args.robot_name, args.x, args.y, args.z))
+
+    node.get_logger().info('Spawning Robot using service: `/spawn_entity`')
+    future = client.call_async(request)
+    rclpy.spin_until_future_complete(node, future)
+    if future.result() is not None:
+        print('response: %r' % future.result())
+    else:
+        raise RuntimeError(
+            'exception while calling service: %r' % future.exception())
+
+    node.get_logger().info('Done! Shutting down node.')
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
+