diff --git a/nav2_bringup/CHANGELOG.rst b/nav2_bringup/bringup/CHANGELOG.rst similarity index 100% rename from nav2_bringup/CHANGELOG.rst rename to nav2_bringup/bringup/CHANGELOG.rst diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/bringup/CMakeLists.txt similarity index 100% rename from nav2_bringup/CMakeLists.txt rename to nav2_bringup/bringup/CMakeLists.txt diff --git a/nav2_bringup/README.md b/nav2_bringup/bringup/README.md similarity index 100% rename from nav2_bringup/README.md rename to nav2_bringup/bringup/README.md diff --git a/nav2_bringup/launch/bt_navigator.xml b/nav2_bringup/bringup/launch/bt_navigator.xml similarity index 100% rename from nav2_bringup/launch/bt_navigator.xml rename to nav2_bringup/bringup/launch/bt_navigator.xml diff --git a/nav2_bringup/launch/nav2_bringup_launch.py b/nav2_bringup/bringup/launch/nav2_bringup_launch.py similarity index 100% rename from nav2_bringup/launch/nav2_bringup_launch.py rename to nav2_bringup/bringup/launch/nav2_bringup_launch.py diff --git a/nav2_bringup/launch/nav2_localization_launch.py b/nav2_bringup/bringup/launch/nav2_localization_launch.py similarity index 100% rename from nav2_bringup/launch/nav2_localization_launch.py rename to nav2_bringup/bringup/launch/nav2_localization_launch.py diff --git a/nav2_bringup/launch/nav2_navigation_launch.py b/nav2_bringup/bringup/launch/nav2_navigation_launch.py similarity index 100% rename from nav2_bringup/launch/nav2_navigation_launch.py rename to nav2_bringup/bringup/launch/nav2_navigation_launch.py diff --git a/nav2_bringup/launch/nav2_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py similarity index 100% rename from nav2_bringup/launch/nav2_tb3_simulation_launch.py rename to nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py diff --git a/nav2_bringup/bringup/launch/spawn_robot_launch.py b/nav2_bringup/bringup/launch/spawn_robot_launch.py new file mode 100644 index 0000000000000000000000000000000000000000..ee2c57f057669d1ac8d17b4bc07f4e9912d52480 --- /dev/null +++ b/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')]), + ]) diff --git a/nav2_bringup/maps/turtlebot3_world.pgm b/nav2_bringup/bringup/maps/turtlebot3_world.pgm similarity index 100% rename from nav2_bringup/maps/turtlebot3_world.pgm rename to nav2_bringup/bringup/maps/turtlebot3_world.pgm diff --git a/nav2_bringup/maps/turtlebot3_world.yaml b/nav2_bringup/bringup/maps/turtlebot3_world.yaml similarity index 100% rename from nav2_bringup/maps/turtlebot3_world.yaml rename to nav2_bringup/bringup/maps/turtlebot3_world.yaml diff --git a/nav2_bringup/package.xml b/nav2_bringup/bringup/package.xml similarity index 100% rename from nav2_bringup/package.xml rename to nav2_bringup/bringup/package.xml diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml similarity index 100% rename from nav2_bringup/params/nav2_params.yaml rename to nav2_bringup/bringup/params/nav2_params.yaml diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz similarity index 100% rename from nav2_bringup/rviz/nav2_default_view.rviz rename to nav2_bringup/bringup/rviz/nav2_default_view.rviz diff --git a/nav2_bringup/worlds/waffle.model b/nav2_bringup/bringup/worlds/waffle.model similarity index 100% rename from nav2_bringup/worlds/waffle.model rename to nav2_bringup/bringup/worlds/waffle.model diff --git a/nav2_bringup/spawn_turtlebot3/package.xml b/nav2_bringup/spawn_turtlebot3/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..9ca2ccc97b340b52ace3ec05fc735d01d9d7211a --- /dev/null +++ b/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> diff --git a/nav2_bringup/spawn_turtlebot3/resource/spawn_turtlebot3 b/nav2_bringup/spawn_turtlebot3/resource/spawn_turtlebot3 new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/nav2_bringup/spawn_turtlebot3/setup.cfg b/nav2_bringup/spawn_turtlebot3/setup.cfg new file mode 100644 index 0000000000000000000000000000000000000000..306d189167c3835b2e8ce3c306538c51ff47a3a5 --- /dev/null +++ b/nav2_bringup/spawn_turtlebot3/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/spawn_turtlebot3 +[install] +install-scripts=$base/lib/spawn_turtlebot3 diff --git a/nav2_bringup/spawn_turtlebot3/setup.py b/nav2_bringup/spawn_turtlebot3/setup.py new file mode 100644 index 0000000000000000000000000000000000000000..1d64f71dee74008c922db4f5b09c0d2ed7098393 --- /dev/null +++ b/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', + ], + }, +) diff --git a/nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/__init__.py b/nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/spawn_turtlebot.py b/nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/spawn_turtlebot.py new file mode 100644 index 0000000000000000000000000000000000000000..d31ac849ba132e3004af794eb408f9669f4e0424 --- /dev/null +++ b/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()