diff --git a/.circleci/config.yml b/.circleci/config.yml index 14a1ec0558c..b50279ebc85 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -573,12 +573,12 @@ jobs: _parameters: core_build_parameters: &core_build_parameters packages_skip_regex: "" - packages_select: "nav2_common nav2_voxel_grid nav_2d_msgs dwb_msgs nav2_msgs nav2_ros_common nav2_simple_commander nav2_loopback_sim nav2_util nav2_amcl nav2_behavior_tree nav2_lifecycle_manager nav2_map_server nav_2d_utils nav2_velocity_smoother nav2_costmap_2d costmap_queue nav2_core" + packages_select: "nav2_common nav2_voxel_grid nav_2d_msgs dwb_msgs nav2_msgs nav2_ros_common nav2_simple_commander nav2_util nav2_amcl nav2_lifecycle_manager nav2_map_server nav_2d_utils nav2_velocity_smoother nav2_costmap_2d costmap_queue" algorithm_build_parameters: &algorithm_build_parameters packages_skip_regex: "" - packages_select: "nav2_collision_monitor dwb_core dwb_critics dwb_plugins nav2_dwb_controller nav2_controller nav2_bt_navigator nav2_constrained_smoother nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_theta_star_planner nav2_graceful_controller" + packages_select: "nav2_core nav2_behavior_tree nav2_collision_monitor dwb_core dwb_critics dwb_plugins nav2_dwb_controller nav2_controller nav2_bt_navigator nav2_constrained_smoother nav2_navfn_planner nav2_planner nav2_regulated_pure_pursuit_controller nav2_theta_star_planner nav2_graceful_controller" system_build_parameters: &system_build_parameters - packages_skip_regex: "'(nav2_common|nav2_voxel_grid|nav_2d_msgs|dwb_msgs|nav2_msgs|nav2_ros_common|nav2_simple_commander|nav2_loopback_sim|nav2_util|nav2_amcl|nav2_behavior_tree|nav2_lifecycle_manager|nav2_map_server|nav_2d_utils|nav2_velocity_smoother|nav2_costmap_2d|costmap_queue|nav2_core|nav2_collision_monitor|dwb_core|dwb_critics|dwb_plugins|nav2_dwb_controller|nav2_controller|nav2_bt_navigator|nav2_constrained_smoother|nav2_navfn_planner|nav2_planner|nav2_regulated_pure_pursuit_controller|nav2_theta_star_planner|nav2_graceful_controller)'" + packages_skip_regex: "'(nav2_common|nav2_voxel_grid|nav_2d_msgs|dwb_msgs|nav2_msgs|nav2_ros_common|nav2_simple_commander|nav2_util|nav2_amcl|nav2_behavior_tree|nav2_lifecycle_manager|nav2_map_server|nav_2d_utils|nav2_velocity_smoother|nav2_costmap_2d|costmap_queue|nav2_core|nav2_collision_monitor|dwb_core|dwb_critics|dwb_plugins|nav2_dwb_controller|nav2_controller|nav2_bt_navigator|nav2_constrained_smoother|nav2_navfn_planner|nav2_planner|nav2_regulated_pure_pursuit_controller|nav2_theta_star_planner|nav2_graceful_controller)'" packages_select: "" release_test_parameters: &release_test_parameters packages_skip_regex: "" diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 7e8cf2a046f..1f910313f73 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -28,6 +28,7 @@ xacro nav2_minimal_tb4_sim nav2_minimal_tb3_sim + nav2_loopback_sim ament_lint_common ament_lint_auto diff --git a/nav2_loopback_sim/CMakeLists.txt b/nav2_loopback_sim/CMakeLists.txt new file mode 100644 index 00000000000..777f4a5fd6b --- /dev/null +++ b/nav2_loopback_sim/CMakeLists.txt @@ -0,0 +1,125 @@ +cmake_minimum_required(VERSION 3.8) +project(nav2_loopback_sim) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_ros_common REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rosgraph_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +nav2_package() + +set(executable_name loopback_simulator) +set(library_name ${executable_name}_core) + +# Main library +add_library(${library_name} SHARED + src/loopback_simulator.cpp + src/clock_publisher.cpp +) +target_include_directories(${library_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + nav2_ros_common::nav2_ros_common + nav2_util::nav2_util_core + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rosgraph_msgs_TARGETS} + ${sensor_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros +) +target_link_libraries(${library_name} PRIVATE + rclcpp_components::component +) + +# Main executable +add_executable(${executable_name} + src/main.cpp +) +target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp) + +rclcpp_components_register_nodes(${library_name} "nav2_loopback_sim::LoopbackSimulator") + +install( + TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + ament_find_gtest() + + nav2_add_gtest(test_clock_publisher test/test_clock_publisher.cpp) + target_link_libraries(test_clock_publisher + ${library_name} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + nav2_ros_common::nav2_ros_common + ${rosgraph_msgs_TARGETS} + ) + + nav2_add_gtest(test_loopback_simulator test/test_loopback_simulator.cpp) + target_link_libraries(test_loopback_simulator + ${library_name} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2::tf2 + ${tf2_msgs_TARGETS} + ) +endif() + +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_libraries(${library_name}) +ament_export_dependencies( + geometry_msgs + nav2_ros_common + rcl_interfaces + nav2_util + nav_msgs + rclcpp + rclcpp_lifecycle + rosgraph_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros +) +ament_package() diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index 62104e05e9b..62692252525 100644 --- a/nav2_loopback_sim/README.md +++ b/nav2_loopback_sim/README.md @@ -1,6 +1,6 @@ # 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. +The Nav2 loopback simulator is a standalone 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! @@ -34,20 +34,24 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated na ### Parameters - `update_duration`: The duration between updates (default 0.01 -- 100hz) -- `base_frame_id`: The base frame to use (default `base_link`) +- `base_frame_id`: The base frame to use (default `base_footprint`) - `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) -- `enable_stamped_cmd_vel`: Whether cmd_vel is stamped or unstamped (i.e. Twist or TwistStamped). Default `false` for `Twist`. -- `scan_publish_dur`: : The duration between publishing scan (default 0.1s -- 10hz) +- `scan_frame_id`: The scan frame to use to publish a scan to keep the collision monitor fed and happy (default `base_scan` for TB3, `rplidar_link` for TB4) +- `enable_stamped_cmd_vel`: Whether cmd_vel is stamped or unstamped (i.e. Twist or TwistStamped). Default `true` for `TwistStamped` (Kilted and newer; was `false` in Jazzy). +- `odom_publish_dur`: The duration between publishing odometry (default: same as `update_duration`) +- `scan_publish_dur`: The duration between publishing scan (default 0.1s -- 10hz) - `publish_map_odom_tf`: Whether or not to publish tf from `map_frame_id` to `odom_frame_id` (default `true`) +- `publish_scan`: Whether or not to publish a fake laser scan (default `true`) - `publish_clock`: Whether or not to publish simulated clock to `/clock` (default `true`) +- `speed_factor`: Speed factor for the simulated clock, e.g. 2.0 runs simulation at 2x wall time (default `1.0`). Dynamically reconfigurable. - `scan_range_min`: Minimum measurable distance from the scan in meters. Values below this are considered invalid (default: `0.05`) - `scan_range_max`: Maximum measurable distance from the scan in meters. Values beyond this are out of range (default: `30.0`) - `scan_angle_min`: Starting angle of the scan in radians (leftmost angle) (default: `-π` / `-3.1415`) - `scan_angle_max`: Ending angle of the scan in radians (rightmost angle) (default: `π` / `3.1415`) - `scan_angle_increment`: Angular resolution of the scan in radians (angle between consecutive measurements) (default: `0.02617`) -- `scan_use_inf`: Whether to use `inf` for out-of-range values. If `false`, uses `scan_range_max - 0.1` instead (default: `True`) +- `scan_use_inf`: Whether to use `inf` for out-of-range values. If `false`, uses `scan_range_max - 0.1` instead (default: `true`) +- `scan_noise_std`: Standard deviation of Gaussian noise added to scan ranges (default: `0.01`) ### Topics @@ -57,7 +61,7 @@ This node subscribes to: - `cmd_vel`: Nav2's output twist to get the commanded velocity This node publishes: -- `clock`: To publish a simulation clock for all other nodes with `use_sim_time=True` +- `clock`: To publish a simulation clock for all other nodes with `use_sim_time=True` (when `publish_clock` is `true`) - `odom`: To publish odometry from twist - `tf`: To publish map->odom and odom->base_link transforms -- `scan`: To publish a range laser scan sensor based on the static map +- `scan`: To publish a range laser scan sensor based on the static map (when `publish_scan` is `true`) diff --git a/nav2_loopback_sim/include/nav2_loopback_sim/clock_publisher.hpp b/nav2_loopback_sim/include/nav2_loopback_sim/clock_publisher.hpp new file mode 100644 index 00000000000..83ed3c764ef --- /dev/null +++ b/nav2_loopback_sim/include/nav2_loopback_sim/clock_publisher.hpp @@ -0,0 +1,86 @@ +// Copyright (c) 2024, Open Navigation LLC +// Copyright (c) 2026, Dexory (Tony Najjar) +// +// 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. + +#ifndef NAV2_LOOPBACK_SIM__CLOCK_PUBLISHER_HPP_ +#define NAV2_LOOPBACK_SIM__CLOCK_PUBLISHER_HPP_ + +#include +#include + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rosgraph_msgs/msg/clock.hpp" + +namespace nav2_loopback_sim +{ + +/** + * @brief Publishes simulated clock to /clock using wall time. + * Uses wall timers so that it works correctly even when owned by a + * node with use_sim_time=true. Supports speed_factor to run sim + * time faster or slower than real time. + */ +class ClockPublisher +{ +public: + /** + * @brief Construct a ClockPublisher. + * @param node Weak pointer to the owning lifecycle node + * @param speed_factor Sim-time speed relative to wall time + */ + ClockPublisher( + nav2::LifecycleNode::WeakPtr node, + double speed_factor = 1.0); + + /** + * @brief Start publishing /clock + */ + void start(); + /** + * @brief Stop publishing /clock + */ + void stop(); + /** + * @brief Update the simulation speed factor + * @param speed_factor New speed multiplier (must be positive) + */ + void setSpeedFactor(double speed_factor); + +protected: + /** + * @brief Wall-timer callback that advances sim time and publishes /clock + */ + void timerCallback(); + /** + * @brief (Re)create the wall timer based on current speed_factor + */ + void resetTimer(); + + nav2::LifecycleNode::WeakPtr node_; + rclcpp::Logger logger_; + + rclcpp::Publisher::SharedPtr clock_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + static constexpr double kResolution = 0.01; // 10ms sim-time step + static constexpr double kMinWallPeriod = 0.001; // 1ms / 1000 Hz max + double speed_factor_; + rclcpp::Time sim_time_; + std::chrono::steady_clock::time_point last_wall_time_; +}; + +} // namespace nav2_loopback_sim + +#endif // NAV2_LOOPBACK_SIM__CLOCK_PUBLISHER_HPP_ diff --git a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp new file mode 100644 index 00000000000..9a4806c0e61 --- /dev/null +++ b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp @@ -0,0 +1,232 @@ +// Copyright (c) 2024, Open Navigation LLC +// Copyright (c) 2026, Dexory (Tony Najjar) +// +// 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. + +#ifndef NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_ +#define NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/service_client.hpp" +#include "nav2_util/twist_subscriber.hpp" +#include "nav2_loopback_sim/clock_publisher.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/srv/get_map.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Matrix3x3.hpp" + +namespace nav2_loopback_sim +{ + +/** + * @brief 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 mimic global localization and simulation. + * It also accepts initialpose messages to set the initial pose of the robot. + */ +class LoopbackSimulator : public nav2::LifecycleNode +{ +public: + /** + * @brief Construct a LoopbackSimulator node. + * @param options Node options + */ + explicit LoopbackSimulator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~LoopbackSimulator() = default; + +protected: + /** + * @brief Configure the node: declare parameters, create pubs/subs/timers + */ + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief Activate the node: start publishing + */ + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Deactivate the node: stop timers, reset cmd_vel + */ + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Cleanup the node: release all resources + */ + nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Shutdown the node + */ + nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Callback for incoming cmd_vel (unstamped Twist) + */ + void cmdVelCallback(const geometry_msgs::msg::Twist::ConstSharedPtr & msg); + /** + * @brief Callback for incoming cmd_vel (stamped TwistStamped) + */ + void cmdVelStampedCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr & msg); + /** + * @brief Callback for incoming initial pose + */ + void initialPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg); + /** + * @brief Periodic setup callback: publishes identity TFs and fetches map + */ + void setupTimerCallback(); + /** + * @brief Main update callback: integrates cmd_vel and publishes TF + */ + void timerCallback(); + /** + * @brief Periodic odometry publishing callback + */ + void odomTimerCallback(); + /** + * @brief Publish a simulated laser scan from the map + */ + void publishLaserScan(); + /** + * @brief Validate dynamic parameter changes (pre-set callback) + * @param parameters The parameters being set + * @return Result indicating whether the parameter change is accepted + */ + rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback( + const std::vector & parameters); + /** + * @brief Apply validated dynamic parameter changes (post-set callback) + * @param parameters The parameters that were set + */ + void updateParametersCallback(const std::vector & parameters); + + /** + * @brief Look up the static transform from base to laser frame + */ + void getBaseToLaserTf(); + /** + * @brief Request the map from the map server + */ + void getMap(); + /** + * @brief Publish map->odom and odom->base_link transforms + */ + void publishTransforms( + geometry_msgs::msg::TransformStamped & map_to_odom, + geometry_msgs::msg::TransformStamped & odom_to_base_link); + /** + * @brief Publish nav_msgs::Odometry from the current odom->base transform + */ + void publishOdometry(const geometry_msgs::msg::TransformStamped & odom_to_base_link); + /** + * @brief Compute the laser pose in the map frame + */ + std::tuple getLaserPose(); + /** + * @brief Raycast the map to fill a LaserScan message + * @param num_samples Number of ray samples + * @param scan_msg Output scan message to populate + */ + void getLaserScan(int num_samples, sensor_msgs::msg::LaserScan & scan_msg); + + /** + * @brief Add a yaw rotation to a quaternion + * @param quaternion Input quaternion + * @param yaw_to_add Yaw angle to add in radians + * @return Resulting quaternion + */ + static geometry_msgs::msg::Quaternion addYawToQuat( + const geometry_msgs::msg::Quaternion & quaternion, double yaw_to_add); + + // Parameters + double update_dur_; + std::string base_frame_id_; + std::string map_frame_id_; + std::string odom_frame_id_; + std::string scan_frame_id_; + double odom_publish_dur_; + double scan_publish_dur_; + bool publish_map_odom_tf_; + double scan_range_min_; + double scan_range_max_; + double scan_angle_min_; + double scan_angle_max_; + double scan_angle_increment_; + bool use_inf_; + double scan_noise_std_; + bool publish_scan_; + bool publish_clock_; + double speed_factor_; + + // State + std::optional curr_cmd_vel_; + rclcpp::Time curr_cmd_vel_time_; + bool has_initial_pose_{false}; + geometry_msgs::msg::Pose initial_pose_; + bool has_map_{false}; + nav_msgs::msg::OccupancyGrid map_; + bool has_base_to_laser_{false}; + tf2::Transform tf_base_to_laser_; + std::mt19937 rng_{std::random_device{}()}; + geometry_msgs::msg::TransformStamped t_map_to_odom_; + geometry_msgs::msg::TransformStamped t_odom_to_base_link_; + + // ROS interfaces + std::unique_ptr tf_broadcaster_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + nav2::Subscription::SharedPtr + initial_pose_sub_; + std::unique_ptr cmd_vel_sub_; + + nav2::Publisher::SharedPtr odom_pub_; + nav2::Publisher::SharedPtr scan_pub_; + + nav2::ServiceClient::SharedPtr map_client_; + + rclcpp::TimerBase::SharedPtr setup_timer_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr odom_timer_; + rclcpp::TimerBase::SharedPtr scan_timer_; + + std::unique_ptr clock_publisher_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_validator_; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr param_updater_; +}; + +} // namespace nav2_loopback_sim + +#endif // NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_ diff --git a/nav2_loopback_sim/launch/loopback_simulation.launch.py b/nav2_loopback_sim/launch/loopback_simulation.launch.py index 18756f240d0..ff3d524ae41 100644 --- a/nav2_loopback_sim/launch/loopback_simulation.launch.py +++ b/nav2_loopback_sim/launch/loopback_simulation.launch.py @@ -18,7 +18,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import LifecycleNode def generate_launch_description() -> LaunchDescription: @@ -36,12 +36,15 @@ def generate_launch_description() -> LaunchDescription: default_value='base_scan', ) - loopback_sim_cmd = Node( + loopback_sim_cmd = LifecycleNode( package='nav2_loopback_sim', executable='loopback_simulator', name='loopback_simulator', + namespace='', output='screen', - parameters=[params_file, {'scan_frame_id': scan_frame_id}], + autostart=True, + parameters=[params_file, {'scan_frame_id': scan_frame_id, + 'use_sim_time': True}], ) ld = LaunchDescription() diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py deleted file mode 100644 index db81df655fb..00000000000 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ /dev/null @@ -1,515 +0,0 @@ -# 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 typing import Optional, Union - -from geometry_msgs.msg import (Pose, PoseWithCovarianceStamped, Quaternion, TransformStamped, - Twist, TwistStamped, Vector3) -from nav2_loopback_sim.utils import (addYawToQuat, matrixToTransform, transformStampedToMatrix, - worldToMap) -from nav_msgs.msg import OccupancyGrid, Odometry -from nav_msgs.srv import GetMap -import numpy as np -import rclpy -from rclpy.client import Client -from rclpy.duration import Duration -from rclpy.node import Node -from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy -from rclpy.subscription import Subscription -from rclpy.timer import Timer -from rosgraph_msgs.msg import Clock -from sensor_msgs.msg import LaserScan -from tf2_ros import Buffer, TransformBroadcaster, TransformListener -import tf_transformations - -""" -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 mimic 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) -> None: - super().__init__(node_name='loopback_simulator') - self.curr_cmd_vel: Optional[Twist] = None - self.curr_cmd_vel_time = self.get_clock().now() - self.initial_pose: Optional[Pose] = None - self.timer: Optional[Timer] = None - self.setupTimer = None - self.map: Optional[OccupancyGrid] = None - self.mat_base_to_laser: Optional[np.ndarray[np.float64, np.dtype[np.float64]]] = 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.declare_parameter('enable_stamped_cmd_vel', True) - use_stamped = self.get_parameter('enable_stamped_cmd_vel').get_parameter_value().bool_value - - self.declare_parameter('scan_publish_dur', 0.1) - self.scan_publish_dur = self.get_parameter( - 'scan_publish_dur').get_parameter_value().double_value - - self.declare_parameter('publish_map_odom_tf', True) - self.publish_map_odom_tf = self.get_parameter( - 'publish_map_odom_tf').get_parameter_value().bool_value - - self.declare_parameter('publish_clock', True) - self.publish_clock = self.get_parameter('publish_clock').get_parameter_value().bool_value - - self.declare_parameter('scan_range_min', 0.05) - self.scan_range_min = \ - self.get_parameter('scan_range_min').get_parameter_value().double_value - - self.declare_parameter('scan_range_max', 30.0) - self.scan_range_max = \ - self.get_parameter('scan_range_max').get_parameter_value().double_value - - self.declare_parameter('scan_angle_min', -math.pi) - self.scan_angle_min = \ - self.get_parameter('scan_angle_min').get_parameter_value().double_value - - self.declare_parameter('scan_angle_max', math.pi) - self.scan_angle_max = \ - self.get_parameter('scan_angle_max').get_parameter_value().double_value - - self.declare_parameter('scan_angle_increment', 0.0261) # 0.0261 rad = 1.5 degrees - self.scan_angle_increment = \ - self.get_parameter('scan_angle_increment').get_parameter_value().double_value - - self.declare_parameter('scan_use_inf', True) - self.use_inf = \ - self.get_parameter('scan_use_inf').get_parameter_value().bool_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: Union[Subscription[Twist], Subscription[TwistStamped]] - - if not use_stamped: - self.cmd_vel_sub = self.create_subscription( - Twist, - 'cmd_vel', self.cmdVelCallback, 10) - else: - self.cmd_vel_sub = self.create_subscription( - TwistStamped, - 'cmd_vel', self.cmdVelStampedCallback, 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) - - if self.publish_clock: - self.clock_timer = self.create_timer(0.1, self.clockTimerCallback) - self.clock_pub = self.create_publisher(Clock, '/clock', 10) - - self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) - - self.map_client: Client[GetMap.Request, GetMap.Response] = \ - self.create_client(GetMap, '/map_server/map') - - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) - - self.getMap() - - self.info('Loopback simulator initialized') - - def getBaseToLaserTf(self) -> None: - try: - # Wait for transform and lookup - transform = self.tf_buffer.lookup_transform( - self.base_frame_id, self.scan_frame_id, rclpy.time.Time()) - self.mat_base_to_laser = transformStampedToMatrix(transform) - - except Exception as ex: - self.get_logger().error(f'Transform lookup failed: {str(ex)}') - - def setupTimerCallback(self) -> None: - # Publish initial identity odom transform & laser scan to warm up system - self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) - if self.mat_base_to_laser is None: - self.getBaseToLaserTf() - - def clockTimerCallback(self) -> None: - msg = Clock() - msg.clock = self.get_clock().now().to_msg() - self.clock_pub.publish(msg) - - def cmdVelCallback(self, msg: Twist) -> None: - 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 cmdVelStampedCallback(self, msg: TwistStamped) -> None: - 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.twist - self.curr_cmd_vel_time = rclpy.time.Time.from_msg(msg.header.stamp) - - def initialPoseCallback(self, msg: PoseWithCovarianceStamped) -> None: - 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) - self.timer_laser = self.create_timer(self.scan_publish_dur, self.publishLaserScan) - 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 = self.base_frame_id - 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) -> None: - # 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) - - def publishLaserScan(self) -> None: - # Publish a bogus laser scan for collision monitor - self.scan_msg = LaserScan() - self.scan_msg.header.stamp = (self.get_clock().now()).to_msg() - self.scan_msg.header.frame_id = self.scan_frame_id - self.scan_msg.angle_min = self.scan_angle_min - self.scan_msg.angle_max = self.scan_angle_max - # 1.5 degrees - self.scan_msg.angle_increment = self.scan_angle_increment - self.scan_msg.time_increment = 0.0 - self.scan_msg.scan_time = 0.1 - self.scan_msg.range_min = self.scan_range_min - self.scan_msg.range_max = self.scan_range_max - num_samples = int( - (self.scan_msg.angle_max - self.scan_msg.angle_min) / - self.scan_msg.angle_increment) - self.scan_msg.ranges = [0.0] * num_samples - self.getLaserScan(num_samples) - self.scan_pub.publish(self.scan_msg) - - def publishTransforms(self, map_to_odom: TransformStamped, - odom_to_base_link: TransformStamped) -> None: - 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() - if self.publish_map_odom_tf: - self.tf_broadcaster.sendTransform(map_to_odom) - self.tf_broadcaster.sendTransform(odom_to_base_link) - - def publishOdometry(self, odom_to_base_link: TransformStamped) -> None: - 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 - if self.curr_cmd_vel is not None: - odom.twist.twist = self.curr_cmd_vel - self.odom_pub.publish(odom) - - def info(self, msg: str) -> None: - self.get_logger().info(msg) - return - - def debug(self, msg: str) -> None: - self.get_logger().debug(msg) - return - - def getMap(self) -> None: - request = GetMap.Request() - if self.map_client.wait_for_service(timeout_sec=5.0): - # Request to get map - future = self.map_client.call_async(request) - rclpy.spin_until_future_complete(self, future) - result = future.result() - if result is not None: - self.map = result.map - self.get_logger().info('Laser scan will be populated using map data') - else: - self.get_logger().warning( - 'Failed to get map, ' - 'Laser scan will be populated using max range' - ) - else: - self.get_logger().warning( - 'Failed to get map, ' - 'Laser scan will be populated using max range' - ) - - def getLaserPose(self) -> tuple[float, float, float]: - mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom) - mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link) - - mat_map_to_laser = tf_transformations.concatenate_matrices( - mat_map_to_odom, - mat_odom_to_base, - self.mat_base_to_laser - ) - transform = matrixToTransform(mat_map_to_laser) - - x = transform.translation.x - y = transform.translation.y - theta = tf_transformations.euler_from_quaternion([ - transform.rotation.x, - transform.rotation.y, - transform.rotation.z, - transform.rotation.w - ])[2] - - return x, y, theta - - def getLaserScan(self, num_samples: int) -> None: - if self.map is None or self.initial_pose is None or self.mat_base_to_laser is None: - if self.use_inf: - self.scan_msg.ranges = [float('inf')] * num_samples - else: - self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples - return - - x0, y0, theta = self.getLaserPose() - - mx0, my0 = worldToMap(x0, y0, self.map) - - if not 0 < mx0 < self.map.info.width or not 0 < my0 < self.map.info.height: - # outside map - if self.use_inf: - self.scan_msg.ranges = [float('inf')] * num_samples - else: - self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples - return - - width = self.map.info.width - height = self.map.info.height - resolution = self.map.info.resolution - map_data = self.map.data - range_max = self.scan_msg.range_max - angle_min = self.scan_msg.angle_min - angle_increment = self.scan_msg.angle_increment - ranges = self.scan_msg.ranges - use_inf = self.use_inf - origin_x = self.map.info.origin.position.x - origin_y = self.map.info.origin.position.y - - for i in range(num_samples): - curr_angle = theta + angle_min + i * angle_increment - x1 = x0 + range_max * math.cos(curr_angle) - y1 = y0 + range_max * math.sin(curr_angle) - - mx1 = int(math.floor((x1 - origin_x) / resolution)) - my1 = int(math.floor((y1 - origin_y) / resolution)) - - # Inline LineIterator with step_size=0.5 for performance - lx = float(mx0) - ly = float(my0) - step_size = 0.5 - if mx1 != mx0 and my1 != my0: - m = (my1 - my0) / (mx1 - mx0) - b = my1 - m * mx1 - if mx1 > mx0: - while lx <= mx1: - mx, my = int(lx), int(ly) - if not 0 < mx < width or not 0 < my < height: - break - if map_data[my * width + mx] >= 60: - ranges[i] = math.sqrt((mx - mx0) ** 2 + (my - my0) ** 2) * resolution - break - lx = lx + step_size - if lx > mx1: - lx = mx1 - ly = m * lx + b - else: - if ranges[i] == 0.0 and use_inf: - ranges[i] = float('inf') - continue - else: - while lx >= mx1: - mx, my = int(lx), int(ly) - if not 0 < mx < width or not 0 < my < height: - break - if map_data[my * width + mx] >= 60: - ranges[i] = math.sqrt((mx - mx0) ** 2 + (my - my0) ** 2) * resolution - break - lx = lx - step_size - if lx < mx1: - lx = mx1 - ly = m * lx + b - else: - if ranges[i] == 0.0 and use_inf: - ranges[i] = float('inf') - continue - elif mx1 == mx0: - # Vertical line - if my1 > my0: - while ly <= my1: - mx, my = int(lx), int(ly) - if not 0 < mx < width or not 0 < my < height: - break - if map_data[my * width + mx] >= 60: - ranges[i] = math.sqrt((mx - mx0) ** 2 + (my - my0) ** 2) * resolution - break - ly = ly + step_size - if ly > my1: - ly = my1 - else: - if ranges[i] == 0.0 and use_inf: - ranges[i] = float('inf') - continue - elif my1 < my0: - while ly >= my1: - mx, my = int(lx), int(ly) - if not 0 < mx < width or not 0 < my < height: - break - if map_data[my * width + mx] >= 60: - ranges[i] = math.sqrt((mx - mx0) ** 2 + (my - my0) ** 2) * resolution - break - ly = ly - step_size - if ly < my1: - ly = my1 - else: - if ranges[i] == 0.0 and use_inf: - ranges[i] = float('inf') - continue - else: - # Horizontal line (my1 == my0) - m = (my1 - my0) / (mx1 - mx0) - b = my1 - m * mx1 - if mx1 > mx0: - while lx <= mx1: - mx, my = int(lx), int(ly) - if not 0 < mx < width or not 0 < my < height: - break - if map_data[my * width + mx] >= 60: - ranges[i] = math.sqrt((mx - mx0) ** 2 + (my - my0) ** 2) * resolution - break - lx = lx + step_size - if lx > mx1: - lx = mx1 - ly = m * lx + b - else: - if ranges[i] == 0.0 and use_inf: - ranges[i] = float('inf') - continue - else: - while lx >= mx1: - mx, my = int(lx), int(ly) - if not 0 < mx < width or not 0 < my < height: - break - if map_data[my * width + mx] >= 60: - ranges[i] = math.sqrt((mx - mx0) ** 2 + (my - my0) ** 2) * resolution - break - lx = lx - step_size - if lx < mx1: - lx = mx1 - ly = m * lx + b - else: - if ranges[i] == 0.0 and use_inf: - ranges[i] = float('inf') - continue - - if ranges[i] == 0.0 and use_inf: - ranges[i] = float('inf') - - -def main() -> None: - 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 deleted file mode 100644 index 7131b8424d9..00000000000 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ /dev/null @@ -1,78 +0,0 @@ -# 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 Quaternion, Transform, TransformStamped -from nav_msgs.msg import OccupancyGrid -import numpy as np -import tf_transformations - -""" -Transformation utilities for the loopback simulator -""" - - -def addYawToQuat(quaternion: Quaternion, yaw_to_add: float) -> Quaternion: - 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: TransformStamped) -> np.ndarray[np.float64, np.dtype[np.float64]]: - 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: np.ndarray[np.float64, np.dtype[np.float64]]) -> Transform: - 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 - - -def worldToMap(world_x: float, world_y: float, map_msg: OccupancyGrid) -> tuple[int, int]: - map_x = int(math.floor((world_x - map_msg.info.origin.position.x) / map_msg.info.resolution)) - map_y = int(math.floor((world_y - map_msg.info.origin.position.y) / map_msg.info.resolution)) - return map_x, map_y - - -def getMapOccupancy(x: int, y: int, map_msg: OccupancyGrid): # type: ignore[no-untyped-def] - return map_msg.data[y * map_msg.info.width + x] diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index 8d61903f770..a863f6e983f 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -7,23 +7,30 @@ steve macenski Apache-2.0 - geometry_msgs - nav2_simple_commander - nav_msgs - python3-transforms3d - rclpy - tf2_ros - tf_transformations + ament_cmake + nav2_common - ament_copyright - ament_flake8 - ament_mypy - ament_pep257 - ament_xmllint - python3-pytest + backward_ros + geometry_msgs + nav2_ros_common + nav2_util + nav_msgs + rcl_interfaces + rclcpp + rclcpp_components + rclcpp_lifecycle + rosgraph_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + ament_lint_common + ament_cmake_gtest ament_cmake_ros - ament_python + ament_cmake diff --git a/nav2_loopback_sim/pytest.ini b/nav2_loopback_sim/pytest.ini deleted file mode 100644 index fe55d2ed64b..00000000000 --- a/nav2_loopback_sim/pytest.ini +++ /dev/null @@ -1,2 +0,0 @@ -[pytest] -junit_family=xunit2 diff --git a/nav2_loopback_sim/resource/nav2_loopback_sim b/nav2_loopback_sim/resource/nav2_loopback_sim deleted file mode 100644 index e69de29bb2d..00000000000 diff --git a/nav2_loopback_sim/setup.cfg b/nav2_loopback_sim/setup.cfg deleted file mode 100644 index 20a46b06d61..00000000000 --- a/nav2_loopback_sim/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[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 deleted file mode 100644 index 8d25201e070..00000000000 --- a/nav2_loopback_sim/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -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/src/clock_publisher.cpp b/nav2_loopback_sim/src/clock_publisher.cpp new file mode 100644 index 00000000000..5388a2700d5 --- /dev/null +++ b/nav2_loopback_sim/src/clock_publisher.cpp @@ -0,0 +1,117 @@ +// Copyright (c) 2024, Open Navigation LLC +// Copyright (c) 2026, Dexory (Tony Najjar) +// +// 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. + +#include "nav2_loopback_sim/clock_publisher.hpp" + +#include +#include +#include + +namespace nav2_loopback_sim +{ + +ClockPublisher::ClockPublisher( + nav2::LifecycleNode::WeakPtr node, + double speed_factor) +: node_(node), + logger_(rclcpp::get_logger("nav2_loopback_sim")), + speed_factor_(speed_factor), + sim_time_(0, 0, RCL_ROS_TIME), + last_wall_time_(std::chrono::steady_clock::now()) +{ + auto shared_node = node_.lock(); + if (!shared_node) { + throw std::runtime_error("Node expired during ClockPublisher construction"); + } + logger_ = shared_node->get_logger(); + clock_pub_ = rclcpp::create_publisher( + shared_node->get_node_topics_interface(), "/clock", 10); +} + +void ClockPublisher::start() +{ + last_wall_time_ = std::chrono::steady_clock::now(); + resetTimer(); + RCLCPP_INFO( + logger_, + "Sim clock publisher started (resolution: %.3fs, speed: %.2fx)", + kResolution, speed_factor_); +} + +void ClockPublisher::stop() +{ + if (timer_) { + timer_->cancel(); + timer_.reset(); + } +} + +void ClockPublisher::setSpeedFactor(double speed_factor) +{ + if (speed_factor <= 0.0) { + RCLCPP_WARN( + logger_, + "Ignoring non-positive speed_factor %.2f", speed_factor); + return; + } + speed_factor_ = speed_factor; + if (timer_) { + resetTimer(); + } + RCLCPP_INFO( + logger_, + "Clock speed factor changed to %.2fx", speed_factor_); +} + +void ClockPublisher::resetTimer() +{ + if (timer_) { + timer_->cancel(); + timer_.reset(); + } + auto shared_node = node_.lock(); + if (!shared_node) { + return; + } + double wall_period = std::max(kResolution / speed_factor_, kMinWallPeriod); + timer_ = rclcpp::create_wall_timer( + std::chrono::duration(wall_period), + std::bind(&ClockPublisher::timerCallback, this), + nullptr, + shared_node->get_node_base_interface().get(), + shared_node->get_node_timers_interface().get()); + if (kResolution / speed_factor_ < kMinWallPeriod) { + RCLCPP_WARN( + logger_, + "Wall period clamped to %.1fms (requested %.3fms from resolution=%.3f, speed=%.1f)", + kMinWallPeriod * 1000.0, (kResolution / speed_factor_) * 1000.0, + kResolution, speed_factor_); + } +} + +void ClockPublisher::timerCallback() +{ + auto now_wall = std::chrono::steady_clock::now(); + double wall_dt = std::chrono::duration(now_wall - last_wall_time_).count(); + last_wall_time_ = now_wall; + + sim_time_ += rclcpp::Duration::from_seconds(wall_dt * speed_factor_); + + auto msg = std::make_unique(); + msg->clock = sim_time_; + clock_pub_->publish(std::move(msg)); +} + +} // namespace nav2_loopback_sim diff --git a/nav2_loopback_sim/src/loopback_simulator.cpp b/nav2_loopback_sim/src/loopback_simulator.cpp new file mode 100644 index 00000000000..0b1f9aa3294 --- /dev/null +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -0,0 +1,554 @@ +// Copyright (c) 2024, Open Navigation LLC +// Copyright (c) 2026, Dexory (Tony Najjar) +// +// 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. + +#include "nav2_loopback_sim/loopback_simulator.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +using namespace std::chrono_literals; +using std::placeholders::_1; + +namespace nav2_loopback_sim +{ + +LoopbackSimulator::LoopbackSimulator(const rclcpp::NodeOptions & options) +: nav2::LifecycleNode("loopback_simulator", options), + curr_cmd_vel_time_(this->now()) +{ +} + +nav2::CallbackReturn +LoopbackSimulator::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Declare and get parameters + update_dur_ = declare_or_get_parameter("update_duration", 0.01); + base_frame_id_ = declare_or_get_parameter("base_frame_id", std::string("base_footprint")); + map_frame_id_ = declare_or_get_parameter("map_frame_id", std::string("map")); + odom_frame_id_ = declare_or_get_parameter("odom_frame_id", std::string("odom")); + scan_frame_id_ = declare_or_get_parameter("scan_frame_id", std::string("base_scan")); + odom_publish_dur_ = declare_or_get_parameter("odom_publish_dur", update_dur_); + scan_publish_dur_ = declare_or_get_parameter("scan_publish_dur", 0.1); + publish_map_odom_tf_ = declare_or_get_parameter("publish_map_odom_tf", true); + scan_range_min_ = declare_or_get_parameter("scan_range_min", 0.05); + scan_range_max_ = declare_or_get_parameter("scan_range_max", 30.0); + scan_angle_min_ = declare_or_get_parameter("scan_angle_min", -M_PI); + scan_angle_max_ = declare_or_get_parameter("scan_angle_max", M_PI); + scan_angle_increment_ = declare_or_get_parameter("scan_angle_increment", 0.0261); + use_inf_ = declare_or_get_parameter("scan_use_inf", true); + scan_noise_std_ = declare_or_get_parameter("scan_noise_std", 0.01); + publish_scan_ = declare_or_get_parameter("publish_scan", true); + publish_clock_ = declare_or_get_parameter("publish_clock", true); + speed_factor_ = declare_or_get_parameter("speed_factor", 1.0); + + // Setup transforms + t_map_to_odom_.header.frame_id = map_frame_id_; + t_map_to_odom_.child_frame_id = odom_frame_id_; + t_odom_to_base_link_.header.frame_id = odom_frame_id_; + t_odom_to_base_link_.child_frame_id = base_frame_id_; + + tf_broadcaster_ = std::make_unique(*this); + + // Subscriptions + initial_pose_sub_ = + create_subscription( + "initialpose", + std::bind(&LoopbackSimulator::initialPoseCallback, this, _1)); + + cmd_vel_sub_ = std::make_unique( + shared_from_this(), "cmd_vel", + std::bind(&LoopbackSimulator::cmdVelCallback, this, _1), + std::bind(&LoopbackSimulator::cmdVelStampedCallback, this, _1)); + + // Publishers + odom_pub_ = create_publisher("odom"); + + if (publish_scan_) { + scan_pub_ = create_publisher( + "scan", nav2::qos::SensorDataQoS()); + } + + if (publish_scan_) { + map_client_ = create_client("/map_server/map"); + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + if (publish_clock_) { + clock_publisher_ = std::make_unique( + weak_from_this(), + speed_factor_); + } + + param_validator_ = add_on_set_parameters_callback( + std::bind( + &LoopbackSimulator::validateParameterUpdatesCallback, this, + std::placeholders::_1)); + param_updater_ = add_post_set_parameters_callback( + std::bind( + &LoopbackSimulator::updateParametersCallback, this, + std::placeholders::_1)); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +LoopbackSimulator::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + odom_pub_->on_activate(); + if (scan_pub_) { + scan_pub_->on_activate(); + } + + setup_timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, + std::bind(&LoopbackSimulator::setupTimerCallback, this)); + + if (clock_publisher_) { + clock_publisher_->start(); + } + + createBond(); + + RCLCPP_INFO(get_logger(), "Loopback simulator activated"); + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +LoopbackSimulator::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + if (setup_timer_) { + setup_timer_->cancel(); + setup_timer_.reset(); + } + if (timer_) { + timer_->cancel(); + timer_.reset(); + } + if (odom_timer_) { + odom_timer_->cancel(); + odom_timer_.reset(); + } + if (scan_timer_) { + scan_timer_->cancel(); + scan_timer_.reset(); + } + + if (clock_publisher_) { + clock_publisher_->stop(); + } + + odom_pub_->on_deactivate(); + if (scan_pub_) { + scan_pub_->on_deactivate(); + } + + has_initial_pose_ = false; + curr_cmd_vel_.reset(); + + destroyBond(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +LoopbackSimulator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + initial_pose_sub_.reset(); + cmd_vel_sub_.reset(); + odom_pub_.reset(); + scan_pub_.reset(); + map_client_.reset(); + tf_listener_.reset(); + tf_buffer_.reset(); + tf_broadcaster_.reset(); + clock_publisher_.reset(); + param_validator_.reset(); + param_updater_.reset(); + + return nav2::CallbackReturn::SUCCESS; +} + +nav2::CallbackReturn +LoopbackSimulator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2::CallbackReturn::SUCCESS; +} + +void LoopbackSimulator::getMap() +{ + if (!map_client_->wait_for_service(0s)) { + return; + } + auto request = std::make_shared(); + map_client_->async_call( + request, + [this](typename rclcpp::Client::SharedFuture future) { + auto response = future.get(); + if (response->map.info.width == 0 || response->map.info.height == 0 || + response->map.info.resolution <= 0.0) + { + RCLCPP_WARN( + get_logger(), + "Map server returned empty/invalid map (%dx%d, res=%.3f), will retry", + response->map.info.width, response->map.info.height, + response->map.info.resolution); + return; + } + map_ = response->map; + has_map_ = true; + RCLCPP_INFO(get_logger(), "Laser scan will be populated using map data"); + }); +} + +void LoopbackSimulator::getBaseToLaserTf() +{ + try { + auto transform = tf_buffer_->lookupTransform( + base_frame_id_, scan_frame_id_, tf2::TimePointZero); + tf2::fromMsg(transform.transform, tf_base_to_laser_); + has_base_to_laser_ = true; + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "Transform lookup failed: %s", ex.what()); + } +} + +void LoopbackSimulator::setupTimerCallback() +{ + t_odom_to_base_link_.header.stamp = this->now(); + tf_broadcaster_->sendTransform(t_odom_to_base_link_); + if (publish_scan_ && !has_map_) { + getMap(); + } + if (publish_scan_ && !has_base_to_laser_) { + getBaseToLaserTf(); + } +} + +void LoopbackSimulator::cmdVelCallback( + const geometry_msgs::msg::Twist::ConstSharedPtr & msg) +{ + RCLCPP_DEBUG(get_logger(), "Received cmd_vel"); + if (!has_initial_pose_) { + return; + } + curr_cmd_vel_ = *msg; + curr_cmd_vel_time_ = this->now(); +} + +void LoopbackSimulator::cmdVelStampedCallback( + const geometry_msgs::msg::TwistStamped::ConstSharedPtr & msg) +{ + RCLCPP_DEBUG(get_logger(), "Received cmd_vel"); + if (!has_initial_pose_) { + return; + } + curr_cmd_vel_ = msg->twist; + curr_cmd_vel_time_ = rclcpp::Time(msg->header.stamp); +} + +void LoopbackSimulator::initialPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg) +{ + RCLCPP_INFO(get_logger(), "Received initial pose!"); + + if (!has_initial_pose_) { + has_initial_pose_ = true; + initial_pose_ = msg->pose.pose; + + // Initialize map->odom from input pose, odom->base_link starts as identity + t_map_to_odom_.transform.translation.x = initial_pose_.position.x; + t_map_to_odom_.transform.translation.y = initial_pose_.position.y; + t_map_to_odom_.transform.rotation = initial_pose_.orientation; + t_odom_to_base_link_.transform.translation = geometry_msgs::msg::Vector3(); + t_odom_to_base_link_.transform.rotation = geometry_msgs::msg::Quaternion(); + publishTransforms(t_map_to_odom_, t_odom_to_base_link_); + + // Cancel setup timer and start update/scan timers + if (setup_timer_) { + setup_timer_->cancel(); + setup_timer_.reset(); + } + timer_ = rclcpp::create_timer( + this, get_clock(), + std::chrono::duration(update_dur_), + std::bind(&LoopbackSimulator::timerCallback, this)); + odom_timer_ = rclcpp::create_timer( + this, get_clock(), + std::chrono::duration(odom_publish_dur_), + std::bind(&LoopbackSimulator::odomTimerCallback, this)); + if (publish_scan_) { + scan_timer_ = rclcpp::create_timer( + this, get_clock(), + std::chrono::duration(scan_publish_dur_), + std::bind(&LoopbackSimulator::publishLaserScan, this)); + } + return; + } + + initial_pose_ = msg->pose.pose; + + // Adjust map->odom based on new initial pose, keeping odom->base_link the same + tf2::Transform tf_map_to_base; + tf_map_to_base.setOrigin( + tf2::Vector3(initial_pose_.position.x, initial_pose_.position.y, 0.0)); + tf_map_to_base.setRotation( + tf2::Quaternion( + initial_pose_.orientation.x, initial_pose_.orientation.y, + initial_pose_.orientation.z, initial_pose_.orientation.w)); + + tf2::Transform tf_odom_to_base; + tf2::fromMsg(t_odom_to_base_link_.transform, tf_odom_to_base); + + tf2::Transform tf_map_to_odom = tf_map_to_base * tf_odom_to_base.inverse(); + t_map_to_odom_.transform = tf2::toMsg(tf_map_to_odom); +} + +void LoopbackSimulator::timerCallback() +{ + // If no data, just republish existing transforms without change + auto one_sec = rclcpp::Duration::from_seconds(1.0); + if (!curr_cmd_vel_.has_value() || (this->now() - curr_cmd_vel_time_) > one_sec) { + publishTransforms(t_map_to_odom_, t_odom_to_base_link_); + curr_cmd_vel_.reset(); + return; + } + + // Update odom->base_link from cmd_vel + double dx = curr_cmd_vel_->linear.x * update_dur_; + double dy = curr_cmd_vel_->linear.y * update_dur_; + double dth = curr_cmd_vel_->angular.z * update_dur_; + + tf2::Quaternion q( + t_odom_to_base_link_.transform.rotation.x, + t_odom_to_base_link_.transform.rotation.y, + t_odom_to_base_link_.transform.rotation.z, + t_odom_to_base_link_.transform.rotation.w); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + + t_odom_to_base_link_.transform.translation.x += dx * std::cos(yaw) - dy * std::sin(yaw); + t_odom_to_base_link_.transform.translation.y += dx * std::sin(yaw) + dy * std::cos(yaw); + t_odom_to_base_link_.transform.rotation = + addYawToQuat(t_odom_to_base_link_.transform.rotation, dth); + + publishTransforms(t_map_to_odom_, t_odom_to_base_link_); +} + +void LoopbackSimulator::odomTimerCallback() +{ + publishOdometry(t_odom_to_base_link_); +} + +void LoopbackSimulator::publishLaserScan() +{ + auto scan_msg = std::make_unique(); + scan_msg->header.stamp = this->now(); + scan_msg->header.frame_id = scan_frame_id_; + scan_msg->angle_min = static_cast(scan_angle_min_); + scan_msg->angle_max = static_cast(scan_angle_max_); + scan_msg->angle_increment = static_cast(scan_angle_increment_); + scan_msg->time_increment = 0.0f; + scan_msg->scan_time = static_cast(scan_publish_dur_); + scan_msg->range_min = static_cast(scan_range_min_); + scan_msg->range_max = static_cast(scan_range_max_); + + int num_samples = static_cast( + (scan_angle_max_ - scan_angle_min_) / scan_angle_increment_); + scan_msg->ranges.assign(num_samples, 0.0f); + if (!has_map_) { + getMap(); + } + if (!has_base_to_laser_) { + getBaseToLaserTf(); + } + getLaserScan(num_samples, *scan_msg); + scan_pub_->publish(std::move(scan_msg)); +} + +void LoopbackSimulator::publishTransforms( + geometry_msgs::msg::TransformStamped & map_to_odom, + geometry_msgs::msg::TransformStamped & odom_to_base_link) +{ + auto now = this->now(); + map_to_odom.header.stamp = now + rclcpp::Duration::from_seconds(update_dur_); + odom_to_base_link.header.stamp = now; + if (publish_map_odom_tf_) { + tf_broadcaster_->sendTransform(map_to_odom); + } + tf_broadcaster_->sendTransform(odom_to_base_link); +} + +void LoopbackSimulator::publishOdometry( + const geometry_msgs::msg::TransformStamped & odom_to_base_link) +{ + auto odom = std::make_unique(); + odom->header.stamp = this->now(); + odom->header.frame_id = odom_frame_id_; + odom->child_frame_id = base_frame_id_; + 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; + if (curr_cmd_vel_.has_value()) { + odom->twist.twist = curr_cmd_vel_.value(); + } + odom_pub_->publish(std::move(odom)); +} + +geometry_msgs::msg::Quaternion LoopbackSimulator::addYawToQuat( + const geometry_msgs::msg::Quaternion & quaternion, double yaw_to_add) +{ + tf2::Quaternion q(quaternion.x, quaternion.y, quaternion.z, quaternion.w); + tf2::Quaternion q_yaw; + q_yaw.setRPY(0.0, 0.0, yaw_to_add); + q = q * q_yaw; + q.normalize(); + return tf2::toMsg(q); +} + +std::tuple LoopbackSimulator::getLaserPose() +{ + tf2::Transform tf_map_to_odom; + tf2::fromMsg(t_map_to_odom_.transform, tf_map_to_odom); + + tf2::Transform tf_odom_to_base; + tf2::fromMsg(t_odom_to_base_link_.transform, tf_odom_to_base); + + tf2::Transform tf_map_to_laser = tf_map_to_odom * tf_odom_to_base * tf_base_to_laser_; + + double x = tf_map_to_laser.getOrigin().x(); + double y = tf_map_to_laser.getOrigin().y(); + double roll, pitch, yaw; + tf2::Matrix3x3(tf_map_to_laser.getRotation()).getRPY(roll, pitch, yaw); + + return {x, y, yaw}; +} + +void LoopbackSimulator::getLaserScan( + int num_samples, sensor_msgs::msg::LaserScan & scan_msg) +{ + float no_hit_range = use_inf_ ? std::numeric_limits::infinity() : + scan_msg.range_max - 0.1f; + + if (!has_map_ || !has_initial_pose_ || !has_base_to_laser_) { + scan_msg.ranges.assign(num_samples, no_hit_range); + return; + } + + auto [x0, y0, theta] = getLaserPose(); + + double resolution = map_.info.resolution; + double origin_x = map_.info.origin.position.x; + double origin_y = map_.info.origin.position.y; + int width = static_cast(map_.info.width); + int height = static_cast(map_.info.height); + + int mx0 = static_cast(std::floor((x0 - origin_x) / resolution)); + int my0 = static_cast(std::floor((y0 - origin_y) / resolution)); + + if (mx0 <= 0 || mx0 >= width || my0 <= 0 || my0 >= height) { + scan_msg.ranges.assign(num_samples, no_hit_range); + return; + } + + const auto & map_data = map_.data; + double range_max = scan_msg.range_max; + double angle_min = scan_msg.angle_min; + double angle_increment = scan_msg.angle_increment; + double step = resolution * 0.5; + + for (int i = 0; i < num_samples; ++i) { + double angle = theta + angle_min + i * angle_increment; + double cos_a = std::cos(angle); + double sin_a = std::sin(angle); + scan_msg.ranges[i] = no_hit_range; + + for (double d = 0.0; d <= range_max; d += step) { + int mx = static_cast(std::floor((x0 + d * cos_a - origin_x) / resolution)); + int my = static_cast(std::floor((y0 + d * sin_a - origin_y) / resolution)); + if (mx <= 0 || mx >= width || my <= 0 || my >= height) { + break; + } + if (map_data[my * width + mx] >= 60) { + scan_msg.ranges[i] = static_cast(d); + break; + } + } + } + + // Add Gaussian noise to valid range measurements + if (scan_noise_std_ > 0.0) { + std::normal_distribution noise(0.0f, static_cast(scan_noise_std_)); + for (int i = 0; i < num_samples; ++i) { + float & r = scan_msg.ranges[i]; + if (std::isfinite(r) && r > 0.0f) { + r = std::max(0.0f, r + noise(rng_)); + } + } + } +} + +rcl_interfaces::msg::SetParametersResult +LoopbackSimulator::validateParameterUpdatesCallback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & param : parameters) { + if (param.get_name() == "speed_factor") { + double factor = param.as_double(); + if (factor <= 0.0) { + result.successful = false; + result.reason = "speed_factor must be positive"; + return result; + } + } + } + return result; +} + +void LoopbackSimulator::updateParametersCallback( + const std::vector & parameters) +{ + for (const auto & param : parameters) { + if (param.get_name() == "speed_factor") { + speed_factor_ = param.as_double(); + if (clock_publisher_) { + clock_publisher_->setSpeedFactor(speed_factor_); + } + } + } +} + +} // namespace nav2_loopback_sim + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_loopback_sim::LoopbackSimulator) diff --git a/nav2_loopback_sim/src/main.cpp b/nav2_loopback_sim/src/main.cpp new file mode 100644 index 00000000000..2ac1fc2e001 --- /dev/null +++ b/nav2_loopback_sim/src/main.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2024, Open Navigation LLC +// Copyright (c) 2026, Dexory (Tony Najjar) +// +// 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. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_loopback_sim/loopback_simulator.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(rclcpp::NodeOptions()); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; +} diff --git a/nav2_loopback_sim/test/test_clock_publisher.cpp b/nav2_loopback_sim/test/test_clock_publisher.cpp new file mode 100644 index 00000000000..640af984e99 --- /dev/null +++ b/nav2_loopback_sim/test/test_clock_publisher.cpp @@ -0,0 +1,194 @@ +// Copyright (c) 2026, Dexory (Tony Najjar) +// +// 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. + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "rosgraph_msgs/msg/clock.hpp" +#include "nav2_loopback_sim/clock_publisher.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" + +using namespace std::chrono_literals; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class ClockPublisherTest : public ::testing::Test +{ +protected: + void SetUp() override + { + node_ = std::make_shared("clock_test_node"); + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); + } + + void TearDown() override + { + executor_->cancel(); + node_.reset(); + executor_.reset(); + } + + void spinFor(std::chrono::milliseconds duration) + { + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < duration) { + executor_->spin_some(10ms); + } + } + + std::unique_ptr makeClockPublisher( + double speed_factor = 1.0) + { + return std::make_unique( + node_->weak_from_this(), + speed_factor); + } + + nav2::LifecycleNode::SharedPtr node_; + std::shared_ptr executor_; +}; + +// Verify that starting the clock publisher results in /clock messages being published +TEST_F(ClockPublisherTest, PublishesClockMessages) +{ + auto clock_pub = makeClockPublisher(); + + int msg_count = 0; + auto sub = node_->create_subscription( + "/clock", + [&](const rosgraph_msgs::msg::Clock::SharedPtr) { + msg_count++; + }, + rclcpp::QoS(10)); + + clock_pub->start(); + spinFor(500ms); + + EXPECT_GE(msg_count, 5); +} + +// Verify that successive /clock timestamps are strictly increasing +TEST_F(ClockPublisherTest, ClockAdvancesMonotonically) +{ + auto clock_pub = makeClockPublisher(); + + std::vector timestamps; + auto sub = node_->create_subscription( + "/clock", + [&](const rosgraph_msgs::msg::Clock::SharedPtr msg) { + timestamps.push_back(rclcpp::Time(msg->clock).nanoseconds()); + }, + rclcpp::QoS(10)); + + clock_pub->start(); + spinFor(500ms); + + ASSERT_GE(timestamps.size(), 10u); + for (size_t i = 1; i < timestamps.size(); ++i) { + EXPECT_GT(timestamps[i], timestamps[i - 1]); + } +} + +// Verify that calling stop() ceases all /clock publication +TEST_F(ClockPublisherTest, StopStopsPublishing) +{ + auto clock_pub = makeClockPublisher(); + + int msg_count = 0; + auto sub = node_->create_subscription( + "/clock", + [&](const rosgraph_msgs::msg::Clock::SharedPtr) { + msg_count++; + }, + rclcpp::QoS(10)); + + clock_pub->start(); + spinFor(200ms); + EXPECT_GT(msg_count, 0); + + clock_pub->stop(); + spinFor(50ms); // drain any in-flight callbacks + int count_after_stop = msg_count; + spinFor(200ms); + EXPECT_EQ(msg_count, count_after_stop); +} + +// Verify that zero and negative speed factors are silently rejected (no crash, no change) +TEST_F(ClockPublisherTest, SetSpeedFactorRejectsNonPositive) +{ + auto clock_pub = makeClockPublisher(1.0); + clock_pub->start(); + + // These should be silently rejected (no crash) + clock_pub->setSpeedFactor(0.0); + clock_pub->setSpeedFactor(-1.0); + + // Valid value should be accepted (no crash) + clock_pub->setSpeedFactor(5.0); + + // Verify clock still works after rejected values + int msg_count = 0; + auto sub = node_->create_subscription( + "/clock", + [&](const rosgraph_msgs::msg::Clock::SharedPtr) { + msg_count++; + }, + rclcpp::QoS(10)); + spinFor(200ms); + EXPECT_GT(msg_count, 0); +} + +// Verify that a 0.5x speed factor produces sim time ≈ half of wall time +TEST_F(ClockPublisherTest, SpeedFactorAffectsRate) +{ + // Slow clock at 0.5x + auto clock_pub = makeClockPublisher(0.5); + + int64_t last_ns = 0; + int count = 0; + auto sub = node_->create_subscription( + "/clock", + [&](const rosgraph_msgs::msg::Clock::SharedPtr msg) { + last_ns = rclcpp::Time(msg->clock).nanoseconds(); + count++; + }, + rclcpp::QoS(10)); + + auto wall_start = std::chrono::steady_clock::now(); + clock_pub->start(); + + while (count < 20 && + std::chrono::steady_clock::now() - wall_start < 3s) + { + executor_->spin_some(10ms); + } + + EXPECT_GE(count, 20); + auto wall_elapsed_ns = std::chrono::duration_cast( + std::chrono::steady_clock::now() - wall_start).count(); + double ratio = static_cast(last_ns) / static_cast(wall_elapsed_ns); + // With 0.5x, sim time should be roughly half of wall time + EXPECT_GT(ratio, 0.45); + EXPECT_LT(ratio, 0.55); +} diff --git a/nav2_loopback_sim/test/test_copyright.py b/nav2_loopback_sim/test/test_copyright.py deleted file mode 100644 index 1f869834559..00000000000 --- a/nav2_loopback_sim/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright() -> None: - 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 deleted file mode 100644 index fe9aa6a300c..00000000000 --- a/nav2_loopback_sim/test/test_flake8.py +++ /dev/null @@ -1,24 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, f'Found {len(errors)} code style errors / warnings:\n' + \ - '\n'.join(errors) diff --git a/nav2_loopback_sim/test/test_loopback_simulator.cpp b/nav2_loopback_sim/test/test_loopback_simulator.cpp new file mode 100644 index 00000000000..cc36b97a47c --- /dev/null +++ b/nav2_loopback_sim/test/test_loopback_simulator.cpp @@ -0,0 +1,333 @@ +// Copyright (c) 2026, Dexory (Tony Najjar) +// +// 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. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_loopback_sim/loopback_simulator.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Matrix3x3.hpp" + +using namespace std::chrono_literals; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class LoopbackSimulatorTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::NodeOptions options; + // Disable sim_time and scan for simpler testing; use TwistStamped (default) + options.parameter_overrides( + { + rclcpp::Parameter("use_sim_time", false), + rclcpp::Parameter("publish_scan", false), + rclcpp::Parameter("publish_clock", false), + rclcpp::Parameter("update_duration", 0.01), + rclcpp::Parameter("base_frame_id", "base_footprint"), + rclcpp::Parameter("odom_frame_id", "odom"), + rclcpp::Parameter("map_frame_id", "map"), + }); + + sim_node_ = std::make_shared(options); + helper_node_ = rclcpp::Node::make_shared("test_helper"); + + executor_ = std::make_shared(); + executor_->add_node(sim_node_->get_node_base_interface()); + executor_->add_node(helper_node_); + + // TwistSubscriber defaults to TwistStamped (enable_stamped_cmd_vel=true) + cmd_vel_pub_ = helper_node_->create_publisher< + geometry_msgs::msg::TwistStamped>("cmd_vel", 10); + initial_pose_pub_ = helper_node_->create_publisher< + geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10); + } + + void TearDown() override + { + executor_->cancel(); + cmd_vel_pub_.reset(); + initial_pose_pub_.reset(); + helper_node_.reset(); + sim_node_.reset(); + executor_.reset(); + } + + void configureAndActivate() + { + sim_node_->configure(); + sim_node_->activate(); + spinFor(200ms); + } + + void spinFor(std::chrono::milliseconds duration) + { + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < duration) { + executor_->spin_some(10ms); + } + } + + void publishInitialPose(double x, double y, double yaw) + { + auto msg = std::make_unique(); + msg->header.stamp = helper_node_->now(); + msg->header.frame_id = "map"; + msg->pose.pose.position.x = x; + msg->pose.pose.position.y = y; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + msg->pose.pose.orientation.x = q.x(); + msg->pose.pose.orientation.y = q.y(); + msg->pose.pose.orientation.z = q.z(); + msg->pose.pose.orientation.w = q.w(); + initial_pose_pub_->publish(std::move(msg)); + } + + void publishCmdVel(double vx, double vy, double wz) + { + auto msg = std::make_unique(); + msg->header.stamp = helper_node_->now(); + msg->twist.linear.x = vx; + msg->twist.linear.y = vy; + msg->twist.angular.z = wz; + cmd_vel_pub_->publish(std::move(msg)); + } + + std::shared_ptr sim_node_; + rclcpp::Node::SharedPtr helper_node_; + std::shared_ptr executor_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Publisher::SharedPtr initial_pose_pub_; +}; + +// Verify that the node transitions through all lifecycle states correctly +TEST_F(LoopbackSimulatorTest, LifecycleTransitions) +{ + auto result = sim_node_->configure(); + EXPECT_EQ(result.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + result = sim_node_->activate(); + EXPECT_EQ(result.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + result = sim_node_->deactivate(); + EXPECT_EQ(result.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + result = sim_node_->cleanup(); + EXPECT_EQ(result.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + result = sim_node_->shutdown(); + EXPECT_EQ(result.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); +} + +// Verify that activating the node starts publishing odom->base_footprint transforms +TEST_F(LoopbackSimulatorTest, PublishesTfOnActivation) +{ + configureAndActivate(); + + bool received_tf = false; + auto tf_sub = helper_node_->create_subscription( + "/tf", 10, + [&](const tf2_msgs::msg::TFMessage::SharedPtr msg) { + for (const auto & t : msg->transforms) { + if (t.header.frame_id == "odom" && t.child_frame_id == "base_footprint") { + received_tf = true; + } + } + }); + + spinFor(500ms); + EXPECT_TRUE(received_tf); +} + +// Verify that publishing an initial pose creates a map->odom transform at the given position +TEST_F(LoopbackSimulatorTest, InitialPoseSetsMapToOdom) +{ + configureAndActivate(); + + std::vector received_tfs; + auto tf_sub = helper_node_->create_subscription( + "/tf", 10, + [&](const tf2_msgs::msg::TFMessage::SharedPtr msg) { + for (const auto & t : msg->transforms) { + received_tfs.push_back(t); + } + }); + + publishInitialPose(1.0, 2.0, 0.0); + spinFor(500ms); + + bool found_map_odom = false; + for (const auto & t : received_tfs) { + if (t.header.frame_id == "map" && t.child_frame_id == "odom") { + found_map_odom = true; + EXPECT_NEAR(t.transform.translation.x, 1.0, 0.1); + EXPECT_NEAR(t.transform.translation.y, 2.0, 0.1); + } + } + EXPECT_TRUE(found_map_odom); +} + +// Verify that a forward cmd_vel causes the robot's odometry position to advance +TEST_F(LoopbackSimulatorTest, CmdVelMovesRobot) +{ + configureAndActivate(); + + // Subscribe to odom before publishing + std::vector odom_msgs; + auto odom_sub = helper_node_->create_subscription( + "odom", 10, + [&](const nav_msgs::msg::Odometry::SharedPtr msg) { + odom_msgs.push_back(*msg); + }); + + publishInitialPose(0.0, 0.0, 0.0); + spinFor(500ms); + + // Drive forward at 1.0 m/s for ~1.5s + for (int i = 0; i < 50; i++) { + publishCmdVel(1.0, 0.0, 0.0); + spinFor(30ms); + } + + ASSERT_FALSE(odom_msgs.empty()); + EXPECT_GT(odom_msgs.back().pose.pose.position.x, 0.0); +} + +// Verify that the published odometry twist matches the commanded velocity +TEST_F(LoopbackSimulatorTest, OdometryContainsTwist) +{ + configureAndActivate(); + + nav_msgs::msg::Odometry latest_odom; + bool got_odom = false; + auto odom_sub = helper_node_->create_subscription( + "odom", 10, + [&](const nav_msgs::msg::Odometry::SharedPtr msg) { + latest_odom = *msg; + got_odom = true; + }); + + publishInitialPose(0.0, 0.0, 0.0); + spinFor(500ms); + + for (int i = 0; i < 30; i++) { + publishCmdVel(0.5, 0.0, 0.1); + spinFor(30ms); + } + + ASSERT_TRUE(got_odom); + EXPECT_NEAR(latest_odom.twist.twist.linear.x, 0.5, 0.01); + EXPECT_NEAR(latest_odom.twist.twist.angular.z, 0.1, 0.01); +} + +// Verify that angular cmd_vel rotates the robot (changes yaw in the odom->base TF) +TEST_F(LoopbackSimulatorTest, RotationUpdatesYaw) +{ + configureAndActivate(); + + geometry_msgs::msg::TransformStamped latest_base_tf; + bool got_base_tf = false; + auto tf_sub = helper_node_->create_subscription( + "/tf", 10, + [&](const tf2_msgs::msg::TFMessage::SharedPtr msg) { + for (const auto & t : msg->transforms) { + if (t.child_frame_id == "base_footprint") { + latest_base_tf = t; + got_base_tf = true; + } + } + }); + + publishInitialPose(0.0, 0.0, 0.0); + spinFor(500ms); + + // Rotate at 1 rad/s + for (int i = 0; i < 50; i++) { + publishCmdVel(0.0, 0.0, 1.0); + spinFor(30ms); + } + + ASSERT_TRUE(got_base_tf); + + tf2::Quaternion q( + latest_base_tf.transform.rotation.x, + latest_base_tf.transform.rotation.y, + latest_base_tf.transform.rotation.z, + latest_base_tf.transform.rotation.w); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + + EXPECT_GT(std::abs(yaw), 0.1); +} + +// Verify that deactivating the node stops odometry publication +TEST_F(LoopbackSimulatorTest, DeactivateStopsPublishing) +{ + configureAndActivate(); + + publishInitialPose(0.0, 0.0, 0.0); + spinFor(500ms); + + sim_node_->deactivate(); + spinFor(100ms); + + int msg_count = 0; + auto odom_sub = helper_node_->create_subscription( + "odom", 10, + [&](const nav_msgs::msg::Odometry::SharedPtr) { + msg_count++; + }); + + publishCmdVel(1.0, 0.0, 0.0); + spinFor(300ms); + + EXPECT_EQ(msg_count, 0); +} + +// Verify that speed_factor can be changed dynamically and non-positive values are rejected +TEST_F(LoopbackSimulatorTest, SpeedFactorDynamicReconfigure) +{ + configureAndActivate(); + + // Valid change + auto result = sim_node_->set_parameter(rclcpp::Parameter("speed_factor", 5.0)); + EXPECT_TRUE(result.successful); + EXPECT_DOUBLE_EQ(sim_node_->get_parameter("speed_factor").as_double(), 5.0); + + // Reject non-positive + result = sim_node_->set_parameter(rclcpp::Parameter("speed_factor", 0.0)); + EXPECT_FALSE(result.successful); + + result = sim_node_->set_parameter(rclcpp::Parameter("speed_factor", -1.0)); + EXPECT_FALSE(result.successful); + + // Should still be last valid value + EXPECT_DOUBLE_EQ(sim_node_->get_parameter("speed_factor").as_double(), 5.0); +} diff --git a/nav2_loopback_sim/test/test_pep257.py b/nav2_loopback_sim/test/test_pep257.py deleted file mode 100644 index b6808e1d80f..00000000000 --- a/nav2_loopback_sim/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings'