diff --git a/ionic_demo/CMakeLists.txt b/ionic_demo/CMakeLists.txt index bced042..5b00c56 100644 --- a/ionic_demo/CMakeLists.txt +++ b/ionic_demo/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR) project(ionic_demo) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED) # further dependencies manually. # find_package( REQUIRED) +install(DIRECTORY configs DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}) diff --git a/ionic_demo/configs/tb4_camera_bridge.yaml b/ionic_demo/configs/tb4_camera_bridge.yaml new file mode 100644 index 0000000..7adbf1f --- /dev/null +++ b/ionic_demo/configs/tb4_camera_bridge.yaml @@ -0,0 +1,12 @@ +# replace clock_bridge +- topic_name: "/rgbd_camera/image" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS + lazy: true + +- topic_name: "/rgbd_camera/depth_image" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS + lazy: true diff --git a/ionic_demo/launch/ionic_navigation_demo_launch.py b/ionic_demo/launch/ionic_navigation_demo_launch.py index 16e1a0c..c8868cc 100644 --- a/ionic_demo/launch/ionic_navigation_demo_launch.py +++ b/ionic_demo/launch/ionic_navigation_demo_launch.py @@ -1,36 +1,65 @@ -from launch_ros.substitutions import FindPackageShare +# Copyright (C) 2024 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. + +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) +from launch.conditions import UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import PathJoinSubstitution, TextSubstitution +from launch.substitutions import LaunchConfiguration def generate_launch_description(): + bringup_dir = Path(get_package_share_directory('nav2_bringup')) + ionic_demo_dir = Path(get_package_share_directory('ionic_demo')) + launch_dir = bringup_dir / 'launch' + headless = LaunchConfiguration('headless') + declare_headless_cmd = DeclareLaunchArgument( + 'headless', default_value='True', description='Whether to execute gzclient)' + ) - return LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('nav2_bringup'), - 'launch', - 'tb4_simulation_launch.py' - ]) - ]), - launch_arguments={ - 'map': PathJoinSubstitution([ - FindPackageShare('ionic_demo'), - 'maps', - 'ionic_demo.yaml' - ]), - 'world': PathJoinSubstitution([ - FindPackageShare('ionic_demo'), - 'worlds', - 'ionic.sdf' - ]), - 'x_pose': '0.0', - 'y_pose': '0.0', - 'z_pose': '0.0' - }.items() - ) - ]) + return LaunchDescription( + [ + declare_headless_cmd, + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(ionic_demo_dir / 'launch' / 'tb4_spawn_launch.py') + ), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(ionic_demo_dir / 'launch' / 'world_launch.py') + ), + ), + ExecuteProcess( + cmd=['gz', 'sim', '-g', '-v4'], + output='screen', + condition=UnlessCondition(headless), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(str(launch_dir / 'rviz_launch.py')), + launch_arguments={ + 'use_sim_time': 'True', + 'rviz_config': str(bringup_dir / 'rviz' / 'nav2_default_view.rviz'), + }.items(), + ), + ] + ) diff --git a/ionic_demo/launch/tb4_spawn_launch.py b/ionic_demo/launch/tb4_spawn_launch.py new file mode 100644 index 0000000..d1b78fa --- /dev/null +++ b/ionic_demo/launch/tb4_spawn_launch.py @@ -0,0 +1,114 @@ +# Copyright (C) 2024 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. + +from pathlib import Path + +import xacro +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from ros_gz_bridge.actions import RosGzBridge + +from launch import LaunchDescription +from launch.actions import ( + AppendEnvironmentVariable, + IncludeLaunchDescription, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + # Get the launch directory + ionic_demo_dir = Path(get_package_share_directory('ionic_demo')) + bringup_dir = Path(get_package_share_directory('nav2_bringup')) + launch_dir = bringup_dir / 'launch' + map_yaml_file = str(ionic_demo_dir / 'maps' / 'ionic_demo.yaml') + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(str(launch_dir / 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'use_composition': 'True', + }.items(), + ) + # This checks that tb4 exists needed for the URDF / simulation files. + # If not using TB4, its safe to remove. + sim_dir = Path(get_package_share_directory('nav2_minimal_tb4_sim')) + desc_dir = Path(get_package_share_directory('nav2_minimal_tb4_description')) + + # Launch configuration variables specific to simulation + robot_name = 'ionic_tb4' + robot_sdf = xacro.process(desc_dir / 'urdf' / 'standard' / 'turtlebot4.urdf.xacro') + + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + 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': robot_sdf + } + ], + remappings=remappings, + ) + + bridge = RosGzBridge( + bridge_name='tb4_bridge', + config_file=str(sim_dir / 'configs' / 'tb4_bridge.yaml'), + use_composition='True', + ) + camera_bridge = RosGzBridge( + bridge_name='tb4_camera_bridge', + config_file=str(ionic_demo_dir / 'configs' / 'tb4_camera_bridge.yaml'), + use_composition='True', + ) + + spawn_model = Node( + package='ros_gz_sim', + executable='create', + output='screen', + parameters=[ + { + 'entity': robot_name, + 'string': robot_sdf, + } + ], + ) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', str(desc_dir.parent.resolve()) + ) + + # Create the launch description and populate + ld = LaunchDescription() + + ld.add_action(bridge) + ld.add_action(camera_bridge) + + # The meshes used for the turtlebot4 visuals seem to be complex and cause + # a drop in RTF on non-GPU machines. + # Comment this to let gazebo to fail to find the mesh files + ld.add_action(set_env_vars_resources) + + ld.add_action(spawn_model) + + # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/ionic_demo/launch/world_launch.py b/ionic_demo/launch/world_launch.py new file mode 100644 index 0000000..1449051 --- /dev/null +++ b/ionic_demo/launch/world_launch.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 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. + +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory +from ros_gz_sim.actions import GzServer + +from launch import LaunchDescription + + +def generate_launch_description(): + ionic_demo_dir = Path(get_package_share_directory("ionic_demo")) + world = str(ionic_demo_dir / "worlds" / "ionic.sdf") + return LaunchDescription([GzServer(world_sdf_file=world, use_composition='True')]) diff --git a/ionic_demo/worlds/ionic.sdf b/ionic_demo/worlds/ionic.sdf index 3a6e7c3..b3f58d8 100644 --- a/ionic_demo/worlds/ionic.sdf +++ b/ionic_demo/worlds/ionic.sdf @@ -2,6 +2,10 @@ + + 0.004 + 1 +