From c57a27850431b96f1d0f5c0a0587b6ea704e0a4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 19 Jun 2023 17:35:38 +0200 Subject: [PATCH 1/9] Added nav2 simple commander with the new Gazebo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../launch/gz_warehouse.world | 272 ++++++++++++++++++ .../launch/nav_to_pose_example_launch.py | 147 ++++++++++ .../nav2_gz_simple_commander/__init__.py | 0 nav2_gz_simple_commander/package.xml | 24 ++ .../resource/nav2_gz_simple_commander | 0 nav2_gz_simple_commander/setup.cfg | 4 + nav2_gz_simple_commander/setup.py | 39 +++ .../test/test_copyright.py | 23 ++ nav2_gz_simple_commander/test/test_flake8.py | 25 ++ .../test/test_footprint_collision_checker.py | 152 ++++++++++ .../test/test_line_iterator.py | 108 +++++++ nav2_gz_simple_commander/test/test_pep257.py | 23 ++ 12 files changed, 817 insertions(+) create mode 100644 nav2_gz_simple_commander/launch/gz_warehouse.world create mode 100644 nav2_gz_simple_commander/launch/nav_to_pose_example_launch.py create mode 100644 nav2_gz_simple_commander/nav2_gz_simple_commander/__init__.py create mode 100644 nav2_gz_simple_commander/package.xml create mode 100644 nav2_gz_simple_commander/resource/nav2_gz_simple_commander create mode 100644 nav2_gz_simple_commander/setup.cfg create mode 100644 nav2_gz_simple_commander/setup.py create mode 100644 nav2_gz_simple_commander/test/test_copyright.py create mode 100644 nav2_gz_simple_commander/test/test_flake8.py create mode 100644 nav2_gz_simple_commander/test/test_footprint_collision_checker.py create mode 100644 nav2_gz_simple_commander/test/test_line_iterator.py create mode 100644 nav2_gz_simple_commander/test/test_pep257.py diff --git a/nav2_gz_simple_commander/launch/gz_warehouse.world b/nav2_gz_simple_commander/launch/gz_warehouse.world new file mode 100644 index 0000000000..7f9b87a49e --- /dev/null +++ b/nav2_gz_simple_commander/launch/gz_warehouse.world @@ -0,0 +1,272 @@ + + + + + 0.001 + 1.0 + + + + + + + + + + + ogre2 + + + + + 0 0 -9.8 + + 0.001 + 1 + 1000 + + + + + + + + model://aws_robomaker_warehouse_ShelfF_01 + + -5.795143 -0.956635 0 0 0 0 + + + + + model://aws_robomaker_warehouse_WallB_01 + + 0.0 0.0 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 0.57943 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -4.827049 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -8.6651 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -1.242668 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -3.038551 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -6.750542 0 0 0 0 + + + + + + + model://aws_robomaker_warehouse_GroundB_01 + + 0.0 0.0 -0.090092 0 0 0 + + + + + model://aws_robomaker_warehouse_Lamp_01 + + 0 0 -4 0 0 0 + + + + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 9.631706 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_Bucket_01 + + -1.8321 -6.3752 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 8.59 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 5.708138 8.616844 -0.017477 0 0 0 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 3.408638 8.616844 -0.017477 0 0 0 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + -1.491287 5.222435 -0.017477 0 0 -1.583185 + + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.324959 3.822449 -0.012064 0 0 1.563871 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.54171 3.816475 -0.015663 0 0 -1.583191 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.384239 6.137154 0 0 0 3.150000 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.236 6.137154 0 0 0 3.150000 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.573677 2.301994 -0.015663 0 0 -3.133191 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.2196 9.407 -0.015663 0 0 1.563871 + + + + + model://aws_robomaker_warehouse_ClutteringD_01 + + -1.634682 -7.811813 -0.319559 0 0 0 + + + + + model://aws_robomaker_warehouse_TrashCanC_01 + + -1.592441 7.715420 0 0 0 0 + + + + + + model://aws_robomaker_warehouse_PalletJackB_01 + + -0.276098 -9.481944 0.023266 0 0 0 + + + + + 0 0 9 0 0 0 + 0.5 0.5 0.5 1 + 0.2 0.2 0.2 1 + + 80 + 0.3 + 0.01 + 0.001 + + 1 + 0.1 0.1 -1 + + + + + -4.70385 10.895 16.2659 -0 0.921795 -1.12701 + orbit + perspective + + + + diff --git a/nav2_gz_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_gz_simple_commander/launch/nav_to_pose_example_launch.py new file mode 100644 index 0000000000..c40fa17e6a --- /dev/null +++ b/nav2_gz_simple_commander/launch/nav_to_pose_example_launch.py @@ -0,0 +1,147 @@ +# Copyright (C) 2023 Open Source Robotics Foundation + +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +import xacro + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_gz_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -v 4 ' + world]}.items(), + ) + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'gz_turtlebot3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join(get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join(get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' + ]) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'tb3', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.01', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_nav_to_pose', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + + return ld diff --git a/nav2_gz_simple_commander/nav2_gz_simple_commander/__init__.py b/nav2_gz_simple_commander/nav2_gz_simple_commander/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_gz_simple_commander/package.xml b/nav2_gz_simple_commander/package.xml new file mode 100644 index 0000000000..c31c7a8c4d --- /dev/null +++ b/nav2_gz_simple_commander/package.xml @@ -0,0 +1,24 @@ + + + + nav2_gz_simple_commander + 1.1.0 + An importable library for writing mobile robot applications in python3 + steve + Apache-2.0 + + rclpy + geometry_msgs + nav2_msgs + action_msgs + lifecycle_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/nav2_gz_simple_commander/resource/nav2_gz_simple_commander b/nav2_gz_simple_commander/resource/nav2_gz_simple_commander new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_gz_simple_commander/setup.cfg b/nav2_gz_simple_commander/setup.cfg new file mode 100644 index 0000000000..8301f8b681 --- /dev/null +++ b/nav2_gz_simple_commander/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/nav2_gz_simple_commander +[install] +install_scripts=$base/lib/nav2_gz_simple_commander diff --git a/nav2_gz_simple_commander/setup.py b/nav2_gz_simple_commander/setup.py new file mode 100644 index 0000000000..c629f9e7a5 --- /dev/null +++ b/nav2_gz_simple_commander/setup.py @@ -0,0 +1,39 @@ +from glob import glob +import os + +from setuptools import setup + + +package_name = 'nav2_gz_simple_commander' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ahcorde', + maintainer_email='ahcorde@gmail.com', + description='An importable library for writing mobile robot applications in python3', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_nav_to_pose = nav2_gz_simple_commander.example_nav_to_pose:main', + # 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', + # 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', + # 'example_follow_path = nav2_simple_commander.example_follow_path:main', + # 'demo_picking = nav2_simple_commander.demo_picking:main', + # 'demo_inspection = nav2_simple_commander.demo_inspection:main', + # 'demo_security = nav2_simple_commander.demo_security:main', + # 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', + # 'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main', + ], + }, +) diff --git a/nav2_gz_simple_commander/test/test_copyright.py b/nav2_gz_simple_commander/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /dev/null +++ b/nav2_gz_simple_commander/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/nav2_gz_simple_commander/test/test_flake8.py b/nav2_gz_simple_commander/test/test_flake8.py new file mode 100644 index 0000000000..27ee1078ff --- /dev/null +++ b/nav2_gz_simple_commander/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/nav2_gz_simple_commander/test/test_footprint_collision_checker.py b/nav2_gz_simple_commander/test/test_footprint_collision_checker.py new file mode 100644 index 0000000000..8ffc4b653b --- /dev/null +++ b/nav2_gz_simple_commander/test/test_footprint_collision_checker.py @@ -0,0 +1,152 @@ +# Copyright 2022 Afif Swaidan +# +# 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 unittest + +from geometry_msgs.msg import Point32, Polygon +from nav2_simple_commander.costmap_2d import PyCostmap2D +from nav2_simple_commander.footprint_collision_checker import FootprintCollisionChecker +from nav_msgs.msg import OccupancyGrid + +LETHAL_OBSTACLE = 254 + + +class TestFootprintCollisionChecker(unittest.TestCase): + + def test_no_costmap(self): + # Test if a type error raised when costmap is not specified yet + fcc_ = FootprintCollisionChecker() + self.assertRaises(ValueError, fcc_.worldToMapValidated, 0.0, 0.0) + self.assertRaises(ValueError, fcc_.pointCost, 0.0, 0.0) + + def test_pointCost(self): + # Test if point cost is calculated correctly + # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel + # AKA 10 meters x 10 meters + occupancyGrid_ = OccupancyGrid() + occupancyGrid_.info.resolution = 1.0 + occupancyGrid_.info.width = 10 + occupancyGrid_.info.height = 10 + occupancyGrid_.info.origin.position.x = 0.0 + occupancyGrid_.info.origin.position.y = 0.0 + map_data = [0] * 10 * 10 + occupancyGrid_.data = map_data + costmap_ = PyCostmap2D(occupancyGrid_) + fcc_ = FootprintCollisionChecker() + fcc_.setCostmap(costmap_) + self.assertEqual(fcc_.pointCost(1, 1), 0) + self.assertRaises(IndexError, fcc_.pointCost, 11, 11) + + def test_worldToMapValidated(self): + # Test if worldToMap conversion is calculated correctly + # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel + # AKA 10 meters x 10 meters + # Map origin is at (5,5) of world coordinates + occupancyGrid_ = OccupancyGrid() + occupancyGrid_.info.resolution = 1.0 + occupancyGrid_.info.width = 10 + occupancyGrid_.info.height = 10 + occupancyGrid_.info.origin.position.x = 5.0 + occupancyGrid_.info.origin.position.y = 5.0 + map_data = [0] * 10 * 10 + occupancyGrid_.data = map_data + costmap_ = PyCostmap2D(occupancyGrid_) + fcc_ = FootprintCollisionChecker() + fcc_.setCostmap(costmap_) + self.assertEqual(fcc_.worldToMapValidated(0, 5), (None, None)) + self.assertEqual(fcc_.worldToMapValidated(5, 0), (None, None)) + self.assertEqual(fcc_.worldToMapValidated(5, 5), (0, 0)) + self.assertEqual(fcc_.worldToMapValidated(14, 14), (9, 9)) + self.assertEqual(fcc_.worldToMapValidated(15, 14), (None, None)) + + def test_lineCost(self): + # Test if line cost is calculated correctly + # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel + # AKA 10 meters x 10 meters + occupancyGrid_ = OccupancyGrid() + occupancyGrid_.info.resolution = 1.0 + occupancyGrid_.info.width = 10 + occupancyGrid_.info.height = 10 + occupancyGrid_.info.origin.position.x = 0.0 + occupancyGrid_.info.origin.position.y = 0.0 + map_data = [0] * 10 * 10 + occupancyGrid_.data = map_data + costmap_ = PyCostmap2D(occupancyGrid_) + fcc_ = FootprintCollisionChecker() + fcc_.setCostmap(costmap_) + self.assertRaises(IndexError, fcc_.lineCost, 0, 15, 0, 9, 1) + self.assertEqual(fcc_.lineCost(0, 9, 0, 9, 1), 0.0) + + def test_footprintCost(self): + # Test if footprint cost is calculated correctly + # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel + # AKA 10 meters x 10 meters + occupancyGrid_ = OccupancyGrid() + occupancyGrid_.info.resolution = 1.0 + occupancyGrid_.info.width = 10 + occupancyGrid_.info.height = 10 + occupancyGrid_.info.origin.position.x = 0.0 + occupancyGrid_.info.origin.position.y = 0.0 + map_data = [0] * 10 * 10 + occupancyGrid_.data = map_data + costmap_ = PyCostmap2D(occupancyGrid_) + fcc_ = FootprintCollisionChecker() + fcc_.setCostmap(costmap_) + # Create square footprint 1m x 1m + footprint = Polygon() + point = Point32() + point.x = 0.0 + point.y = 0.0 + point.z = 0.0 + footprint.points.append(point) + point = Point32() + point.x = 1.0 + point.y = 1.0 + point.z = 0.0 + footprint.points.append(point) + point = Point32() + point.x = 1.0 + point.y = 0.0 + point.z = 0.0 + footprint.points.append(point) + point = Point32() + point.x = 0.0 + point.y = 1.0 + point.z = 0.0 + footprint.points.append(point) + self.assertEqual(fcc_.footprintCost(footprint), 0.0) + # Test none-zero cost + # Create in the map center a full box of cost value 100 + for i in range(24, 28): + map_data[i] = 100 + map_data[i + 10] = 100 + map_data[i + 20] = 100 + map_data[i + 30] = 100 + map_data[i + 40] = 100 + occupancyGrid_.data = map_data + costmap_ = PyCostmap2D(occupancyGrid_) + fcc_ = FootprintCollisionChecker() + fcc_.setCostmap(costmap_) + self.assertEqual(fcc_.footprintCostAtPose(4.0, 4.0, 0.0, footprint), 100) + # Append a point that is outside the map + point = Point32() + point.x = 30.0 + point.y = 5.0 + point.z = 3.0 + footprint.points.append(point) + self.assertEqual(fcc_.footprintCost(footprint), LETHAL_OBSTACLE) + + +if __name__ == '__main__': + unittest.main() diff --git a/nav2_gz_simple_commander/test/test_line_iterator.py b/nav2_gz_simple_commander/test/test_line_iterator.py new file mode 100644 index 0000000000..87c348ea70 --- /dev/null +++ b/nav2_gz_simple_commander/test/test_line_iterator.py @@ -0,0 +1,108 @@ +# Copyright 2022 Afif Swaidan +# +# 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. + +from cmath import sqrt +import unittest + +from nav2_simple_commander.line_iterator import LineIterator + + +class TestLineIterator(unittest.TestCase): + + def test_type_error(self): + # Test if a type error raised when passing invalid arguements types + self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') + + def test_value_error(self): + # Test if a value error raised when passing negative or zero step_size + self.assertRaises(ValueError, LineIterator, 0, 0, 10, 10, -2) + # Test if a value error raised when passing zero length line + self.assertRaises(ValueError, LineIterator, 2, 2, 2, 2, 1) + + def test_get_xy(self): + # Test if the initial and final coordinates are returned correctly + lt = LineIterator(0, 0, 5, 5, 1) + self.assertEqual(lt.getX0(), 0) + self.assertEqual(lt.getY0(), 0) + self.assertEqual(lt.getX1(), 5) + self.assertEqual(lt.getY1(), 5) + + def test_line_length(self): + # Test if the line length is calculated correctly + lt = LineIterator(0, 0, 5, 5, 1) + self.assertEqual(lt.get_line_length(), sqrt(pow(5, 2) + pow(5, 2))) + + def test_straight_line(self): + # Test if the calculations are correct for y = x + lt = LineIterator(0, 0, 5, 5, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + i) + lt.advance() + i += 1 + + # Test if the calculations are correct for y = 2x (positive slope) + lt = LineIterator(0, 0, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + (i*2)) + lt.advance() + i += 1 + + # Test if the calculations are correct for y = -2x (negative slope) + lt = LineIterator(0, 0, 5, -10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + (-i*2)) + lt.advance() + i += 1 + + def test_hor_line(self): + # Test if the calculations are correct for y = 0x+b (horizontal line) + lt = LineIterator(0, 10, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0()) + lt.advance() + i += 1 + + def test_ver_line(self): + # Test if the calculations are correct for x = n (vertical line) + lt = LineIterator(5, 0, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0()) + self.assertEqual(lt.getY(), lt.getY0() + i) + lt.advance() + i += 1 + + def test_clamp(self): + # Test if the increments are clamped to avoid crossing the final points + # when step_size is large with respect to line length + lt = LineIterator(0, 0, 5, 5, 10) + self.assertEqual(lt.getX(), 0) + self.assertEqual(lt.getY(), 0) + lt.advance() + while lt.isValid(): + self.assertEqual(lt.getX(), 5) + self.assertEqual(lt.getY(), 5) + lt.advance() + + +if __name__ == '__main__': + unittest.main() diff --git a/nav2_gz_simple_commander/test/test_pep257.py b/nav2_gz_simple_commander/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/nav2_gz_simple_commander/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 6e708e9c5e2aad4ccffdbd1190d76f4a6a0390cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Jun 2023 12:58:43 +0200 Subject: [PATCH 2/9] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../nav2_gz_simple_commander/__init__.py | 0 nav2_gz_simple_commander/package.xml | 24 --- .../resource/nav2_gz_simple_commander | 0 nav2_gz_simple_commander/setup.cfg | 4 - nav2_gz_simple_commander/setup.py | 39 ----- .../test/test_copyright.py | 23 --- nav2_gz_simple_commander/test/test_flake8.py | 25 --- .../test/test_footprint_collision_checker.py | 152 ------------------ .../test/test_line_iterator.py | 108 ------------- nav2_gz_simple_commander/test/test_pep257.py | 23 --- .../launch/gz_nav_to_pose_example_launch.py | 81 ++++++++-- .../launch/gz_warehouse.world | 7 - 12 files changed, 65 insertions(+), 421 deletions(-) delete mode 100644 nav2_gz_simple_commander/nav2_gz_simple_commander/__init__.py delete mode 100644 nav2_gz_simple_commander/package.xml delete mode 100644 nav2_gz_simple_commander/resource/nav2_gz_simple_commander delete mode 100644 nav2_gz_simple_commander/setup.cfg delete mode 100644 nav2_gz_simple_commander/setup.py delete mode 100644 nav2_gz_simple_commander/test/test_copyright.py delete mode 100644 nav2_gz_simple_commander/test/test_flake8.py delete mode 100644 nav2_gz_simple_commander/test/test_footprint_collision_checker.py delete mode 100644 nav2_gz_simple_commander/test/test_line_iterator.py delete mode 100644 nav2_gz_simple_commander/test/test_pep257.py rename nav2_gz_simple_commander/launch/nav_to_pose_example_launch.py => nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py (69%) rename {nav2_gz_simple_commander => nav2_simple_commander}/launch/gz_warehouse.world (97%) diff --git a/nav2_gz_simple_commander/nav2_gz_simple_commander/__init__.py b/nav2_gz_simple_commander/nav2_gz_simple_commander/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/nav2_gz_simple_commander/package.xml b/nav2_gz_simple_commander/package.xml deleted file mode 100644 index c31c7a8c4d..0000000000 --- a/nav2_gz_simple_commander/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - nav2_gz_simple_commander - 1.1.0 - An importable library for writing mobile robot applications in python3 - steve - Apache-2.0 - - rclpy - geometry_msgs - nav2_msgs - action_msgs - lifecycle_msgs - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/nav2_gz_simple_commander/resource/nav2_gz_simple_commander b/nav2_gz_simple_commander/resource/nav2_gz_simple_commander deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/nav2_gz_simple_commander/setup.cfg b/nav2_gz_simple_commander/setup.cfg deleted file mode 100644 index 8301f8b681..0000000000 --- a/nav2_gz_simple_commander/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/nav2_gz_simple_commander -[install] -install_scripts=$base/lib/nav2_gz_simple_commander diff --git a/nav2_gz_simple_commander/setup.py b/nav2_gz_simple_commander/setup.py deleted file mode 100644 index c629f9e7a5..0000000000 --- a/nav2_gz_simple_commander/setup.py +++ /dev/null @@ -1,39 +0,0 @@ -from glob import glob -import os - -from setuptools import setup - - -package_name = 'nav2_gz_simple_commander' - -setup( - name=package_name, - version='1.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name), glob('launch/*')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='ahcorde', - maintainer_email='ahcorde@gmail.com', - description='An importable library for writing mobile robot applications in python3', - license='Apache-2.0', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'example_nav_to_pose = nav2_gz_simple_commander.example_nav_to_pose:main', - # 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', - # 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', - # 'example_follow_path = nav2_simple_commander.example_follow_path:main', - # 'demo_picking = nav2_simple_commander.demo_picking:main', - # 'demo_inspection = nav2_simple_commander.demo_inspection:main', - # 'demo_security = nav2_simple_commander.demo_security:main', - # 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', - # 'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main', - ], - }, -) diff --git a/nav2_gz_simple_commander/test/test_copyright.py b/nav2_gz_simple_commander/test/test_copyright.py deleted file mode 100644 index cc8ff03f79..0000000000 --- a/nav2_gz_simple_commander/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# 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. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/nav2_gz_simple_commander/test/test_flake8.py b/nav2_gz_simple_commander/test/test_flake8.py deleted file mode 100644 index 27ee1078ff..0000000000 --- a/nav2_gz_simple_commander/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# 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. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/nav2_gz_simple_commander/test/test_footprint_collision_checker.py b/nav2_gz_simple_commander/test/test_footprint_collision_checker.py deleted file mode 100644 index 8ffc4b653b..0000000000 --- a/nav2_gz_simple_commander/test/test_footprint_collision_checker.py +++ /dev/null @@ -1,152 +0,0 @@ -# Copyright 2022 Afif Swaidan -# -# 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 unittest - -from geometry_msgs.msg import Point32, Polygon -from nav2_simple_commander.costmap_2d import PyCostmap2D -from nav2_simple_commander.footprint_collision_checker import FootprintCollisionChecker -from nav_msgs.msg import OccupancyGrid - -LETHAL_OBSTACLE = 254 - - -class TestFootprintCollisionChecker(unittest.TestCase): - - def test_no_costmap(self): - # Test if a type error raised when costmap is not specified yet - fcc_ = FootprintCollisionChecker() - self.assertRaises(ValueError, fcc_.worldToMapValidated, 0.0, 0.0) - self.assertRaises(ValueError, fcc_.pointCost, 0.0, 0.0) - - def test_pointCost(self): - # Test if point cost is calculated correctly - # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel - # AKA 10 meters x 10 meters - occupancyGrid_ = OccupancyGrid() - occupancyGrid_.info.resolution = 1.0 - occupancyGrid_.info.width = 10 - occupancyGrid_.info.height = 10 - occupancyGrid_.info.origin.position.x = 0.0 - occupancyGrid_.info.origin.position.y = 0.0 - map_data = [0] * 10 * 10 - occupancyGrid_.data = map_data - costmap_ = PyCostmap2D(occupancyGrid_) - fcc_ = FootprintCollisionChecker() - fcc_.setCostmap(costmap_) - self.assertEqual(fcc_.pointCost(1, 1), 0) - self.assertRaises(IndexError, fcc_.pointCost, 11, 11) - - def test_worldToMapValidated(self): - # Test if worldToMap conversion is calculated correctly - # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel - # AKA 10 meters x 10 meters - # Map origin is at (5,5) of world coordinates - occupancyGrid_ = OccupancyGrid() - occupancyGrid_.info.resolution = 1.0 - occupancyGrid_.info.width = 10 - occupancyGrid_.info.height = 10 - occupancyGrid_.info.origin.position.x = 5.0 - occupancyGrid_.info.origin.position.y = 5.0 - map_data = [0] * 10 * 10 - occupancyGrid_.data = map_data - costmap_ = PyCostmap2D(occupancyGrid_) - fcc_ = FootprintCollisionChecker() - fcc_.setCostmap(costmap_) - self.assertEqual(fcc_.worldToMapValidated(0, 5), (None, None)) - self.assertEqual(fcc_.worldToMapValidated(5, 0), (None, None)) - self.assertEqual(fcc_.worldToMapValidated(5, 5), (0, 0)) - self.assertEqual(fcc_.worldToMapValidated(14, 14), (9, 9)) - self.assertEqual(fcc_.worldToMapValidated(15, 14), (None, None)) - - def test_lineCost(self): - # Test if line cost is calculated correctly - # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel - # AKA 10 meters x 10 meters - occupancyGrid_ = OccupancyGrid() - occupancyGrid_.info.resolution = 1.0 - occupancyGrid_.info.width = 10 - occupancyGrid_.info.height = 10 - occupancyGrid_.info.origin.position.x = 0.0 - occupancyGrid_.info.origin.position.y = 0.0 - map_data = [0] * 10 * 10 - occupancyGrid_.data = map_data - costmap_ = PyCostmap2D(occupancyGrid_) - fcc_ = FootprintCollisionChecker() - fcc_.setCostmap(costmap_) - self.assertRaises(IndexError, fcc_.lineCost, 0, 15, 0, 9, 1) - self.assertEqual(fcc_.lineCost(0, 9, 0, 9, 1), 0.0) - - def test_footprintCost(self): - # Test if footprint cost is calculated correctly - # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel - # AKA 10 meters x 10 meters - occupancyGrid_ = OccupancyGrid() - occupancyGrid_.info.resolution = 1.0 - occupancyGrid_.info.width = 10 - occupancyGrid_.info.height = 10 - occupancyGrid_.info.origin.position.x = 0.0 - occupancyGrid_.info.origin.position.y = 0.0 - map_data = [0] * 10 * 10 - occupancyGrid_.data = map_data - costmap_ = PyCostmap2D(occupancyGrid_) - fcc_ = FootprintCollisionChecker() - fcc_.setCostmap(costmap_) - # Create square footprint 1m x 1m - footprint = Polygon() - point = Point32() - point.x = 0.0 - point.y = 0.0 - point.z = 0.0 - footprint.points.append(point) - point = Point32() - point.x = 1.0 - point.y = 1.0 - point.z = 0.0 - footprint.points.append(point) - point = Point32() - point.x = 1.0 - point.y = 0.0 - point.z = 0.0 - footprint.points.append(point) - point = Point32() - point.x = 0.0 - point.y = 1.0 - point.z = 0.0 - footprint.points.append(point) - self.assertEqual(fcc_.footprintCost(footprint), 0.0) - # Test none-zero cost - # Create in the map center a full box of cost value 100 - for i in range(24, 28): - map_data[i] = 100 - map_data[i + 10] = 100 - map_data[i + 20] = 100 - map_data[i + 30] = 100 - map_data[i + 40] = 100 - occupancyGrid_.data = map_data - costmap_ = PyCostmap2D(occupancyGrid_) - fcc_ = FootprintCollisionChecker() - fcc_.setCostmap(costmap_) - self.assertEqual(fcc_.footprintCostAtPose(4.0, 4.0, 0.0, footprint), 100) - # Append a point that is outside the map - point = Point32() - point.x = 30.0 - point.y = 5.0 - point.z = 3.0 - footprint.points.append(point) - self.assertEqual(fcc_.footprintCost(footprint), LETHAL_OBSTACLE) - - -if __name__ == '__main__': - unittest.main() diff --git a/nav2_gz_simple_commander/test/test_line_iterator.py b/nav2_gz_simple_commander/test/test_line_iterator.py deleted file mode 100644 index 87c348ea70..0000000000 --- a/nav2_gz_simple_commander/test/test_line_iterator.py +++ /dev/null @@ -1,108 +0,0 @@ -# Copyright 2022 Afif Swaidan -# -# 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. - -from cmath import sqrt -import unittest - -from nav2_simple_commander.line_iterator import LineIterator - - -class TestLineIterator(unittest.TestCase): - - def test_type_error(self): - # Test if a type error raised when passing invalid arguements types - self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') - - def test_value_error(self): - # Test if a value error raised when passing negative or zero step_size - self.assertRaises(ValueError, LineIterator, 0, 0, 10, 10, -2) - # Test if a value error raised when passing zero length line - self.assertRaises(ValueError, LineIterator, 2, 2, 2, 2, 1) - - def test_get_xy(self): - # Test if the initial and final coordinates are returned correctly - lt = LineIterator(0, 0, 5, 5, 1) - self.assertEqual(lt.getX0(), 0) - self.assertEqual(lt.getY0(), 0) - self.assertEqual(lt.getX1(), 5) - self.assertEqual(lt.getY1(), 5) - - def test_line_length(self): - # Test if the line length is calculated correctly - lt = LineIterator(0, 0, 5, 5, 1) - self.assertEqual(lt.get_line_length(), sqrt(pow(5, 2) + pow(5, 2))) - - def test_straight_line(self): - # Test if the calculations are correct for y = x - lt = LineIterator(0, 0, 5, 5, 1) - i = 0 - while lt.isValid(): - self.assertEqual(lt.getX(), lt.getX0() + i) - self.assertEqual(lt.getY(), lt.getY0() + i) - lt.advance() - i += 1 - - # Test if the calculations are correct for y = 2x (positive slope) - lt = LineIterator(0, 0, 5, 10, 1) - i = 0 - while lt.isValid(): - self.assertEqual(lt.getX(), lt.getX0() + i) - self.assertEqual(lt.getY(), lt.getY0() + (i*2)) - lt.advance() - i += 1 - - # Test if the calculations are correct for y = -2x (negative slope) - lt = LineIterator(0, 0, 5, -10, 1) - i = 0 - while lt.isValid(): - self.assertEqual(lt.getX(), lt.getX0() + i) - self.assertEqual(lt.getY(), lt.getY0() + (-i*2)) - lt.advance() - i += 1 - - def test_hor_line(self): - # Test if the calculations are correct for y = 0x+b (horizontal line) - lt = LineIterator(0, 10, 5, 10, 1) - i = 0 - while lt.isValid(): - self.assertEqual(lt.getX(), lt.getX0() + i) - self.assertEqual(lt.getY(), lt.getY0()) - lt.advance() - i += 1 - - def test_ver_line(self): - # Test if the calculations are correct for x = n (vertical line) - lt = LineIterator(5, 0, 5, 10, 1) - i = 0 - while lt.isValid(): - self.assertEqual(lt.getX(), lt.getX0()) - self.assertEqual(lt.getY(), lt.getY0() + i) - lt.advance() - i += 1 - - def test_clamp(self): - # Test if the increments are clamped to avoid crossing the final points - # when step_size is large with respect to line length - lt = LineIterator(0, 0, 5, 5, 10) - self.assertEqual(lt.getX(), 0) - self.assertEqual(lt.getY(), 0) - lt.advance() - while lt.isValid(): - self.assertEqual(lt.getX(), 5) - self.assertEqual(lt.getY(), 5) - lt.advance() - - -if __name__ == '__main__': - unittest.main() diff --git a/nav2_gz_simple_commander/test/test_pep257.py b/nav2_gz_simple_commander/test/test_pep257.py deleted file mode 100644 index b234a3840f..0000000000 --- a/nav2_gz_simple_commander/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# 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. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/nav2_gz_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py similarity index 69% rename from nav2_gz_simple_commander/launch/nav_to_pose_example_launch.py rename to nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py index c40fa17e6a..8000ac2bc3 100644 --- a/nav2_gz_simple_commander/launch/nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py @@ -1,11 +1,11 @@ -# Copyright (C) 2023 Open Source Robotics Foundation - -# Licensed under the Apache License, Version 2.0 (the "License") +# Copyright 2023 Open Source Robotics Foundation, Inc. +# +# 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 - +# +# 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. @@ -18,12 +18,14 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution import xacro + def generate_launch_description(): warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') nav2_bringup_dir = get_package_share_directory('nav2_bringup') @@ -32,6 +34,21 @@ def generate_launch_description(): map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') world = os.path.join(python_commander_dir, 'gz_warehouse.world') + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + # Launch gazebo environment start_gazebo_server_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -41,14 +58,25 @@ def generate_launch_description(): 'gz_sim.launch.py' ]) ), - launch_arguments={'gz_args': ['-r -v 4 ' + world]}.items(), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'gz_turtlebot3_waffle.urdf') doc = xacro.parse(open(urdf)) xacro.process_doc(doc) - params = {'robot_description': doc.toxml()} start_robot_state_publisher_cmd = Node( package='robot_state_publisher', @@ -58,29 +86,35 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': True, 'robot_description': doc.toxml() - }]) + }] + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), launch_arguments={'namespace': '', 'use_namespace': 'False'}.items()) - os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'models') - os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join(get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') - os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join(get_package_share_directory('turtlebot3_gazebo'), '..') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') # Clock bridge - clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', name='clock_bridge', output='screen', parameters=[{ 'use_sim_time': True }], arguments=[ - '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' - ]) + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) # lidar bridge lidar_bridge = Node( @@ -96,6 +130,17 @@ def generate_launch_description(): ] ) + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -133,10 +178,14 @@ def generate_launch_description(): output='screen') ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) ld.add_action(start_gazebo_spawner_cmd) ld.add_action(clock_bridge) ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) ld.add_action(load_joint_state_controller) ld.add_action(load_diffdrive_controller) ld.add_action(start_robot_state_publisher_cmd) diff --git a/nav2_gz_simple_commander/launch/gz_warehouse.world b/nav2_simple_commander/launch/gz_warehouse.world similarity index 97% rename from nav2_gz_simple_commander/launch/gz_warehouse.world rename to nav2_simple_commander/launch/gz_warehouse.world index 7f9b87a49e..5d54bca1b7 100644 --- a/nav2_gz_simple_commander/launch/gz_warehouse.world +++ b/nav2_simple_commander/launch/gz_warehouse.world @@ -261,12 +261,5 @@ 0.1 0.1 -1 - - - -4.70385 10.895 16.2659 -0 0.921795 -1.12701 - orbit - perspective - - From 0fa0f011c40e083a50651aa2b34809a942459f85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Jun 2023 13:14:44 +0200 Subject: [PATCH 3/9] get the right package MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py index 8000ac2bc3..65dd1f4b0b 100644 --- a/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') nav2_bringup_dir = get_package_share_directory('nav2_bringup') - python_commander_dir = get_package_share_directory('nav2_gz_simple_commander') + python_commander_dir = get_package_share_directory('nav2_simple_commander') map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') world = os.path.join(python_commander_dir, 'gz_warehouse.world') From 48dc0d24e9a90340a2be10b5e0fec507422e0705 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Jun 2023 23:21:48 +0200 Subject: [PATCH 4/9] Added more changes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/launch/rviz_launch.py | 13 + .../launch/gz_nav_to_pose_example_launch.py | 10 +- .../launch/gz_tb3_waffle.urdf | 412 ++++++++++++++++ .../launch/gz_warehouse.world | 451 ++++++++---------- 4 files changed, 628 insertions(+), 258 deletions(-) create mode 100644 nav2_simple_commander/launch/gz_tb3_waffle.urdf diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index c87ae01b41..10eabf007e 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -34,6 +34,7 @@ def generate_launch_description(): namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') rviz_config_file = LaunchConfiguration('rviz_config') + use_sim_time = LaunchConfiguration('use_sim_time') # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( @@ -52,9 +53,17 @@ def generate_launch_description(): default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use') + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation (Gazebo) clock if true') + # Launch rviz start_rviz_cmd = Node( condition=UnlessCondition(use_namespace), + parameters=[{ + 'use_sim_time': True, + }], package='rviz2', executable='rviz2', arguments=['-d', rviz_config_file], @@ -66,6 +75,9 @@ def generate_launch_description(): start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), + parameters=[{ + 'use_sim_time': True, + }], package='rviz2', executable='rviz2', namespace=namespace, @@ -97,6 +109,7 @@ def generate_launch_description(): ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_sim_time_cmd) # Add any conditioned actions ld.add_action(start_rviz_cmd) diff --git a/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py index 65dd1f4b0b..99a6983863 100644 --- a/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(['not ', headless])), ) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'gz_turtlebot3_waffle.urdf') + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') doc = xacro.parse(open(urdf)) xacro.process_doc(doc) @@ -95,7 +95,8 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), condition=IfCondition(use_rviz), launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( get_package_share_directory('turtlebot3_gazebo'), 'models') @@ -152,7 +153,7 @@ def generate_launch_description(): executable='create', output='screen', arguments=[ - '-entity', 'tb3', + '-entity', 'turtlebot3_waffle', '-string', doc.toxml(), '-x', '3.45', '-y', '2.15', '-z', '0.01', '-R', '0', '-P', '0', '-Y', '3.14' @@ -175,6 +176,9 @@ def generate_launch_description(): package='nav2_simple_commander', executable='example_nav_to_pose', emulate_tty=True, + parameters=[{ + 'use_sim_time': True + }], output='screen') ld = LaunchDescription() diff --git a/nav2_simple_commander/launch/gz_tb3_waffle.urdf b/nav2_simple_commander/launch/gz_tb3_waffle.urdf new file mode 100644 index 0000000000..017f4a8d6e --- /dev/null +++ b/nav2_simple_commander/launch/gz_tb3_waffle.urdf @@ -0,0 +1,412 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + " + scan + base_scan + -0.064 0 0.121 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + 1 + true + + + + + + true + 200 + imu + imu_link + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + + + + + + $(find nav2_bringup)/params/control.yaml + + ~/odom:=odom + /tf:=tf + /tf_static:=tf_static + /diagnostics:=diagnostics + /diffdrive_controller/cmd_vel_unstamped:=cmd_vel + + + + + true + true + false + false + true + true + true + + + + diff --git a/nav2_simple_commander/launch/gz_warehouse.world b/nav2_simple_commander/launch/gz_warehouse.world index 5d54bca1b7..7f50bd819d 100644 --- a/nav2_simple_commander/launch/gz_warehouse.world +++ b/nav2_simple_commander/launch/gz_warehouse.world @@ -1,265 +1,206 @@ - - - 0.001 - 1.0 - - + - - + - - + - - + - - + - ogre2 - - ogre2 + + - - - 0 0 -9.8 - - 0.001 - 1 - 1000 - - - - - - - - model://aws_robomaker_warehouse_ShelfF_01 - - -5.795143 -0.956635 0 0 0 0 - - - - - model://aws_robomaker_warehouse_WallB_01 - - 0.0 0.0 0 0 0 0 - - - - - model://aws_robomaker_warehouse_ShelfE_01 - - 4.73156 0.57943 0 0 0 0 - - - - - model://aws_robomaker_warehouse_ShelfE_01 - - 4.73156 -4.827049 0 0 0 0 - - - - - model://aws_robomaker_warehouse_ShelfE_01 - - 4.73156 -8.6651 0 0 0 0 - - - - - model://aws_robomaker_warehouse_ShelfD_01 - - 4.73156 -1.242668 0 0 0 0 - - - - - model://aws_robomaker_warehouse_ShelfD_01 - - 4.73156 -3.038551 0 0 0 0 - - - - - model://aws_robomaker_warehouse_ShelfD_01 - - 4.73156 -6.750542 0 0 0 0 - - - - - - - model://aws_robomaker_warehouse_GroundB_01 - - 0.0 0.0 -0.090092 0 0 0 - - - - - model://aws_robomaker_warehouse_Lamp_01 - - 0 0 -4 0 0 0 - - - - - - - model://aws_robomaker_warehouse_Bucket_01 - - 0.433449 9.631706 0 0 0 -1.563161 - - - - - model://aws_robomaker_warehouse_Bucket_01 - - -1.8321 -6.3752 0 0 0 -1.563161 - - - - - model://aws_robomaker_warehouse_Bucket_01 - - 0.433449 8.59 0 0 0 -1.563161 - - - - - model://aws_robomaker_warehouse_ClutteringA_01 - - 5.708138 8.616844 -0.017477 0 0 0 - - - - - model://aws_robomaker_warehouse_ClutteringA_01 - - 3.408638 8.616844 -0.017477 0 0 0 - - - - - model://aws_robomaker_warehouse_ClutteringA_01 - - -1.491287 5.222435 -0.017477 0 0 -1.583185 - - - - - - model://aws_robomaker_warehouse_ClutteringC_01 - - 3.324959 3.822449 -0.012064 0 0 1.563871 - - - - - model://aws_robomaker_warehouse_ClutteringC_01 - - 5.54171 3.816475 -0.015663 0 0 -1.583191 - - - - - model://aws_robomaker_warehouse_ClutteringC_01 - - 5.384239 6.137154 0 0 0 3.150000 - - - - - model://aws_robomaker_warehouse_ClutteringC_01 - - 3.236 6.137154 0 0 0 3.150000 - - - - - model://aws_robomaker_warehouse_ClutteringC_01 - - -1.573677 2.301994 -0.015663 0 0 -3.133191 - - - - - model://aws_robomaker_warehouse_ClutteringC_01 - - -1.2196 9.407 -0.015663 0 0 1.563871 - - - - - model://aws_robomaker_warehouse_ClutteringD_01 - - -1.634682 -7.811813 -0.319559 0 0 0 - - - - - model://aws_robomaker_warehouse_TrashCanC_01 - - -1.592441 7.715420 0 0 0 0 - - - - - - model://aws_robomaker_warehouse_PalletJackB_01 - - -0.276098 -9.481944 0.023266 0 0 0 - - - - - 0 0 9 0 0 0 - 0.5 0.5 0.5 1 - 0.2 0.2 0.2 1 - - 80 - 0.3 - 0.01 - 0.001 - - 1 - 0.1 0.1 -1 - - - + name="ignition::gazebo::systems::Imu"> + + 0 0 -9.8 + + 0.001 + 1 + 1000.0 + + + quick + 150 + 0 + 1.400000 + 1 + + + + + + + model://aws_robomaker_warehouse_ShelfF_01 + + -5.795143 -0.956635 0 0 0 0 + + + + model://aws_robomaker_warehouse_WallB_01 + + 0.0 0.0 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 0.57943 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -4.827049 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -8.6651 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -1.242668 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -3.038551 0 0 0 0 + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -6.750542 0 0 0 0 + + + + model://aws_robomaker_warehouse_GroundB_01 + + 0.0 0.0 -0.090092 0 0 0 + + + + model://aws_robomaker_warehouse_Lamp_01 + + 0 0 -4 0 0 0 + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 9.631706 0 0 0 -1.563161 + + + + model://aws_robomaker_warehouse_Bucket_01 + + -1.8321 -6.3752 0 0 0 -1.563161 + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 8.59 0 0 0 -1.563161 + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 5.708138 8.616844 -0.017477 0 0 0 + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 3.408638 8.616844 -0.017477 0 0 0 + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + -1.491287 5.222435 -0.017477 0 0 -1.583185 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.324959 3.822449 -0.012064 0 0 1.563871 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.54171 3.816475 -0.015663 0 0 -1.583191 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.384239 6.137154 0 0 0 3.150000 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.236 6.137154 0 0 0 3.150000 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.573677 2.301994 -0.015663 0 0 -3.133191 + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.2196 9.407 -0.015663 0 0 1.563871 + + + + model://aws_robomaker_warehouse_ClutteringD_01 + + -1.634682 -7.811813 -0.319559 0 0 0 + + + + model://aws_robomaker_warehouse_TrashCanC_01 + + -1.592441 7.715420 0 0 0 0 + + + + model://aws_robomaker_warehouse_PalletJackB_01 + + -0.276098 -9.481944 0.023266 0 0 0 + + + + 0 0 8.5 0 0 0 + 0.5 0.5 0.5 1 + 0.2 0.2 0.2 1 + + 80 + 0.3 + 0.01 + 0.001 + + 1 + 0.1 0.1 -1 + + From a90ef92adf42b63ff598e54617b913b86ad0e825 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 22 Jun 2023 01:26:23 +0200 Subject: [PATCH 5/9] Added more launch files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../gz_assisted_teleop_example_launch.py | 209 +++++++++++++++++ .../launch/gz_follow_path_example_launch.py | 206 ++++++++++++++++ .../launch/gz_inspection_demo_launch.py | 206 ++++++++++++++++ .../gz_nav_through_poses_example_launch.py | 98 ++++++++ .../launch/gz_nav_to_pose_example_launch.py | 14 +- .../launch/gz_recoveries_example_launch.py | 205 ++++++++++++++++ .../launch/gz_tb3_waffle.urdf | 220 ++++++++++++------ 7 files changed, 1083 insertions(+), 75 deletions(-) create mode 100644 nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py create mode 100644 nav2_simple_commander/launch/gz_follow_path_example_launch.py create mode 100644 nav2_simple_commander/launch/gz_inspection_demo_launch.py create mode 100644 nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py create mode 100644 nav2_simple_commander/launch/gz_recoveries_example_launch.py diff --git a/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py b/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py new file mode 100644 index 0000000000..5eb9a6bdba --- /dev/null +++ b/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py @@ -0,0 +1,209 @@ +# Copyright (c) 2021 Samsung Research America +# Copyright (c) 2022 Joshua Wallace +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_assisted_teleop', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_follow_path_example_launch.py b/nav2_simple_commander/launch/gz_follow_path_example_launch.py new file mode 100644 index 0000000000..32106b3e8e --- /dev/null +++ b/nav2_simple_commander/launch/gz_follow_path_example_launch.py @@ -0,0 +1,206 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_follow_path', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_inspection_demo_launch.py b/nav2_simple_commander/launch/gz_inspection_demo_launch.py new file mode 100644 index 0000000000..9be518d3b3 --- /dev/null +++ b/nav2_simple_commander/launch/gz_inspection_demo_launch.py @@ -0,0 +1,206 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_inspection', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py b/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py new file mode 100644 index 0000000000..0c9fedaad9 --- /dev/null +++ b/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py @@ -0,0 +1,98 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + condition=IfCondition(PythonExpression(['not ', headless])), + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_nav_through_poses', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py index 99a6983863..bd0a547a4a 100644 --- a/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/gz_nav_to_pose_example_launch.py @@ -38,6 +38,15 @@ def generate_launch_description(): use_rviz = LaunchConfiguration('use_rviz') headless = LaunchConfiguration('headless') + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( 'use_rviz', @@ -86,7 +95,8 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': True, 'robot_description': doc.toxml() - }] + }], + remappings=remappings ) # start the visualization @@ -155,7 +165,7 @@ def generate_launch_description(): arguments=[ '-entity', 'turtlebot3_waffle', '-string', doc.toxml(), - '-x', '3.45', '-y', '2.15', '-z', '0.01', + '-x', '3.45', '-y', '2.15', '-z', '0.1', '-R', '0', '-P', '0', '-Y', '3.14' ] ) diff --git a/nav2_simple_commander/launch/gz_recoveries_example_launch.py b/nav2_simple_commander/launch/gz_recoveries_example_launch.py new file mode 100644 index 0000000000..23c85849c0 --- /dev/null +++ b/nav2_simple_commander/launch/gz_recoveries_example_launch.py @@ -0,0 +1,205 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_recoveries', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_tb3_waffle.urdf b/nav2_simple_commander/launch/gz_tb3_waffle.urdf index 017f4a8d6e..2be185c222 100644 --- a/nav2_simple_commander/launch/gz_tb3_waffle.urdf +++ b/nav2_simple_commander/launch/gz_tb3_waffle.urdf @@ -1,45 +1,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -49,13 +9,22 @@ + + + + 0.4 0.4 0.4 1 + 0.4 0.4 0.4 1 + 0.0 0.0 1.0 1 + + + + - @@ -67,10 +36,10 @@ - - + + @@ -81,13 +50,21 @@ + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.0 0.0 1.0 1 + + + - @@ -95,15 +72,36 @@ + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + - - - + + + @@ -113,13 +111,22 @@ + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.0 0.0 1.0 1 + + + + - @@ -127,15 +134,37 @@ + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + - - - + + + @@ -148,16 +177,35 @@ - + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + + - - + + @@ -171,16 +219,35 @@ - + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + + - - + + @@ -199,13 +266,22 @@ + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.0 0.0 1.0 1 + + + + - @@ -216,8 +292,8 @@ - + @@ -371,14 +447,12 @@ ign_ros2_control/IgnitionSystem - - + - - + From 99b77b328b5fb4006237bd8ce11794fa6004bb7a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 22 Jun 2023 10:04:07 +0200 Subject: [PATCH 6/9] Added all launch files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../gz_assisted_teleop_example_launch.py | 2 - .../launch/gz_follow_path_example_launch.py | 1 - .../launch/gz_picking_demo_launch.py | 206 +++++++++++++++++ .../launch/gz_security_demo_launch.py | 206 +++++++++++++++++ .../gz_waypoint_follower_example_launch.py | 210 ++++++++++++++++++ 5 files changed, 622 insertions(+), 3 deletions(-) create mode 100644 nav2_simple_commander/launch/gz_picking_demo_launch.py create mode 100644 nav2_simple_commander/launch/gz_security_demo_launch.py create mode 100644 nav2_simple_commander/launch/gz_waypoint_follower_example_launch.py diff --git a/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py b/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py index 5eb9a6bdba..d815536fcf 100644 --- a/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/gz_assisted_teleop_example_launch.py @@ -48,7 +48,6 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( 'use_rviz', @@ -110,7 +109,6 @@ def generate_launch_description(): 'use_namespace': 'False', 'use_sim_time': 'true'}.items()) - os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( get_package_share_directory('turtlebot3_gazebo'), 'models') os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( diff --git a/nav2_simple_commander/launch/gz_follow_path_example_launch.py b/nav2_simple_commander/launch/gz_follow_path_example_launch.py index 32106b3e8e..0501a8e517 100644 --- a/nav2_simple_commander/launch/gz_follow_path_example_launch.py +++ b/nav2_simple_commander/launch/gz_follow_path_example_launch.py @@ -106,7 +106,6 @@ def generate_launch_description(): os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( get_package_share_directory('turtlebot3_gazebo'), '..') - # Clock bridge clock_bridge = Node( package='ros_gz_bridge', executable='parameter_bridge', diff --git a/nav2_simple_commander/launch/gz_picking_demo_launch.py b/nav2_simple_commander/launch/gz_picking_demo_launch.py new file mode 100644 index 0000000000..fcb4611424 --- /dev/null +++ b/nav2_simple_commander/launch/gz_picking_demo_launch.py @@ -0,0 +1,206 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_picking', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_security_demo_launch.py b/nav2_simple_commander/launch/gz_security_demo_launch.py new file mode 100644 index 0000000000..00c1c5661a --- /dev/null +++ b/nav2_simple_commander/launch/gz_security_demo_launch.py @@ -0,0 +1,206 @@ +# Copyright (c) 2021 Samsung Research America +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_security', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/gz_waypoint_follower_example_launch.py b/nav2_simple_commander/launch/gz_waypoint_follower_example_launch.py new file mode 100644 index 0000000000..e83a4d4718 --- /dev/null +++ b/nav2_simple_commander/launch/gz_waypoint_follower_example_launch.py @@ -0,0 +1,210 @@ +# Copyright 2023 Open Source Robotics Foundation, Inc. +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') + + # Launch configuration variables + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), + condition=IfCondition(PythonExpression(['not ', headless])), + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_waypoint_follower', + emulate_tty=True, + parameters=[{ + 'use_sim_time': True + }], + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + + return ld From 46e60ca9bf4387d6e53f118107d592cf9f902f08 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 22 Jun 2023 10:06:27 +0200 Subject: [PATCH 7/9] Fixed warning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- nav2_bringup/launch/rviz_launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 10eabf007e..f6dd99cc97 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -62,7 +62,7 @@ def generate_launch_description(): start_rviz_cmd = Node( condition=UnlessCondition(use_namespace), parameters=[{ - 'use_sim_time': True, + 'use_sim_time': use_sim_time, }], package='rviz2', executable='rviz2', @@ -76,7 +76,7 @@ def generate_launch_description(): start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), parameters=[{ - 'use_sim_time': True, + 'use_sim_time': use_sim_time, }], package='rviz2', executable='rviz2', From 8e0a97fa17cf84166e93157cb5a5263565950138 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 23 Jun 2023 10:55:44 +0200 Subject: [PATCH 8/9] Fixed CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 87cf7be47e..5f6c56adfc 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v14\ + - "<< parameters.key >>-v15\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v14\ + - "<< parameters.key >>-v15\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v14\ + key: "<< parameters.key >>-v15\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ From 55bd8bf3dc186f5971b76b5e7b80ac8e1f425c00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 28 Jun 2023 13:41:06 +0200 Subject: [PATCH 9/9] Port to Gazebo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../gz_nav_through_poses_example_launch.py | 142 ++++++++++++++++-- 1 file changed, 127 insertions(+), 15 deletions(-) diff --git a/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py b/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py index 0c9fedaad9..a880a2c053 100644 --- a/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py +++ b/nav2_simple_commander/launch/gz_nav_through_poses_example_launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2021 Samsung Research America +# Copyright 2023 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -20,8 +20,10 @@ from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import xacro def generate_launch_description(): @@ -30,12 +32,21 @@ def generate_launch_description(): python_commander_dir = get_package_share_directory('nav2_simple_commander') map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') - world = os.path.join(python_commander_dir, 'warehouse.world') + world = os.path.join(python_commander_dir, 'gz_warehouse.world') # Launch configuration variables use_rviz = LaunchConfiguration('use_rviz') headless = LaunchConfiguration('headless') + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( 'use_rviz', @@ -47,23 +58,46 @@ def generate_launch_description(): default_value='False', description='Whether to execute gzclient)') - # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') - - start_gazebo_client_cmd = ExecuteProcess( + # Launch gazebo environment + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-r -s ' + world]}.items(), + ) + + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py' + ]) + ), + launch_arguments={'gz_args': ['-g ' + world]}.items(), condition=IfCondition(PythonExpression(['not ', headless])), - cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + ) + + urdf = os.path.join(python_commander_dir, 'gz_tb3_waffle.urdf') + + doc = xacro.parse(open(urdf)) + xacro.process_doc(doc) - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + parameters=[{ + 'use_sim_time': True, + 'robot_description': doc.toxml() + }], + remappings=remappings + ) # start the visualization rviz_cmd = IncludeLaunchDescription( @@ -71,19 +105,90 @@ def generate_launch_description(): os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), condition=IfCondition(use_rviz), launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + 'use_namespace': 'False', + 'use_sim_time': 'true'}.items()) + + os.environ['IGN_GAZEBO_RESOURCE_PATH'] = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('aws_robomaker_small_warehouse_world'), 'models') + os.environ['IGN_GAZEBO_RESOURCE_PATH'] += ':' + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), '..') + + # Clock bridge + clock_bridge = Node( + package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock'] + ) + + # lidar bridge + lidar_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[ + ['/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ] + ) + + imu_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='imu_bridge', + output='screen', + parameters=[{ + 'use_sim_time': True + }], + arguments=[['/imu' + '@sensor_msgs/msg/Imu[ignition.msgs.IMU']], + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + launch_arguments={'map': map_yaml_file, 'use_sim_time': 'true'}.items()) + + start_gazebo_spawner_cmd = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'turtlebot3_waffle', + '-string', doc.toxml(), + '-x', '3.45', '-y', '2.15', '-z', '0.1', + '-R', '0', '-P', '0', '-Y', '3.14' + ] + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + load_diffdrive_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'diffdrive_controller'], + output='screen' + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='example_nav_through_poses', emulate_tty=True, + parameters=[{ + 'use_sim_time': True + }], output='screen') ld = LaunchDescription() @@ -91,8 +196,15 @@ def generate_launch_description(): ld.add_action(declare_simulator_cmd) ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + ld.add_action(clock_bridge) + ld.add_action(lidar_bridge) + ld.add_action(imu_bridge) + ld.add_action(load_joint_state_controller) + ld.add_action(load_diffdrive_controller) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) ld.add_action(demo_cmd) + return ld