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..9e598e854c --- /dev/null +++ b/nav2_bringup/launch/tb3_loopback_simulation.launch.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. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile + +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 + 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_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_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..0ad911ddc7 --- /dev/null +++ b/nav2_bringup/launch/tb4_loopback_simulation.launch.py @@ -0,0 +1,232 @@ +# 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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +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 + + +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') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') + + # Create the launch configuration variables + 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_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_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..2581635e7f 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.02 diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md new file mode 100644 index 0000000000..1e1aec6569 --- /dev/null +++ b/nav2_loopback_sim/README.md @@ -0,0 +1,51 @@ +# 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 +- Simulate N robots simultaneously with a lower compute footprint +- 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 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 + +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..dd476aa986 --- /dev/null +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -0,0 +1,223 @@ +# 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 math + +from geometry_msgs.msg import PoseWithCovarianceStamped, Twist +from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 +from nav_msgs.msg import Odometry +import rclpy +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import LaserScan +from tf2_ros import TransformBroadcaster +import tf_transformations + +from .utils import addYawToQuat, matrixToTransform, transformStampedToMatrix + + +""" +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.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.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() + 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 = 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) + 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 + 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 + 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) + scan.ranges = [scan.range_max - 0.01] * num_samples + 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..0ed85689dd --- /dev/null +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -0,0 +1,65 @@ +# 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. + + +from geometry_msgs.msg import Quaternion, Transform +import numpy as np +import tf_transformations + + +""" +Transformation utilities for the loopback simulator +""" + + +def addYawToQuat(quaternion, yaw_to_add): + q = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] + 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 = q_new[0] + new_quaternion.y = q_new[1] + new_quaternion.z = q_new[2] + new_quaternion.w = q_new[3] + return new_quaternion + + +def transformStampedToMatrix(transform): + 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): + transform = Transform() + transform.translation.x = matrix[0, 3] + transform.translation.y = matrix[1, 3] + transform.translation.z = matrix[2, 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'