Convert nav2_loopback_sim to C++

.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: ""

nav2_bringup/package.xml

@@ -28,6 +28,7 @@
   <exec_depend>xacro</exec_depend>
   <exec_depend>nav2_minimal_tb4_sim</exec_depend>
   <exec_depend>nav2_minimal_tb3_sim</exec_depend>
+  <exec_depend>nav2_loopback_sim</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

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
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+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()

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`)

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 <chrono>
+#include <memory>
+
+#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<rosgraph_msgs::msg::Clock>::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_