From 5f2c337b75fe09e339bcbb9e8dd74cdd1d04c4cb Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 9 Aug 2024 12:08:53 -0700 Subject: [PATCH 1/6] adding Nav2 loopback sim Signed-off-by: Steve Macenski --- nav2_bringup/launch/bringup_launch.py | 11 +- .../launch/tb3_loopback_simulation.launch.py | 235 +++++++++++++++++ .../launch/tb4_loopback_simulation.launch.py | 244 ++++++++++++++++++ nav2_bringup/params/nav2_params.yaml | 8 + nav2_loopback_sim/README.md | 50 ++++ .../launch/loopback_simulation.launch.py | 52 ++++ .../nav2_loopback_sim/loopback_simulator.py | 224 ++++++++++++++++ nav2_loopback_sim/nav2_loopback_sim/utils.py | 71 +++++ nav2_loopback_sim/package.xml | 25 ++ nav2_loopback_sim/pytest.ini | 2 + nav2_loopback_sim/resource/nav2_loopback_sim | 0 nav2_loopback_sim/setup.cfg | 4 + nav2_loopback_sim/setup.py | 30 +++ nav2_loopback_sim/test/test_copyright.py | 23 ++ nav2_loopback_sim/test/test_flake8.py | 25 ++ nav2_loopback_sim/test/test_pep257.py | 23 ++ 16 files changed, 1025 insertions(+), 2 deletions(-) create mode 100644 nav2_bringup/launch/tb3_loopback_simulation.launch.py create mode 100644 nav2_bringup/launch/tb4_loopback_simulation.launch.py create mode 100644 nav2_loopback_sim/README.md create mode 100644 nav2_loopback_sim/launch/loopback_simulation.launch.py create mode 100644 nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py create mode 100644 nav2_loopback_sim/nav2_loopback_sim/utils.py create mode 100644 nav2_loopback_sim/package.xml create mode 100644 nav2_loopback_sim/pytest.ini create mode 100644 nav2_loopback_sim/resource/nav2_loopback_sim create mode 100644 nav2_loopback_sim/setup.cfg create mode 100644 nav2_loopback_sim/setup.py create mode 100644 nav2_loopback_sim/test/test_copyright.py create mode 100644 nav2_loopback_sim/test/test_flake8.py create mode 100644 nav2_loopback_sim/test/test_pep257.py diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index f39a0b8e88..eee19f6b36 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -48,6 +48,7 @@ def generate_launch_description(): use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') + use_localization = LaunchConfiguration('use_localization') # 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 @@ -99,6 +100,11 @@ def generate_launch_description(): 'map', default_value='', description='Full path to map yaml file to load' ) + declare_use_localization_cmd = DeclareLaunchArgument( + 'use_localization', default_value='True', + description='Whether to enable localization or not' + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', @@ -151,7 +157,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(launch_dir, 'slam_launch.py') ), - condition=IfCondition(slam), + condition=IfCondition(PythonExpression([slam, ' and ', use_localization])), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, @@ -164,7 +170,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(launch_dir, 'localization_launch.py') ), - condition=IfCondition(PythonExpression(['not ', slam])), + condition=IfCondition(PythonExpression(['not ', slam, ' and ', use_localization])), launch_arguments={ 'namespace': namespace, 'map': map_yaml_file, @@ -210,6 +216,7 @@ def generate_launch_description(): ld.add_action(declare_use_composition_cmd) ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) + ld.add_action(declare_use_localization_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation.launch.py new file mode 100644 index 0000000000..2d31edf0f3 --- /dev/null +++ b/nav2_bringup/launch/tb3_loopback_simulation.launch.py @@ -0,0 +1,235 @@ +# Copyright (C) 2024 Open Navigation LLC +# +# 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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os +import tempfile + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.descriptions import ParameterFile +from launch_ros.actions import Node, SetParameter + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + loopback_sim_dir = get_package_share_directory('nav2_loopback_sim') + launch_dir = os.path.join(bringup_dir, 'launch') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + + # Create the launch configuration variables + slam = LaunchConfiguration('slam') + namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') + map_yaml_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration('rviz_config_file') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack', + ) + + declare_slam_cmd = DeclareLaunchArgument( + 'slam', default_value='False', description='Whether run a SLAM' + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use', + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher', + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + namespace=namespace, + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], + remappings=remappings, + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'use_sim_time': 'True', + 'rviz_config': rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'use_localization': 'False', # Don't use SLAM, AMCL + }.items(), + ) + + loopback_sim_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(loopback_sim_dir, 'loopback_simulation.launch.py')), + launch_arguments={ + 'params_file': params_file, + }.items(), + ) + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + start_map_server = GroupAction( + actions=[ + SetParameter('use_sim_time', True), + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, {'yaml_filename': map_yaml_file}], + remappings=remappings, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_map_server', + output='screen', + parameters=[ + configured_params, + {'autostart': autostart}, {'node_names': ['map_server']}], + ), + ] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_use_respawn_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(start_map_server) + ld.add_action(loopback_sim_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/nav2_bringup/launch/tb4_loopback_simulation.launch.py b/nav2_bringup/launch/tb4_loopback_simulation.launch.py new file mode 100644 index 0000000000..6002c635de --- /dev/null +++ b/nav2_bringup/launch/tb4_loopback_simulation.launch.py @@ -0,0 +1,244 @@ +# Copyright (C) 2024 Open Navigation LLC +# +# 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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os +import tempfile + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch_ros.descriptions import ParameterFile +from launch_ros.actions import Node, SetParameter + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + loopback_sim_dir = get_package_share_directory('nav2_loopback_sim') + launch_dir = os.path.join(bringup_dir, 'launch') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') + + # Create the launch configuration variables + slam = LaunchConfiguration('slam') + namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') + map_yaml_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration('rviz_config_file') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack', + ) + + declare_slam_cmd = DeclareLaunchArgument( + 'slam', default_value='False', description='Whether run a SLAM' + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'depot.yaml'), # Try warehouse.yaml! + description='Full path to map file to load', + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use', + ) + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher', + ) + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) + + sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + namespace=namespace, + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', sdf])} + ], + remappings=remappings, + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'use_sim_time': 'True', + 'rviz_config': rviz_config_file, + }.items(), + ) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'use_localization': 'False', # Don't use SLAM, AMCL + }.items(), + ) + + loopback_sim_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(loopback_sim_dir, 'loopback_simulation.launch.py')), + launch_arguments={ + 'params_file': params_file, + 'scan_frame_id': 'rplidar_link', + }.items(), + ) + + static_publisher_cmd = Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=[ + '0.0', '0.0', '0.0', '0', '0', '0', + 'base_footprint', 'base_link'] + ) + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + start_map_server = GroupAction( + actions=[ + SetParameter('use_sim_time', True), + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, {'yaml_filename': map_yaml_file}], + remappings=remappings, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_map_server', + output='screen', + parameters=[ + configured_params, + {'autostart': autostart}, {'node_names': ['map_server']}], + ), + ] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_use_respawn_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(static_publisher_cmd) + ld.add_action(start_map_server) + ld.add_action(loopback_sim_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 307eb2f37a..7549315b06 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -419,3 +419,11 @@ docking_server: k_delta: 2.0 v_linear_min: 0.15 v_linear_max: 0.15 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.01 diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md new file mode 100644 index 0000000000..c0b05d1645 --- /dev/null +++ b/nav2_loopback_sim/README.md @@ -0,0 +1,50 @@ +# Nav2 Loopback Simulation + +The Nav2 loopback simulator is a stand-alone simulator to create a "loopback" for non-physical simulation to replace robot hardware, physics simulators (Gazebo, Bullet, Isaac Sim, etc). It computes the robot's odometry based on the command velocity's output request to create a perfect 'frictionless plane'-style simulation for unit testing, system testing, R&D on higher level systems, and testing behaviors without concerning yourself with localization accuracy or system dynamics. + +This was created by Steve Macenski of [Open Navigation LLC](https://opennav.org) and donated to Nav2 by the support of our project sponsors. If you rely on Nav2, please consider supporting the project! + +**⚠️ If you need professional services related to Nav2, please contact [Open Navigation](https://www.opennav.org/) at info@opennav.org.** + +It is drop-in replacable with AMR simulators and global localization by providing: +- Map -> Odom transform +- Odom -> Base Link transform, `nav_msgs/Odometry` odometry +- Accepts the standard `/initialpose` topic for transporting the robot to another location + +Note: This does not provide sensor data, so it is required that the global (and probably local) costmap contain the `StaticLayer` to avoid obstacles. + +It is convenient to be able to test systems by being able to: +- Arbitrarily transport the robot to any location and accurately navigate without waiting for a particle filter to converge for testing behaviors and reproducing higher-level issues +- Write unit or system tests on areas that are not dependent on low-level controller or localization performance without needing to spin up a compute-heavy process like Gazebo or Isaac Sim to provide odometry and sensor data, such as global planning, autonomy behavior trees, etc +- Perform R&D on various sensitive systems easily without concerning yourself with the errors accumulated with localization performance or imperfect dynamic models to get a proof of concept started +- When otherwise highly compute constrained and need to simulate a robotic system + +## How to Use + +``` +ros2 run nav2_loopback_sim loopback_simulator # As a node, if in simulation +ros2 launch nav2_loopback_sim loopback_simulation.launch.py # As a launch file +ros2 launch nav2_bringup tb3_loopback_simulation.launch.py # Nav2 integrated navigation demo using it +ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated navigation demo using it +``` + +## API + +### Parameters + +- `update_duration`: the duration between updates (default 0.01 -- 100hz) +- `base_frame_id`: The base frame to use (default `base_link`) +- `odom_frame_id`: The odom frame to use (default `odom`) +- `map_frame_id`: The map frame to use (default `map`) +- `scan_frame_id`: The map frame to use (default `base_scan` for TB3, `rplidar_link` for TB4) + +### Topics + +This node subscribes to: +- `initialpose`: To set the initial robot pose or relocalization request analog to other localization systems +- `cmd_vel`: Nav2's output twist to get the commanded velocity + +This node publishes: +- `odom`: To publish odometry from twist +- `tf`: To publish map->odom and odom->base_link transforms +- `scan`: To publish a bogus max range laser scan sensor to make the collision monitor happy diff --git a/nav2_loopback_sim/launch/loopback_simulation.launch.py b/nav2_loopback_sim/launch/loopback_simulation.launch.py new file mode 100644 index 0000000000..0124cb848f --- /dev/null +++ b/nav2_loopback_sim/launch/loopback_simulation.launch.py @@ -0,0 +1,52 @@ +# Copyright (c) 2024 Open Navigation LLC +# +# 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 +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = LaunchConfiguration('params_file') + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + scan_frame_id = LaunchConfiguration('scan_frame_id') + declare_scan_frame_id_cmd = DeclareLaunchArgument( + 'scan_frame_id', + default_value='base_scan', + ) + + loopback_sim_cmd = Node( + package='nav2_loopback_sim', + executable='loopback_simulator', + name='loopback_simulator', + output='screen', + parameters=[params_file, {'scan_frame_id': scan_frame_id}], + ) + + ld = LaunchDescription() + ld.add_action(declare_scan_frame_id_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(loopback_sim_cmd) + return ld diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py new file mode 100644 index 0000000000..471c2f497f --- /dev/null +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -0,0 +1,224 @@ +# Copyright (c) 2024 Open Navigation LLC +# +# 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 copy +import math + +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist +from geometry_msgs.msg import TransformStamped, Vector3, Quaternion +from nav_msgs.msg import Odometry +import numpy as np +import rclpy +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy +from sensor_msgs.msg import LaserScan +import tf_transformations +from tf2_ros import Buffer, TransformBroadcaster, TransformListener +from .utils import transformStampedToMatrix, matrixToTransform, addYawToQuat + + +""" +This is a loopback simulator that replaces a physics simulator to create a +frictionless, inertialess, and collisionless simulation environment. It +accepts cmd_vel messages and publishes odometry & TF messages based on the +cumulative velocities received to mimick global localization and simulation. +It also accepts initialpose messages to set the initial pose of the robot +to place anywhere. +""" + + +class LoopbackSimulator(Node): + + def __init__(self): + super().__init__(node_name='loopback_simulator') + self.curr_cmd_vel = None + self.curr_cmd_vel_time = self.get_clock().now() + self.initial_pose = None + self.timer = None + self.setupTimer = None + + self.declare_parameter('update_duration', 0.01) + self.update_dur = self.get_parameter('update_duration').get_parameter_value().double_value + + self.declare_parameter('base_frame_id', 'base_footprint') + self.base_frame_id = self.get_parameter('base_frame_id').get_parameter_value().string_value + + self.declare_parameter('map_frame_id', 'map') + self.map_frame_id = self.get_parameter('map_frame_id').get_parameter_value().string_value + + self.declare_parameter('odom_frame_id', 'odom') + self.odom_frame_id = self.get_parameter('odom_frame_id').get_parameter_value().string_value + + self.declare_parameter('scan_frame_id', 'base_scan') + self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value + + self.t_map_to_odom = TransformStamped() + self.t_map_to_odom.header.frame_id = self.map_frame_id + self.t_map_to_odom.child_frame_id = self.odom_frame_id + self.t_odom_to_base_link = TransformStamped() + self.t_odom_to_base_link.header.frame_id = self.odom_frame_id + self.t_odom_to_base_link.child_frame_id = self.base_frame_id + + self.tf_broadcaster = TransformBroadcaster(self) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.initial_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, + 'initialpose', self.initialPoseCallback, 10) + self.cmd_vel_sub = self.create_subscription( + Twist, + 'cmd_vel', self.cmdVelCallback, 10) + self.odom_pub = self.create_publisher(Odometry, 'odom', 10) + + sensor_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + depth=10) + self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos) + + self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) + self.info('Loopback simulator initialized') + + def setupTimerCallback(self): + # Publish initial identity odom transform & laser scan to warm up system + self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) + self.publishLaserScan() + + def cmdVelCallback(self, msg): + self.debug('Received cmd_vel') + if self.initial_pose is None: + # Don't consider velocities before the initial pose is set + return + self.curr_cmd_vel = msg + self.curr_cmd_vel_time = self.get_clock().now() + + def initialPoseCallback(self, msg): + self.info('Received initial pose!') + if self.initial_pose is None: + # Initialize transforms (map->odom as input pose, odom->base_link start from identity) + self.initial_pose = msg.pose.pose + self.t_map_to_odom.transform.translation.x = self.initial_pose.position.x + self.t_map_to_odom.transform.translation.y = self.initial_pose.position.y + self.t_map_to_odom.transform.rotation = self.initial_pose.orientation + self.t_odom_to_base_link.transform.translation = Vector3() + self.t_odom_to_base_link.transform.rotation = Quaternion() + self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) + + # Start republication timer and velocity processing + if self.setupTimer is not None: + self.setupTimer.cancel() + self.setupTimer.destroy() + self.setupTimer = None + self.timer = self.create_timer(self.update_dur, self.timerCallback) + return + + self.initial_pose = msg.pose.pose + + # Adjust map->odom transform based on new initial pose, keeping odom->base_link the same + t_map_to_base_link = TransformStamped() + t_map_to_base_link.header = msg.header + t_map_to_base_link.child_frame_id = 'base_link' + t_map_to_base_link.transform.translation.x = self.initial_pose.position.x + t_map_to_base_link.transform.translation.y = self.initial_pose.position.y + t_map_to_base_link.transform.translation.z = self.initial_pose.position.z + t_map_to_base_link.transform.rotation = self.initial_pose.orientation + mat_map_to_base_link = transformStampedToMatrix(t_map_to_base_link) + mat_odom_to_base_link = transformStampedToMatrix(self.t_odom_to_base_link) + mat_base_link_to_odom = tf_transformations.inverse_matrix(mat_odom_to_base_link) + mat_map_to_odom = tf_transformations.concatenate_matrices(mat_map_to_base_link, mat_base_link_to_odom) + self.t_map_to_odom.transform = matrixToTransform(mat_map_to_odom) + + def timerCallback(self): + # If no data, just republish existing transforms without change + if self.curr_cmd_vel is None or self.get_clock().now() - self.curr_cmd_vel_time > Duration(seconds=1): + self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) + self.curr_cmd_vel= None + return + + # Update odom->base_link from cmd_vel + dx = self.curr_cmd_vel.linear.x * self.update_dur + dy = self.curr_cmd_vel.linear.y * self.update_dur + dth = self.curr_cmd_vel.angular.z * self.update_dur + q = [self.t_odom_to_base_link.transform.rotation.x, + self.t_odom_to_base_link.transform.rotation.y, + self.t_odom_to_base_link.transform.rotation.z, + self.t_odom_to_base_link.transform.rotation.w] + _, _, yaw = tf_transformations.euler_from_quaternion(q) + self.t_odom_to_base_link.transform.translation.x += dx * math.cos(yaw) - dy * math.sin(yaw) + self.t_odom_to_base_link.transform.translation.y += dx * math.sin(yaw) + dy * math.cos(yaw) + self.t_odom_to_base_link.transform.rotation = \ + addYawToQuat(self.t_odom_to_base_link.transform.rotation, dth) + + self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) + self.publishOdometry(self.t_odom_to_base_link) + self.publishLaserScan() + + def publishLaserScan(self): + # Publish a bogus laser scan for collision monitor + scan = LaserScan() + # scan.header.stamp = (self.get_clock().now()).to_msg() + scan.header.frame_id = self.scan_frame_id + scan.angle_min = -math.pi + scan.angle_max = math.pi + scan.angle_increment = 0.005817705996 # 0.333 degrees + scan.time_increment = 0.0 + scan.scan_time = 0.1 + scan.range_min = 0.1 + scan.range_max = 100.0 + num_samples = int((scan.angle_max - scan.angle_min) / scan.angle_increment) + for i in range(num_samples): + scan.ranges.append(scan.range_max - 0.01) + self.scan_pub.publish(scan) + + def publishTransforms(self, map_to_odom, odom_to_base_link): + map_to_odom.header.stamp = \ + (self.get_clock().now() + Duration(seconds=self.update_dur)).to_msg() + odom_to_base_link.header.stamp = self.get_clock().now().to_msg() + self.tf_broadcaster.sendTransform(map_to_odom) + self.tf_broadcaster.sendTransform(odom_to_base_link) + + def publishOdometry(self, odom_to_base_link): + odom = Odometry() + odom.header.stamp = self.get_clock().now().to_msg() + odom.header.frame_id = 'odom' + odom.child_frame_id = 'base_link' + odom.pose.pose.position.x = odom_to_base_link.transform.translation.x + odom.pose.pose.position.y = odom_to_base_link.transform.translation.y + odom.pose.pose.orientation = odom_to_base_link.transform.rotation + odom.twist.twist = self.curr_cmd_vel + self.odom_pub.publish(odom) + + def info(self, msg): + self.get_logger().info(msg) + return + + def debug(self, msg): + self.get_logger().debug(msg) + return + + +def main(): + rclpy.init() + loopback_simulator = LoopbackSimulator() + rclpy.spin(loopback_simulator) + loopback_simulator.destroy_node() + rclpy.shutdown() + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py new file mode 100644 index 0000000000..ba4dc8a3f0 --- /dev/null +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -0,0 +1,71 @@ +# Copyright (c) 2024 Open Navigation LLC +# +# 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 numpy as np +import tf_transformations +from geometry_msgs.msg import Transform, TransformStamped, Quaternion + + +""" +Transformation utilities for the loopback simulator +""" + + +def addYawToQuat(quaternion, yaw_to_add): + q = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] + _, _, yaw = tf_transformations.euler_from_quaternion(q) + new_yaw = yaw + yaw_to_add + new_quaternion_array = tf_transformations.quaternion_from_euler(0.0, 0.0, new_yaw) + new_quaternion = Quaternion() + new_quaternion.x = new_quaternion_array[0] + new_quaternion.y = new_quaternion_array[1] + new_quaternion.z = new_quaternion_array[2] + new_quaternion.w = new_quaternion_array[3] + return new_quaternion + +def transformStampedToMatrix(transform): + """ + Converts a geometry_msgs/TransformStamped to a 4x4 numpy matrix. + """ + translation = transform.transform.translation + rotation = transform.transform.rotation + matrix = np.eye(4) + matrix[0, 3] = translation.x + matrix[1, 3] = translation.y + matrix[2, 3] = translation.z + rotation_matrix = tf_transformations.quaternion_matrix([ + rotation.x, + rotation.y, + rotation.z, + rotation.w + ]) + matrix[:3, :3] = rotation_matrix[:3, :3] + return matrix + +def matrixToTransform(matrix): + """ + Converts a 4x4 numpy matrix to a geometry_msgs/Transform. + """ + transform = Transform() + transform.translation.x = matrix[0, 3] + transform.translation.y = matrix[1, 3] + transform.translation.z = matrix[2, 3] + rotation_matrix = matrix[:3, :3] + quaternion = tf_transformations.quaternion_from_matrix(matrix) + transform.rotation.x = quaternion[0] + transform.rotation.y = quaternion[1] + transform.rotation.z = quaternion[2] + transform.rotation.w = quaternion[3] + return transform diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml new file mode 100644 index 0000000000..a2a253def9 --- /dev/null +++ b/nav2_loopback_sim/package.xml @@ -0,0 +1,25 @@ + + + + nav2_loopback_sim + 1.3.1 + A loopback simulator to replace physics simulation + steve macenski + Apache-2.0 + + rclpy + geometry_msgs + nav_msgs + tf_transformations + tf2_ros + python3-transforms3d + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/nav2_loopback_sim/pytest.ini b/nav2_loopback_sim/pytest.ini new file mode 100644 index 0000000000..fe55d2ed64 --- /dev/null +++ b/nav2_loopback_sim/pytest.ini @@ -0,0 +1,2 @@ +[pytest] +junit_family=xunit2 diff --git a/nav2_loopback_sim/resource/nav2_loopback_sim b/nav2_loopback_sim/resource/nav2_loopback_sim new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_loopback_sim/setup.cfg b/nav2_loopback_sim/setup.cfg new file mode 100644 index 0000000000..20a46b06d6 --- /dev/null +++ b/nav2_loopback_sim/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/nav2_loopback_sim +[install] +install_scripts=$base/lib/nav2_loopback_sim diff --git a/nav2_loopback_sim/setup.py b/nav2_loopback_sim/setup.py new file mode 100644 index 0000000000..0d56733614 --- /dev/null +++ b/nav2_loopback_sim/setup.py @@ -0,0 +1,30 @@ +from glob import glob +import os + +from setuptools import setup + + +package_name = 'nav2_loopback_sim' + +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='stevemacenski', + maintainer_email='steve@opennav.org', + description='A loopback simulator to replace physics simulation', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'loopback_simulator = nav2_loopback_sim.loopback_simulator:main', + ], + }, +) diff --git a/nav2_loopback_sim/test/test_copyright.py b/nav2_loopback_sim/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /dev/null +++ b/nav2_loopback_sim/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_loopback_sim/test/test_flake8.py b/nav2_loopback_sim/test/test_flake8.py new file mode 100644 index 0000000000..26030113cc --- /dev/null +++ b/nav2_loopback_sim/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_loopback_sim/test/test_pep257.py b/nav2_loopback_sim/test/test_pep257.py new file mode 100644 index 0000000000..b234a3840f --- /dev/null +++ b/nav2_loopback_sim/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 4f12b72f784e1aae32129ef4b9100a5e90e6fefa Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 9 Aug 2024 12:17:55 -0700 Subject: [PATCH 2/6] drop performance by half --- nav2_bringup/params/nav2_params.yaml | 2 +- nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 7549315b06..2581635e7f 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -426,4 +426,4 @@ loopback_simulator: odom_frame_id: "odom" map_frame_id: "map" scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' - update_duration: 0.01 + update_duration: 0.02 diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 471c2f497f..8e02d09849 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -73,8 +73,6 @@ def __init__(self): self.t_odom_to_base_link.child_frame_id = self.base_frame_id self.tf_broadcaster = TransformBroadcaster(self) - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) self.initial_pose_sub = self.create_subscription( PoseWithCovarianceStamped, @@ -180,8 +178,7 @@ def publishLaserScan(self): scan.range_min = 0.1 scan.range_max = 100.0 num_samples = int((scan.angle_max - scan.angle_min) / scan.angle_increment) - for i in range(num_samples): - scan.ranges.append(scan.range_max - 0.01) + scan.ranges = [scan.range_max - 0.01] * num_samples self.scan_pub.publish(scan) def publishTransforms(self, map_to_odom, odom_to_base_link): From b4c436b8dd876a9eaad1cc7dc538dfbc22ad89c0 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 9 Aug 2024 12:26:48 -0700 Subject: [PATCH 3/6] lintin --- .../launch/tb3_loopback_simulation.launch.py | 15 ++-------- .../launch/tb4_loopback_simulation.launch.py | 16 ++-------- .../nav2_loopback_sim/loopback_simulator.py | 29 ++++++++++--------- nav2_loopback_sim/nav2_loopback_sim/utils.py | 11 ++----- 4 files changed, 22 insertions(+), 49 deletions(-) diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation.launch.py index 2d31edf0f3..9e598e854c 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation.launch.py @@ -15,25 +15,20 @@ """This is all-in-one launch script intended for use by nav2 developers.""" import os -import tempfile from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, - ExecuteProcess, GroupAction, IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, ) from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.descriptions import ParameterFile +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml @@ -46,7 +41,6 @@ def generate_launch_description(): sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') # Create the launch configuration variables - slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') @@ -73,10 +67,6 @@ def generate_launch_description(): description='Whether to apply a namespace to the navigation stack', ) - declare_slam_cmd = DeclareLaunchArgument( - 'slam', default_value='False', description='Whether run a SLAM' - ) - declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), @@ -214,7 +204,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) - ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_bringup/launch/tb4_loopback_simulation.launch.py b/nav2_bringup/launch/tb4_loopback_simulation.launch.py index 6002c635de..0ad911ddc7 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation.launch.py @@ -15,25 +15,20 @@ """This is all-in-one launch script intended for use by nav2 developers.""" import os -import tempfile from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, - ExecuteProcess, GroupAction, IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, ) from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration, PythonExpression -from launch_ros.descriptions import ParameterFile +from launch.substitutions import Command, LaunchConfiguration from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml @@ -43,11 +38,9 @@ def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') loopback_sim_dir = get_package_share_directory('nav2_loopback_sim') launch_dir = os.path.join(bringup_dir, 'launch') - sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') desc_dir = get_package_share_directory('nav2_minimal_tb4_description') # Create the launch configuration variables - slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') @@ -74,10 +67,6 @@ def generate_launch_description(): description='Whether to apply a namespace to the navigation stack', ) - declare_slam_cmd = DeclareLaunchArgument( - 'slam', default_value='False', description='Whether run a SLAM' - ) - declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'depot.yaml'), # Try warehouse.yaml! @@ -222,7 +211,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) - ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 8e02d09849..3990797cbe 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -13,21 +13,20 @@ # limitations under the License. -import copy import math -from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist -from geometry_msgs.msg import TransformStamped, Vector3, Quaternion +from geometry_msgs.msg import PoseWithCovarianceStamped, Twist +from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 from nav_msgs.msg import Odometry -import numpy as np import rclpy from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import LaserScan +from tf2_ros import TransformBroadcaster import tf_transformations -from tf2_ros import Buffer, TransformBroadcaster, TransformListener -from .utils import transformStampedToMatrix, matrixToTransform, addYawToQuat + +from .utils import addYawToQuat, matrixToTransform, transformStampedToMatrix """ @@ -87,10 +86,10 @@ def __init__(self): durability=DurabilityPolicy.VOLATILE, depth=10) self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos) - + self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) self.info('Loopback simulator initialized') - + def setupTimerCallback(self): # Publish initial identity odom transform & laser scan to warm up system self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) @@ -123,7 +122,7 @@ def initialPoseCallback(self, msg): self.setupTimer = None self.timer = self.create_timer(self.update_dur, self.timerCallback) return - + self.initial_pose = msg.pose.pose # Adjust map->odom transform based on new initial pose, keeping odom->base_link the same @@ -137,14 +136,16 @@ def initialPoseCallback(self, msg): mat_map_to_base_link = transformStampedToMatrix(t_map_to_base_link) mat_odom_to_base_link = transformStampedToMatrix(self.t_odom_to_base_link) mat_base_link_to_odom = tf_transformations.inverse_matrix(mat_odom_to_base_link) - mat_map_to_odom = tf_transformations.concatenate_matrices(mat_map_to_base_link, mat_base_link_to_odom) + mat_map_to_odom = \ + tf_transformations.concatenate_matrices(mat_map_to_base_link, mat_base_link_to_odom) self.t_map_to_odom.transform = matrixToTransform(mat_map_to_odom) def timerCallback(self): # If no data, just republish existing transforms without change - if self.curr_cmd_vel is None or self.get_clock().now() - self.curr_cmd_vel_time > Duration(seconds=1): + one_sec = Duration(seconds=1) + if self.curr_cmd_vel is None or self.get_clock().now() - self.curr_cmd_vel_time > one_sec: self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) - self.curr_cmd_vel= None + self.curr_cmd_vel = None return # Update odom->base_link from cmd_vel @@ -187,7 +188,7 @@ def publishTransforms(self, map_to_odom, odom_to_base_link): odom_to_base_link.header.stamp = self.get_clock().now().to_msg() self.tf_broadcaster.sendTransform(map_to_odom) self.tf_broadcaster.sendTransform(odom_to_base_link) - + def publishOdometry(self, odom_to_base_link): odom = Odometry() odom.header.stamp = self.get_clock().now().to_msg() diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index ba4dc8a3f0..f6ae5f5e7d 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -13,9 +13,9 @@ # limitations under the License. +from geometry_msgs.msg import Quaternion, Transform import numpy as np import tf_transformations -from geometry_msgs.msg import Transform, TransformStamped, Quaternion """ @@ -35,10 +35,8 @@ def addYawToQuat(quaternion, yaw_to_add): new_quaternion.w = new_quaternion_array[3] return new_quaternion + def transformStampedToMatrix(transform): - """ - Converts a geometry_msgs/TransformStamped to a 4x4 numpy matrix. - """ translation = transform.transform.translation rotation = transform.transform.rotation matrix = np.eye(4) @@ -54,15 +52,12 @@ def transformStampedToMatrix(transform): matrix[:3, :3] = rotation_matrix[:3, :3] return matrix + def matrixToTransform(matrix): - """ - Converts a 4x4 numpy matrix to a geometry_msgs/Transform. - """ transform = Transform() transform.translation.x = matrix[0, 3] transform.translation.y = matrix[1, 3] transform.translation.z = matrix[2, 3] - rotation_matrix = matrix[:3, :3] quaternion = tf_transformations.quaternion_from_matrix(matrix) transform.rotation.x = quaternion[0] transform.rotation.y = quaternion[1] From 44e9a117017eeb9a9a3fcc95a755d87b749aec4e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 9 Aug 2024 13:40:45 -0700 Subject: [PATCH 4/6] Add multirobot usecase comment --- nav2_loopback_sim/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index c0b05d1645..3919146012 100644 --- a/nav2_loopback_sim/README.md +++ b/nav2_loopback_sim/README.md @@ -17,6 +17,7 @@ It is convenient to be able to test systems by being able to: - Arbitrarily transport the robot to any location and accurately navigate without waiting for a particle filter to converge for testing behaviors and reproducing higher-level issues - Write unit or system tests on areas that are not dependent on low-level controller or localization performance without needing to spin up a compute-heavy process like Gazebo or Isaac Sim to provide odometry and sensor data, such as global planning, autonomy behavior trees, etc - Perform R&D on various sensitive systems easily without concerning yourself with the errors accumulated with localization performance or imperfect dynamic models to get a proof of concept started +- Simulate N robots simultaneously with a lower compute footprint - When otherwise highly compute constrained and need to simulate a robotic system ## How to Use From 267bd5800f72eab756731691c38b55c637fd1fa7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 9 Aug 2024 14:52:49 -0700 Subject: [PATCH 5/6] fixing copy paste error --- nav2_loopback_sim/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index 3919146012..1e1aec6569 100644 --- a/nav2_loopback_sim/README.md +++ b/nav2_loopback_sim/README.md @@ -37,7 +37,7 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated na - `base_frame_id`: The base frame to use (default `base_link`) - `odom_frame_id`: The odom frame to use (default `odom`) - `map_frame_id`: The map frame to use (default `map`) -- `scan_frame_id`: The map frame to use (default `base_scan` for TB3, `rplidar_link` for TB4) +- `scan_frame_id`: The can frame to use to publish a scan to keep the collision monitor fed and happy (default `base_scan` for TB3, `rplidar_link` for TB4) ### Topics From 783af26acd50bc66d44aadda164e5912deb54482 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 12 Aug 2024 12:19:47 -0700 Subject: [PATCH 6/6] fixing review comments --- .../nav2_loopback_sim/loopback_simulator.py | 3 ++- nav2_loopback_sim/nav2_loopback_sim/utils.py | 13 ++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 3990797cbe..dd476aa986 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -110,6 +110,7 @@ def initialPoseCallback(self, msg): self.initial_pose = msg.pose.pose self.t_map_to_odom.transform.translation.x = self.initial_pose.position.x self.t_map_to_odom.transform.translation.y = self.initial_pose.position.y + self.t_map_to_odom.transform.translation.z = 0.0 self.t_map_to_odom.transform.rotation = self.initial_pose.orientation self.t_odom_to_base_link.transform.translation = Vector3() self.t_odom_to_base_link.transform.rotation = Quaternion() @@ -131,7 +132,7 @@ def initialPoseCallback(self, msg): t_map_to_base_link.child_frame_id = 'base_link' t_map_to_base_link.transform.translation.x = self.initial_pose.position.x t_map_to_base_link.transform.translation.y = self.initial_pose.position.y - t_map_to_base_link.transform.translation.z = self.initial_pose.position.z + t_map_to_base_link.transform.translation.z = 0.0 t_map_to_base_link.transform.rotation = self.initial_pose.orientation mat_map_to_base_link = transformStampedToMatrix(t_map_to_base_link) mat_odom_to_base_link = transformStampedToMatrix(self.t_odom_to_base_link) diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index f6ae5f5e7d..0ed85689dd 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -25,14 +25,13 @@ def addYawToQuat(quaternion, yaw_to_add): q = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] - _, _, yaw = tf_transformations.euler_from_quaternion(q) - new_yaw = yaw + yaw_to_add - new_quaternion_array = tf_transformations.quaternion_from_euler(0.0, 0.0, new_yaw) + q2 = tf_transformations.quaternion_from_euler(0.0, 0.0, yaw_to_add) + q_new = tf_transformations.quaternion_multiply(q, q2) new_quaternion = Quaternion() - new_quaternion.x = new_quaternion_array[0] - new_quaternion.y = new_quaternion_array[1] - new_quaternion.z = new_quaternion_array[2] - new_quaternion.w = new_quaternion_array[3] + new_quaternion.x = q_new[0] + new_quaternion.y = q_new[1] + new_quaternion.z = q_new[2] + new_quaternion.w = q_new[3] return new_quaternion