[multirobot] Spawn multiple robots into Gazebo (ros-navigation#1146)

Moving original files to bringup folder
Adding node for spawning TB3's into Gazebo

nav2_bringup/bringup/launch/spawn_robot_launch.py

@@ -0,0 +1,36 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# 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.
+
+import os
+
+from launch import LaunchDescription
+
+import launch.actions
+import launch_ros.actions
+
+def generate_launch_description():
+
+    return LaunchDescription([
+        #TODO(orduno) might not be necessary to have it's own package
+        launch_ros.actions.Node(
+            package='spawn_turtlebot3',
+            node_executable='spawn_turtlebot',
+            output='screen',
+            arguments=[
+                    launch.substitutions.LaunchConfiguration('robot_name'),
+                    launch.substitutions.LaunchConfiguration('robot_name'),
+                    launch.substitutions.LaunchConfiguration('x_pose'),
+                    launch.substitutions.LaunchConfiguration('y_pose'),
+                    launch.substitutions.LaunchConfiguration('z_pose')]),
+    ])

nav2_bringup/spawn_turtlebot3/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>spawn_turtlebot3</name>
+  <version>0.2.4</version>
+  <description>Package for spawning Turtlebot3 into Gazebo</description>
+  <maintainer email="carlos.orduno@intel.com">lkumarbe</maintainer>
+  <maintainer email="lalit.kumar.begani@intel.com">lkumarbe</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

nav2_bringup/spawn_turtlebot3/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/spawn_turtlebot3
+[install]
+install-scripts=$base/lib/spawn_turtlebot3

nav2_bringup/spawn_turtlebot3/setup.py

@@ -0,0 +1,23 @@
+from setuptools import setup
+
+PACKAGE_NAME = 'spawn_turtlebot3'
+
+setup(
+    name=PACKAGE_NAME,
+    version='1.0.0',
+    package_dir={'': 'src'},
+    packages=[PACKAGE_NAME],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + PACKAGE_NAME]),
+        ('share/' + PACKAGE_NAME, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'spawn_turtlebot = spawn_turtlebot3.spawn_turtlebot:main',
+        ],
+    },
+)

nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/spawn_turtlebot.py

@@ -0,0 +1,94 @@
+"""
+spawn_turtlebot.py
+
+Script used to spawn a turtlebot in a generic position
+"""
+import os
+import sys
+import rclpy
+import argparse
+from ament_index_python.packages import get_package_share_directory
+from gazebo_msgs.srv import SpawnEntity
+import xml.etree.ElementTree as ET
+
+
+def main():
+    """ Main for spawning turtlebot node """
+    # Get input arguments from user
+    parser = argparse.ArgumentParser(description='Spawn Turtlebot3 into Gazebo')
+    parser.add_argument('-n', '--robot_name', type=str, default='robot',
+                        help='Name of the robot to spawn')
+    parser.add_argument('-ns', '--robot_namespace', type=str, default='robot',
+                        help='ROS namespace to apply to the tf and plugins')
+    parser.add_argument('-t', '--turtlebot_type', type=str, default='waffle',
+                        choices=['waffle', 'burger'])
+    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 = parser.parse_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!")
+
+    node.get_logger().info('spawning a `{}` with name `{}` on namespace `{}` at {}, {}, {}'.format(
+        args.turtlebot_type, args.robot_name, args.robot_namespace, args.x, args.y, args.z))
+
+    # Get path to the turtlebot3 burgerbot
+    sdf_file_path = os.path.join(
+        get_package_share_directory("turtlebot3_gazebo"), "models",
+        "turtlebot3_{}".format(args.turtlebot_type), "model.sdf")
+
+    # We need to remap the transform (/tf) topic so each robot has its own.
+    # We do this by adding `ROS argument entries` to the sdf file for
+    # each plugin broadcasting a transform. These argument entries provide the
+    # remapping rule, i.e. /tf -> /<robot_id>/tf
+    tree = ET.parse(sdf_file_path)
+    root = tree.getroot()
+    for plugin in root.iter('plugin'):
+        if 'turtlebot3_diff_drive' in plugin.attrib.values():
+            # The only plugin we care for now is 'diff_drive' which is
+            # broadcasting a transform between`odom` and `base_footprint`
+            break
+
+    ros_params = plugin.find('ros')
+    ros_tf_remap = ET.SubElement(ros_params, 'argument')
+    ros_tf_remap.text = '/tf:=/' + args.robot_namespace + '/tf'
+
+    # Set data for request
+    request = SpawnEntity.Request()
+    request.name = args.robot_name
+    request.xml = ET.tostring(root, encoding="unicode")
+    request.robot_namespace = args.robot_namespace
+    request.initial_pose.position.x = float(args.x)
+    request.initial_pose.position.y = float(args.y)
+    request.initial_pose.position.z = float(args.z)
+
+    node.get_logger().info("Sending service request to `/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()