From 3378e2fb48867b2deb94b590a365bfbcd1a558ee Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Thu, 26 Mar 2026 10:59:36 -0400 Subject: [PATCH 01/21] building --- .claude/settings.local.json | 11 + src/description/urdf/athena_drive.urdf.xacro | 5 + src/description/urdf/magnetometer.xacro | 31 + src/msgs/CMakeLists.txt | 6 + src/msgs/action/NavigateToTarget.action | 22 + src/msgs/msg/ActiveTarget.msg | 12 + src/msgs/msg/NavStatus.msg | 10 + src/msgs/msg/PlannerEvent.msg | 5 + src/msgs/srv/LatLonToENU.srv | 6 + src/msgs/srv/SetTarget.srv | 17 + src/simulation/CMakeLists.txt | 1 + src/simulation/launch/bridge.launch.py | 10 + src/simulation/launch/bringup.launch.py | 19 +- src/simulation/scripts/heading_publisher.py | 64 + src/subsystems/minimal_navigation/PLAN.md | 508 ++++ .../ackermann_mppi/CMakeLists.txt | 121 + .../include/ackermann_mppi/critic_data.hpp | 85 + .../ackermann_mppi/critic_function.hpp | 120 + .../critics/constraint_critic.hpp | 46 + .../ackermann_mppi/critics/goal_critic.hpp | 39 + .../critics/obstacles_critic.hpp | 61 + .../critics/path_align_critic.hpp | 43 + .../critics/path_follow_critic.hpp | 40 + .../ackermann_mppi/models/constraints.hpp | 47 + .../models/control_sequence.hpp | 50 + .../models/optimizer_settings.hpp | 47 + .../include/ackermann_mppi/models/path.hpp | 46 + .../include/ackermann_mppi/models/state.hpp | 55 + .../ackermann_mppi/models/trajectories.hpp | 46 + .../include/ackermann_mppi/motion_models.hpp | 133 + .../include/ackermann_mppi/optimizer.hpp | 238 ++ .../ackermann_mppi/tools/noise_generator.hpp | 97 + .../include/ackermann_mppi/tools/utils.hpp | 387 +++ .../ackermann_mppi/package.xml | 36 + .../src/ackermann_mppi_node.cpp | 301 ++ .../src/critics/constraint_critic.cpp | 65 + .../src/critics/goal_critic.cpp | 55 + .../src/critics/obstacles_critic.cpp | 214 ++ .../src/critics/path_align_critic.cpp | 141 + .../src/critics/path_follow_critic.cpp | 69 + .../ackermann_mppi/src/noise_generator.cpp | 103 + .../ackermann_mppi/src/optimizer.cpp | 486 ++++ .../athena_smac_planner/CMakeLists.txt | 114 + .../include/athena_smac_planner/a_star.hpp | 323 +++ .../analytic_expansion.hpp | 200 ++ .../athena_smac_planner/collision_checker.hpp | 139 + .../include/athena_smac_planner/constants.hpp | 105 + .../costmap_downsampler.hpp | 99 + .../distance_heuristic.hpp | 77 + .../athena_smac_planner/goal_manager.hpp | 289 ++ .../athena_smac_planner/node_basic.hpp | 69 + .../athena_smac_planner/node_hybrid.hpp | 386 +++ .../obstacle_heuristic.hpp | 96 + .../smac_planner_hybrid.hpp | 111 + .../include/athena_smac_planner/smoother.hpp | 208 ++ .../thirdparty/robin_hood.h | 2539 +++++++++++++++++ .../include/athena_smac_planner/types.hpp | 257 ++ .../include/athena_smac_planner/utils.hpp | 231 ++ .../athena_smac_planner/package.xml | 31 + .../athena_smac_planner/src/a_star.cpp | 526 ++++ .../src/analytic_expansion.cpp | 449 +++ .../src/collision_checker.cpp | 196 ++ .../src/costmap_downsampler.cpp | 127 + .../src/distance_heuristic.cpp | 157 + .../src/global_planner_node.cpp | 159 ++ .../athena_smac_planner/src/node_basic.cpp | 54 + .../athena_smac_planner/src/node_hybrid.cpp | 540 ++++ .../src/obstacle_heuristic.cpp | 230 ++ .../src/smac_planner_hybrid.cpp | 478 ++++ .../athena_smac_planner/src/smoother.cpp | 436 +++ .../dem_costmap/CMakeLists.txt | 65 + .../minimal_navigation/dem_costmap/LICENSE | 202 ++ .../dem_costmap/config/dem_costmap.yaml | 8 + .../dem_costmap/launch/dem_costmap.launch.py | 30 + .../dem_costmap/maps/north_site800m.tif | 3 + .../dem_costmap/package.xml | 22 + .../dem_costmap/src/map_node.cpp | 236 ++ .../gps_pose_publisher/CMakeLists.txt | 32 + .../gps_pose_publisher/package.xml | 24 + .../src/gps_pose_publisher_node.cpp | 192 ++ .../minimal_nav_bringup/CMakeLists.txt | 10 + .../config/nav_params.yaml | 92 + .../minimal_nav_bringup/launch/nav.launch.py | 147 + .../minimal_nav_bringup/package.xml | 23 + .../mission_executive/CMakeLists.txt | 33 + .../mission_executive/package.xml | 23 + .../src/mission_executive_node.cpp | 784 +++++ src/subsystems/minimal_navigation/todo.md | 69 + 88 files changed, 14517 insertions(+), 2 deletions(-) create mode 100644 .claude/settings.local.json create mode 100644 src/description/urdf/magnetometer.xacro create mode 100644 src/msgs/action/NavigateToTarget.action create mode 100644 src/msgs/msg/ActiveTarget.msg create mode 100644 src/msgs/msg/NavStatus.msg create mode 100644 src/msgs/msg/PlannerEvent.msg create mode 100644 src/msgs/srv/LatLonToENU.srv create mode 100644 src/msgs/srv/SetTarget.srv create mode 100644 src/simulation/scripts/heading_publisher.py create mode 100644 src/subsystems/minimal_navigation/PLAN.md create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/package.xml create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/package.xml create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp create mode 100644 src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/dem_costmap/LICENSE create mode 100644 src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml create mode 100644 src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py create mode 100644 src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif create mode 100644 src/subsystems/minimal_navigation/dem_costmap/package.xml create mode 100644 src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp create mode 100644 src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/gps_pose_publisher/package.xml create mode 100644 src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp create mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml create mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py create mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml create mode 100644 src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/mission_executive/package.xml create mode 100644 src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp create mode 100644 src/subsystems/minimal_navigation/todo.md diff --git a/.claude/settings.local.json b/.claude/settings.local.json new file mode 100644 index 00000000..4fe02daa --- /dev/null +++ b/.claude/settings.local.json @@ -0,0 +1,11 @@ +{ + "permissions": { + "allow": [ + "Bash(find:*)", + "Bash(git rm:*)", + "Bash(git checkout:*)", + "Bash(git -C /Users/mdurrani/Development/umdloop/ros2-devenv/main/athena-code diff main...HEAD)", + "Bash(xargs sed:*)" + ] + } +} diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 66e8d7a2..93f7dcfb 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -14,6 +14,7 @@ + @@ -54,6 +55,10 @@ + + + + diff --git a/src/description/urdf/magnetometer.xacro b/src/description/urdf/magnetometer.xacro new file mode 100644 index 00000000..ad53565d --- /dev/null +++ b/src/description/urdf/magnetometer.xacro @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + true + 50 + /mag + mag_link + + + + + diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 83a12abd..8d6c9814 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -19,9 +19,15 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Currentdraw.msg" "msg/JointReceiver.msg" "msg/SystemInfo.msg" + "msg/PlannerEvent.msg" + "msg/ActiveTarget.msg" + "msg/NavStatus.msg" "msg/Heading.msg" "srv/SetController.srv" + "srv/LatLonToENU.srv" + "srv/SetTarget.srv" "action/NavigateToGPS.action" + "action/NavigateToTarget.action" DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs ) diff --git a/src/msgs/action/NavigateToTarget.action b/src/msgs/action/NavigateToTarget.action new file mode 100644 index 00000000..f52ecbc1 --- /dev/null +++ b/src/msgs/action/NavigateToTarget.action @@ -0,0 +1,22 @@ +uint8 GPS=0 +uint8 METER=1 +uint8 GNSS_ONLY=0 +uint8 ARUCO_POST=1 +uint8 OBJECT=2 +uint8 LOCAL=3 +string target_id +float64 lat +float64 lon +float64 x_m +float64 y_m +uint8 goal_type +uint8 target_type +float64 tolerance_m +bool is_return +--- +bool success +string message +--- +float64 distance_to_goal_m +float64 cross_track_error_m +string state diff --git a/src/msgs/msg/ActiveTarget.msg b/src/msgs/msg/ActiveTarget.msg new file mode 100644 index 00000000..c9c70300 --- /dev/null +++ b/src/msgs/msg/ActiveTarget.msg @@ -0,0 +1,12 @@ +uint8 GNSS_ONLY=0 +uint8 ARUCO_POST=1 +uint8 OBJECT=2 +uint8 LOCAL=3 +uint8 GPS=0 +uint8 METER=1 +string target_id +uint8 target_type +float64 tolerance_m +geometry_msgs/PoseStamped goal_enu +uint8 goal_source +string status diff --git a/src/msgs/msg/NavStatus.msg b/src/msgs/msg/NavStatus.msg new file mode 100644 index 00000000..a1360d83 --- /dev/null +++ b/src/msgs/msg/NavStatus.msg @@ -0,0 +1,10 @@ +string state +string active_target_id +uint8 active_target_type +uint8 goal_source +float64 distance_to_goal_m +float64 cross_track_error_m +float64 heading_error_rad +float64 robot_speed_mps +bool is_return +uint8 last_planner_event diff --git a/src/msgs/msg/PlannerEvent.msg b/src/msgs/msg/PlannerEvent.msg new file mode 100644 index 00000000..3efbbb55 --- /dev/null +++ b/src/msgs/msg/PlannerEvent.msg @@ -0,0 +1,5 @@ +uint8 NEW_GOAL=0 +uint8 PLANNING=1 +uint8 PLAN_SUCCEEDED=2 +uint8 PLAN_FAILED=3 +uint8 event diff --git a/src/msgs/srv/LatLonToENU.srv b/src/msgs/srv/LatLonToENU.srv new file mode 100644 index 00000000..e4ced1d9 --- /dev/null +++ b/src/msgs/srv/LatLonToENU.srv @@ -0,0 +1,6 @@ +float64 lat +float64 lon +--- +float64 x +float64 y +float64 z diff --git a/src/msgs/srv/SetTarget.srv b/src/msgs/srv/SetTarget.srv new file mode 100644 index 00000000..790f2240 --- /dev/null +++ b/src/msgs/srv/SetTarget.srv @@ -0,0 +1,17 @@ +uint8 GPS=0 +uint8 METER=1 +uint8 GNSS_ONLY=0 +uint8 ARUCO_POST=1 +uint8 OBJECT=2 +uint8 LOCAL=3 +string target_id +float64 lat +float64 lon +float64 x_m +float64 y_m +uint8 goal_type +uint8 target_type +float64 tolerance_m +--- +bool success +string message diff --git a/src/simulation/CMakeLists.txt b/src/simulation/CMakeLists.txt index 3e08edaa..ee5162d3 100644 --- a/src/simulation/CMakeLists.txt +++ b/src/simulation/CMakeLists.txt @@ -28,6 +28,7 @@ endforeach() # Install Python scripts install(PROGRAMS scripts/ground_truth_tf_publisher.py + scripts/heading_publisher.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/simulation/launch/bridge.launch.py b/src/simulation/launch/bridge.launch.py index 1fc2e6b7..e8ec9b5a 100644 --- a/src/simulation/launch/bridge.launch.py +++ b/src/simulation/launch/bridge.launch.py @@ -47,4 +47,14 @@ def generate_launch_description(): '/odom/ground_truth@nav_msgs/msg/Odometry@gz.msgs.Odometry', ] ), + + Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='magnetometer_bridge', + output='screen', + arguments=[ + '/mag@sensor_msgs/msg/MagneticField[gz.msgs.Magnetometer', + ] + ), ]) diff --git a/src/simulation/launch/bringup.launch.py b/src/simulation/launch/bringup.launch.py index 772d9798..1d2b3dc2 100644 --- a/src/simulation/launch/bringup.launch.py +++ b/src/simulation/launch/bringup.launch.py @@ -41,11 +41,17 @@ description='Publish ground truth odom -> base_link transform' ), DeclareLaunchArgument( - 'rqt', + 'rqt', default_value='false', choices=['true', 'false'], description='Open RQt.' ), + DeclareLaunchArgument( + 'publish_sim_heading', + default_value='false', + choices=['true', 'false'], + description='Run heading_publisher.py to derive heading from simulated magnetometer' + ), ] def generate_launch_description(): @@ -92,11 +98,20 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('publish_ground_truth_tf')) ) + sim_heading = Node( + package='simulation', + executable='heading_publisher.py', + name='heading_publisher', + output='screen', + condition=IfCondition(LaunchConfiguration('publish_sim_heading')), + ) + ld = LaunchDescription(ARGUMENTS) ld.add_action(gazebo) ld.add_action(robot_spawn) ld.add_action(bridge) ld.add_action(control) ld.add_action(ground_truth_tf) - + ld.add_action(sim_heading) + return ld diff --git a/src/simulation/scripts/heading_publisher.py b/src/simulation/scripts/heading_publisher.py new file mode 100644 index 00000000..7c5df039 --- /dev/null +++ b/src/simulation/scripts/heading_publisher.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 + +import math + +import rclpy +from rclpy.node import Node +from rclpy.qos import SensorDataQoS +from sensor_msgs.msg import MagneticField +from msgs.msg import Heading + + +class HeadingPublisher(Node): + def __init__(self): + super().__init__('heading_publisher') + + self.declare_parameter('mag_topic', '/mag') + self.declare_parameter('heading_topic', '/gps/heading') + self.declare_parameter('heading_acc', 5.0) + + mag_topic = self.get_parameter('mag_topic').get_parameter_value().string_value + heading_topic = self.get_parameter('heading_topic').get_parameter_value().string_value + self.heading_acc_ = self.get_parameter('heading_acc').get_parameter_value().double_value + + self.pub_ = self.create_publisher(Heading, heading_topic, 10) + + self.sub_ = self.create_subscription( + MagneticField, + mag_topic, + self.on_mag, + SensorDataQoS(), + ) + + self.get_logger().info( + f'Heading publisher: {mag_topic} -> {heading_topic}' + ) + + def on_mag(self, msg: MagneticField): + bx = msg.magnetic_field.x + by = msg.magnetic_field.y + + # compass bearing: degrees clockwise from north + compass_bearing = (math.degrees(math.atan2(-by, bx)) + 360.0) % 360.0 + + # ENU heading: degrees CCW from east + heading_enu = (90.0 - compass_bearing + 360.0) % 360.0 + + out = Heading() + out.header = msg.header + out.heading = heading_enu + out.heading_acc = self.heading_acc_ + out.compass_bearing = compass_bearing + self.pub_.publish(out) + + +def main(args=None): + rclpy.init(args=args) + node = HeadingPublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/subsystems/minimal_navigation/PLAN.md b/src/subsystems/minimal_navigation/PLAN.md new file mode 100644 index 00000000..09d8b107 --- /dev/null +++ b/src/subsystems/minimal_navigation/PLAN.md @@ -0,0 +1,508 @@ +# Athena Navigation Stack — Implementation Spec + +A GPS-only, Nav2-free navigation system in a single ROS 2 package. No IMU-GNSS fusion — localization is GPS + compass heading only. + + +--- + +## 1. Architecture Overview + +Four nodes form the runtime pipeline: + +``` +gps_pose_publisher → mission_executive → global_planner → mppi_runner + │ │ │ │ + map→base_link TF state machine Hybrid-A* MPPI control + /robot_pose /goal_pose /global_path /cmd_vel +``` + +**TF tree:** `map → base_link` (no `odom` frame — see §1.2). + +### 1.1 Existing Code + +| Source | Role | Required Changes | +|--------|------|------------------| +| `ackermann_mppi/` | Local planner / path follower | Rename `/plan` → `/global_path`; add `/nav_enabled` guard; add costmap debug publisher | +| `smac/` | Global planner (Hybrid-A\*) | Rename package to `athena_smac_planner`; integrate `athena_map` costmap input | +| `athena_map/` | Publishes `nav_msgs/OccupancyGrid` on `/map` | Ensure `frame_id: "map"`, `transient_local` QoS, and resolution matches `global_costmap` (0.5 m). We control this node — adapt as needed for clean integration. | +| `athena_planner/` | BT nodes (ArUco pose extraction, etc.) | Reuse in Phase 2 | +| `aruco_detection/` | ArUco refinement (~80% complete) | Wire in Phase 2c | + +**Do not use `localizer/`** — IMU-GNSS fusion is explicitly excluded from the localization path. + +### 1.2 TF Convention — Intentional `odom` Omission + +REP-105 expects `map → odom → base_link`. This stack uses `map → base_link` directly because there is no fused odometry source suitable for the `odom` frame. `gps_pose_publisher` broadcasts this transform. + +The ZED SDK provides `/odom` (`nav_msgs/Odometry`) with visual-inertial velocity estimates. This is used **only** for measured velocity in stop detection (§3.2.4) — it does not participate in localization or TF. + +### 1.3 Package Rename: `smac/` → `athena_smac_planner` + +The vendored `smac/` package declared `nav2_smac_planner` in `package.xml`, colliding with the system-installed `ros-$ROS_DISTRO-nav2-smac-planner`. Fix: rename to `athena_smac_planner` in both `package.xml` and `CMakeLists.txt`. + +--- + +## 2. Operator Interface + +All operator commands use ROS 2 services and actions (the legacy `OperatorCmd.msg` topic is retired). + +| Command | Interface | Type | Rationale | +|---------|-----------|------|-----------| +| `GO_TO` | Action | `NavigateToTarget` | Long-running; preemptable; provides distance feedback | +| `RETURN` | Action | `NavigateToTarget` | Same semantics as `GO_TO` (with `is_return=true`) | +| `ABORT` | Service | `std_srvs/Trigger` | Instant; needs confirmation | +| `SET_TARGET` | Service | `SetTarget` | Pre-register a target by ID for later navigation | +| `TELEOP_ON` / `TELEOP_OFF` | Service | `std_srvs/SetBool` | Needs ack; returns success + optional message | + +**Intended workflow:** Use `SetTarget` to pre-load named targets (with coordinates, type, and tolerance) into `mission_executive`'s target registry. Then use `NavigateToTarget` with just a `target_id` (+ `is_return` flag) to begin navigation. `NavigateToTarget` also accepts inline coordinates for ad-hoc goals that don't need pre-registration — when `target_id` is empty, the coordinates in the goal message are used directly. + +All custom types live in the `navigation_msgs` package (see §6). + +--- + +## 3. Node Specifications + +### 3.1 `gps_pose_publisher` + +Converts raw GPS + heading into a `map`-frame pose and broadcasts `map → base_link`. + +**Subscriptions:** + +| Topic | Type | Notes | +|-------|------|-------| +| `/gps/fix` | `sensor_msgs/NavSatFix` | | +| `/gps/heading` (configurable) | `std_msgs/Float64` | Input always in degrees. Internally converted to ENU radians (0 = East, CCW+). | + +**Publications:** + +| Topic | Type | Notes | +|-------|------|-------| +| `/robot_pose` | `geometry_msgs/PoseStamped` | Frame: `map` | +| `/gps_status` | `std_msgs/String` | Fix quality + heading source each update | +| TF: `map → base_link` | | | + +**Services:** + +| Service | Type | Purpose | +|---------|------|---------| +| `~/latlon_to_enu` | `LatLonToENU` | Centralizes WGS84 → ENU projection | + +**Core logic:** + +- WGS84 → ENU via `GeographicLib::LocalCartesian`: + ```cpp + GeographicLib::LocalCartesian proj(lat0, lon0, alt0); + proj.Forward(lat, lon, alt, e, n, u); + ``` +- Origin set from first fix, or from `/start_gate_ref` if `use_start_gate_ref: true`. +- Heading conversion: input degrees → ENU radians, then `tf2::Quaternion::setRPY(0, 0, heading_rad)`. + +--- + +### 3.2 `mission_executive` + +Operator-driven state machine managing target sequencing, abort/return, mode switching, and arrival detection. + +#### 3.2.1 States + +`IDLE`, `NAVIGATING`, `ARRIVING`, `STOPPED_AT_TARGET`, `ABORTING`, `RETURNING`, `STOPPED_AT_RETURN`, `TELEOP`. + +`TELEOP_ON` from any state → `TELEOP`. `TELEOP_OFF` → `IDLE` (never auto-resumes navigation). + +#### 3.2.2 Transition Table + +| State | GO_TO | ABORT | RETURN | `PLAN_FAILED` | TELEOP_ON | TELEOP_OFF | +|-------|-------|-------|--------|---------------|-----------|------------| +| `IDLE` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | +| `NAVIGATING` | → `NAVIGATING` (preempt) | → `ABORTING` | ignore | → `ABORTING` | → `TELEOP` | ignore | +| `ARRIVING` | → `NAVIGATING` (cancel) | → `ABORTING` | ignore | ignore | → `TELEOP` | ignore | +| `STOPPED_AT_TARGET` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | +| `ABORTING` | queued | ignore | queued | ignore | → `TELEOP` | ignore | +| `RETURNING` | → `NAVIGATING` (preempt) | → `ABORTING` | ignore | → `ABORTING` | → `TELEOP` | ignore | +| `STOPPED_AT_RETURN` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | +| `TELEOP` | ignore | ignore | ignore | ignore | ignore | → `IDLE` | + +\* `RETURN` requires the target to have been previously visited. Otherwise the action server returns failure immediately. + +#### 3.2.3 `PLAN_FAILED` Handling + +Subscribes to `/planner_event` (`PlannerEvent.msg`). On `event == PLAN_FAILED`: + +- In `NAVIGATING` or `RETURNING`: transition to `ABORTING`, publish `/nav_enabled = false`. +- In all other states: ignore (the executive is authoritative on state). + +#### 3.2.4 Stopped Detection + +Arrival confirmation uses **measured velocity from `/odom`** (published by the ZED SDK — not commanded `/cmd_vel`). The robot is considered stopped when `|twist.linear| < velocity_zero_threshold` is held for `arrival_hold_time` seconds. + +#### 3.2.5 Replanning + +The global planner does **not** self-trigger replans. `mission_executive` computes the robot's cross-track error (perpendicular distance from `/robot_pose` to the nearest segment of the current `/global_path`). When this exceeds `replan_distance_m`, the executive re-publishes `/goal_pose` to trigger a fresh plan. This keeps replanning fully observable via the `/goal_pose` topic stream. + +The executive caches the latest `/global_path` (subscribed with `transient_local` QoS) for this computation. + +#### 3.2.6 Executor Configuration + +`mission_executive` calls `gps_pose_publisher`'s `latlon_to_enu` service during `GO_TO` goal conversion. To avoid deadlock from blocking on `future.get()` inside a callback, **use a `MultiThreadedExecutor` with a reentrant callback group** for the service client. + +#### 3.2.7 Interfaces + +**Subscriptions:** + +| Topic | Type | Purpose | +|-------|------|---------| +| `/robot_pose` | `PoseStamped` | Current position | +| `/odom` | `nav_msgs/Odometry` | Measured velocity for stop detection (ZED SDK) | +| `/planner_event` | `PlannerEvent` | `PLAN_FAILED` triggers abort | +| `/global_path` | `nav_msgs/Path` | Cached for cross-track error (QoS: `transient_local`) | + +**Action servers:** + +| Name | Type | Handles | +|------|------|---------| +| `~/navigate_to_target` | `NavigateToTarget` | `GO_TO`, `RETURN` | + +**Services:** + +| Name | Type | Purpose | +|------|------|---------| +| `~/abort` | `std_srvs/Trigger` | Transition to `ABORTING` | +| `~/set_target` | `SetTarget` | Register/update a named target in the registry | + +**Publications:** + +| Topic | Type | Notes | +|-------|------|-------| +| `/goal_pose` | `PoseStamped` | Always ENU / `map` frame; consumed by global planner | +| `/nav_enabled` | `std_msgs/Bool` | `false` during teleop/stop; MPPI respects this | +| `/nav_mode` | `std_msgs/String` | `"autonomous"` / `"teleop"` / `"stopped"` | +| `/active_target` | `ActiveTarget` | Target metadata + status | +| `/nav_status` | `NavStatus` | 2 Hz + on every state change | + +#### 3.2.8 Target Types + +| Type | Tolerance | Notes | +|------|-----------|-------| +| `GNSS_ONLY` | 3.0 m | GPS-sourced goal | +| `ARUCO_POST` | 2.0 m | GNSS approach → ArUco refines (Phase 2c) | +| `OBJECT` | stop on detection | First two: GNSS < 3 m; third: < 10 m | +| `LOCAL` | user-defined | Meter-sourced goal; no GPS required | + +--- + +### 3.3 `global_planner` (Hybrid-A\* via `athena_smac_planner`) + +Wraps the vendored SMAC planner with `athena_map` costmap input. Plans **only** on receipt of a new `/goal_pose` — no self-triggered replanning. + +**Startup behavior:** On receiving a `/goal_pose` before a valid `/robot_pose` has been received, the planner logs a `WARN` and waits (does not publish `PLAN_FAILED`). Planning proceeds once both pose and map are available. This handles the GPS cold-start window gracefully. + +**Subscriptions:** + +| Topic | Type | Notes | +|-------|------|-------| +| `/goal_pose` | `PoseStamped` | Triggers planning | +| `/robot_pose` | `PoseStamped` | Planner start pose — must be received before planning | +| `/map` | `nav_msgs/OccupancyGrid` | Feeds `StaticLayer` (QoS: `transient_local`) | + +**Publications:** + +| Topic | Type | QoS | Notes | +|-------|------|-----|-------| +| `/global_path` | `nav_msgs/Path` | `transient_local` | Consumed by MPPI + mission_executive | +| `/planner_event` | `PlannerEvent` | | Enum-based event (see §6) | + +--- + +### 3.4 `mppi_runner` (existing `ackermann_mppi`) + +Minimal changes to the existing package. + +**Changes required:** + +1. Rename input topic `/plan` → `/global_path` +2. Subscribe to `/nav_enabled` — skip `evalControl()` when false +3. Add costmap debug publisher via `nav2_costmap_2d::CostmapPublisher` + +**Local costmap** is owned by `mppi_runner` as a child `Costmap2DROS` node, configured under the `local_costmap` namespace nested within `mppi_runner` parameters (see §7). + +**Subscriptions:** `/robot_pose`, `/odom`, `/global_path`, `/goal_pose`, `/nav_enabled` + +**Publications:** `/cmd_vel` (`TwistStamped`) + +**Control loop:** Timer at `controller_frequency` Hz (default 20). Calls `evalControl()` only when pose + path + goal are all available **and** `/nav_enabled` is true. + +**Initial critics:** +`ConstraintCritic`, `GoalCritic`, `GoalAngleCritic`, `PathFollowCritic`, `PathAlignCritic`, `PreferForwardCritic` + +--- + +## 4. Debugging & Observability + +### 4.1 `/nav_status` (published by `mission_executive`) + +Published at 2 Hz and on every state change. See §6 for full message definition. + +### 4.2 Logging Conventions + +| Event | Level | Format | +|-------|-------|--------| +| State transition | `INFO` | `[mission_executive] → : ` | +| Replan trigger | `INFO` | `[mission_executive] replan triggered: xtrack=X.Xm` | +| `evalControl()` skip | `DEBUG` | (fires at 20 Hz — never INFO) | +| GPS origin set | `INFO` | `[gps_pose_publisher] origin set: lat=X lon=Y alt=Z` | +| `PLAN_FAILED` received | `WARN` | `[mission_executive] PLAN_FAILED — transitioning to ABORTING` | +| Planner waiting for fix | `WARN` | `[global_planner] goal received but no robot_pose yet — waiting` | +| Service/action rejection | `WARN` | Reason logged; error returned to caller | + +### 4.3 Topic QoS + +| Topic | QoS | +|-------|-----| +| `/global_path` | `transient_local` (rviz2/Foxglove sees current path on connect) | +| `/map` | `transient_local` (from `athena_map`) | +| `/nav_status` | `keep_last(1)`, reliable | + +--- + +## 5. Extension Phases + +| Phase | Feature | Effort | +|-------|---------|--------| +| **2a** | Depth camera obstacle avoidance | Config only — add `ObstacleLayer` + `InflationLayer` to MPPI local costmap plugins, enable `CostCritic` + `ObstaclesCritic` | +| **2b** | GeoTIFF terrain obstacles | Config only — set `geotiff_path` on `global_planner` | +| **2c** | ArUco post refinement | Wire `correction_node` output to override `/goal_pose` when `ARUCO_POST` target is within ~10 m. Reuse `GetArucoPose` BT node. | +| **2d** | Object detection | `yolo_ros/` detects objects within tolerance → publish to C2 overlay → `mission_executive` transitions to `STOPPED_AT_TARGET` | +| **2e** | Spiral/lawnmower search | `SpiralCoverageAction` BT node (Archimedean spiral, $r = 15$ m, spacing $= 2$ m). Sub-mode when target reached via GNSS but not visually acquired. | + +--- + +## 6. Custom Message / Service / Action Definitions + +All defined in the `navigation_msgs` package. + +### Services + +``` +# LatLonToENU.srv +float64 lat +float64 lon +--- +float64 x +float64 y +float64 z +``` + +``` +# SetTarget.srv — pre-register a named target +string target_id +float64 lat # GPS goal (WGS84) — ignored if goal_type == METER +float64 lon +float64 x_m # Meter goal (ENU) — ignored if goal_type == GPS +float64 y_m +uint8 goal_type # GPS=0, METER=1 +uint8 GPS=0 +uint8 METER=1 +uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 +uint8 GNSS_ONLY=0 +uint8 ARUCO_POST=1 +uint8 OBJECT=2 +uint8 LOCAL=3 +float64 tolerance_m +--- +bool success +string message +``` + +### Actions + +``` +# NavigateToTarget.action +# --- Goal --- +# If target_id is non-empty, coordinates are looked up from the registry +# (previously loaded via SetTarget). If target_id is empty, inline +# coordinates are used as an ad-hoc goal. +string target_id # empty string = use inline coordinates +float64 lat # ad-hoc GPS goal (ignored if target_id set) +float64 lon +float64 x_m # ad-hoc meter goal (ignored if target_id set) +float64 y_m +uint8 goal_type # GPS=0, METER=1 +uint8 GPS=0 +uint8 METER=1 +uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 +uint8 GNSS_ONLY=0 +uint8 ARUCO_POST=1 +uint8 OBJECT=2 +uint8 LOCAL=3 +float64 tolerance_m # only used for ad-hoc goals; registry targets use stored tolerance +bool is_return +--- +# --- Result --- +bool success +string message +--- +# --- Feedback --- +float64 distance_to_goal_m +float64 cross_track_error_m +string state +``` + +### Messages + +``` +# PlannerEvent.msg +uint8 NEW_GOAL=0 +uint8 PLANNING=1 +uint8 PLAN_SUCCEEDED=2 +uint8 PLAN_FAILED=3 +uint8 event +``` + +``` +# ActiveTarget.msg +string target_id +uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 +float64 tolerance_m +geometry_msgs/PoseStamped goal_enu +uint8 goal_source # GPS=0, METER=1 +string status # NAVIGATING, SEARCHING, ARRIVED, ABORTED +``` + +``` +# NavStatus.msg +string state # IDLE, NAVIGATING, ARRIVING, STOPPED_AT_TARGET, + # ABORTING, RETURNING, STOPPED_AT_RETURN, TELEOP +string active_target_id +uint8 active_target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 +uint8 goal_source # GPS=0, METER=1 +float64 distance_to_goal_m # -1.0 if no active goal +float64 cross_track_error_m # -1.0 if no active path +float64 heading_error_rad +float64 robot_speed_mps # from /odom twist (ZED SDK) +bool is_return +uint8 last_planner_event # last PlannerEvent.event value +``` + +--- + +## 7. Parameter Reference (`nav_params.yaml`) + +```yaml +# ── Global ────────────────────────────────────────────── +use_sim_time: false # propagated to all nodes via launch + +# ── Robot geometry (referenced by costmaps + planner) ─── +robot_radius: &robot_radius 0.45 # meters — inscribed radius + +# ── gps_pose_publisher ───────────────────────────────── +gps_pose_publisher: + ros__parameters: + use_sim_time: false + heading_topic: "/gps/heading" + origin_lat: NaN # auto-set from first fix if NaN + origin_lon: NaN + origin_alt: 0.0 + use_start_gate_ref: true # if true, /start_gate_ref sets origin + +# ── mission_executive ────────────────────────────────── +mission_executive: + ros__parameters: + use_sim_time: false + default_targets: [] + velocity_zero_threshold: 0.05 # m/s — applied to /odom twist + arrival_hold_time: 1.0 # seconds of confirmed zero velocity + replan_distance_m: 3.0 # cross-track error threshold to trigger replan + latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" + +# ── global_planner (athena_smac_planner) ─────────────── +global_planner: + ros__parameters: + use_sim_time: false + geotiff_path: "" + grid_size_m: 500.0 + grid_resolution_m: 0.5 + obstacle_threshold: 50 + min_turning_r: 1.2 # must match mppi_runner + allow_reverse: false + +global_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 1.0 + publish_frequency: 0.5 + transform_tolerance: 1.0 # generous — GPS-only localization has jitter + rolling_window: false + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_topic: /map + subscribe_to_updates: true + map_subscribe_transient_local: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 1.0 # >= robot_radius + +# ── mppi_runner (ackermann_mppi) ─────────────────────── +mppi_runner: + ros__parameters: + use_sim_time: false + controller_frequency: 20.0 + motion_model: "Ackermann" + min_turning_r: 1.2 + vx_max: 3.0 + vx_min: 0.0 + wz_max: 1.0 + time_steps: 56 + batch_size: 1000 + model_dt: 0.05 + critics: + - ConstraintCritic + - GoalCritic + - GoalAngleCritic + - PathFollowCritic + - PathAlignCritic + - PreferForwardCritic + # Local costmap — child Costmap2DROS node owned by mppi_runner + local_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 1.0 + rolling_window: true + width: 20 + height: 20 + resolution: 0.1 + plugins: [] # Phase 2a: add ObstacleLayer + InflationLayer +``` + +--- + +## 8. Implementation Order + +1. **`gps_pose_publisher`** — full implementation; validates TF + ENU projection before anything else runs. +2. **`ackermann_mppi` modifications** — topic rename, `/nav_enabled` guard, costmap debug publisher. +3. **`global_planner` / `athena_smac_planner`** — package rename, `athena_map` integration, costmap wiring. +4. **`mission_executive`** — state machine, action/service servers, all operator commands. +5. **`nav.launch.py`** + `CMakeLists.txt` + `package.xml` — single-package build and launch. + +In the final launch file, you need to launch main/athena-code/src/subsystems/navigation/athena_gps/launch/gps_launch.py, and we will use the /odom/ground_truth topic for odom. you also need to map the output using a twist stamper like + + twist_stamper_node = Node( + package='twist_stamper', + executable='twist_stamper', + name='cmd_vel_stamper', + parameters=[{'use_sim_time': sim}], + remappings=[ + ('cmd_vel_in', '/cmd_vel_nav'), + ('cmd_vel_out', '/rear_ackermann_controller/reference'), + ], + ) + diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt b/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt new file mode 100644 index 00000000..8b73412f --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt @@ -0,0 +1,121 @@ +cmake_minimum_required(VERSION 3.8) +project(ackermann_mppi) + +find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(builtin_interfaces REQUIRED) + +include_directories( + include + ${EIGEN3_INCLUDE_DIR} +) + +include(CheckCXXCompilerFlag) +check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA) +if(COMPILER_SUPPORTS_FMA) + add_compile_options(-mfma) +endif() + +# --- Core optimizer library (no ROS node, no pluginlib) --- +add_library(ackermann_mppi_core SHARED + src/noise_generator.cpp + src/optimizer.cpp + src/critics/constraint_critic.cpp + src/critics/goal_critic.cpp + src/critics/obstacles_critic.cpp + src/critics/path_align_critic.cpp + src/critics/path_follow_critic.cpp +) + +# Enable C++17 (required for inline constexpr, structured bindings, etc.) +target_compile_features(ackermann_mppi_core PUBLIC cxx_std_17) + +# Apply -O3 only for Release and RelWithDebInfo builds so Debug stays debuggable. +# Do not force -O3 unconditionally — it disrespects the developer's chosen build type. +target_compile_options(ackermann_mppi_core PUBLIC + $<$:-O3> + $<$:-O3> +) + +target_include_directories(ackermann_mppi_core + PUBLIC + "$" + "$") + +target_link_libraries(ackermann_mppi_core PUBLIC + angles::angles + ${geometry_msgs_TARGETS} + nav2_costmap_2d::layers + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + ${std_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} + ${builtin_interfaces_TARGETS} +) + +# --- ROS2 node executable --- +add_executable(ackermann_mppi_node + src/ackermann_mppi_node.cpp +) + +target_compile_options(ackermann_mppi_node PRIVATE + $<$:-O3> + $<$:-O3> +) + +target_include_directories(ackermann_mppi_node PRIVATE + "$") + +target_link_libraries(ackermann_mppi_node + ackermann_mppi_core + rclcpp::rclcpp + tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + ${geometry_msgs_TARGETS} +) + +# --- Install --- +install(TARGETS ackermann_mppi_core ackermann_mppi_node + EXPORT ackermann_mppi + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +ament_export_targets(ackermann_mppi HAS_LIBRARY_TARGET) +ament_export_dependencies( + angles + geometry_msgs + nav2_costmap_2d + nav_msgs + rclcpp + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + Eigen3 +) +ament_export_include_directories(include/${PROJECT_NAME}) + +ament_package() diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp new file mode 100644 index 00000000..14e89c51 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp @@ -0,0 +1,85 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITIC_DATA_HPP_ +#define ACKERMANN_MPPI__CRITIC_DATA_HPP_ + +#include + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/models/trajectories.hpp" +#include "ackermann_mppi/models/path.hpp" +#include "ackermann_mppi/motion_models.hpp" + +namespace mppi +{ + +/** + * @struct mppi::CriticData + * @brief Data passed to critics for scoring. GoalChecker replaced with a simple + * distance tolerance float — the optimizer sets this from its parameters. + * + * Has an explicit constructor to prevent silent wrong-field binding when using + * aggregate initialization in C++17 (designated initializers require C++20). + */ +struct CriticData +{ + const models::State & state; + const models::Trajectories & trajectories; + const models::Path & path; + const geometry_msgs::msg::Pose & goal; + + Eigen::ArrayXf & costs; + float & model_dt; + + bool fail_flag{false}; + + // Replaces nav2_core::GoalChecker — set by Optimizer::prepare() from settings + float goal_dist_tolerance{0.25f}; + + std::shared_ptr motion_model; + std::optional> path_pts_valid; + std::optional furthest_reached_path_point; + std::vector trajectories_in_collision; + + // Explicit constructor so call sites use named parameters, preventing silent + // mis-binding if fields are reordered. + CriticData( + const models::State & state_in, + const models::Trajectories & trajectories_in, + const models::Path & path_in, + const geometry_msgs::msg::Pose & goal_in, + Eigen::ArrayXf & costs_in, + float & model_dt_in, + float goal_dist_tolerance_in = 0.25f, + std::shared_ptr motion_model_in = nullptr) + : state(state_in), + trajectories(trajectories_in), + path(path_in), + goal(goal_in), + costs(costs_in), + model_dt(model_dt_in), + fail_flag(false), + goal_dist_tolerance(goal_dist_tolerance_in), + motion_model(std::move(motion_model_in)) {} +}; + +} // namespace mppi + +#endif // ACKERMANN_MPPI__CRITIC_DATA_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp new file mode 100644 index 00000000..90cff7f6 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp @@ -0,0 +1,120 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ +#define ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +#include "ackermann_mppi/critic_data.hpp" + +namespace mppi::critics +{ + +struct CollisionCost +{ + float cost{0}; + bool using_footprint{false}; +}; + +/** + * @class mppi::critics::CriticFunction + * @brief Abstract base for MPPI trajectory scoring critics. + * + * Compared to the original nav2 version: + * - Uses rclcpp::Node::SharedPtr instead of LifecycleNode::WeakPtr + * - No ParametersHandler dependency — uses a simple getParam() helper inline + * - Instantiated directly by the Optimizer (no pluginlib) + */ +class CriticFunction +{ +public: + CriticFunction() = default; + virtual ~CriticFunction() = default; + + /** + * @brief Configure critic on startup. + * @param node ROS2 node (used for parameter access and logging) + * @param parent_name Name of the parent optimizer (for reading shared params) + * @param name Name of this critic (used as param namespace) + * @param costmap_ros Costmap for collision/cost queries + */ + void on_configure( + rclcpp::Node::SharedPtr node, + const std::string & parent_name, + const std::string & name, + std::shared_ptr costmap_ros) + { + node_ = node; + logger_ = node_->get_logger(); + name_ = name; + parent_name_ = parent_name; + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + + // Declare and read the common 'enabled' parameter + declare_param(name_ + ".enabled", true); + node_->get_parameter(name_ + ".enabled", enabled_); + + initialize(); + } + + virtual void score(CriticData & data) = 0; + virtual void initialize() = 0; + + std::string getName() { return name_; } + +protected: + /** + * @brief Declare a parameter with a default value if not already declared. + */ + template + void declare_param(const std::string & full_name, T default_value) + { + if (!node_->has_parameter(full_name)) { + node_->declare_parameter(full_name, default_value); + } + } + + /** + * @brief Get a parameter accessor scoped to a given namespace. + * + * Usage (same pattern as original ParametersHandler::getParamGetter): + * auto getParam = getParamGetter(name_); + * getParam(weight_, "cost_weight", 5.0f); + */ + auto getParamGetter(const std::string & ns) + { + return [this, ns](auto & setting, const std::string & name, auto default_value) { + std::string full_name = ns.empty() ? name : ns + "." + name; + declare_param(full_name, default_value); + node_->get_parameter(full_name, setting); + }; + } + + bool enabled_{true}; + std::string name_, parent_name_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_{nullptr}; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp new file mode 100644 index 00000000..ba4901a3 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ + +#include "ackermann_mppi/critic_function.hpp" + +namespace mppi::critics +{ + +/** + * @class ConstraintCritic (Ackermann-only) + * @brief Penalizes trajectories that violate velocity and turning radius constraints. + * + * For Ackermann: penalizes vx outside [vx_min, vx_max] and steering ratios + * (|vx|/|wz|) that would require a tighter turn than min_turning_r. + */ +class ConstraintCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + // min_turning_r is vehicle geometry — fixed. Velocity limits are read from + // data.motion_model->getConstraints() in score() so speed-limit changes take effect. + float min_turning_r_{0.2f}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp new file mode 100644 index 00000000..f8d69ed1 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp @@ -0,0 +1,39 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ + +#include "ackermann_mppi/critic_function.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +namespace mppi::critics +{ + +class GoalCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + float threshold_to_consider_{0}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp new file mode 100644 index 00000000..25a9ca14 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "ackermann_mppi/critic_function.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +namespace mppi::critics +{ + +class ObstaclesCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + inline bool inCollision(float cost) const; + inline CollisionCost costAtPose(float x, float y, float theta); + inline float distanceToObstacle(const CollisionCost & cost); + float findCircumscribedCost(std::shared_ptr costmap); + +protected: + nav2_costmap_2d::FootprintCollisionChecker + collision_checker_{nullptr}; + + bool consider_footprint_{true}; + float collision_cost_{0}; + float inflation_scale_factor_{0}, inflation_radius_{0}; + float possible_collision_cost_; + float collision_margin_distance_; + float near_goal_distance_; + float circumscribed_cost_{0}, circumscribed_radius_{0}; + + unsigned int power_{0}; + float repulsion_weight_, critical_weight_{0}; + std::string inflation_layer_name_; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp new file mode 100644 index 00000000..db17564e --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023 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. + +#ifndef ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ + +#include "ackermann_mppi/critic_function.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +namespace mppi::critics +{ + +class PathAlignCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + size_t offset_from_furthest_{0}; + int trajectory_point_step_{0}; + float threshold_to_consider_{0}; + float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; + unsigned int power_{0}; + float weight_{0}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp new file mode 100644 index 00000000..ef5485b7 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp @@ -0,0 +1,40 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ + +#include "ackermann_mppi/critic_function.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +namespace mppi::critics +{ + +class PathFollowCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + float threshold_to_consider_{0}; + size_t offset_from_furthest_{0}; + unsigned int power_{0}; + float weight_{0}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp new file mode 100644 index 00000000..8f54c8d6 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ +#define ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ + +namespace mppi::models +{ + +/** + * @struct mppi::models::ControlConstraints + * @brief Constraints on control (Ackermann: no vy/ay terms) + */ +struct ControlConstraints +{ + float vx_max; + float vx_min; + float wz; + float ax_max; + float ax_min; + float az_max; +}; + +/** + * @struct mppi::models::SamplingStd + * @brief Noise standard deviations for MPPI sampling (Ackermann: vx and wz only) + */ +struct SamplingStd +{ + float vx; + float wz; +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp new file mode 100644 index 00000000..b4781787 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ +#define ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ + +#include + +namespace mppi::models +{ + +/** + * @struct mppi::models::Control + * @brief A single control step (Ackermann: vx and wz only) + */ +struct Control +{ + float vx, wz; +}; + +/** + * @struct mppi::models::ControlSequence + * @brief A control sequence over time (Ackermann: vx and wz only) + */ +struct ControlSequence +{ + Eigen::ArrayXf vx; + Eigen::ArrayXf wz; + + void reset(unsigned int time_steps) + { + vx.setZero(time_steps); + wz.setZero(time_steps); + } +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp new file mode 100644 index 00000000..af0ad621 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ +#define ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ + +#include +#include "ackermann_mppi/models/constraints.hpp" + +namespace mppi::models +{ + +/** + * @struct mppi::models::OptimizerSettings + * @brief Settings for the optimizer to use + */ +struct OptimizerSettings +{ + models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; + models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; + models::SamplingStd sampling_std{0.0f, 0.0f}; + float model_dt{0.0f}; + float temperature{0.0f}; + float gamma{0.0f}; + unsigned int batch_size{0u}; + unsigned int time_steps{0u}; + unsigned int iteration_count{0u}; + bool shift_control_sequence{false}; + size_t retry_attempt_limit{0}; + bool open_loop{false}; + bool visualize{false}; +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp new file mode 100644 index 00000000..a9171d78 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__PATH_HPP_ +#define ACKERMANN_MPPI__MODELS__PATH_HPP_ + +#include + +namespace mppi::models +{ + +/** + * @struct mppi::models::Path + * @brief Path represented as a tensor + */ +struct Path +{ + Eigen::ArrayXf x; + Eigen::ArrayXf y; + Eigen::ArrayXf yaws; + + /** + * @brief Reset path data + */ + void reset(unsigned int size) + { + x.setZero(size); + y.setZero(size); + yaws.setZero(size); + } +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__PATH_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp new file mode 100644 index 00000000..cb5018f4 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__STATE_HPP_ +#define ACKERMANN_MPPI__MODELS__STATE_HPP_ + +#include + +#include +#include + + +namespace mppi::models +{ + +/** + * @struct mppi::models::State + * @brief State information: velocities, controls, poses, speed + */ +struct State +{ + // Propagated velocities: [batch_size x time_steps] + Eigen::ArrayXXf vx; // longitudinal velocity + Eigen::ArrayXXf wz; // angular velocity (vy is always zero for Ackermann) + + // Noised controls: [batch_size x time_steps] + Eigen::ArrayXXf cvx; + Eigen::ArrayXXf cwz; + + geometry_msgs::msg::PoseStamped pose; + geometry_msgs::msg::Twist speed; + float local_path_length; + + void reset(unsigned int batch_size, unsigned int time_steps) + { + vx.setZero(batch_size, time_steps); + wz.setZero(batch_size, time_steps); + cvx.setZero(batch_size, time_steps); + cwz.setZero(batch_size, time_steps); + } +}; +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__STATE_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp new file mode 100644 index 00000000..8284872a --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ +#define ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ + +#include + +namespace mppi::models +{ + +/** + * @class mppi::models::Trajectories + * @brief Candidate Trajectories + */ +struct Trajectories +{ + Eigen::ArrayXXf x; + Eigen::ArrayXXf y; + Eigen::ArrayXXf yaws; + + /** + * @brief Reset state data + */ + void reset(unsigned int batch_size, unsigned int time_steps) + { + x.setZero(batch_size, time_steps); + y.setZero(batch_size, time_steps); + yaws.setZero(batch_size, time_steps); + } +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp new file mode 100644 index 00000000..87487587 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp @@ -0,0 +1,133 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2025 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. + +#ifndef ACKERMANN_MPPI__MOTION_MODELS_HPP_ +#define ACKERMANN_MPPI__MOTION_MODELS_HPP_ + +#include +#include +#include + +#include "ackermann_mppi/models/control_sequence.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/models/constraints.hpp" + +namespace mppi +{ + +// Forward declaration used in utils.hpp to avoid circular include +namespace utils +{ +float clamp(const float lower_bound, const float upper_bound, const float input); +} + +/** + * @class mppi::MotionModel + * @brief Abstract motion model base class + */ +class MotionModel +{ +public: + MotionModel() = default; + virtual ~MotionModel() = default; + + void initialize(const models::ControlConstraints & control_constraints, float model_dt) + { + control_constraints_ = control_constraints; + model_dt_ = model_dt; + } + + /** + * @brief Propagate velocities forward one step using acceleration constraints. + * Batch operation: state matrices are [batch_size x time_steps]. + */ + virtual void predict(models::State & state) + { + float max_delta_vx = model_dt_ * control_constraints_.ax_max; + float min_delta_vx = model_dt_ * control_constraints_.ax_min; + float max_delta_wz = model_dt_ * control_constraints_.az_max; + + unsigned int n_cols = state.vx.cols(); + for (unsigned int i = 1; i < n_cols; i++) { + auto lower_bound_vx = (state.vx.col(i - 1) > 0).select( + state.vx.col(i - 1) + min_delta_vx, + state.vx.col(i - 1) - max_delta_vx); + auto upper_bound_vx = (state.vx.col(i - 1) > 0).select( + state.vx.col(i - 1) + max_delta_vx, + state.vx.col(i - 1) - min_delta_vx); + + state.cvx.col(i - 1) = state.cvx.col(i - 1) + .cwiseMax(lower_bound_vx) + .cwiseMin(upper_bound_vx); + state.vx.col(i) = state.cvx.col(i - 1); + + state.cwz.col(i - 1) = state.cwz.col(i - 1) + .cwiseMax(state.wz.col(i - 1) - max_delta_wz) + .cwiseMin(state.wz.col(i - 1) + max_delta_wz); + state.wz.col(i) = state.cwz.col(i - 1); + } + } + + virtual bool isHolonomic() = 0; + + /** + * @brief Apply model-specific hard constraints to the optimal control sequence. + * Called after softmax weighting, before returning the command. + */ + virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {} + + /** + * @brief Return the currently active constraints (may differ from base if speed limit is set). + * Critics should read from this instead of caching their own copies. + */ + const models::ControlConstraints & getConstraints() const { return control_constraints_; } + +protected: + float model_dt_{0.0}; + models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; +}; + +/** + * @class mppi::AckermannMotionModel + * @brief Ackermann steering motion model. + * + * Key constraint: angular velocity is bounded by |wz| <= |vx| / min_turning_r + * This prevents commanding steering angles that are physically impossible at a given speed. + */ +class AckermannMotionModel : public MotionModel +{ +public: + explicit AckermannMotionModel(float min_turning_r = 0.2f) + : min_turning_r_(min_turning_r) {} + + bool isHolonomic() override { return false; } + + void applyConstraints(models::ControlSequence & control_sequence) override + { + const auto wz_constrained = control_sequence.vx.abs() / min_turning_r_; + control_sequence.wz = control_sequence.wz + .max(-wz_constrained) + .min(wz_constrained); + } + + float getMinTurningRadius() const { return min_turning_r_; } + +private: + float min_turning_r_{0.2f}; +}; + +} // namespace mppi + +#endif // ACKERMANN_MPPI__MOTION_MODELS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp new file mode 100644 index 00000000..bfcd2e41 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp @@ -0,0 +1,238 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2025 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. + +#ifndef ACKERMANN_MPPI__OPTIMIZER_HPP_ +#define ACKERMANN_MPPI__OPTIMIZER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "tf2_ros/buffer.hpp" + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" + +#include "ackermann_mppi/models/optimizer_settings.hpp" +#include "ackermann_mppi/motion_models.hpp" +#include "ackermann_mppi/critic_data.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/models/trajectories.hpp" +#include "ackermann_mppi/models/path.hpp" +#include "ackermann_mppi/tools/noise_generator.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +// Forward declarations for critic types +namespace mppi::critics +{ +class CriticFunction; +class PathFollowCritic; +class PathAlignCritic; +class GoalCritic; +class ObstaclesCritic; +class ConstraintCritic; +} + +namespace mppi +{ + +/** + * @class mppi::Optimizer + * @brief Ackermann-specific MPPI optimizer. + * + * Differences from the nav2_mppi_controller Optimizer: + * - Initialized with rclcpp::Node::SharedPtr (not LifecycleNode) + * - Critics are created directly (no pluginlib) + * - No OptimalTrajectoryValidator — simple fallback on fail_flag + * - Motion model hardcoded to AckermannMotionModel + * - GoalChecker replaced with a distance threshold parameter + * - Parameters read directly via node->declare_parameter / node->get_parameter + */ +class Optimizer +{ +public: + // Both constructor and destructor defined in optimizer.cpp so critic forward declarations are sufficient here + Optimizer(); + ~Optimizer(); + + /** + * @brief Initialize the optimizer. + * @param node ROS2 node for parameters, logging, and clock + * @param name Parameter namespace prefix (e.g. "mppi") + * @param costmap_ros Costmap for obstacle avoidance critics + * @param tf_buffer TF buffer for robot pose lookups + */ + void initialize( + rclcpp::Node::SharedPtr node, + const std::string & name, + std::shared_ptr costmap_ros, + std::shared_ptr tf_buffer); + + void shutdown(); + + /** + * @brief Run MPPI to compute the next control command. + * @param robot_pose Current robot pose (in costmap frame) + * @param robot_speed Current robot velocity (from odometry) + * @param plan Global path to follow + * @param goal Goal pose (last pose of plan, or explicit goal) + * @return [TwistStamped command, optimal trajectory as [time_steps x 3] (x, y, yaw)] + * @throws std::runtime_error if all trajectories are in collision after retries + */ + std::tuple evalControl( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal); + + // --- Accessors for visualization / debugging --- + + models::Trajectories & getGeneratedTrajectories() { return generated_trajectories_; } + Eigen::ArrayXXf getOptimizedTrajectory(); + const models::ControlSequence & getOptimalControlSequence() { return control_sequence_; } + const Eigen::ArrayXf & getCosts() const { return costs_; } + + /** + * @brief Per-critic cost breakdown from last evalControl call. + * Each entry is (critic_name, cost_delta_array[batch_size]). + */ + const std::vector> & getCriticCosts() const + { + return critic_costs_; + } + + const std::vector & getCollisionFlags() const + { + return critics_data_.trajectories_in_collision; + } + + /** + * @brief Scale max speeds by a ratio (e.g. from a speed zone costmap filter). + * @param speed_limit Absolute speed OR percentage of max speed + * @param percentage True if speed_limit is a percentage [0–100] + */ + void setSpeedLimit(double speed_limit, bool percentage); + + void reset(bool reset_dynamic_speed_limits = true); + + bool isSpeedLimitActive() const; + + const models::OptimizerSettings & getSettings() const { return settings_; } + +protected: + void optimize(); + void evalTrajectoriesScores(); + + void prepare( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal); + + void getParams(); + void loadCritics(); + + void shiftControlSequence(); + void generateNoisedTrajectories(); + void applyControlSequenceConstraints(); + void updateStateVelocities(models::State & state) const; + void updateInitialStateVelocities(models::State & state) const; + void propagateStateVelocitiesFromInitials(models::State & state) const; + + void integrateStateVelocities( + models::Trajectories & trajectories, + const models::State & state) const; + + void integrateStateVelocities( + Eigen::Array & trajectory, + const Eigen::ArrayXXf & sequence) const; + + void updateControlSequence(); + + geometry_msgs::msg::TwistStamped + getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time & stamp); + + bool fallback(bool fail); + + size_t fallback_counter_{0}; // member — not static, so reset() clears it correctly + + template + void declareParam(const std::string & full_name, T default_value) + { + if (!node_->has_parameter(full_name)) { + node_->declare_parameter(full_name, default_value); + } + } + + auto getParamGetter(const std::string & ns) + { + return [this, ns](auto & setting, const std::string & name, auto default_value) { + std::string full_name = ns.empty() ? name : ns + "." + name; + declareParam(full_name, default_value); + node_->get_parameter(full_name, setting); + }; + } + +protected: + rclcpp::Node::SharedPtr node_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_{nullptr}; + std::string name_; + std::string base_frame_; + std::shared_ptr tf_buffer_; + + std::shared_ptr motion_model_; + + // Critics owned directly (no pluginlib) + std::vector> critics_; + std::vector> critic_costs_; + + NoiseGenerator noise_generator_; + + models::OptimizerSettings settings_; + float goal_dist_tolerance_{0.25f}; + + models::State state_; + models::ControlSequence control_sequence_; + std::array control_history_; + models::Trajectories generated_trajectories_; + models::Path path_; + geometry_msgs::msg::Pose goal_; + Eigen::ArrayXf costs_; + + CriticData critics_data_{ + state_, + generated_trajectories_, + path_, + goal_, + costs_, + settings_.model_dt, + goal_dist_tolerance_}; + + rclcpp::Logger logger_{rclcpp::get_logger("AckermannMPPI")}; + geometry_msgs::msg::Twist last_command_vel_; +}; + +} // namespace mppi + +#endif // ACKERMANN_MPPI__OPTIMIZER_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp new file mode 100644 index 00000000..c4b69720 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ +#define ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "ackermann_mppi/models/optimizer_settings.hpp" +#include "ackermann_mppi/models/control_sequence.hpp" +#include "ackermann_mppi/models/state.hpp" + +namespace mppi +{ + +/** + * @class mppi::NoiseGenerator + * @brief Generates Gaussian-perturbed control sequences for MPPI sampling. + * + * When regenerate_noises=true (set via optimizer param), noise generation runs in a + * background thread that pre-generates noise for the *next* iteration while the + * current one scores trajectories. This hides latency on slower hardware. + * + * When regenerate_noises=false (default), noise is generated synchronously each call. + */ +class NoiseGenerator +{ +public: + NoiseGenerator() = default; + + /** + * @brief Initialize noise generator. + * @param settings Optimizer settings (batch_size, time_steps, sampling_std) + * @param regenerate_noises If true, run noise generation in background thread + */ + void initialize( + mppi::models::OptimizerSettings & settings, + bool regenerate_noises = false); + + void shutdown(); + + /** + * @brief Signal the background thread to generate next iteration's noises. + * No-op when regenerate_noises=false. + */ + void generateNextNoises(); + + /** + * @brief Apply noises to the current optimal control sequence to get noised controls. + * Stores in state.cvx / state.cwz for use by the motion model predict(). + */ + void setNoisedControls( + models::State & state, + const models::ControlSequence & control_sequence); + + void reset(mppi::models::OptimizerSettings & settings); + +protected: + void noiseThread(); + void generateNoisedControls(); + + Eigen::ArrayXXf noises_vx_; + Eigen::ArrayXXf noises_wz_; + + std::default_random_engine generator_; + std::normal_distribution ndistribution_vx_; + std::normal_distribution ndistribution_wz_; + + mppi::models::OptimizerSettings settings_; + bool regenerate_noises_{false}; + + std::thread noise_thread_; + std::condition_variable noise_cond_; + std::mutex noise_lock_; + bool active_{false}, ready_{false}; +}; + +} // namespace mppi + +#endif // ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp new file mode 100644 index 00000000..f499ddea --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp @@ -0,0 +1,387 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#ifndef ACKERMANN_MPPI__TOOLS__UTILS_HPP_ +#define ACKERMANN_MPPI__TOOLS__UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "std_msgs/msg/color_rgba.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include "ackermann_mppi/models/optimizer_settings.hpp" +#include "ackermann_mppi/models/control_sequence.hpp" +#include "ackermann_mppi/models/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "ackermann_mppi/critic_data.hpp" + +namespace mppi::utils +{ + +// Use constexpr instead of #define to avoid macro pollution and enable type checking. +inline constexpr float M_PIF = 3.141592653589793238462643383279502884e+00F; +inline constexpr float M_PIF_2 = 1.5707963267948966e+00F; + +inline geometry_msgs::msg::Pose createPose(double x, double y, double z) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + pose.orientation.w = 1; + pose.orientation.x = 0; + pose.orientation.y = 0; + pose.orientation.z = 0; + return pose; +} + +inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +inline visualization_msgs::msg::Marker createMarker( + int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns) +{ + using visualization_msgs::msg::Marker; + Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = ns; + marker.id = id; + marker.type = Marker::SPHERE; + marker.action = Marker::ADD; + marker.pose = pose; + marker.scale = scale; + marker.color = color; + return marker; +} + +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) +{ + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = frame; + twist.header.stamp = stamp; + twist.twist.linear.x = vx; + twist.twist.angular.z = wz; + return twist; +} + +/** + * @brief Convert path to a tensor of (x, y, yaw) arrays + */ +inline models::Path toTensor(const nav_msgs::msg::Path & path) +{ + auto result = models::Path{}; + result.reset(path.poses.size()); + for (size_t i = 0; i < path.poses.size(); ++i) { + result.x(i) = path.poses[i].pose.position.x; + result.y(i) = path.poses[i].pose.position.y; + result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation); + } + return result; +} + +/** + * @brief Get the last pose in the path tensor + */ +inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) +{ + const unsigned int path_last_idx = path.x.size() - 1; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, path.yaws(path_last_idx)); + geometry_msgs::msg::Pose p; + p.position.x = path.x(path_last_idx); + p.position.y = path.y(path_last_idx); + p.orientation = tf2::toMsg(q); + return p; +} + +template +auto normalize_angles(const T & angles) +{ + return (angles + M_PIF).unaryExpr( + [&](const float x) { + float remainder = std::fmod(x, 2.0f * M_PIF); + return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; + }); +} + +template +auto shortest_angular_distance(const F & from, const T & to) +{ + return normalize_angles(to - from); +} + +/** + * @brief Find the path point index furthest along the path that any trajectory in the batch + * reaches (by closest-point mapping). Used by PathFollow and PathAlign critics. + * + * Vectorized: outer loop iterates over path points (n_path), inner computation is a + * batch-wide Eigen array op over all trajectories simultaneously. This is significantly + * faster than the naive scalar O(n_batch × n_path) nested loop because each iteration + * of the outer loop operates on a [batch_size] SIMD-vectorizable array. + */ +inline size_t findPathFurthestReachedPoint(const CriticData & data) +{ + const int last_col = data.trajectories.x.cols() - 1; + // End-positions of all trajectories: [batch_size] arrays. + const auto traj_x = data.trajectories.x.col(last_col); + const auto traj_y = data.trajectories.y.col(last_col); + + const Eigen::Index n_batch = traj_x.rows(); + const Eigen::Index n_path = static_cast(data.path.x.size()); + + // Per-trajectory: closest path index found so far and its squared distance. + Eigen::ArrayXf min_dist2 = Eigen::ArrayXf::Constant(n_batch, std::numeric_limits::max()); + Eigen::ArrayXi best_idx = Eigen::ArrayXi::Zero(n_batch); + + for (Eigen::Index j = 0; j != n_path; ++j) { + // Vectorized over all batch_size trajectories at once. + const Eigen::ArrayXf dx = traj_x - data.path.x(j); + const Eigen::ArrayXf dy = traj_y - data.path.y(j); + const Eigen::ArrayXf d2 = dx * dx + dy * dy; + + const auto better = (d2 < min_dist2).eval(); + min_dist2 = better.select(d2, min_dist2); + best_idx = better.select(Eigen::ArrayXi::Constant(n_batch, static_cast(j)), best_idx); + + // Early exit: already found a trajectory that reaches the last path point. + if (best_idx.maxCoeff() == static_cast(n_path) - 1) { + break; + } + } + + return static_cast(best_idx.maxCoeff()); +} + +inline void setPathFurthestPointIfNotSet(CriticData & data) +{ + if (!data.furthest_reached_path_point) { + data.furthest_reached_path_point = findPathFurthestReachedPoint(data); + } +} + +/** + * @brief Check which path points are collision-free according to the costmap. + * Stores results in data.path_pts_valid for reuse across critics. + */ +inline void findPathCosts( + CriticData & data, + std::shared_ptr costmap_ros) +{ + auto * costmap = costmap_ros->getCostmap(); + unsigned int map_x, map_y; + const size_t path_segments_count = data.path.x.size() - 1; + data.path_pts_valid = std::vector(path_segments_count, false); + const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); + + for (unsigned int idx = 0; idx < path_segments_count; idx++) { + if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) { + (*data.path_pts_valid)[idx] = false; + continue; + } + switch (costmap->getCost(map_x, map_y)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::NO_INFORMATION): + (*data.path_pts_valid)[idx] = tracking_unknown ? true : false; + continue; + } + (*data.path_pts_valid)[idx] = true; + } +} + +inline void setPathCostsIfNotSet( + CriticData & data, + std::shared_ptr costmap_ros) +{ + if (!data.path_pts_valid) { + findPathCosts(data, costmap_ros); + } +} + +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) +{ + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); + float yaw = atan2f(point_y - pose_y, point_x - pose_x); + + if (!forward_preference) { + return std::min( + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); + } + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief Apply Savitzky-Golay smoothing filter to the optimal control sequence. + * Uses a 9-point quadratic filter to smooth sharp changes between control steps. + */ +inline void savitskyGolayFilter( + models::ControlSequence & control_sequence, + std::array & control_history, + const models::OptimizerSettings & settings) +{ + // Standard 9-point quadratic Savitzky-Golay smoothing coefficients (window = ±4). + // Denominator 231 is the normalization factor for this window/polynomial order. + // Reference: Savitzky & Golay, Analytical Chemistry, 1964, Table I (m=4, n=2). + Eigen::Array filter = { + -21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, -21.0f}; + filter /= 231.0f; + + const unsigned int num_sequences = control_sequence.vx.size() - 1; + // Skip smoothing for very short horizons: the 9-point window needs at least + // 5 look-ahead points (indices 0..4), so sequences shorter than 20 provide + // too little context for meaningful smoothing. + if (num_sequences < 20) {return;} + + auto applyFilter = [&](const Eigen::Array & data) -> float { + return (data * filter).eval().sum(); + }; + + auto applyFilterOverAxis = + [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence, + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + { + float pt_m4 = hist_0, pt_m3 = hist_1, pt_m2 = hist_2, pt_m1 = hist_3; + float pt = initial_sequence(0); + float pt_p1 = initial_sequence(1), pt_p2 = initial_sequence(2); + float pt_p3 = initial_sequence(3), pt_p4 = initial_sequence(4); + + for (unsigned int idx = 0; idx != num_sequences; idx++) { + sequence(idx) = applyFilter( + {pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); + pt_m4 = pt_m3; pt_m3 = pt_m2; pt_m2 = pt_m1; pt_m1 = pt; + pt = pt_p1; pt_p1 = pt_p2; pt_p2 = pt_p3; pt_p3 = pt_p4; + pt_p4 = (idx + 5 < num_sequences) ? + initial_sequence(idx + 5) : initial_sequence(num_sequences); + } + }; + + const models::ControlSequence initial_control_sequence = control_sequence; + applyFilterOverAxis( + control_sequence.vx, initial_control_sequence.vx, + control_history[0].vx, control_history[1].vx, + control_history[2].vx, control_history[3].vx); + applyFilterOverAxis( + control_sequence.wz, initial_control_sequence.wz, + control_history[0].wz, control_history[1].wz, + control_history[2].wz, control_history[3].wz); + + unsigned int offset = settings.shift_control_sequence ? 1 : 0; + control_history[0] = control_history[1]; + control_history[1] = control_history[2]; + control_history[2] = control_history[3]; + control_history[3] = {control_sequence.vx(offset), control_sequence.wz(offset)}; +} + +inline unsigned int findClosestPathPt( + const std::vector & vec, const float dist, const unsigned int init = 0u) +{ + float distim1 = init != 0u ? vec[init] : 0.0f; + float disti = 0.0f; + const unsigned int size = vec.size(); + for (unsigned int i = init + 1; i != size; i++) { + disti = vec[i]; + if (disti > dist) { + if (i > 0 && dist - distim1 < disti - dist) {return i - 1;} + return i; + } + distim1 = disti; + } + return size - 1; +} + +struct Pose2D { float x, y, theta; }; + +/** + * @brief Shift columns of an Eigen Array left (-1) or right (1) by one place. + * Used in trajectory integration and control sequence shifting. + */ +inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) +{ + int size = e.size(); + if (size == 1) {return;} + if (abs(direction) != 1) { + throw std::logic_error("Invalid direction, only 1 and -1 are valid values."); + } + if ((e.cols() == 1 || e.rows() == 1) && size > 1) { + auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; + auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; + while (start_ptr != end_ptr) { + *(start_ptr + direction) = *start_ptr; + start_ptr -= direction; + } + *(start_ptr + direction) = *start_ptr; + } else { + auto start_ptr = direction == 1 ? + e.data() + size - 2 * e.rows() : e.data() + e.rows(); + auto end_ptr = direction == 1 ? + e.data() : e.data() + size - e.rows(); + auto span = e.rows(); + while (start_ptr != end_ptr) { + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + start_ptr -= (direction * span); + } + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + } +} + +inline float clamp(const float lower_bound, const float upper_bound, const float input) +{ + return std::min(upper_bound, std::max(input, lower_bound)); +} + +} // namespace mppi::utils + +#endif // ACKERMANN_MPPI__TOOLS__UTILS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/package.xml b/src/subsystems/minimal_navigation/ackermann_mppi/package.xml new file mode 100644 index 00000000..cf682381 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/package.xml @@ -0,0 +1,36 @@ + + + + ackermann_mppi + 0.1.0 + + Standalone MPPI controller for Ackermann steering robots. + A minimal extraction of nav2_mppi_controller with the nav2 plugin + scaffolding removed — no pluginlib, no LifecycleNode dependency, + no GoalChecker, no OptimalTrajectoryValidator. + Runs as a plain rclcpp::Node. + + UMD Loop + Apache-2.0 + + ament_cmake + + angles + builtin_interfaces + geometry_msgs + nav2_costmap_2d + nav_msgs + rclcpp + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + + eigen3_cmake_module + Eigen3 + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp new file mode 100644 index 00000000..730e1081 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp @@ -0,0 +1,301 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/bool.hpp" + +#include "ackermann_mppi/optimizer.hpp" + +/** + * @class AckermannMPPINode + * @brief Standalone ROS2 node that runs MPPI for an Ackermann steering vehicle. + * + * Inputs: + * /global_path (nav_msgs/msg/Path) — global path to follow + * /odom (nav_msgs/msg/Odometry) — robot velocity feedback + * + * Outputs: + * /cmd_vel (geometry_msgs/msg/Twist) — velocity command + * + * The node also owns a Costmap2DROS instance (subscribes to sensor topics + * independently). Configure it via the standard nav2 costmap parameters. + * + * Key parameters (under the node's namespace): + * controller_frequency (double, default 10.0) — control loop rate in Hz + * plan_timeout (double, default 2.0) — seconds before stale plan triggers stop + * mppi.model_dt (float, default 0.05) — must equal 1/controller_frequency + * mppi.batch_size (int, default 1000) + * mppi.time_steps (int, default 56) + * mppi.vx_max (float, default 0.5) + * mppi.vx_min (float, default -0.35) + * mppi.wz_max (float, default 1.9) + * mppi.AckermannConstraints.min_turning_r (float, default 0.2) + * ... (see optimizer.cpp getParams() for full list) + */ +class AckermannMPPINode : public rclcpp::Node +{ +public: + explicit AckermannMPPINode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("ackermann_mppi", options) + { + // Declare parameters in constructor so they're available before configure() + declare_parameter("controller_frequency", 10.0); + declare_parameter("plan_timeout", 2.0); + declare_parameter("odom_topic", std::string("/odom")); + declare_parameter("plan_topic", std::string("/global_path")); + declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); + declare_parameter("cmd_vel_topic", std::string("/cmd_vel")); + declare_parameter("robot_base_frame", std::string("base_link")); + declare_parameter("global_frame", std::string("map")); + } + + /** + * @brief Finish setup after construction (requires shared_from_this()). + * Call this immediately after make_shared(). + */ + void configure() + { + RCLCPP_INFO(get_logger(), "AckermannMPPINode configuring..."); + + // Cache frame names — these never change at runtime. + get_parameter("global_frame", global_frame_); + get_parameter("robot_base_frame", base_frame_); + get_parameter("plan_timeout", plan_timeout_s_); + + // TF buffer and listener + tf_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_); + + // Costmap2DROS manages its own ROS2 lifecycle (sensors, inflation layers, etc). + // It needs to be spun in the same executor as this node. + // Parameters are read from the "local_costmap" namespace (standard nav2 costmap params). + costmap_ros_ = std::make_shared( + "local_costmap", + get_namespace(), // parent namespace + "local_costmap" // local namespace + ); + // Transfer the TF buffer so the costmap shares our TF instance + costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", get_parameter("use_sim_time").as_bool())); + costmap_ros_->configure(); + costmap_ros_->activate(); + + optimizer_.initialize(shared_from_this(), "mppi", costmap_ros_, tf_); + + double controller_frequency; + get_parameter("controller_frequency", controller_frequency); + + // Subscriptions + std::string odom_topic, plan_topic, nav_enabled_topic; + get_parameter("odom_topic", odom_topic); + get_parameter("plan_topic", plan_topic); + get_parameter("nav_enabled_topic", nav_enabled_topic); + + // NOTE: both callbacks write shared state that is read by the control timer. + // The mutexes (plan_mutex_, odom_mutex_) below protect against the data race + // in the MultiThreadedExecutor where these run in parallel with controlLoop(). + odom_sub_ = create_subscription( + odom_topic, rclcpp::SensorDataQoS(), + [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(odom_mutex_); + last_odom_ = *msg; + }); + + plan_sub_ = create_subscription( + plan_topic, rclcpp::QoS(1).transient_local(), + [this](const nav_msgs::msg::Path::SharedPtr msg) { + if (!msg->poses.empty()) { + std::lock_guard lock(plan_mutex_); + current_plan_ = *msg; + last_plan_time_ = now(); + } + }); + + nav_enabled_sub_ = create_subscription( + nav_enabled_topic, 1, + [this](const std_msgs::msg::Bool::SharedPtr msg) { + nav_enabled_.store(msg->data); + }); + + // Publisher + std::string cmd_vel_topic; + get_parameter("cmd_vel_topic", cmd_vel_topic); + cmd_vel_pub_ = create_publisher(cmd_vel_topic, 1); + + // Control loop timer + const auto period = std::chrono::duration(1.0 / controller_frequency); + control_timer_ = create_wall_timer(period, [this]() { controlLoop(); }); + + RCLCPP_INFO( + get_logger(), + "AckermannMPPINode configured. Control loop: %.1f Hz, plan timeout: %.1f s", + controller_frequency, plan_timeout_s_); + } + + std::shared_ptr getCostmapROS() + { + return costmap_ros_; + } + + void deactivate() + { + control_timer_->cancel(); + optimizer_.shutdown(); + costmap_ros_->deactivate(); + costmap_ros_->cleanup(); + } + +private: + void controlLoop() + { + if (!nav_enabled_.load()) { + RCLCPP_DEBUG(get_logger(), "Navigation disabled — sending zero velocity."); + cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); + return; + } + + // --- Snapshot shared state under locks --- + nav_msgs::msg::Path plan; + nav_msgs::msg::Odometry odom; + rclcpp::Time plan_time; + { + std::lock_guard lock(plan_mutex_); + if (current_plan_.poses.empty()) { + return; // No plan received yet + } + plan = current_plan_; + plan_time = last_plan_time_; + } + { + std::lock_guard lock(odom_mutex_); + odom = last_odom_; + } + + // --- Staleness check --- + const double plan_age = (now() - plan_time).seconds(); + if (plan_age > plan_timeout_s_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, + "Plan is stale (%.1f s old, timeout %.1f s) — sending zero velocity.", + plan_age, plan_timeout_s_); + cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); + optimizer_.reset(); + return; + } + + // --- Look up robot pose in the global frame --- + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = global_frame_; + robot_pose.header.stamp = now(); + + try { + auto transform = tf_->lookupTransform( + global_frame_, base_frame_, + tf2::TimePointZero); + robot_pose.pose.position.x = transform.transform.translation.x; + robot_pose.pose.position.y = transform.transform.translation.y; + robot_pose.pose.position.z = transform.transform.translation.z; + robot_pose.pose.orientation = transform.transform.rotation; + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, + "Could not get robot pose: %s", ex.what()); + return; + } + + // Robot speed from odometry + geometry_msgs::msg::Twist robot_speed; + robot_speed.linear.x = odom.twist.twist.linear.x; + robot_speed.linear.y = odom.twist.twist.linear.y; + robot_speed.angular.z = odom.twist.twist.angular.z; + + // Use the last pose of the plan as the goal + const auto & goal = plan.poses.back().pose; + + try { + auto [cmd, optimal_traj] = optimizer_.evalControl( + robot_pose, robot_speed, plan, goal); + cmd_vel_pub_->publish(cmd.twist); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR(get_logger(), "MPPI failed: %s — sending zero velocity.", ex.what()); + cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); + optimizer_.reset(); + } + } + + // TF + std::shared_ptr tf_; + std::shared_ptr tf_listener_; + + // Costmap (sensor integration + inflation) + std::shared_ptr costmap_ros_; + + // MPPI optimizer + mppi::Optimizer optimizer_; + + // ROS interfaces + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr plan_sub_; + rclcpp::Subscription::SharedPtr nav_enabled_sub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::TimerBase::SharedPtr control_timer_; + + std::atomic nav_enabled_{true}; + + // Shared state — protected by corresponding mutexes. + // Written by subscription callbacks, read by control timer; all run in a + // MultiThreadedExecutor so concurrent access is real. + std::mutex plan_mutex_; + std::mutex odom_mutex_; + nav_msgs::msg::Path current_plan_; + nav_msgs::msg::Odometry last_odom_; + rclcpp::Time last_plan_time_{0, 0, RCL_ROS_TIME}; + + // Cached parameters (never change after configure()) + std::string global_frame_{"map"}; + std::string base_frame_{"base_link"}; + double plan_timeout_s_{2.0}; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + node->configure(); + + // Use a multi-threaded executor so the Costmap2DROS lifecycle node + // (which has its own callbacks for sensor data) runs alongside the control loop. + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.add_node(node->getCostmapROS()->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp new file mode 100644 index 00000000..3b985feb --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp @@ -0,0 +1,65 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "ackermann_mppi/critics/constraint_critic.hpp" + +namespace mppi::critics +{ + +void ConstraintCritic::initialize() +{ + auto getParam = getParamGetter(name_); + auto getParentParam = getParamGetter(parent_name_); + + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 4.0f); + + // min_turning_r is vehicle geometry — fixed at init. + // vx_min/vx_max are read live from data.motion_model in score() so speed limits take effect. + getParentParam(min_turning_r_, "AckermannConstraints.min_turning_r", 0.2f); + + RCLCPP_INFO( + logger_, "ConstraintCritic (Ackermann) instantiated with %d power and %f weight.", + power_, weight_); +} + +void ConstraintCritic::score(CriticData & data) +{ + if (!enabled_) {return;} + + // Read velocity limits live so that setSpeedLimit() changes take effect immediately. + const auto & c = data.motion_model->getConstraints(); + const float max_vel = c.vx_max; + // Preserve original sign convention: vx_min is typically negative for reverse. + const float min_vel = (c.vx_min > 0.0f ? 1.0f : -1.0f) * std::abs(c.vx_min); + + auto & vx = data.state.vx; + auto & wz = data.state.wz; + + // Penalize steering ratio violations: |wz| <= |vx| / min_turning_r + constexpr float kEpsilon = 1e-6f; + auto wz_safe = wz.abs().max(kEpsilon); + auto out_of_turning_rad_motion = (min_turning_r_ - (vx.abs() / wz_safe)).max(0.0f); + + if (power_ > 1u) { + data.costs += ((((vx - max_vel).max(0.0f) + (min_vel - vx).max(0.0f) + + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * + weight_).pow(power_).eval(); + } else { + data.costs += ((((vx - max_vel).max(0.0f) + (min_vel - vx).max(0.0f) + + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_).eval(); + } +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp new file mode 100644 index 00000000..27c9baa7 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp @@ -0,0 +1,55 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "ackermann_mppi/critics/goal_critic.hpp" + +namespace mppi::critics +{ + +void GoalCritic::initialize() +{ + auto getParam = getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 5.0f); + getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); + + RCLCPP_INFO( + logger_, "GoalCritic instantiated with %d power and %f weight.", + power_, weight_); +} + +void GoalCritic::score(CriticData & data) +{ + // Only activate when near the goal to provide terminal cost + if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); + const auto goal_x = goal.position.x; + const auto goal_y = goal.position.y; + + const auto delta_x = data.trajectories.x - goal_x; + const auto delta_y = data.trajectories.y - goal_y; + + if (power_ > 1u) { + data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * + weight_).pow(power_); + } else { + data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * + weight_).eval(); + } +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp new file mode 100644 index 00000000..128204e2 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp @@ -0,0 +1,214 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#include +#include "ackermann_mppi/critics/obstacles_critic.hpp" + +namespace mppi::critics +{ + +void ObstaclesCritic::initialize() +{ + auto getParam = getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(repulsion_weight_, "repulsion_weight", 1.5f); + getParam(critical_weight_, "critical_weight", 20.0f); + getParam(collision_cost_, "collision_cost", 100000.0f); + getParam(collision_margin_distance_, "collision_margin_distance", 0.10f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + + collision_checker_.setCostmap(costmap_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + + if (possible_collision_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation radius is not set sufficiently for " + "non-circular collision checking. Set inflation_radius >= half of the robot's largest " + "cross-section. This will substantially impact run-time performance."); + } + + RCLCPP_INFO( + logger_, + "ObstaclesCritic instantiated with %d power and %f / %f weights. " + "Collision check based on %s cost.", + power_, critical_weight_, repulsion_weight_, + consider_footprint_ ? "footprint" : "circular"); +} + +float ObstaclesCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + return circumscribed_cost_; + } + + std::shared_ptr inflation_layer; + for (auto & layer : *costmap->getLayeredCostmap()->getPlugins()) { + auto candidate = std::dynamic_pointer_cast(layer); + if (candidate && + (inflation_layer_name_.empty() || candidate->getName() == inflation_layer_name_)) + { + inflation_layer = candidate; + break; + } + } + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + const std::string & ln = inflation_layer->getName(); + rclcpp::Parameter p; + if (costmap->get_parameter(ln + ".cost_scaling_factor", p)) { + inflation_scale_factor_ = static_cast(p.as_double()); + } + if (costmap->get_parameter(ln + ".inflation_radius", p)) { + inflation_radius_ = static_cast(p.as_double()); + } + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found. Cannot use costmap potential field for fast collision " + "pre-screening. Only absolute collisions will be detected."); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + return circumscribed_cost_; +} + +float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) +{ + const float scale_factor = inflation_scale_factor_; + const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + // nav2 inflation layer maps INSCRIBED_INFLATED_OBSTACLE (254) - 1 = 253 to the inscribed + // radius. We invert the exponential cost formula to recover metric distance. + constexpr float kNavInscribedCostThreshold = 253.0f; + float dist_to_obj = + (scale_factor * min_radius - logf(cost.cost) + logf(kNavInscribedCostThreshold)) / + scale_factor; + if (!cost.using_footprint) { + dist_to_obj -= min_radius; + } + return dist_to_obj; +} + +void ObstaclesCritic::score(CriticData & data) +{ + if (!enabled_) {return;} + + if (consider_footprint_) { + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + } + + bool near_goal = data.state.local_path_length < near_goal_distance_; + + Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size()); + Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size()); + + const unsigned int traj_len = data.trajectories.x.cols(); + const unsigned int batch_size = data.trajectories.x.rows(); + bool all_trajectories_collide = true; + + auto & collisions = data.trajectories_in_collision; + const bool track_collisions = !collisions.empty(); + + for (unsigned int i = 0; i != batch_size; i++) { + bool trajectory_collide = false; + float traj_cost = 0.0f; + const auto & traj = data.trajectories; + CollisionCost pose_cost; + + for (unsigned int j = 0; j != traj_len; j++) { + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); + if (pose_cost.cost < 1.0f) {continue;} + + if (inCollision(pose_cost.cost)) { + trajectory_collide = true; + break; + } + + if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {continue;} + + const float dist_to_obj = distanceToObstacle(pose_cost); + if (dist_to_obj < collision_margin_distance_) { + traj_cost += (collision_margin_distance_ - dist_to_obj); + } + if (!near_goal) { + repulsive_cost[i] += inflation_radius_ - dist_to_obj; + } + } + + if (!trajectory_collide) {all_trajectories_collide = false;} + raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost; + if (trajectory_collide && track_collisions) {collisions[i] = true;} + } + + auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len; + + if (power_ > 1u) { + data.costs += + ((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized)) + .pow(power_); + } else { + data.costs += + (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized); + } + + data.fail_flag = all_trajectories_collide; +} + +bool ObstaclesCritic::inCollision(float cost) const +{ + bool is_tracking_unknown = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + using namespace nav2_costmap_2d; // NOLINT + switch (static_cast(cost)) { + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint_ ? false : true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + return false; +} + +CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) +{ + CollisionCost collision_cost; + float & cost = collision_cost.cost; + collision_cost.using_footprint = false; + + unsigned int x_i, y_i; + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { + cost = nav2_costmap_2d::NO_INFORMATION; + return collision_cost; + } + cost = collision_checker_.pointCost(x_i, y_i); + + if (consider_footprint_ && + (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) + { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + collision_cost.using_footprint = true; + } + return collision_cost; +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp new file mode 100644 index 00000000..be4813a5 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp @@ -0,0 +1,141 @@ +// Copyright (c) 2023 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. + +#include "ackermann_mppi/critics/path_align_critic.hpp" + +namespace mppi::critics +{ + +void PathAlignCritic::initialize() +{ + auto getParam = getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 10.0f); + getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); + getParam(offset_from_furthest_, "offset_from_furthest", 20); + getParam(trajectory_point_step_, "trajectory_point_step", 4); + getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); + getParam(use_path_orientations_, "use_path_orientations", false); + + RCLCPP_INFO( + logger_, "PathAlignCritic instantiated with %d power and %f weight", + power_, weight_); +} + +void PathAlignCritic::score(CriticData & data) +{ + if (!enabled_ || data.state.local_path_length < threshold_to_consider_) {return;} + + utils::setPathFurthestPointIfNotSet(data); + const size_t path_segments_count = *data.furthest_reached_path_point; + float path_segments_flt = static_cast(path_segments_count); + if (path_segments_count < offset_from_furthest_) {return;} + + utils::setPathCostsIfNotSet(data, costmap_ros_); + std::vector & path_pts_valid = *data.path_pts_valid; + float invalid_ctr = 0.0f; + for (size_t i = 0; i < path_segments_count; i++) { + if (!path_pts_valid[i]) {invalid_ctr += 1.0f;} + if (invalid_ctr / path_segments_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) { + return; + } + } + + const size_t batch_size = data.trajectories.x.rows(); + Eigen::ArrayXf cost(data.costs.rows()); + cost.setZero(); + + std::vector path_integrated_distances(path_segments_count, 0.0f); + std::vector path(path_segments_count); + float dx = 0.0f, dy = 0.0f; + for (unsigned int i = 1; i != path_segments_count; i++) { + auto & pose = path[i - 1]; + pose.x = data.path.x(i - 1); + pose.y = data.path.y(i - 1); + pose.theta = data.path.yaws(i - 1); + dx = data.path.x(i) - pose.x; + dy = data.path.y(i) - pose.y; + path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy); + } + + auto & final_pose = path[path_segments_count - 1]; + final_pose.x = data.path.x(path_segments_count - 1); + final_pose.y = data.path.y(path_segments_count - 1); + final_pose.theta = data.path.yaws(path_segments_count - 1); + + int strided_traj_rows = data.trajectories.x.rows(); + int strided_traj_cols = + static_cast(floor((data.trajectories.x.cols() - 1) / trajectory_point_step_)) + 1; + int outer_stride = strided_traj_rows * trajectory_point_step_; + + const auto T_x = Eigen::Map>( + data.trajectories.x.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_y = Eigen::Map>( + data.trajectories.y.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_yaw = Eigen::Map>( + data.trajectories.yaws.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_sampled_size = T_x.cols(); + + float summed_path_dist = 0.0f, dyaw = 0.0f; + unsigned int num_samples = 0u, path_pt = 0u; + float traj_integrated_distance = 0.0f; + float Tx_m1, Ty_m1; + + for (size_t t = 0; t < batch_size; ++t) { + summed_path_dist = 0.0f; + num_samples = 0u; + traj_integrated_distance = 0.0f; + path_pt = 0u; + Tx_m1 = T_x(t, 0); + Ty_m1 = T_y(t, 0); + + for (int p = 1; p < traj_sampled_size; p++) { + const float Tx = T_x(t, p); + const float Ty = T_y(t, p); + dx = Tx - Tx_m1; + dy = Ty - Ty_m1; + Tx_m1 = Tx; + Ty_m1 = Ty; + traj_integrated_distance += sqrtf(dx * dx + dy * dy); + path_pt = utils::findClosestPathPt(path_integrated_distances, traj_integrated_distance, + path_pt); + + if (path_pts_valid[path_pt]) { + const auto & pose = path[path_pt]; + dx = pose.x - Tx; + dy = pose.y - Ty; + num_samples++; + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(pose.theta, T_yaw(t, p)); + summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); + } else { + summed_path_dist += sqrtf(dx * dx + dy * dy); + } + } + } + cost(t) = num_samples > 0u ? + summed_path_dist / static_cast(num_samples) : 0.0f; + } + + if (power_ > 1u) { + data.costs += (cost * weight_).pow(power_).eval(); + } else { + data.costs += (cost * weight_).eval(); + } +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp new file mode 100644 index 00000000..d6e1207e --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp @@ -0,0 +1,69 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "ackermann_mppi/critics/path_follow_critic.hpp" +#include + +namespace mppi::critics +{ + +void PathFollowCritic::initialize() +{ + auto getParam = getParamGetter(name_); + getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); + getParam(offset_from_furthest_, "offset_from_furthest", 6); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 5.0f); +} + +void PathFollowCritic::score(CriticData & data) +{ + if (!enabled_) {return;} + + if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) { + return; + } + + utils::setPathFurthestPointIfNotSet(data); + utils::setPathCostsIfNotSet(data, costmap_ros_); + const size_t path_size = data.path.x.size() - 1; + + auto offsetted_idx = std::min( + *data.furthest_reached_path_point + offset_from_furthest_, path_size); + + // Drive to first valid (non-obstructed) path point at or after the lookahead + bool valid = false; + while (!valid && offsetted_idx < path_size - 1) { + valid = (*data.path_pts_valid)[offsetted_idx]; + if (!valid) {offsetted_idx++;} + } + + const auto path_x = data.path.x(offsetted_idx); + const auto path_y = data.path.y(offsetted_idx); + + const int rightmost_idx = data.trajectories.x.cols() - 1; + const auto last_x = data.trajectories.x.col(rightmost_idx); + const auto last_y = data.trajectories.y.col(rightmost_idx); + + const auto delta_x = last_x - path_x; + const auto delta_y = last_y - path_y; + + if (power_ > 1u) { + data.costs += (((delta_x.square() + delta_y.square()).sqrt()) * weight_).pow(power_); + } else { + data.costs += ((delta_x.square() + delta_y.square()).sqrt()) * weight_; + } +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp new file mode 100644 index 00000000..2160d7f2 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp @@ -0,0 +1,103 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "ackermann_mppi/tools/noise_generator.hpp" + +namespace mppi +{ + +void NoiseGenerator::initialize( + mppi::models::OptimizerSettings & settings, + bool regenerate_noises) +{ + settings_ = settings; + regenerate_noises_ = regenerate_noises; + active_ = true; + + ndistribution_vx_ = std::normal_distribution(0.0f, settings_.sampling_std.vx); + ndistribution_wz_ = std::normal_distribution(0.0f, settings_.sampling_std.wz); + + if (regenerate_noises_) { + noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + } else { + generateNoisedControls(); + } +} + +void NoiseGenerator::shutdown() +{ + active_ = false; + ready_ = true; + noise_cond_.notify_all(); + if (noise_thread_.joinable()) { + noise_thread_.join(); + } +} + +void NoiseGenerator::generateNextNoises() +{ + { + std::unique_lock guard(noise_lock_); + ready_ = true; + } + noise_cond_.notify_all(); +} + +void NoiseGenerator::setNoisedControls( + models::State & state, + const models::ControlSequence & control_sequence) +{ + std::unique_lock guard(noise_lock_); + state.cvx = noises_vx_.rowwise() + control_sequence.vx.transpose(); + state.cwz = noises_wz_.rowwise() + control_sequence.wz.transpose(); +} + +void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings) +{ + settings_ = settings; + + { + std::unique_lock guard(noise_lock_); + noises_vx_.setZero(settings_.batch_size, settings_.time_steps); + noises_wz_.setZero(settings_.batch_size, settings_.time_steps); + ready_ = true; + } + + if (regenerate_noises_) { + noise_cond_.notify_all(); + } else { + generateNoisedControls(); + } +} + +void NoiseGenerator::noiseThread() +{ + do { + std::unique_lock guard(noise_lock_); + noise_cond_.wait(guard, [this]() {return ready_;}); + ready_ = false; + generateNoisedControls(); + } while (active_); +} + +void NoiseGenerator::generateNoisedControls() +{ + auto & s = settings_; + noises_vx_ = Eigen::ArrayXXf::NullaryExpr( + s.batch_size, s.time_steps, [&]() {return ndistribution_vx_(generator_);}); + noises_wz_ = Eigen::ArrayXXf::NullaryExpr( + s.batch_size, s.time_steps, [&]() {return ndistribution_wz_(generator_);}); +} + +} // namespace mppi diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp new file mode 100644 index 00000000..17d75f66 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp @@ -0,0 +1,486 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2025 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. + +#include "ackermann_mppi/optimizer.hpp" + +#include +#include +#include +#include +#include +#include + +#include "ackermann_mppi/critics/path_follow_critic.hpp" +#include "ackermann_mppi/critics/path_align_critic.hpp" +#include "ackermann_mppi/critics/goal_critic.hpp" +#include "ackermann_mppi/critics/obstacles_critic.hpp" +#include "ackermann_mppi/critics/constraint_critic.hpp" + +namespace mppi +{ + +Optimizer::Optimizer() = default; + +Optimizer::~Optimizer() +{ + shutdown(); +} + +void Optimizer::initialize( + rclcpp::Node::SharedPtr node, + const std::string & name, + std::shared_ptr costmap_ros, + std::shared_ptr tf_buffer) +{ + node_ = node; + name_ = name; + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + tf_buffer_ = tf_buffer; + logger_ = node_->get_logger(); + base_frame_ = costmap_ros_->getBaseFrameID(); + + getParams(); + loadCritics(); + + bool regenerate_noises = false; + declareParam(name_ + ".regenerate_noises", false); + node_->get_parameter(name_ + ".regenerate_noises", regenerate_noises); + noise_generator_.initialize(settings_, regenerate_noises); + + reset(); +} + +void Optimizer::loadCritics() +{ + // Direct instantiation — add or remove critics here to change scoring behavior. + // Order matters: critics run in order and can set fail_flag to short-circuit the rest. + auto add = [&](auto * raw, const std::string & short_name) { + critics_.emplace_back(raw); + const std::string full_name = name_ + "." + short_name; + critics_.back()->on_configure(node_, name_, full_name, costmap_ros_); + RCLCPP_INFO(logger_, "Loaded critic: %s", short_name.c_str()); + }; + + add(new critics::ConstraintCritic(), "ConstraintCritic"); + add(new critics::ObstaclesCritic(), "ObstaclesCritic"); + add(new critics::PathFollowCritic(), "PathFollowCritic"); + add(new critics::PathAlignCritic(), "PathAlignCritic"); + add(new critics::GoalCritic(), "GoalCritic"); +} + +void Optimizer::shutdown() +{ + noise_generator_.shutdown(); +} + +void Optimizer::getParams() +{ + auto & s = settings_; + auto getParam = getParamGetter(name_); + + getParam(s.model_dt, "model_dt", 0.05f); + getParam(s.time_steps, "time_steps", 56); + getParam(s.batch_size, "batch_size", 1000); + getParam(s.iteration_count, "iteration_count", 1); + getParam(s.temperature, "temperature", 0.3f); + getParam(s.gamma, "gamma", 0.015f); + getParam(s.base_constraints.vx_max, "vx_max", 0.5f); + getParam(s.base_constraints.vx_min, "vx_min", -0.35f); + getParam(s.base_constraints.wz, "wz_max", 1.9f); + getParam(s.base_constraints.ax_max, "ax_max", 3.0f); + getParam(s.base_constraints.ax_min, "ax_min", -3.0f); + getParam(s.base_constraints.az_max, "az_max", 3.5f); + getParam(s.sampling_std.vx, "vx_std", 0.2f); + getParam(s.sampling_std.wz, "wz_std", 0.4f); + getParam(s.retry_attempt_limit, "retry_attempt_limit", 1); + getParam(s.open_loop, "open_loop", false); + getParam(s.visualize, "visualize", false); + getParam(goal_dist_tolerance_, "goal_dist_tolerance", 0.25f); + + s.base_constraints.ax_max = fabs(s.base_constraints.ax_max); + if (s.base_constraints.ax_min > 0.0f) { + s.base_constraints.ax_min = -s.base_constraints.ax_min; + RCLCPP_WARN(logger_, "ax_min sign incorrect, setting negative."); + } + + float min_turning_r = 0.2f; + getParam(min_turning_r, "AckermannConstraints.min_turning_r", 0.2f); + motion_model_ = std::make_shared(min_turning_r); + + s.constraints = s.base_constraints; + + // Determine if control period matches model_dt to enable sequence shifting + double controller_frequency = 10.0; + declareParam("controller_frequency", 10.0); + node_->get_parameter("controller_frequency", controller_frequency); + const double controller_period = 1.0 / controller_frequency; + constexpr double eps = 1e-6; + if (std::abs(controller_period - s.model_dt) < eps) { + s.shift_control_sequence = true; + RCLCPP_INFO(logger_, "Control sequence shifting enabled (controller_period == model_dt)."); + } else if (controller_period > s.model_dt + eps) { + RCLCPP_WARN( + logger_, + "controller_frequency (%.2f Hz, period=%.4f s) > model_dt (%.4f s). " + "Set controller_frequency = 1/model_dt for best performance.", + controller_frequency, controller_period, s.model_dt); + } +} + +void Optimizer::reset(bool reset_dynamic_speed_limits) +{ + state_.reset(settings_.batch_size, settings_.time_steps); + control_sequence_.reset(settings_.time_steps); + control_history_.fill({}); + + if (settings_.open_loop) { + last_command_vel_ = geometry_msgs::msg::Twist(); + } + + if (reset_dynamic_speed_limits) { + settings_.constraints = settings_.base_constraints; + } + + costs_.setZero(settings_.batch_size); + generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); + noise_generator_.reset(settings_); + motion_model_->initialize(settings_.constraints, settings_.model_dt); + + // Update critic_data_ references (they hold refs to member variables) + critics_data_.goal_dist_tolerance = goal_dist_tolerance_; + + RCLCPP_INFO(logger_, "Optimizer reset"); +} + +bool Optimizer::isSpeedLimitActive() const +{ + const auto & base = settings_.base_constraints; + const auto & curr = settings_.constraints; + return base.vx_max != curr.vx_max || + base.vx_min != curr.vx_min || + base.wz != curr.wz; +} + +std::tuple Optimizer::evalControl( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal) +{ + prepare(robot_pose, robot_speed, plan, goal); + Eigen::ArrayXXf optimal_trajectory; + + do { + optimize(); + optimal_trajectory = getOptimizedTrajectory(); + } while (fallback(critics_data_.fail_flag)); + + auto control = getControlFromSequenceAsTwist(plan.header.stamp); + last_command_vel_ = control.twist; + + if (settings_.shift_control_sequence) { + shiftControlSequence(); + } + + return std::make_tuple(control, optimal_trajectory); +} + +void Optimizer::optimize() +{ + for (size_t i = 0; i < settings_.iteration_count; ++i) { + generateNoisedTrajectories(); + evalTrajectoriesScores(); + updateControlSequence(); + } +} + +void Optimizer::evalTrajectoriesScores() +{ + critic_costs_.clear(); + + for (auto & critic : critics_) { + if (critics_data_.fail_flag) {break;} + + if (settings_.visualize) { + Eigen::ArrayXf costs_before = critics_data_.costs; + critic->score(critics_data_); + critic_costs_.emplace_back(critic->getName(), critics_data_.costs - costs_before); + } else { + critic->score(critics_data_); + } + } +} + +bool Optimizer::fallback(bool fail) +{ + if (!fail) { + fallback_counter_ = 0; + return false; + } + + reset(false); + + if (++fallback_counter_ > settings_.retry_attempt_limit) { + fallback_counter_ = 0; + throw std::runtime_error("AckermannMPPI: all trajectories in collision, no valid control."); + } + return true; +} + +void Optimizer::prepare( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal) +{ + state_.pose = robot_pose; + state_.speed = settings_.open_loop ? last_command_vel_ : robot_speed; + + // Compute approximate path length (sum of Euclidean segment distances) + float path_length = 0.0f; + for (size_t i = 1; i < plan.poses.size(); ++i) { + float dx = plan.poses[i].pose.position.x - plan.poses[i - 1].pose.position.x; + float dy = plan.poses[i].pose.position.y - plan.poses[i - 1].pose.position.y; + path_length += sqrtf(dx * dx + dy * dy); + } + state_.local_path_length = path_length; + + path_ = utils::toTensor(plan); + costs_.setZero(settings_.batch_size); + goal_ = goal; + + critics_data_.fail_flag = false; + critics_data_.motion_model = motion_model_; + critics_data_.furthest_reached_path_point.reset(); + critics_data_.path_pts_valid.reset(); + critics_data_.trajectories_in_collision.clear(); +} + +void Optimizer::shiftControlSequence() +{ + auto size = control_sequence_.vx.size(); + utils::shiftColumnsByOnePlace(control_sequence_.vx, -1); + utils::shiftColumnsByOnePlace(control_sequence_.wz, -1); + control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2); + control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2); +} + +void Optimizer::generateNoisedTrajectories() +{ + noise_generator_.setNoisedControls(state_, control_sequence_); + noise_generator_.generateNextNoises(); + updateStateVelocities(state_); + integrateStateVelocities(generated_trajectories_, state_); +} + +void Optimizer::applyControlSequenceConstraints() +{ + auto & s = settings_; + + float max_delta_vx = s.model_dt * s.constraints.ax_max; + float min_delta_vx = s.model_dt * s.constraints.ax_min; + float max_delta_wz = s.model_dt * s.constraints.az_max; + + float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, + control_sequence_.vx(0)); + float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0)); + control_sequence_.vx(0) = vx_last; + control_sequence_.wz(0) = wz_last; + + for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { + float & vx_curr = control_sequence_.vx(i); + vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr); + if (vx_last > 0) { + vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr); + } else { + vx_curr = utils::clamp(vx_last - max_delta_vx, vx_last - min_delta_vx, vx_curr); + } + vx_last = vx_curr; + + float & wz_curr = control_sequence_.wz(i); + wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr); + wz_curr = utils::clamp(wz_last - max_delta_wz, wz_last + max_delta_wz, wz_curr); + wz_last = wz_curr; + } + + motion_model_->applyConstraints(control_sequence_); +} + +void Optimizer::updateStateVelocities(models::State & state) const +{ + updateInitialStateVelocities(state); + propagateStateVelocitiesFromInitials(state); +} + +void Optimizer::updateInitialStateVelocities(models::State & state) const +{ + state.vx.col(0) = static_cast(state.speed.linear.x); + state.wz.col(0) = static_cast(state.speed.angular.z); + // vy is always zero for Ackermann (non-holonomic) +} + +void Optimizer::propagateStateVelocitiesFromInitials(models::State & state) const +{ + motion_model_->predict(state); +} + +void Optimizer::integrateStateVelocities( + Eigen::Array & trajectory, + const Eigen::ArrayXXf & sequence) const +{ + float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); + + const auto vx = sequence.col(0); + const auto wz = sequence.col(1); + auto traj_x = trajectory.col(0); + auto traj_y = trajectory.col(1); + auto traj_yaws = trajectory.col(2); + + const size_t n_size = traj_yaws.size(); + if (n_size == 0) {return;} + + float last_yaw = initial_yaw; + for (size_t i = 0; i != n_size; i++) { + last_yaw += wz(i) * settings_.model_dt; + traj_yaws(i) = last_yaw; + } + + Eigen::ArrayXf yaw_cos = traj_yaws.cos(); + Eigen::ArrayXf yaw_sin = traj_yaws.sin(); + utils::shiftColumnsByOnePlace(yaw_cos, 1); + utils::shiftColumnsByOnePlace(yaw_sin, 1); + yaw_cos(0) = cosf(initial_yaw); + yaw_sin(0) = sinf(initial_yaw); + + float last_x = state_.pose.pose.position.x; + float last_y = state_.pose.pose.position.y; + for (size_t i = 0; i != n_size; i++) { + last_x += vx(i) * yaw_cos(i) * settings_.model_dt; + last_y += vx(i) * yaw_sin(i) * settings_.model_dt; + traj_x(i) = last_x; + traj_y(i) = last_y; + } +} + +void Optimizer::integrateStateVelocities( + models::Trajectories & trajectories, + const models::State & state) const +{ + auto initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); + const size_t n_cols = trajectories.yaws.cols(); + + Eigen::ArrayXf last_yaws = Eigen::ArrayXf::Constant(trajectories.yaws.rows(), initial_yaw); + for (size_t i = 0; i != n_cols; i++) { + last_yaws += state.wz.col(i) * settings_.model_dt; + trajectories.yaws.col(i) = last_yaws; + } + + Eigen::ArrayXXf yaw_cos = trajectories.yaws.cos(); + Eigen::ArrayXXf yaw_sin = trajectories.yaws.sin(); + utils::shiftColumnsByOnePlace(yaw_cos, 1); + utils::shiftColumnsByOnePlace(yaw_sin, 1); + yaw_cos.col(0) = cosf(initial_yaw); + yaw_sin.col(0) = sinf(initial_yaw); + + // Ackermann: no vy term + auto dx = (state.vx * yaw_cos).eval(); + auto dy = (state.vx * yaw_sin).eval(); + + Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant( + trajectories.x.rows(), state.pose.pose.position.x); + Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant( + trajectories.y.rows(), state.pose.pose.position.y); + + for (size_t i = 0; i != n_cols; i++) { + last_x += dx.col(i) * settings_.model_dt; + last_y += dy.col(i) * settings_.model_dt; + trajectories.x.col(i) = last_x; + trajectories.y.col(i) = last_y; + } +} + +Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() +{ + // Build [time_steps x 2] sequence (vx, wz) and integrate to poses + Eigen::ArrayXXf sequence(settings_.time_steps, 2); + Eigen::Array trajectory(settings_.time_steps, 3); + + sequence.col(0) = control_sequence_.vx; + sequence.col(1) = control_sequence_.wz; + + integrateStateVelocities(trajectory, sequence); + return trajectory; +} + +void Optimizer::updateControlSequence() +{ + auto & s = settings_; + + auto vx_T = control_sequence_.vx.transpose(); + auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; + const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx); + costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); + + if (s.sampling_std.wz > 0.0f) { + auto wz_T = control_sequence_.wz.transpose(); + auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; + const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz); + costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); + } + + auto costs_normalized = costs_ - costs_.minCoeff(); + const float inv_temp = 1.0f / s.temperature; + auto softmaxes = (-inv_temp * costs_normalized).exp().eval(); + softmaxes /= softmaxes.sum(); + + auto softmax_mat = softmaxes.matrix(); + control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat; + control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat; + + utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); + applyControlSequenceConstraints(); +} + +geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( + const builtin_interfaces::msg::Time & stamp) +{ + unsigned int offset = settings_.shift_control_sequence ? 1 : 0; + return utils::toTwistStamped( + control_sequence_.vx(offset), + control_sequence_.wz(offset), + stamp, base_frame_); +} + +void Optimizer::setSpeedLimit(double speed_limit, bool percentage) +{ + auto & s = settings_; + // nav2 costmap speed filter sentinel: 255.0 means "no limit active". + // Matches nav2_costmap_2d::NO_SPEED_LIMIT from filter_values.hpp. + constexpr double NO_SPEED_LIMIT = 255.0; + if (speed_limit == NO_SPEED_LIMIT) { + s.constraints.vx_max = s.base_constraints.vx_max; + s.constraints.vx_min = s.base_constraints.vx_min; + s.constraints.wz = s.base_constraints.wz; + } else { + double ratio = percentage ? speed_limit / 100.0 : speed_limit / s.base_constraints.vx_max; + s.constraints.vx_max = s.base_constraints.vx_max * ratio; + s.constraints.vx_min = s.base_constraints.vx_min * ratio; + s.constraints.wz = s.base_constraints.wz * ratio; + } + motion_model_->initialize(settings_.constraints, settings_.model_dt); +} + +} // namespace mppi diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt b/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt new file mode 100644 index 00000000..80e5cd00 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt @@ -0,0 +1,114 @@ +cmake_minimum_required(VERSION 3.8) +project(athena_smac_planner) + +find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_smoother REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(nlohmann_json REQUIRED) +find_package(ompl REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(msgs REQUIRED) + +nav2_package() + +if(MSVC) + add_compile_definitions(_USE_MATH_DEFINES) +else() + add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) +endif() + +set(library_name athena_smac_planner) + +add_library(${library_name} SHARED + src/a_star.cpp + src/analytic_expansion.cpp + src/collision_checker.cpp + src/costmap_downsampler.cpp + src/node_basic.cpp + src/node_hybrid.cpp + src/obstacle_heuristic.cpp + src/distance_heuristic.cpp + src/smoother.cpp + src/smac_planner_hybrid.cpp +) +target_include_directories(${library_name} + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} + ${nav2_core_INCLUDE_DIRS} + ${nav2_smoother_INCLUDE_DIRS} +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_costmap_2d::layers + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2::tf2 + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name} PRIVATE + angles::angles +) + +install(TARGETS ${library_name} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +add_executable(global_planner_node src/global_planner_node.cpp) +target_include_directories(global_planner_node PRIVATE + "$" + ${OMPL_INCLUDE_DIRS} + ${nav2_core_INCLUDE_DIRS} +) +target_link_libraries(global_planner_node + ${library_name} + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + ${msgs_TARGETS} +) + +install(TARGETS global_planner_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_libraries(${library_name}) +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_costmap_2d + nav_msgs + nlohmann_json + ompl + rclcpp + rclcpp_lifecycle + tf2 + tf2_ros + visualization_msgs +) +ament_export_targets(${PROJECT_NAME}) +ament_package() diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp new file mode 100644 index 00000000..fc587fb8 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp @@ -0,0 +1,323 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__A_STAR_HPP_ +#define ATHENA_SMAC_PLANNER__A_STAR_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_core/exceptions.hpp" + +#include "athena_smac_planner/thirdparty/robin_hood.h" +#include "athena_smac_planner/analytic_expansion.hpp" +#include "athena_smac_planner/node_hybrid.hpp" +#include "athena_smac_planner/node_basic.hpp" +#include "athena_smac_planner/goal_manager.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/constants.hpp" + +namespace athena_smac_planner +{ + +/** + * @class athena_smac_planner::AStarAlgorithm + * @brief An A* implementation for planning in a costmap. Templated based on the Node type. + */ +template +class AStarAlgorithm +{ +public: + typedef NodeT * NodePtr; + typedef robin_hood::unordered_node_map Graph; + typedef std::vector NodeVector; + typedef std::pair> NodeElement; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + typedef typename NodeVector::iterator NeighborIterator; + typedef std::function NodeGetter; + typedef GoalManager GoalManagerT; + using NodeContext = typename NodeT::NodeContext; + + + /** + * @struct athena_smac_planner::NodeComparator + * @brief Node comparison for priority queue sorting + */ + struct NodeComparator + { + bool operator()(const NodeElement & a, const NodeElement & b) const + { + return a.first > b.first; + } + }; + + typedef std::priority_queue, NodeComparator> NodeQueue; + + /** + * @brief A constructor for athena_smac_planner::AStarAlgorithm + */ + explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); + + /** + * @brief A destructor for athena_smac_planner::AStarAlgorithm + */ + ~AStarAlgorithm(); + + /** + * @brief Initialization of the planner with defaults + * @param allow_unknown Allow search in unknown space, good for navigation while mapping + * @param max_iterations Maximum number of iterations to use while expanding search + * @param max_on_approach_iterations Maximum number of iterations before returning a valid + * path once within thresholds to refine path + * comes at more compute time but smoother paths. + * @param terminal_checking_interval Number of iterations to check if the task has been canceled or + * or planning time exceeded + * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns + * false after this timeout + * @param lookup_table_size Size of the lookup table to store heuristic values + * @param dim_3_size Number of quantization bins + */ + void initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations, + const int & terminal_checking_interval, + const double & max_planning_time, + const float & lookup_table_size, + const unsigned int & dim_3_size); + + /** + * @brief Creating path from given costmap, start, and goal + * @param path Reference to a vector of indices of generated path + * @param num_iterations Reference to number of iterations to create plan + * @param tolerance Reference to tolerance in costmap nodes + * @param cancel_checker Function to check if the task has been canceled + * @param expansions_log Optional expansions logged for debug + * @return if plan was successful + */ + bool createPath( + CoordinateVector & path, int & num_iterations, const float & tolerance, + std::function cancel_checker, + std::vector> * expansions_log = nullptr); + + /** + * @brief Update the search info used by the algorithm + * @param search_info Search info to update + */ + void setSearchInfo(const SearchInfo & search_info) {_search_info = search_info;} + + /** + * @brief Sets the collision checker to use + * @param collision_checker Collision checker to use for checking state validity + */ + void setCollisionChecker(GridCollisionChecker * collision_checker); + + /** + * @brief Set the goal for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + * @param goal_heading_mode The goal heading mode to use + * @param coarse_search_resolution The resolution to search for goal heading + */ + void setGoal( + const float & mx, + const float & my, + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT, + const int & coarse_search_resolution = 1); + + /** + * @brief Set the starting pose for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setStart( + const float & mx, + const float & my, + const unsigned int & dim_3); + + /** + * @brief Get maximum number of iterations to plan + * @return Reference to Maximum iterations parameter + */ + int & getMaxIterations(); + + /** + * @brief Get pointer reference to starting node + * @return Node pointer reference to starting node + */ + NodePtr & getStart(); + + /** + * @brief Get maximum number of on-approach iterations after within threshold + * @return Reference to Maximum on-approach iterations parameter + */ + int & getOnApproachMaxIterations(); + + /** + * @brief Get tolerance, in node nodes + * @return Reference to tolerance parameter + */ + float & getToleranceHeuristic(); + + /** + * @brief Get size of graph in X + * @return Size in X + */ + unsigned int & getSizeX(); + + /** + * @brief Get size of graph in Y + * @return Size in Y + */ + unsigned int & getSizeY(); + + /** + * @brief Get number of angle quantization bins (SE2) or Z coordinate (XYZ) + * @return Number of angle bins / Z dimension + */ + unsigned int & getSizeDim3(); + + /** + * @brief Get the resolution of the coarse search + * @return Size of the goals to expand + */ + unsigned int getCoarseSearchResolution(); + + /** + * @brief Get the goals manager class + * @return Goal manager class + */ + GoalManagerT getGoalManager(); + + /** + * @brief Get pointer to shared node context + * @return Node context pointer + */ + NodeContext * getContext(); + +protected: + /** + * @brief Get pointer to next goal in open set + * @return Node pointer reference to next heuristically scored node + */ + inline NodePtr getNextNode(); + + /** + * @brief Add a node to the open set + * @param cost The cost to sort into the open set of the node + * @param node Node pointer reference to add to open set + */ + inline void addNode(const float & cost, NodePtr & node); + + /** + * @brief Adds node to graph + * @param index Node index to add + */ + inline NodePtr addToGraph(const uint64_t & index); + + /** + * @brief Get cost of heuristic of node + * @param node Node pointer to get heuristic for + * @return Heuristic cost for node + */ + inline float getHeuristicCost(const NodePtr & node); + + /** + * @brief Check if inputs to planner are valid + * @return Are valid + */ + inline bool areInputsValid(); + + /** + * @brief Get the closest path within tolerance if available + * @param path Vector of coordinates to fill with path + * @return if a valid path was found within tolerance + */ + inline bool getClosestPathWithinTolerance(CoordinateVector & path); + + /** + * @brief Clear heuristic queue of nodes to search + */ + inline void clearQueue(); + + /** + * @brief Clear graph of nodes searched + */ + inline void clearGraph(); + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param dim3 Z coordinate / theta bin of point + * @return Index + */ + inline uint64_t getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & dim3); + + /** + * @brief Check if node has been visited + * @param current_node Node to check if visited + * @return if node has been visited + */ + inline bool onVisitationCheckNode(const NodePtr & node); + + /** + * @brief Populate a debug log of expansions for Hybrid-A* for visualization + * @param node Node expanded + * @param expansions_log Log to add not expanded to + */ + inline void populateExpansionsLog( + const NodePtr & node, std::vector> * expansions_log); + + bool _traverse_unknown; + bool _is_initialized; + int _max_iterations; + int _max_on_approach_iterations; + int _terminal_checking_interval; + double _max_planning_time; + float _tolerance; + unsigned int _x_size; + unsigned int _y_size; + unsigned int _dim3_size; + unsigned int _coarse_search_resolution; + SearchInfo _search_info; + + NodePtr _start; + GoalManagerT _goal_manager; + Graph _graph; + NodeQueue _queue; + + MotionModel _motion_model; + NodeHeuristicPair _best_heuristic_node; + + GridCollisionChecker * _collision_checker; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr> _expander; + std::shared_ptr _shared_ctx; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__A_STAR_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp new file mode 100644 index 00000000..1c3263cc --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp @@ -0,0 +1,200 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ +#define ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "athena_smac_planner/node_hybrid.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/constants.hpp" + +namespace athena_smac_planner +{ + +template +class AnalyticExpansion +{ +public: + typedef NodeT * NodePtr; + typedef std::vector NodeVector; + typedef typename NodeT::Coordinates Coordinates; + typedef std::function NodeGetter; + typedef typename NodeT::CoordinateVector CoordinateVector; + using NodeContext = typename NodeT::NodeContext; + + /** + * @struct athena_smac_planner::AnalyticExpansion::AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + */ + struct AnalyticExpansionNode + { + AnalyticExpansionNode( + NodePtr & node_in, + Coordinates & initial_coords_in, + Coordinates & proposed_coords_in) + : node(node_in), + initial_coords(initial_coords_in), + proposed_coords(proposed_coords_in) + { + } + + NodePtr node; + Coordinates initial_coords; + Coordinates proposed_coords; + }; + + /** + * @struct AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + * + * This structure holds a collection of analytic expansion nodes and the number of direction + * changes encountered during the expansion. + */ + struct AnalyticExpansionNodes + { + AnalyticExpansionNodes() = default; + + void add( + NodePtr & node, + Coordinates & initial_coords, + Coordinates & proposed_coords) + { + nodes.emplace_back(node, initial_coords, proposed_coords); + } + + void setDirectionChanges(int changes) + { + direction_changes = changes; + } + + std::vector nodes; + int direction_changes{0}; + }; + + /** + * @brief Constructor for analytic expansion object + */ + AnalyticExpansion( + const MotionModel & motion_model, + const SearchInfo & search_info, + const bool & traverse_unknown, + const unsigned int & dim_3_size); + + /** + * @brief Sets the collision checker and costmap to use in expansion validation + * @param collision_checker Collision checker to use + */ + void setCollisionChecker(GridCollisionChecker * collision_checker); + + /** + * @brief Sets the shared context to use + * @param ctx Shared context pointer + */ + void setContext(NodeContext * ctx); + + /** + * @brief Attempt an analytic path completion + * @param node The node to start the analytic path from + * @param coarse_check_goals Coarse list of goals nodes to plan to + * @param fine_check_goals Fine list of goals nodes to plan to + * @param goals_coords vector of goal coordinates to plan to + * @param getter Gets a node at a set of coordinates + * @param iterations Iterations to run over + * @param closest_distance Closest distance to goal + * @return Node pointer reference to goal node with the best score out of the goals node if + * successful, else return nullptr + */ + NodePtr tryAnalyticExpansion( + const NodePtr & current_node, + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, + const NodeGetter & getter, int & iterations, + int & closest_distance); + + /** + * @brief Perform an analytic path expansion to the goal + * @param node The node to start the analytic path from + * @param goal The goal node to plan to + * @param getter The function object that gets valid nodes from the graph + * @param state_space State space to use for computing analytic expansions + * @return A set of analytically expanded nodes to the goal from current node, if possible + */ + AnalyticExpansionNodes getAnalyticPath( + const NodePtr & node, const NodePtr & goal, + const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space); + + /** + * @brief Refined analytic path from the current node to the goal + * @param node The node to start the analytic path from. Node head may + * change as a result of refinement + * @param goal_node The goal node to plan to + * @param getter The function object that gets valid nodes from the graph + * @param analytic_nodes The set of analytic nodes to refine + * @return The score of the refined path + */ + float refineAnalyticPath( + NodePtr & node, + const NodePtr & goal_node, + const NodeGetter & getter, + AnalyticExpansionNodes & analytic_nodes); + + /** + * @brief Takes final analytic expansion and appends to current expanded node + * @param node The node to start the analytic path from + * @param goal The goal node to plan to + * @param expanded_nodes Expanded nodes to append to end of current search path + * @return Node pointer to goal node if successful, else return nullptr + */ + NodePtr setAnalyticPath( + const NodePtr & node, const NodePtr & goal, + const AnalyticExpansionNodes & expanded_nodes); + + /** + * @brief Counts the number of direction changes in a Reeds-Shepp path + * @param path The Reeds-Shepp path to count direction changes in + * @return The number of direction changes in the path + */ + int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path); + + /** + * @brief Takes an expanded nodes to clean up, if necessary, of any state + * information that may be polluting it from a prior search iteration + * @param expanded_nodes Expanded node to clean up from search + */ + void cleanNode(const NodePtr & nodes); + +protected: + MotionModel _motion_model; + SearchInfo _search_info; + bool _traverse_unknown; + unsigned int _dim_3_size; + GridCollisionChecker * _collision_checker; + std::list> _detached_nodes; + NodeContext * _ctx = nullptr; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp new file mode 100644 index 00000000..ebd5023f --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp @@ -0,0 +1,139 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "athena_smac_planner/constants.hpp" +#include "rclcpp/rclcpp.hpp" + +#ifndef ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ +#define ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ + +namespace athena_smac_planner +{ + +/** + * @class athena_smac_planner::GridCollisionChecker + * @brief A costmap grid collision checker + */ +class GridCollisionChecker + : public nav2_costmap_2d::FootprintCollisionChecker +{ +public: + /** + * @brief A constructor for athena_smac_planner::GridCollisionChecker + * for use when regular bin intervals are appropriate + * @param costmap The costmap to collision check against + * @param num_quantizations The number of quantizations to precompute footprint + * @param node Node to extract clock and logger from + * orientations for to speed up collision checking + */ + GridCollisionChecker( + std::shared_ptr costmap, + unsigned int num_quantizations, + rclcpp::Node::SharedPtr node); + + /** + * @brief A constructor for athena_smac_planner::GridCollisionChecker + * for use when irregular bin intervals are appropriate + * @param costmap The costmap to collision check against + * @param angles The vector of possible angle bins to precompute for + * orientations for to speed up collision checking, in radians + */ + // GridCollisionChecker( + // nav2_costmap_2d::Costmap2D * costmap, + // std::vector & angles); + + /** + * @brief Set the footprint to use with collision checker + * @param footprint The footprint to collision check against + * @param radius Whether or not the footprint is a circle and use radius collision checking + */ + void setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_collision_cost); + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param x X coordinate of pose to check against + * @param y Y coordinate of pose to check against + * @param theta Angle bin number of pose to check against (NOT radians) + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const float & x, + const float & y, + const float & theta, + const bool & traverse_unknown); + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param i Index to search collision status of + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const unsigned int & i, + const bool & traverse_unknown); + + /** + * @brief Get cost at footprint pose in costmap + * @return the cost at the pose in costmap + */ + float getCost(); + + /** + * @brief Get the angles of the precomputed footprint orientations + * @return the ordered vector of angles corresponding to footprints + */ + std::vector & getPrecomputedAngles() + { + return angles_; + } + + /** + * @brief Get costmap ros object for inflation layer params + * @return Costmap ros + */ + std::shared_ptr getCostmapROS() {return costmap_ros_;} + + /** + * @brief Check if value outside the range + * @param min Minimum value of the range + * @param max Maximum value of the range + * @param value the value to check if it is within the range + * @return boolean if in range or not + */ + bool outsideRange(const unsigned int & max, const float & value); + +protected: + std::shared_ptr costmap_ros_; + std::vector oriented_footprints_; + nav2_costmap_2d::Footprint unoriented_footprint_; + float center_cost_; + bool footprint_is_radius_{false}; + std::vector angles_; + float possible_collision_cost_{-1}; + rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp new file mode 100644 index 00000000..78e145ef --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ +#define ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ + +#include + +namespace athena_smac_planner +{ +enum class MotionModel +{ + UNKNOWN = 0, + TWOD = 1, + DUBIN = 2, + REEDS_SHEPP = 3, + STATE_LATTICE = 4, +}; + +enum class GoalHeadingMode +{ + UNKNOWN = 0, + DEFAULT = 1, + BIDIRECTIONAL = 2, + ALL_DIRECTION = 3, +}; + +inline std::string toString(const MotionModel & n) +{ + switch (n) { + case MotionModel::TWOD: + return "2D"; + case MotionModel::DUBIN: + return "Dubin"; + case MotionModel::REEDS_SHEPP: + return "Reeds-Shepp"; + case MotionModel::STATE_LATTICE: + return "State Lattice"; + default: + return "Unknown"; + } +} + +inline MotionModel fromString(const std::string & n) +{ + if (n == "2D") { + return MotionModel::TWOD; + } else if (n == "DUBIN") { + return MotionModel::DUBIN; + } else if (n == "REEDS_SHEPP") { + return MotionModel::REEDS_SHEPP; + } else if (n == "STATE_LATTICE") { + return MotionModel::STATE_LATTICE; + } else { + return MotionModel::UNKNOWN; + } +} + +inline std::string toString(const GoalHeadingMode & n) +{ + switch (n) { + case GoalHeadingMode::DEFAULT: + return "DEFAULT"; + case GoalHeadingMode::BIDIRECTIONAL: + return "BIDIRECTIONAL"; + case GoalHeadingMode::ALL_DIRECTION: + return "ALL_DIRECTION"; + default: + return "Unknown"; + } +} + +inline GoalHeadingMode fromStringToGH(const std::string & n) +{ + if (n == "DEFAULT") { + return GoalHeadingMode::DEFAULT; + } else if (n == "BIDIRECTIONAL") { + return GoalHeadingMode::BIDIRECTIONAL; + } else if (n == "ALL_DIRECTION") { + return GoalHeadingMode::ALL_DIRECTION; + } else { + return GoalHeadingMode::UNKNOWN; + } +} + +const float UNKNOWN_COST = 255.0; +const float OCCUPIED_COST = 254.0; +const float INSCRIBED_COST = 253.0; +const float MAX_NON_OBSTACLE_COST = 252.0; +const float FREE_COST = 0; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp new file mode 100644 index 00000000..00d49f3f --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2020, Carlos Luis +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ +#define ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ + +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "athena_smac_planner/constants.hpp" + +namespace athena_smac_planner +{ + +/** + * @class athena_smac_planner::CostmapDownsampler + * @brief A costmap downsampler for more efficient path planning + */ +class CostmapDownsampler +{ +public: + /** + * @brief A constructor for CostmapDownsampler + */ + CostmapDownsampler(); + + /** + * @brief A destructor for CostmapDownsampler + */ + ~CostmapDownsampler(); + + /** + * @brief Configure the downsampled costmap object + * @param costmap The costmap we want to downsample + * @param downsampling_factor Multiplier for the costmap resolution + * @param use_min_cost_neighbor If true, min function is used instead of max for downsampling + */ + void on_configure( + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor, + const bool & use_min_cost_neighbor = false); + + /** + * @brief Cleanup the downsampled costmap + */ + void on_cleanup(); + + /** + * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap + * @param downsampling_factor Multiplier for the costmap resolution + * @return A ptr to the downsampled costmap + */ + nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor); + + /** + * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version + */ + void resizeCostmap(); + +protected: + /** + * @brief Update the sizes X-Y of the costmap and its downsampled version + */ + void updateCostmapSize(); + + /** + * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell + * @param new_mx The X-coordinate of the cell in the new costmap + * @param new_my The Y-coordinate of the cell in the new costmap + */ + void setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my); + + unsigned int _size_x; + unsigned int _size_y; + unsigned int _downsampled_size_x; + unsigned int _downsampled_size_y; + unsigned int _downsampling_factor; + bool _use_min_cost_neighbor; + float _downsampled_resolution; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _downsampled_costmap; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp new file mode 100644 index 00000000..3a74bf2e --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp @@ -0,0 +1,77 @@ +// Copyright (c) 2026, 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ +#define ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ + +#include "athena_smac_planner/constants.hpp" +#include "athena_smac_planner/types.hpp" + +namespace athena_smac_planner +{ +struct HybridMotionTable; +class NodeHybrid; + +/** + * @class athena_smac_planner::DistanceHeuristic + * @brief Distance Heuristic implementation for graph, Hybrid-A* + */ +template +class DistanceHeuristic +{ +public: + /** + * @brief A constructor for athena_smac_planner::DistanceHeuristic + */ + DistanceHeuristic() {} + + /** + * @brief Compute the SE2 distance heuristic + * @param lookup_table_dim Size, in costmap pixels, of the + * each lookup table dimension to populate + * @param motion_model Motion model to use for state space + * @param dim_3_size Number of quantization bins for caching + * @param search_info Info containing minimum radius to use + */ + template + void precomputeDistanceHeuristic( + const float & lookup_table_dim, + const MotionModel & motion_model, + const unsigned int & dim_3_size, + const SearchInfo & search_info, + MotionTableT & motion_table); + + /** + * @brief Compute the Distance heuristic + * @param node_coords Coordinates to get heuristic at + * @param goal_coords Coordinates to compute heuristic to + * @param obstacle_heuristic Value of the obstacle heuristic to compute + * additional motion heuristics if required + * @return heuristic Heuristic value + */ + template + float getDistanceHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const float & obstacle_heuristic, + MotionTableT & motion_table); + +protected: + // Dubin / Reeds-Shepp lookup and size for dereferencing + LookupTable dist_heuristic_lookup_table_; + float size_lookup_; +}; + +} // namespace athena_smac_planner +#endif // ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp new file mode 100644 index 00000000..4c2c1d87 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp @@ -0,0 +1,289 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// Copyright (c) 2024, Stevedan Ogochukwu Omodolor Omodia +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ +#define ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ + +#include +#include +#include +#include + +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/node_hybrid.hpp" +#include "athena_smac_planner/node_basic.hpp" +#include "athena_smac_planner/collision_checker.hpp" + + +namespace athena_smac_planner +{ + +/** +* @class athena_smac_planner::GoalManager +* @brief Responsible for managing multiple variables storing information on the goal +*/ +template +class GoalManager +{ +public: + typedef NodeT * NodePtr; + typedef std::vector NodeVector; + typedef std::unordered_set NodeSet; + typedef std::vector> GoalStateVector; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + using NodeContext = typename NodeT::NodeContext; + + /** + * @brief Constructor: Initializes empty goal state. sets and coordinate lists. + */ + GoalManager() + : _goals_set(NodeSet()), + _goals_state(GoalStateVector()), + _goals_coordinate(CoordinateVector()), + _ref_goal_coord(Coordinates()) + { + } + + /** + * @brief Destructor for the GoalManager + */ + ~GoalManager() = default; + + /** + * @brief Sets the node context for goal nodes + * @param ctx Pointer to the NodeContext + */ + void setContext(NodeContext * ctx) + { + _ctx = ctx; + } + + /** + * @brief Checks if the goals set is empty + * @return true if the goals set is empty + */ + bool goalsIsEmpty() + { + return _goals_state.empty(); + } + + /** + * @brief Adds goal to the goal vector + *@param goal Reference to the NodePtr + */ + void addGoal(NodePtr & goal) + { + _goals_state.push_back({goal, true}); + } + + /** + * @brief Clears all internal goal data, including goals, states, and coordinates. + */ + void clear() + { + _goals_set.clear(); + _goals_state.clear(); + _goals_coordinate.clear(); + } + + /** + * @brief Populates coarse and fine goal lists for analytic expansion. + * @param coarse_check_goals Output list of goals for coarse search expansion. + * @param fine_check_goals Output list of goals for fine search refinement. + * @param coarse_search_resolution Number of fine goals per coarse goal. + */ + void prepareGoalsForAnalyticExpansion( + NodeVector & coarse_check_goals, NodeVector & fine_check_goals, + int coarse_search_resolution) + { + for (unsigned int i = 0; i < _goals_state.size(); i++) { + if (_goals_state[i].is_valid) { + if (i % coarse_search_resolution == 0) { + coarse_check_goals.push_back(_goals_state[i].goal); + } else { + fine_check_goals.push_back(_goals_state[i].goal); + } + } + } + } + + /** + * @brief Checks if zone within the radius of a node is feasible. Returns true if + * there's at least one non-lethal cell within the node radius. + * @param node Input node. + * @param radius Search radius. + * @param collision_checker Collision checker to validate nearby nodes. + * @param traverse_unknown Flag whether traversal through unknown space is allowed. + * @return true + * @return false + */ + bool isZoneValid( + const NodePtr node, const float & radius, GridCollisionChecker * collision_checker, + const bool & traverse_unknown) const + { + if (radius < 1) { + return false; + } + + const auto size_x = collision_checker->getCostmap()->getSizeInCellsX(); + const auto size_y = collision_checker->getCostmap()->getSizeInCellsY(); + + auto getIndexFromPoint = [this](const Coordinates & point) { + const auto mx = static_cast(point.x); + const auto my = static_cast(point.y); + const auto angle = static_cast(point.theta); + return NodeT::getIndex(mx, my, angle, _ctx->motion_table.size_x, + _ctx->motion_table.num_angle_quantization); + }; + + const Coordinates & center_point = node->pose; + const float min_x = std::max(0.0f, std::floor(center_point.x - radius)); + const float min_y = std::max(0.0f, std::floor(center_point.y - radius)); + const float max_x = + std::min(static_cast(size_x - 1), std::ceil(center_point.x + radius)); + const float max_y = + std::min(static_cast(size_y - 1), std::ceil(center_point.y + radius)); + const float radius_sq = radius * radius; + + Coordinates m; + for (m.x = min_x; m.x <= max_x; m.x += 1.0f) { + for (m.y = min_y; m.y <= max_y; m.y += 1.0f) { + const float dx = m.x - center_point.x; + const float dy = m.y - center_point.y; + + if (dx * dx + dy * dy > radius_sq) { + continue; + } + + NodeT current_node(getIndexFromPoint(m), _ctx); + current_node.setPose(m); + + if (current_node.isNodeValid(traverse_unknown, collision_checker)) { + return true; + } + } + } + + return false; + } + + /** + * @brief Filters and marks invalid goals based on collision checking and tolerance thresholds. + * + * Stores only valid (or tolerably infeasible) goals into internal goal sets and coordinates. + * + * @param tolerance Heuristic tolerance allowed for infeasible goals. + * @param collision_checker Collision checker to validate goal positions. + * @param traverse_unknown Flag whether traversal through unknown space is allowed. + */ + void removeInvalidGoals( + const float & tolerance, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown) + { + // Make sure that there was a goal clear before this was run + if (!_goals_set.empty() || !_goals_coordinate.empty()) { + throw std::runtime_error( + "Goal set should be cleared before calling " + "removeinvalidgoals"); + } + for (unsigned int i = 0; i < _goals_state.size(); i++) { + if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) || + isZoneValid(_goals_state[i].goal, tolerance, collision_checker, traverse_unknown)) + { + _goals_state[i].is_valid = true; + _goals_set.insert(_goals_state[i].goal); + _goals_coordinate.push_back(_goals_state[i].goal->pose); + } else { + _goals_state[i].is_valid = false; + } + } + } + + /** + * @brief Check if a given node is part of the goal set. + * @param node Node pointer to check. + * @return if node matches any goal in the goal set. + */ + inline bool isGoal(const NodePtr & node) + { + return _goals_set.find(node) != _goals_set.end(); + } + + /** + * @brief Get pointer reference to goals set vector + * @return unordered_set of node pointers reference to the goals nodes + */ + inline NodeSet & getGoalsSet() + { + return _goals_set; + } + + /** + * @brief Get pointer reference to goals state + * @return vector of node pointers reference to the goals state + */ + inline GoalStateVector & getGoalsState() + { + return _goals_state; + } + + /** + * @brief Get pointer reference to goals coordinates + * @return vector of goals coordinates reference to the goals coordinates + */ + inline CoordinateVector & getGoalsCoordinates() + { + return _goals_coordinate; + } + + /** + * @brief Set the Reference goal coordinate + * @param coord Coordinates to set as Reference goal + */ + inline void setRefGoalCoordinates(const Coordinates & coord) + { + _ref_goal_coord = coord; + } + + /** + * @brief Checks whether the Reference goal coordinate has changed. + * @param coord Coordinates to compare with the current Reference goal coordinate. + * @return true if the Reference goal coordinate has changed, false otherwise. + */ + inline bool hasGoalChanged(const Coordinates & coord) + { + /** + * Note: This function checks if the goal has changed. This has to be done with + * the coordinates not the Node pointer because the Node pointer + * can be reused for different goals, but the coordinates will always + * be unique for each goal. + */ + return _ref_goal_coord != coord; + } + +protected: + NodeSet _goals_set; + GoalStateVector _goals_state; + CoordinateVector _goals_coordinate; + Coordinates _ref_goal_coord; + NodeContext * _ctx = nullptr; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp new file mode 100644 index 00000000..70f464c9 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ +#define ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ + +#include "athena_smac_planner/constants.hpp" +#include "athena_smac_planner/node_hybrid.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/collision_checker.hpp" + +namespace athena_smac_planner +{ + +/** + * @class athena_smac_planner::NodeBasic + * @brief NodeBasic implementation for priority queue insertion + */ +template +class NodeBasic +{ +public: + /** + * @brief A constructor for athena_smac_planner::NodeBasic + * @param index The index of this node for self-reference + */ + explicit NodeBasic(const uint64_t new_index) + : graph_node_ptr(nullptr), + index(new_index) + { + } + + /** + * @brief Take a NodeBasic and populate it with any necessary state + * cached in the queue for NodeT. + * @param node NodeT ptr to populate metadata into NodeBasic + */ + void populateSearchNode(NodeT * & node); + + /** + * @brief Take a NodeBasic and populate it with any necessary state + * cached in the queue for NodeTs. + * @param node Search node (basic) object to initialize internal node + * with state + */ + void processSearchNode(); + + typename NodeT::Coordinates pose; // Used by NodeHybrid + NodeT * graph_node_ptr; + uint64_t index; + unsigned int motion_index; + bool backward; + TurnDirection turn_dir; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp new file mode 100644 index 00000000..b9ed6092 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp @@ -0,0 +1,386 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ +#define ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ + +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#include "athena_smac_planner/constants.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/collision_checker.hpp" +#include "athena_smac_planner/costmap_downsampler.hpp" +#include "athena_smac_planner/obstacle_heuristic.hpp" +#include "athena_smac_planner/distance_heuristic.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + +namespace athena_smac_planner +{ + +// Must forward declare +class NodeHybrid; + +/** + * @struct athena_smac_planner::HybridMotionTable + * @brief A table of motion primitives and related functions + */ +struct HybridMotionTable +{ + /** + * @brief A constructor for athena_smac_planner::HybridMotionTable + */ + HybridMotionTable() {} + + /** + * @brief Initializing using Dubin model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initDubin( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Initializing using Reeds-Shepp model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initReedsShepp( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Get projections of motion models + * @param node Ptr to NodeHybrid + * @return A set of motion poses + */ + MotionPoses getProjections(const NodeHybrid * node); + + /** + * @brief Get the angular bin to use from a raw orientation + * @param theta Angle in radians + * @return bin index of closest angle to request + */ + unsigned int getClosestAngularBin(const double & theta); + + /** + * @brief Get the raw orientation from an angular bin + * @param bin_idx Index of the bin + * @return Raw orientation in radians + */ + float getAngleFromBin(const unsigned int & bin_idx); + + /** + * @brief Get the angle scaled across bins from a raw orientation + * @param theta Angle in radians + * @return angle scaled across bins + */ + double getAngle(const double & theta); + + MotionModel motion_model = MotionModel::UNKNOWN; + MotionPoses projections; + unsigned int size_x; + unsigned int num_angle_quantization; + float num_angle_quantization_float; + float min_turning_radius; + float bin_size; + float change_penalty; + float non_straight_penalty; + float cost_penalty; + float reverse_penalty; + float travel_distance_reward; + bool downsample_obstacle_heuristic; + bool use_quadratic_cost_penalty; + bool allow_primitive_interpolation; + ompl::base::StateSpacePtr state_space; + std::vector> delta_xs; + std::vector> delta_ys; + std::vector trig_values; + std::vector travel_costs; +}; + +/** + * @class athena_smac_planner::NodeHybrid + * @brief NodeHybrid implementation for graph, Hybrid-A* + */ +class NodeHybrid +{ +public: + typedef NodeHybrid * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + using Coordinates = athena_smac_planner::Coordinates; + typedef std::vector CoordinateVector; + + struct NodeContext + { + /** + * @brief A constructor for athena_smac_planner::NodeContext + */ + NodeContext() + { + obstacle_heuristic = std::make_unique(); + distance_heuristic = std::make_unique>(); + } + HybridMotionTable motion_table; + std::unique_ptr obstacle_heuristic; + std::unique_ptr> distance_heuristic; + }; + /** + * @brief A constructor for athena_smac_planner::NodeHybrid + * @param index The index of this node for self-reference + */ + explicit NodeHybrid(const uint64_t index, NodeContext * ctx); + + /** + * @brief A destructor for athena_smac_planner::NodeHybrid + */ + ~NodeHybrid(); + + /** + * @brief operator== for comparisons + * @param NodeHybrid right hand side node reference + * @return If cell indices are equal + */ + bool operator==(const NodeHybrid & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + + /** + * @brief Reset method for new search + */ + void reset(); + + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float & cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Sets the motion primitive index used to achieve node in search + * @param reference to motion primitive idx + */ + inline void setMotionPrimitiveIndex(const unsigned int & idx, const TurnDirection & turn_dir) + { + _motion_primitive_index = idx; + _turn_dir = turn_dir; + } + + /** + * @brief Gets the motion primitive index used to achieve node in search + * @return reference to motion primitive idx + */ + inline unsigned int & getMotionPrimitiveIndex() + { + return _motion_primitive_index; + } + + /** + * @brief Gets the motion primitive turning direction used to achieve node in search + * @return reference to motion primitive turning direction + */ + inline TurnDirection & getTurnDirection() + { + return _turn_dir; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + inline void visited() + { + _was_visited = true; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline uint64_t getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @param traverse_unknown If we can explore unknown nodes on the graph + * @param collision_checker: Collision checker object + * @return whether this node is valid and collision free + */ + bool isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker * collision_checker); + + /** + * @brief Get traversal cost of parent node to child node + * @param child Node pointer to child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @param width Width of costmap + * @param angle_quantization Number of theta bins + * @return Index + */ + static inline uint64_t getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle, + const unsigned int & width, const unsigned int & angle_quantization) + { + return static_cast(angle) + static_cast(x) * + static_cast(angle_quantization) + + static_cast(y) * static_cast(width) * + static_cast(angle_quantization); + } + + /** + * @brief Get coordinates at index + * @param index Index of point + * @param width Width of costmap + * @param angle_quantization Theta size of costmap + * @return Coordinates + */ + static inline Coordinates getCoords( + const uint64_t & index, + const unsigned int & width, const unsigned int & angle_quantization) + { + return Coordinates( + (index / angle_quantization) % width, // x + index / (angle_quantization * width), // y + index % angle_quantization); // theta + } + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + float getHeuristicCost( + const Coordinates & node_coords, + const CoordinateVector & goals_coords); + + /** + * @brief Initialize motion models + * @param motion_model Motion model enum to use + * @param size_x Size of X of graph + * @param size_y Size of y of graph + * @param angle_quantization Size of theta bins of graph + * @param search_info Search info to use + */ + static void initMotionModel( + NodeContext * ctx, + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & angle_quantization, + SearchInfo & search_info); + + /** + * @brief Retrieve all valid neighbors of a node. + * @param validity_checker Functor for state validity checking + * @param collision_checker Collision checker to use + * @param traverse_unknown If unknown costs are valid to traverse + * @param neighbors Vector of neighbors to be filled + */ + void getNeighbors( + std::function & validity_checker, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + /** + * @brief Set the starting pose for planning, as a node index + * @param path Reference to a vector of indices of generated path + * @return whether the path was able to be backtraced + */ + bool backtracePath(CoordinateVector & path); + + NodeHybrid * parent; + Coordinates pose; + +private: + float _cell_cost; + float _accumulated_cost; + uint64_t _index; + bool _was_visited; + unsigned int _motion_primitive_index; + TurnDirection _turn_dir; + bool _is_node_valid{false}; + NodeContext * _ctx = nullptr; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp new file mode 100644 index 00000000..2a439d02 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp @@ -0,0 +1,96 @@ +// Copyright (c) 2026, 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ +#define ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ + +#include +#include +#include +#include "athena_smac_planner/constants.hpp" +#include "athena_smac_planner/types.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace athena_smac_planner +{ + +typedef std::pair ObstacleHeuristicElement; +struct ObstacleHeuristicComparator +{ + bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const + { + return a.first > b.first; + } +}; + +typedef std::vector ObstacleHeuristicQueue; + +/** + * @class athena_smac_planner::ObstacleHeuristic + * @brief Obstacle Heuristic implementation for graph, Hybrid-A* + */ +class ObstacleHeuristic +{ +public: + /** + * @brief A constructor for athena_smac_planner::ObstacleHeuristic + */ + ObstacleHeuristic() {} + + /** + * @brief A destructor for athena_smac_planner::ObstacleHeuristic + */ + ~ObstacleHeuristic() {} + + /** + * @brief Compute the wavefront heuristic + * @param costmap Costmap to use + * @param goal_coords Coordinates to start heuristic expansion at + */ + void resetObstacleHeuristic( + std::shared_ptr costmap_ros_i, + const unsigned int & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y, + const bool downsample_obstacle_heuristic); + + /** + * @brief Compute the Obstacle heuristic + * @param node_coords Coordinates to get heuristic at + * @param goal_coords Coordinates to compute heuristic to + * @return heuristic Heuristic value + */ + float getObstacleHeuristic( + const Coordinates & node_coords, + const float & cost_penalty, + const bool use_quadratic_cost_penalty, + const bool downsample_obstacle_heuristic); + + inline float distanceHeuristic2D( + const uint64_t idx, const unsigned int size_x, + const unsigned int target_x, const unsigned int target_y) + { + int dx = static_cast(idx % size_x) - static_cast(target_x); + int dy = static_cast(idx / size_x) - static_cast(target_y); + return std::sqrt(dx * dx + dy * dy); + } + +protected: + LookupTable obstacle_heuristic_lookup_table_; + ObstacleHeuristicQueue obstacle_heuristic_queue_; + std::shared_ptr costmap_ros; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp new file mode 100644 index 00000000..c5a2cd84 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ +#define ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ + +#include +#include +#include +#include + +#include "athena_smac_planner/a_star.hpp" +#include "athena_smac_planner/smoother.hpp" +#include "athena_smac_planner/utils.hpp" +#include "athena_smac_planner/costmap_downsampler.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_array.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.hpp" + +namespace athena_smac_planner +{ + +class SmacPlannerHybrid +{ +public: + /** + * @brief Constructor — initializes the planner from ROS parameters. + * @param node Shared ptr to an rclcpp::Node for parameter reading and publisher creation. + * @param costmap_ros Costmap2DROS providing the collision costmap. + * @param name Parameter namespace prefix (default "SmacPlannerHybrid"). + */ + SmacPlannerHybrid( + rclcpp::Node::SharedPtr node, + std::shared_ptr costmap_ros, + const std::string & name = "SmacPlannerHybrid"); + + /** + * @brief Destructor + */ + ~SmacPlannerHybrid(); + + /** + * @brief Create a path from start to goal. + * @param start Start pose in map frame. + * @param goal Goal pose in map frame. + * @param cancel_checker Callable that returns true when planning should abort. + * @return nav_msgs::msg::Path of the generated path. + */ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker); + +protected: + std::unique_ptr> _a_star; + GridCollisionChecker _collision_checker; + std::unique_ptr _smoother; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")}; + nav2_costmap_2d::Costmap2D * _costmap; + std::shared_ptr _costmap_ros; + std::unique_ptr _costmap_downsampler; + std::string _global_frame, _name; + float _lookup_table_dim; + float _tolerance; + bool _downsample_costmap; + int _downsampling_factor; + double _angle_bin_size; + unsigned int _angle_quantizations; + bool _allow_unknown; + int _max_iterations; + int _max_on_approach_iterations; + int _terminal_checking_interval; + SearchInfo _search_info; + double _max_planning_time; + double _lookup_table_size; + double _minimum_turning_radius_global_coords; + bool _debug_visualizations; + std::string _motion_model_for_search; + MotionModel _motion_model; + GoalHeadingMode _goal_heading_mode; + int _coarse_search_resolution; + rclcpp::Publisher::SharedPtr _raw_plan_publisher; + rclcpp::Publisher::SharedPtr + _planned_footprints_publisher; + rclcpp::Publisher::SharedPtr + _smoothed_footprints_publisher; + rclcpp::Publisher::SharedPtr + _expansions_publisher; + std::mutex _mutex; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp new file mode 100644 index 00000000..06997543 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp @@ -0,0 +1,208 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ +#define ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ + +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/constants.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "ompl/base/StateSpace.h" + +namespace athena_smac_planner +{ + +/** + * @struct athena_smac_planner::BoundaryPoints + * @brief Set of boundary condition points from expansion + */ +struct BoundaryPoints +{ + /** + * @brief A constructor for BoundaryPoints + */ + BoundaryPoints(double & x_in, double & y_in, double & theta_in) + : x(x_in), y(y_in), theta(theta_in) + {} + + double x; + double y; + double theta; +}; + +/** + * @struct athena_smac_planner::BoundaryExpansion + * @brief Boundary expansion state + */ +struct BoundaryExpansion +{ + double path_end_idx{0.0}; + double expansion_path_length{0.0}; + double original_path_length{0.0}; + std::vector pts; + bool in_collision{false}; +}; + +typedef std::vector BoundaryExpansions; +typedef std::vector::iterator PathIterator; +typedef std::vector::reverse_iterator ReversePathIterator; + +/** + * @class athena_smac_planner::Smoother + * @brief A path smoother implementation + */ +class Smoother +{ +public: + /** + * @brief A constructor for athena_smac_planner::Smoother + */ + explicit Smoother(const SmootherParams & params); + + /** + * @brief A destructor for athena_smac_planner::Smoother + */ + ~Smoother() {} + + /** + * @brief Initialization of the smoother + * @param min_turning_radius Minimum turning radius (m) + * @param motion_model Motion model type + */ + void initialize( + const double & min_turning_radius); + + /** + * @brief Smoother API method + * @param path Reference to path + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smooth( + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time); + +protected: + /** + * @brief Smoother method - does the smoothing on a segment + * @param path Reference to path + * @param reversing_segment Return if this is a reversing segment + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time); + + /** + * @brief Get the field value for a given dimension + * @param msg Current pose to sample + * @param dim Dimension ID of interest + * @return dim value + */ + inline double getFieldByDim( + const geometry_msgs::msg::PoseStamped & msg, + const unsigned int & dim); + + /** + * @brief Set the field value for a given dimension + * @param msg Current pose to sample + * @param dim Dimension ID of interest + * @param value to set the dimension to for the pose + */ + inline void setFieldByDim( + geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, + const double & value); + + /** + * @brief Enforced minimum curvature boundary conditions on plan output + * the robot is traveling in the same direction (e.g. forward vs reverse) + * @param start_pose Start pose of the feasible path to maintain + * @param path Path to modify for curvature constraints on start / end of path + * @param costmap Costmap to check for collisions + * @param reversing_segment Whether this path segment is in reverse + */ + void enforceStartBoundaryConditions( + const geometry_msgs::msg::Pose & start_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment); + + /** + * @brief Enforced minimum curvature boundary conditions on plan output + * the robot is traveling in the same direction (e.g. forward vs reverse) + * @param end_pose End pose of the feasible path to maintain + * @param path Path to modify for curvature constraints on start / end of path + * @param costmap Costmap to check for collisions + * @param reversing_segment Whether this path segment is in reverse + */ + void enforceEndBoundaryConditions( + const geometry_msgs::msg::Pose & end_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment); + + /** + * @brief Given a set of boundary expansion, find the one which is shortest + * such that it is least likely to contain a loop-de-loop when working with + * close-by primitive markers. Instead, select a further away marker which + * generates a shorter ` + * @param boundary_expansions Set of boundary expansions + * @return Idx of the shorest boundary expansion option + */ + unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions & boundary_expansions); + + /** + * @brief Populate a motion model expansion from start->end into expansion + * @param start Start pose of the feasible path to maintain + * @param end End pose of the feasible path to maintain + * @param expansion Expansion object to populate + * @param costmap Costmap to check for collisions + * @param reversing_segment Whether this path segment is in reverse + */ + void findBoundaryExpansion( + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & end, + BoundaryExpansion & expansion, + const nav2_costmap_2d::Costmap2D * costmap); + + /** + * @brief Generates boundary expansions with end idx at least strategic + * distances away, using either Reverse or (Forward) Path Iterators. + * @param start iterator to start search in path for + * @param end iterator to end search for + * @return Boundary expansions with end idxs populated + */ + template + BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end); + + double min_turning_rad_, tolerance_, data_w_, smooth_w_; + int max_its_, refinement_ctr_, refinement_num_; + bool is_holonomic_, do_refinement_; + MotionModel motion_model_; + ompl::base::StateSpacePtr state_space_; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h new file mode 100644 index 00000000..c6b0fd60 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h @@ -0,0 +1,2539 @@ +// Copyright (c) 2018-2021 Martin Ankerl +// ______ _____ ______ _________ +// ______________ ___ /_ ___(_)_______ ___ /_ ______ ______ ______ / +// __ ___/_ __ \__ __ \__ / __ __ \ __ __ \_ __ \_ __ \_ __ / +// _ / / /_/ /_ /_/ /_ / _ / / / _ / / // /_/ // /_/ // /_/ / +// /_/ \____/ /_.___/ /_/ /_/ /_/ ________/_/ /_/ \____/ \____/ \__,_/ +// _/_____/ +// +// Fast & memory efficient hashtable based on robin hood hashing for C++11/14/17/20 +// https://github.com/martinus/robin-hood-hashing +// +// Licensed under the MIT License . +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2021 Martin Ankerl +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ +#define NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ + +// see https://semver.org/ +#define ROBIN_HOOD_VERSION_MAJOR 3 // for incompatible API changes +#define ROBIN_HOOD_VERSION_MINOR 11 // for adding functionality in a backwards-compatible manner +#define ROBIN_HOOD_VERSION_PATCH 5 // for backwards-compatible bug fixes + +#include +#include +#include +#include +#include +#include // only to support hash of smart pointers +#include +#include +#include +#include +#include +#if __cplusplus >= 201703L +# include +#endif +/* *INDENT-OFF* */ + +// #define ROBIN_HOOD_LOG_ENABLED +#ifdef ROBIN_HOOD_LOG_ENABLED +# include +# define ROBIN_HOOD_LOG(...) \ + std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; +#else +# define ROBIN_HOOD_LOG(x) +#endif + +// #define ROBIN_HOOD_TRACE_ENABLED +#ifdef ROBIN_HOOD_TRACE_ENABLED +# include +# define ROBIN_HOOD_TRACE(...) \ + std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; +#else +# define ROBIN_HOOD_TRACE(x) +#endif + +// #define ROBIN_HOOD_COUNT_ENABLED +#ifdef ROBIN_HOOD_COUNT_ENABLED +# include +# define ROBIN_HOOD_COUNT(x) ++counts().x; +namespace robin_hood { +struct Counts { + uint64_t shiftUp{}; + uint64_t shiftDown{}; +}; +inline std::ostream& operator<<(std::ostream& os, Counts const& c) { + return os << c.shiftUp << " shiftUp" << std::endl << c.shiftDown << " shiftDown" << std::endl; +} + +static Counts& counts() { + static Counts counts{}; + return counts; +} +} // namespace robin_hood +#else +# define ROBIN_HOOD_COUNT(x) +#endif + +// all non-argument macros should use this facility. See +// https://www.fluentcpp.com/2019/05/28/better-macros-better-flags/ +#define ROBIN_HOOD(x) ROBIN_HOOD_PRIVATE_DEFINITION_##x() + +// mark unused members with this macro +#define ROBIN_HOOD_UNUSED(identifier) + +// bitness +#if SIZE_MAX == UINT32_MAX +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 32 +#elif SIZE_MAX == UINT64_MAX +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 +#else +# error Unsupported bitness +#endif + +// endianness +#ifdef _MSC_VER +# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() 1 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() 0 +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() \ + (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) +# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) +#endif + +// inline +#ifdef _MSC_VER +# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __declspec(noinline) +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __attribute__((noinline)) +#endif + +// exceptions +#if !defined(__cpp_exceptions) && !defined(__EXCEPTIONS) && !defined(_CPPUNWIND) +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 0 +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 1 +#endif + +// count leading/trailing bits +#if !defined(ROBIN_HOOD_DISABLE_INTRINSICS) +# ifdef _MSC_VER +# if ROBIN_HOOD(BITNESS) == 32 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward64 +# endif +# include +# pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD)) +# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ + [](size_t mask) noexcept -> int { \ + unsigned long index; \ // NOLINT + return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast(index) \ + : ROBIN_HOOD(BITNESS); \ + }(x) +# else +# if ROBIN_HOOD(BITNESS) == 32 +# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzl +# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzl +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzll +# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzll +# endif +# define ROBIN_HOOD_COUNT_LEADING_ZEROES(x) ((x) ? ROBIN_HOOD(CLZ)(x) : ROBIN_HOOD(BITNESS)) +# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) ((x) ? ROBIN_HOOD(CTZ)(x) : ROBIN_HOOD(BITNESS)) +# endif +#endif + +// fallthrough +#ifndef __has_cpp_attribute // For backwards compatibility +# define __has_cpp_attribute(x) 0 +#endif +#if __has_cpp_attribute(clang::fallthrough) +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[clang::fallthrough]] +#elif __has_cpp_attribute(gnu::fallthrough) +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[gnu::fallthrough]] +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() +#endif + +// likely/unlikely +#ifdef _MSC_VER +# define ROBIN_HOOD_LIKELY(condition) condition +# define ROBIN_HOOD_UNLIKELY(condition) condition +#else +# define ROBIN_HOOD_LIKELY(condition) __builtin_expect(condition, 1) +# define ROBIN_HOOD_UNLIKELY(condition) __builtin_expect(condition, 0) +#endif + +// detect if native wchar_t type is available in MSVC +#ifdef _MSC_VER +# ifdef _NATIVE_WCHAR_T_DEFINED +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 0 +# endif +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 +#endif + +// detect if MSVC supports the pair(std::piecewise_construct_t,...) constructor being constexpr +#ifdef _MSC_VER +# if _MSC_VER <= 1900 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 1 +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 +# endif +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 +#endif + +// workaround missing "is_trivially_copyable" in g++ < 5.0 +// See https://stackoverflow.com/a/31798726/48181 +#if defined(__GNUC__) && __GNUC__ < 5 +# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) __has_trivial_copy(__VA_ARGS__) +#else +# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) std::is_trivially_copyable<__VA_ARGS__>::value +#endif + +// helpers for C++ versions, see https://gcc.gnu.org/onlinedocs/cpp/Standard-Predefined-Macros.html +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX() __cplusplus +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX98() 199711L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX11() 201103L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX14() 201402L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX17() 201703L + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) +# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() [[nodiscard]] +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() +#endif + +namespace robin_hood { + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) +# define ROBIN_HOOD_STD std +#else + +// c++11 compatibility layer +namespace ROBIN_HOOD_STD { +template +struct alignment_of + : std::integral_constant::type)> {}; + +template +class integer_sequence { +public: + using value_type = T; + static_assert(std::is_integral::value, "not integral type"); + static constexpr std::size_t size() noexcept { + return sizeof...(Ints); + } +}; +template +using index_sequence = integer_sequence; + +namespace detail_ { +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0 && Begin < End, "unexpected argument (Begin<0 || Begin<=End)"); + + template + struct IntSeqCombiner; + + template + struct IntSeqCombiner, integer_sequence> { + using TResult = integer_sequence; + }; + + using TResult = + typename IntSeqCombiner::TResult, + typename IntSeqImpl::TResult>::TResult; +}; + +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0, "unexpected argument (Begin<0)"); + using TResult = integer_sequence; +}; + +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0, "unexpected argument (Begin<0)"); + using TResult = integer_sequence; +}; +} // namespace detail_ + +template +using make_integer_sequence = typename detail_::IntSeqImpl::TResult; + +template +using make_index_sequence = make_integer_sequence; + +template +using index_sequence_for = make_index_sequence; + +} // namespace ROBIN_HOOD_STD + +#endif + +namespace detail { + +// make sure we static_cast to the correct type for hash_int +#if ROBIN_HOOD(BITNESS) == 64 +using SizeT = uint64_t; +#else +using SizeT = uint32_t; +#endif + +template +T rotr(T x, unsigned k) { + return (x >> k) | (x << (8U * sizeof(T) - k)); +} + +// This cast gets rid of warnings like "cast from 'uint8_t*' {aka 'unsigned char*'} to +// 'uint64_t*' {aka 'long unsigned int*'} increases required alignment of target type". Use with +// care! +template +inline T reinterpret_cast_no_cast_align_warning(void* ptr) noexcept { + return reinterpret_cast(ptr); +} + +template +inline T reinterpret_cast_no_cast_align_warning(void const* ptr) noexcept { + return reinterpret_cast(ptr); +} + +// make sure this is not inlined as it is slow and dramatically enlarges code, thus making other +// inlinings more difficult. Throws are also generally the slow path. +template +[[noreturn]] ROBIN_HOOD(NOINLINE) +#if ROBIN_HOOD(HAS_EXCEPTIONS) + void doThrow(Args&&... args) { + throw E(std::forward(args)...); +} +#else + void doThrow(Args&&... ROBIN_HOOD_UNUSED(args) /*unused*/) { + abort(); +} +#endif + +template +T* assertNotNull(T* t, Args&&... args) { + if (ROBIN_HOOD_UNLIKELY(nullptr == t)) { + doThrow(std::forward(args)...); + } + return t; +} + +template +inline T unaligned_load(void const* ptr) noexcept { + // using memcpy so we don't get into unaligned load problems. + // compiler should optimize this very well anyways. + T t; + std::memcpy(&t, ptr, sizeof(T)); + return t; +} + +// Allocates bulks of memory for objects of type T. This deallocates the memory in the destructor, +// and keeps a linked list of the allocated memory around. Overhead per allocation is the size of a +// pointer. +template +class BulkPoolAllocator { +public: + BulkPoolAllocator() noexcept = default; + + // does not copy anything, just creates a new allocator. + BulkPoolAllocator(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept + : mHead(nullptr) + , mListForFree(nullptr) {} + + BulkPoolAllocator(BulkPoolAllocator&& o) noexcept + : mHead(o.mHead) + , mListForFree(o.mListForFree) { + o.mListForFree = nullptr; + o.mHead = nullptr; + } + + BulkPoolAllocator& operator=(BulkPoolAllocator&& o) noexcept { + reset(); + mHead = o.mHead; + mListForFree = o.mListForFree; + o.mListForFree = nullptr; + o.mHead = nullptr; + return *this; + } + + BulkPoolAllocator& + operator=(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept { + // does not do anything + return *this; + } + + ~BulkPoolAllocator() noexcept { + reset(); + } + + // Deallocates all allocated memory. + void reset() noexcept { + while (mListForFree) { + T* tmp = *mListForFree; + ROBIN_HOOD_LOG("std::free") + std::free(mListForFree); + mListForFree = reinterpret_cast_no_cast_align_warning(tmp); + } + mHead = nullptr; + } + + // allocates, but does NOT initialize. Use in-place new constructor, e.g. + // T* obj = pool.allocate(); + // ::new (static_cast(obj)) T(); + T* allocate() { + T* tmp = mHead; + if (!tmp) { + tmp = performAllocation(); + } + + mHead = *reinterpret_cast_no_cast_align_warning(tmp); + return tmp; + } + + // does not actually deallocate but puts it in store. + // make sure you have already called the destructor! e.g. with + // obj->~T(); + // pool.deallocate(obj); + void deallocate(T* obj) noexcept { + *reinterpret_cast_no_cast_align_warning(obj) = mHead; + mHead = obj; + } + + // Adds an already allocated block of memory to the allocator. This allocator is from now on + // responsible for freeing the data (with free()). If the provided data is not large enough to + // make use of, it is immediately freed. Otherwise it is reused and freed in the destructor. + void addOrFree(void* ptr, const size_t numBytes) noexcept { + // calculate number of available elements in ptr + if (numBytes < ALIGNMENT + ALIGNED_SIZE) { + // not enough data for at least one element. Free and return. + ROBIN_HOOD_LOG("std::free") + std::free(ptr); + } else { + ROBIN_HOOD_LOG("add to buffer") + add(ptr, numBytes); + } + } + + void swap(BulkPoolAllocator& other) noexcept { + using std::swap; + swap(mHead, other.mHead); + swap(mListForFree, other.mListForFree); + } + +private: + // iterates the list of allocated memory to calculate how many to alloc next. + // Recalculating this each time saves us a size_t member. + // This ignores the fact that memory blocks might have been added manually with addOrFree. In + // practice, this should not matter much. + ROBIN_HOOD(NODISCARD) size_t calcNumElementsToAlloc() const noexcept { + auto tmp = mListForFree; + size_t numAllocs = MinNumAllocs; + + while (numAllocs * 2 <= MaxNumAllocs && tmp) { + auto x = reinterpret_cast(tmp); // NOLINT + tmp = *x; + numAllocs *= 2; + } + + return numAllocs; + } + + // WARNING: Underflow if numBytes < ALIGNMENT! This is guarded in addOrFree(). + void add(void* ptr, const size_t numBytes) noexcept { + const size_t numElements = (numBytes - ALIGNMENT) / ALIGNED_SIZE; + auto data = reinterpret_cast(ptr); + + // link free list + auto x = reinterpret_cast(data); + *x = mListForFree; + mListForFree = data; + + // create linked list for newly allocated data + auto* const headT = + reinterpret_cast_no_cast_align_warning(reinterpret_cast(ptr) + ALIGNMENT); + + auto* const head = reinterpret_cast(headT); + + // Visual Studio compiler automatically unrolls this loop, which is pretty cool + for (size_t i = 0; i < numElements; ++i) { + *reinterpret_cast_no_cast_align_warning(head + i * ALIGNED_SIZE) = + head + (i + 1) * ALIGNED_SIZE; + } + + // last one points to 0 + *reinterpret_cast_no_cast_align_warning(head + (numElements - 1) * ALIGNED_SIZE) = + mHead; + mHead = headT; + } + + // Called when no memory is available (mHead == 0). + // Don't inline this slow path. + ROBIN_HOOD(NOINLINE) T* performAllocation() { + size_t const numElementsToAlloc = calcNumElementsToAlloc(); + + // alloc new memory: [prev |T, T, ... T] + size_t const bytes = ALIGNMENT + ALIGNED_SIZE * numElementsToAlloc; + ROBIN_HOOD_LOG("std::malloc " << bytes << " = " << ALIGNMENT << " + " << ALIGNED_SIZE + << " * " << numElementsToAlloc) + add(assertNotNull(std::malloc(bytes)), bytes); + return mHead; + } + + // enforce byte alignment of the T's +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) + static constexpr size_t ALIGNMENT = + (std::max)(std::alignment_of::value, std::alignment_of::value); +#else + static const size_t ALIGNMENT = + (ROBIN_HOOD_STD::alignment_of::value > ROBIN_HOOD_STD::alignment_of::value) + ? ROBIN_HOOD_STD::alignment_of::value + : +ROBIN_HOOD_STD::alignment_of::value; // the + is for walkarround +#endif + + static constexpr size_t ALIGNED_SIZE = ((sizeof(T) - 1) / ALIGNMENT + 1) * ALIGNMENT; + + static_assert(MinNumAllocs >= 1, "MinNumAllocs"); + static_assert(MaxNumAllocs >= MinNumAllocs, "MaxNumAllocs"); + static_assert(ALIGNED_SIZE >= sizeof(T*), "ALIGNED_SIZE"); + static_assert(0 == (ALIGNED_SIZE % sizeof(T*)), "ALIGNED_SIZE mod"); + static_assert(ALIGNMENT >= sizeof(T*), "ALIGNMENT"); + + T* mHead{nullptr}; + T** mListForFree{nullptr}; +}; + +template +struct NodeAllocator; + +// dummy allocator that does nothing +template +struct NodeAllocator { + // we are not using the data, so just free it. + void addOrFree(void* ptr, size_t ROBIN_HOOD_UNUSED(numBytes)/*unused*/) noexcept { + ROBIN_HOOD_LOG("std::free") + std::free(ptr); + } +}; + +template +struct NodeAllocator : public BulkPoolAllocator {}; + +// c++14 doesn't have is_nothrow_swappable, and clang++ 6.0.1 doesn't like it either, so I'm making +// my own here. +namespace swappable { +#if ROBIN_HOOD(CXX) < ROBIN_HOOD(CXX17) +using std::swap; +template +struct nothrow { + static const bool value = noexcept(swap(std::declval(), std::declval())); +}; +#else +template +struct nothrow { + static const bool value = std::is_nothrow_swappable::value; +}; +#endif +} // namespace swappable + +} // namespace detail + +struct is_transparent_tag {}; + +// A custom pair implementation is used in the map because std::pair is not is_trivially_copyable, +// which means it would not be allowed to be used in std::memcpy. This struct is copiable, which is +// also tested. +template +struct pair { + using first_type = T1; + using second_type = T2; + + template ::value && + std::is_default_constructible::value>::type> + constexpr pair() noexcept(noexcept(U1()) && noexcept(U2())) + : first() + , second() {} + + // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. + explicit constexpr pair(std::pair const& o) noexcept( + noexcept(T1(std::declval())) && noexcept(T2(std::declval()))) + : first(o.first) + , second(o.second) {} + + // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. + explicit constexpr pair(std::pair&& o) noexcept(noexcept( + T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) + : first(std::move(o.first)) + , second(std::move(o.second)) {} + + constexpr pair(T1&& a, T2&& b) noexcept(noexcept( + T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) + : first(std::move(a)) + , second(std::move(b)) {} + + template + constexpr pair(U1&& a, U2&& b) noexcept(noexcept(T1(std::forward( + std::declval()))) && noexcept(T2(std::forward(std::declval())))) + : first(std::forward(a)) + , second(std::forward(b)) {} + + template + // MSVC 2015 produces error "C2476: ‘constexpr’ constructor does not initialize all members" + // if this constructor is constexpr +#if !ROBIN_HOOD(BROKEN_CONSTEXPR) + constexpr +#endif + pair(std::piecewise_construct_t /*unused*/, std::tuple a, + std::tuple + b) noexcept(noexcept(pair(std::declval&>(), + std::declval&>(), + ROBIN_HOOD_STD::index_sequence_for(), + ROBIN_HOOD_STD::index_sequence_for()))) + : pair(a, b, ROBIN_HOOD_STD::index_sequence_for(), + ROBIN_HOOD_STD::index_sequence_for()) { + } + + // constructor called from the std::piecewise_construct_t ctor + template + pair( + std::tuple& a, std::tuple& b, + ROBIN_HOOD_STD::index_sequence /*unused*/, + ROBIN_HOOD_STD::index_sequence /*unused*/) noexcept( + noexcept(T1(std::forward(std::get( + std::declval&>()))...)) && noexcept(T2(std:: + forward(std::get( + std::declval&>()))...))) + : first(std::forward(std::get(a))...) + , second(std::forward(std::get(b))...) { + // make visual studio compiler happy about warning about unused a & b. + // Visual studio's pair implementation disables warning 4100. + (void)a; + (void)b; + } + + void swap(pair& o) noexcept((detail::swappable::nothrow::value) && + (detail::swappable::nothrow::value)) { + using std::swap; + swap(first, o.first); + swap(second, o.second); + } + + T1 first; // NOLINT + T2 second; // NOLINT +}; + +template +inline void swap(pair& a, pair& b) noexcept( + noexcept(std::declval&>().swap(std::declval&>()))) { + a.swap(b); +} + +template +inline constexpr bool operator==(pair const& x, pair const& y) { + return (x.first == y.first) && (x.second == y.second); +} +template +inline constexpr bool operator!=(pair const& x, pair const& y) { + return !(x == y); +} +template +inline constexpr bool operator<(pair const& x, pair const& y) noexcept(noexcept( + std::declval() < std::declval()) && noexcept(std::declval() < + std::declval())) { + return x.first < y.first || (!(y.first < x.first) && x.second < y.second); +} +template +inline constexpr bool operator>(pair const& x, pair const& y) { + return y < x; +} +template +inline constexpr bool operator<=(pair const& x, pair const& y) { + return !(x > y); +} +template +inline constexpr bool operator>=(pair const& x, pair const& y) { + return !(x < y); +} + +inline size_t hash_bytes(void const* ptr, size_t len) noexcept { + static constexpr uint64_t m = UINT64_C(0xc6a4a7935bd1e995); + static constexpr uint64_t seed = UINT64_C(0xe17a1465); + static constexpr unsigned int r = 47; + + auto const* const data64 = static_cast(ptr); + uint64_t h = seed ^ (len * m); + + size_t const n_blocks = len / 8; + for (size_t i = 0; i < n_blocks; ++i) { + auto k = detail::unaligned_load(data64 + i); + + k *= m; + k ^= k >> r; + k *= m; + + h ^= k; + h *= m; + } + + auto const* const data8 = reinterpret_cast(data64 + n_blocks); + switch (len & 7U) { + case 7: + h ^= static_cast(data8[6]) << 48U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 6: + h ^= static_cast(data8[5]) << 40U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 5: + h ^= static_cast(data8[4]) << 32U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 4: + h ^= static_cast(data8[3]) << 24U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 3: + h ^= static_cast(data8[2]) << 16U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 2: + h ^= static_cast(data8[1]) << 8U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 1: + h ^= static_cast(data8[0]); + h *= m; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + default: + break; + } + + h ^= h >> r; + + // not doing the final step here, because this will be done by keyToIdx anyways + // h *= m; + // h ^= h >> r; + return static_cast(h); +} + +inline size_t hash_int(uint64_t x) noexcept { + // tried lots of different hashes, let's stick with murmurhash3. It's simple, fast, well tested, + // and doesn't need any special 128bit operations. + x ^= x >> 33U; + x *= UINT64_C(0xff51afd7ed558ccd); + x ^= x >> 33U; + + // not doing the final step here, because this will be done by keyToIdx anyways + // x *= UINT64_C(0xc4ceb9fe1a85ec53); + // x ^= x >> 33U; + return static_cast(x); +} + +// A thin wrapper around std::hash, performing an additional simple mixing step of the result. +template +struct hash : public std::hash { + size_t operator()(T const& obj) const + noexcept(noexcept(std::declval>().operator()(std::declval()))) { + // call base hash + auto result = std::hash::operator()(obj); + // return mixed of that, to be save against identity has + return hash_int(static_cast(result)); + } +}; + +template +struct hash> { + size_t operator()(std::basic_string const& str) const noexcept { + return hash_bytes(str.data(), sizeof(CharT) * str.size()); + } +}; + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) +template +struct hash> { + size_t operator()(std::basic_string_view const& sv) const noexcept { + return hash_bytes(sv.data(), sizeof(CharT) * sv.size()); + } +}; +#endif + +template +struct hash { + size_t operator()(T* ptr) const noexcept { + return hash_int(reinterpret_cast(ptr)); + } +}; + +template +struct hash> { + size_t operator()(std::unique_ptr const& ptr) const noexcept { + return hash_int(reinterpret_cast(ptr.get())); + } +}; + +template +struct hash> { + size_t operator()(std::shared_ptr const& ptr) const noexcept { + return hash_int(reinterpret_cast(ptr.get())); + } +}; + +template +struct hash::value>::type> { + size_t operator()(Enum e) const noexcept { + using Underlying = typename std::underlying_type::type; + return hash{}(static_cast(e)); + } +}; + +#define ROBIN_HOOD_HASH_INT(T) \ + template <> \ + struct hash { \ + size_t operator()(T const& obj) const noexcept { \ + return hash_int(static_cast(obj)); \ + } \ + } + +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wuseless-cast" +#endif +// see https://en.cppreference.com/w/cpp/utility/hash +ROBIN_HOOD_HASH_INT(bool); +ROBIN_HOOD_HASH_INT(char); +ROBIN_HOOD_HASH_INT(signed char); +ROBIN_HOOD_HASH_INT(unsigned char); +ROBIN_HOOD_HASH_INT(char16_t); +ROBIN_HOOD_HASH_INT(char32_t); +#if ROBIN_HOOD(HAS_NATIVE_WCHART) +ROBIN_HOOD_HASH_INT(wchar_t); +#endif +ROBIN_HOOD_HASH_INT(short); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned short); // NOLINT +ROBIN_HOOD_HASH_INT(int); +ROBIN_HOOD_HASH_INT(unsigned int); +ROBIN_HOOD_HASH_INT(long); // NOLINT +ROBIN_HOOD_HASH_INT(long long); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned long); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned long long); // NOLINT +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic pop +#endif +namespace detail { + +template +struct void_type { + using type = void; +}; + +template +struct has_is_transparent : public std::false_type {}; + +template +struct has_is_transparent::type> + : public std::true_type {}; + +// using wrapper classes for hash and key_equal prevents the diamond problem when the same type +// is used. see https://stackoverflow.com/a/28771920/48181 +template +struct WrapHash : public T { + WrapHash() = default; + explicit WrapHash(T const& o) noexcept(noexcept(T(std::declval()))) + : T(o) {} +}; + +template +struct WrapKeyEqual : public T { + WrapKeyEqual() = default; + explicit WrapKeyEqual(T const& o) noexcept(noexcept(T(std::declval()))) + : T(o) {} +}; + +// A highly optimized hashmap implementation, using the Robin Hood algorithm. +// +// In most cases, this map should be usable as a drop-in replacement for std::unordered_map, but +// be about 2x faster in most cases and require much less allocations. +// +// This implementation uses the following memory layout: +// +// [Node, Node, ... Node | info, info, ... infoSentinel ] +// +// * Node: either a DataNode that directly has the std::pair as member, +// or a DataNode with a pointer to std::pair. Which DataNode representation to use +// depends on how fast the swap() operation is. Heuristically, this is automatically chosen +// based on sizeof(). there are always 2^n Nodes. +// +// * info: Each Node in the map has a corresponding info byte, so there are 2^n info bytes. +// Each byte is initialized to 0, meaning the corresponding Node is empty. Set to 1 means the +// corresponding node contains data. Set to 2 means the corresponding Node is filled, but it +// actually belongs to the previous position and was pushed out because that place is already +// taken. +// +// * infoSentinel: Sentinel byte set to 1, so that iterator's ++ can stop at end() without the +// need for a idx variable. +// +// According to STL, order of templates has effect on throughput. That's why I've moved the +// boolean to the front. +// https://www.reddit.com/r/cpp/comments/ahp6iu/compile_time_binary_size_reductions_and_cs_future/eeguck4/ +template +class Table + : public WrapHash, + public WrapKeyEqual, + detail::NodeAllocator< + typename std::conditional< + std::is_void::value, Key, + robin_hood::pair::type, T>>::type, + 4, 16384, IsFlat> { +public: + static constexpr bool is_flat = IsFlat; + static constexpr bool is_map = !std::is_void::value; + static constexpr bool is_set = !is_map; + static constexpr bool is_transparent = + has_is_transparent::value && has_is_transparent::value; + + using key_type = Key; + using mapped_type = T; + using value_type = typename std::conditional< + is_set, Key, + robin_hood::pair::type, T>>::type; + using size_type = size_t; + using hasher = Hash; + using key_equal = KeyEqual; + using Self = Table; + +private: + static_assert(MaxLoadFactor100 > 10 && MaxLoadFactor100 < 100, + "MaxLoadFactor100 needs to be >10 && < 100"); + + using WHash = WrapHash; + using WKeyEqual = WrapKeyEqual; + + // configuration defaults + + // make sure we have 8 elements, needed to quickly rehash mInfo + static constexpr size_t InitialNumElements = sizeof(uint64_t); + static constexpr uint32_t InitialInfoNumBits = 5; + static constexpr uint8_t InitialInfoInc = 1U << InitialInfoNumBits; + static constexpr size_t InfoMask = InitialInfoInc - 1U; + static constexpr uint8_t InitialInfoHashShift = 0; + using DataPool = detail::NodeAllocator; + + // type needs to be wider than uint8_t. + using InfoType = uint32_t; + + // DataNode //////////////////////////////////////////////////////// + + // Primary template for the data node. We have special implementations for small and big + // objects. For large objects it is assumed that swap() is fairly slow, so we allocate these + // on the heap so swap merely swaps a pointer. + template + class DataNode {}; + + // Small: just allocate on the stack. + template + class DataNode final { + public: + template + explicit DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, Args&&... args) noexcept( + noexcept(value_type(std::forward(args)...))) + : mData(std::forward(args)...) {} + + DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept( + std::is_nothrow_move_constructible::value) + : mData(std::move(n.mData)) {} + + // doesn't do anything + void destroy(M& ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {} + void destroyDoNotDeallocate() noexcept {} + + value_type const* operator->() const noexcept { + return &mData; + } + value_type* operator->() noexcept { + return &mData; + } + + const value_type& operator*() const noexcept { + return mData; + } + + value_type& operator*() noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData.first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type + getFirst() const noexcept { + return mData.first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() const noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() noexcept { + return mData.second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() const noexcept { + return mData.second; + } + + void swap(DataNode& o) noexcept( + noexcept(std::declval().swap(std::declval()))) { + mData.swap(o.mData); + } + + private: + value_type mData; + }; + + // big object: allocate on heap. + template + class DataNode { + public: + template + explicit DataNode(M& map, Args&&... args) + : mData(map.allocate()) { + ::new (static_cast(mData)) value_type(std::forward(args)...); + } + + DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept + : mData(std::move(n.mData)) {} + + void destroy(M& map) noexcept { + // don't deallocate, just put it into list of datapool. + mData->~value_type(); + map.deallocate(mData); + } + + void destroyDoNotDeallocate() noexcept { + mData->~value_type(); + } + + value_type const* operator->() const noexcept { + return mData; + } + + value_type* operator->() noexcept { + return mData; + } + + const value_type& operator*() const { + return *mData; + } + + value_type& operator*() { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData->first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type + getFirst() const noexcept { + return mData->first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() const noexcept { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() noexcept { + return mData->second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() const noexcept { + return mData->second; + } + + void swap(DataNode& o) noexcept { + using std::swap; + swap(mData, o.mData); + } + + private: + value_type* mData; + }; + + using Node = DataNode; + + // helpers for insertKeyPrepareEmptySpot: extract first entry (only const required) + ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(Node const& n) const noexcept { + return n.getFirst(); + } + + // in case we have void mapped_type, we are not using a pair, thus we just route k through. + // No need to disable this because it's just not used if not applicable. + ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(key_type const& k) const noexcept { + return k; + } + + // in case we have non-void mapped_type, we have a standard robin_hood::pair + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, key_type const&>::type + getFirstConst(value_type const& vt) const noexcept { + return vt.first; + } + + // Cloner ////////////////////////////////////////////////////////// + + template + struct Cloner; + + // fast path: Just copy data, without allocating anything. + template + struct Cloner { + void operator()(M const& source, M& target) const { + auto const* const src = reinterpret_cast(source.mKeyVals); + auto* tgt = reinterpret_cast(target.mKeyVals); + auto const numElementsWithBuffer = target.calcNumElementsWithBuffer(target.mMask + 1); + std::copy(src, src + target.calcNumBytesTotal(numElementsWithBuffer), tgt); + } + }; + + template + struct Cloner { + void operator()(M const& s, M& t) const { + auto const numElementsWithBuffer = t.calcNumElementsWithBuffer(t.mMask + 1); + std::copy(s.mInfo, s.mInfo + t.calcNumBytesInfo(numElementsWithBuffer), t.mInfo); + + for (size_t i = 0; i < numElementsWithBuffer; ++i) { + if (t.mInfo[i]) { + ::new (static_cast(t.mKeyVals + i)) Node(t, *s.mKeyVals[i]); + } + } + } + }; + + // Destroyer /////////////////////////////////////////////////////// + + template + struct Destroyer {}; + + template + struct Destroyer { + void nodes(M& m) const noexcept { + m.mNumElements = 0; + } + + void nodesDoNotDeallocate(M& m) const noexcept { + m.mNumElements = 0; + } + }; + + template + struct Destroyer { + void nodes(M& m) const noexcept { + m.mNumElements = 0; + // clear also resets mInfo to 0, that's sometimes not necessary. + auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); + + for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { + if (0 != m.mInfo[idx]) { + Node& n = m.mKeyVals[idx]; + n.destroy(m); + n.~Node(); + } + } + } + + void nodesDoNotDeallocate(M& m) const noexcept { + m.mNumElements = 0; + // clear also resets mInfo to 0, that's sometimes not necessary. + auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); + for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { + if (0 != m.mInfo[idx]) { + Node& n = m.mKeyVals[idx]; + n.destroyDoNotDeallocate(); + n.~Node(); + } + } + } + }; + + // Iter //////////////////////////////////////////////////////////// + + struct fast_forward_tag {}; + + // generic iterator for both const_iterator and iterator. + template + class Iter { + private: + using NodePtr = typename std::conditional::type; + + public: + using difference_type = std::ptrdiff_t; + using value_type = typename Self::value_type; + using reference = typename std::conditional::type; + using pointer = typename std::conditional::type; + using iterator_category = std::forward_iterator_tag; + + // default constructed iterator can be compared to itself, but WON'T return true when + // compared to end(). + Iter() = default; + + // Rule of zero: nothing specified. The conversion constructor is only enabled for + // iterator to const_iterator, so it doesn't accidentally work as a copy ctor. + + // Conversion constructor from iterator to const_iterator. + template ::type> + Iter(Iter const& other) noexcept + : mKeyVals(other.mKeyVals) + , mInfo(other.mInfo) {} + + Iter(NodePtr valPtr, uint8_t const* infoPtr) noexcept + : mKeyVals(valPtr) + , mInfo(infoPtr) {} + + Iter(NodePtr valPtr, uint8_t const* infoPtr, + fast_forward_tag ROBIN_HOOD_UNUSED(tag) /*unused*/) noexcept + : mKeyVals(valPtr) + , mInfo(infoPtr) { + fastForward(); + } + + template ::type> + Iter& operator=(Iter const& other) noexcept { + mKeyVals = other.mKeyVals; + mInfo = other.mInfo; + return *this; + } + + // prefix increment. Undefined behavior if we are at end()! + Iter& operator++() noexcept { + mInfo++; + mKeyVals++; + fastForward(); + return *this; + } + + Iter operator++(int) noexcept { + Iter tmp = *this; + ++(*this); + return tmp; + } + + reference operator*() const { + return **mKeyVals; + } + + pointer operator->() const { + return &**mKeyVals; + } + + template + bool operator==(Iter const& o) const noexcept { + return mKeyVals == o.mKeyVals; + } + + template + bool operator!=(Iter const& o) const noexcept { + return mKeyVals != o.mKeyVals; + } + + private: + // fast forward to the next non-free info byte + // I've tried a few variants that don't depend on intrinsics, but unfortunately they are + // quite a bit slower than this one. So I've reverted that change again. See map_benchmark. + void fastForward() noexcept { + size_t n = 0; + while (0U == (n = detail::unaligned_load(mInfo))) { + mInfo += sizeof(size_t); + mKeyVals += sizeof(size_t); + } +#if defined(ROBIN_HOOD_DISABLE_INTRINSICS) + // we know for certain that within the next 8 bytes we'll find a non-zero one. + if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { + mInfo += 4; + mKeyVals += 4; + } + if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { + mInfo += 2; + mKeyVals += 2; + } + if (ROBIN_HOOD_UNLIKELY(0U == *mInfo)) { + mInfo += 1; + mKeyVals += 1; + } +#else +# if ROBIN_HOOD(LITTLE_ENDIAN) + auto inc = ROBIN_HOOD_COUNT_TRAILING_ZEROES(n) / 8; +# else + auto inc = ROBIN_HOOD_COUNT_LEADING_ZEROES(n) / 8; +# endif + mInfo += inc; + mKeyVals += inc; +#endif + } + + friend class Table; + NodePtr mKeyVals{nullptr}; + uint8_t const* mInfo{nullptr}; + }; + + //////////////////////////////////////////////////////////////////// + + // highly performance relevant code. + // Lower bits are used for indexing into the array (2^n size) + // The upper 1-5 bits need to be a reasonable good hash, to save comparisons. + template + void keyToIdx(HashKey&& key, size_t* idx, InfoType* info) const { + // In addition to whatever hash is used, add another mul & shift so we get better hashing. + // This serves as a bad hash prevention, if the given data is + // badly mixed. + auto h = static_cast(WHash::operator()(key)); + + h *= mHashMultiplier; + h ^= h >> 33U; + + // the lower InitialInfoNumBits are reserved for info. + *info = mInfoInc + static_cast((h & InfoMask) >> mInfoHashShift); + *idx = (static_cast(h) >> InitialInfoNumBits) & mMask; + } + + // forwards the index by one, wrapping around at the end + void next(InfoType* info, size_t* idx) const noexcept { + *idx = *idx + 1; + *info += mInfoInc; + } + + void nextWhileLess(InfoType* info, size_t* idx) const noexcept { + // unrolling this by hand did not bring any speedups. + while (*info < mInfo[*idx]) { + next(info, idx); + } + } + + // Shift everything up by one element. Tries to move stuff around. + void + shiftUp(size_t startIdx, + size_t const insertion_idx) noexcept(std::is_nothrow_move_assignable::value) { + auto idx = startIdx; + ::new (static_cast(mKeyVals + idx)) Node(std::move(mKeyVals[idx - 1])); + while (--idx != insertion_idx) { + mKeyVals[idx] = std::move(mKeyVals[idx - 1]); + } + + idx = startIdx; + while (idx != insertion_idx) { + ROBIN_HOOD_COUNT(shiftUp) + mInfo[idx] = static_cast(mInfo[idx - 1] + mInfoInc); + if (ROBIN_HOOD_UNLIKELY(mInfo[idx] + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + --idx; + } + } + + void shiftDown(size_t idx) noexcept(std::is_nothrow_move_assignable::value) { + // until we find one that is either empty or has zero offset. + // TODO(martinus) we don't need to move everything, just the last one for the same + // bucket. + mKeyVals[idx].destroy(*this); + + // until we find one that is either empty or has zero offset. + while (mInfo[idx + 1] >= 2 * mInfoInc) { + ROBIN_HOOD_COUNT(shiftDown) + mInfo[idx] = static_cast(mInfo[idx + 1] - mInfoInc); + mKeyVals[idx] = std::move(mKeyVals[idx + 1]); + ++idx; + } + + mInfo[idx] = 0; + // don't destroy, we've moved it + // mKeyVals[idx].destroy(*this); + mKeyVals[idx].~Node(); + } + + // copy of find(), except that it returns iterator instead of const_iterator. + template + ROBIN_HOOD(NODISCARD) + size_t findIdx(Other const& key) const { + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + + do { + // unrolling this twice gives a bit of a speedup. More unrolling did not help. + if (info == mInfo[idx] && + ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { + return idx; + } + next(&info, &idx); + if (info == mInfo[idx] && + ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { + return idx; + } + next(&info, &idx); + } while (info <= mInfo[idx]); + + // nothing found! + return mMask == 0 ? 0 + : static_cast(std::distance( + mKeyVals, reinterpret_cast_no_cast_align_warning(mInfo))); + } + + void cloneData(const Table& o) { + Cloner()(o, *this); + } + + // inserts a keyval that is guaranteed to be new, e.g. when the hashmap is resized. + // @return True on success, false if something went wrong + void insert_move(Node&& keyval) { + // we don't retry, fail if overflowing + // don't need to check max num elements + if (0 == mMaxNumElementsAllowed && !try_increase_info()) { + throwOverflowError(); + } + + size_t idx{}; + InfoType info{}; + keyToIdx(keyval.getFirst(), &idx, &info); + + // skip forward. Use <= because we are certain that the element is not there. + while (info <= mInfo[idx]) { + idx = idx + 1; + info += mInfoInc; + } + + // key not found, so we are now exactly where we want to insert it. + auto const insertion_idx = idx; + auto const insertion_info = static_cast(info); + if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + + // find an empty spot + while (0 != mInfo[idx]) { + next(&info, &idx); + } + + auto& l = mKeyVals[insertion_idx]; + if (idx == insertion_idx) { + ::new (static_cast(&l)) Node(std::move(keyval)); + } else { + shiftUp(idx, insertion_idx); + l = std::move(keyval); + } + + // put at empty spot + mInfo[insertion_idx] = insertion_info; + + ++mNumElements; + } + +public: + using iterator = Iter; + using const_iterator = Iter; + + Table() noexcept(noexcept(Hash()) && noexcept(KeyEqual())) + : WHash() + , WKeyEqual() { + ROBIN_HOOD_TRACE(this) + } + + // Creates an empty hash map. Nothing is allocated yet, this happens at the first insert. + // This tremendously speeds up ctor & dtor of a map that never receives an element. The + // penalty is paid at the first insert, and not before. Lookup of this empty map works + // because everybody points to DummyInfoByte::b. parameter bucket_count is dictated by the + // standard, but we can ignore it. + explicit Table( + size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/, const Hash& h = Hash{}, + const KeyEqual& equal = KeyEqual{}) noexcept(noexcept(Hash(h)) && noexcept(KeyEqual(equal))) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + } + + template + Table(Iter first, Iter last, size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, + const Hash& h = Hash{}, const KeyEqual& equal = KeyEqual{}) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + insert(first, last); + } + + Table(std::initializer_list initlist, + size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, const Hash& h = Hash{}, + const KeyEqual& equal = KeyEqual{}) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + insert(initlist.begin(), initlist.end()); + } + + Table(Table&& o) noexcept + : WHash(std::move(static_cast(o))) + , WKeyEqual(std::move(static_cast(o))) + , DataPool(std::move(static_cast(o))) { + ROBIN_HOOD_TRACE(this) + if (o.mMask) { + mHashMultiplier = std::move(o.mHashMultiplier); + mKeyVals = std::move(o.mKeyVals); + mInfo = std::move(o.mInfo); + mNumElements = std::move(o.mNumElements); + mMask = std::move(o.mMask); + mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); + mInfoInc = std::move(o.mInfoInc); + mInfoHashShift = std::move(o.mInfoHashShift); + // set other's mask to 0 so its destructor won't do anything + o.init(); + } + } + + Table& operator=(Table&& o) noexcept { + ROBIN_HOOD_TRACE(this) + if (&o != this) { + if (o.mMask) { + // only move stuff if the other map actually has some data + destroy(); + mHashMultiplier = std::move(o.mHashMultiplier); + mKeyVals = std::move(o.mKeyVals); + mInfo = std::move(o.mInfo); + mNumElements = std::move(o.mNumElements); + mMask = std::move(o.mMask); + mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); + mInfoInc = std::move(o.mInfoInc); + mInfoHashShift = std::move(o.mInfoHashShift); + WHash::operator=(std::move(static_cast(o))); + WKeyEqual::operator=(std::move(static_cast(o))); + DataPool::operator=(std::move(static_cast(o))); + + o.init(); + + } else { + // nothing in the other map => just clear us. + clear(); + } + } + return *this; + } + + Table(const Table& o) + : WHash(static_cast(o)) + , WKeyEqual(static_cast(o)) + , DataPool(static_cast(o)) { + ROBIN_HOOD_TRACE(this) + if (!o.empty()) { + // not empty: create an exact copy. it is also possible to just iterate through all + // elements and insert them, but copying is probably faster. + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + + ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mHashMultiplier = o.mHashMultiplier; + mKeyVals = static_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + // no need for calloc because clonData does memcpy + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + mNumElements = o.mNumElements; + mMask = o.mMask; + mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; + mInfoInc = o.mInfoInc; + mInfoHashShift = o.mInfoHashShift; + cloneData(o); + } + } + + // Creates a copy of the given map. Copy constructor of each entry is used. + // Not sure why clang-tidy thinks this doesn't handle self assignment, it does + Table& operator=(Table const& o) { + ROBIN_HOOD_TRACE(this) + if (&o == this) { + // prevent assigning of itself + return *this; + } + + // we keep using the old allocator and not assign the new one, because we want to keep + // the memory available. when it is the same size. + if (o.empty()) { + if (0 == mMask) { + // nothing to do, we are empty too + return *this; + } + + // not empty: destroy what we have there + // clear also resets mInfo to 0, that's sometimes not necessary. + destroy(); + init(); + WHash::operator=(static_cast(o)); + WKeyEqual::operator=(static_cast(o)); + DataPool::operator=(static_cast(o)); + + return *this; + } + + // clean up old stuff + Destroyer::value>{}.nodes(*this); + + if (mMask != o.mMask) { + // no luck: we don't have the same array size allocated, so we need to realloc. + if (0 != mMask) { + // only deallocate if we actually have data! + ROBIN_HOOD_LOG("std::free") + std::free(mKeyVals); + } + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mKeyVals = static_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + + // no need for calloc here because cloneData performs a memcpy. + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + // sentinel is set in cloneData + } + WHash::operator=(static_cast(o)); + WKeyEqual::operator=(static_cast(o)); + DataPool::operator=(static_cast(o)); + mHashMultiplier = o.mHashMultiplier; + mNumElements = o.mNumElements; + mMask = o.mMask; + mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; + mInfoInc = o.mInfoInc; + mInfoHashShift = o.mInfoHashShift; + cloneData(o); + + return *this; + } + + // Swaps everything between the two maps. + void swap(Table& o) { + ROBIN_HOOD_TRACE(this) + using std::swap; + swap(o, *this); + } + + // Clears all data, without resizing. + void clear() { + ROBIN_HOOD_TRACE(this) + if (empty()) { + // don't do anything! also important because we don't want to write to + // DummyInfoByte::b, even though we would just write 0 to it. + return; + } + + Destroyer::value>{}.nodes(*this); + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + // clear everything, then set the sentinel again + uint8_t const z = 0; + std::fill(mInfo, mInfo + calcNumBytesInfo(numElementsWithBuffer), z); + mInfo[numElementsWithBuffer] = 1; + + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + // Destroys the map and all it's contents. + ~Table() { + ROBIN_HOOD_TRACE(this) + destroy(); + } + + // Checks if both tables contain the same entries. Order is irrelevant. + bool operator==(const Table& other) const { + ROBIN_HOOD_TRACE(this) + if (other.size() != size()) { + return false; + } + for (auto const& otherEntry : other) { + if (!has(otherEntry)) { + return false; + } + } + + return true; + } + + bool operator!=(const Table& other) const { + ROBIN_HOOD_TRACE(this) + return !operator==(other); + } + + template + typename std::enable_if::value, Q&>::type operator[](const key_type& key) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) + Node(*this, std::piecewise_construct, std::forward_as_tuple(key), + std::forward_as_tuple()); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(key), std::forward_as_tuple()); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + } + + return mKeyVals[idxAndState.first].getSecond(); + } + + template + typename std::enable_if::value, Q&>::type operator[](key_type&& key) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) + Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), + std::forward_as_tuple()); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = + Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), + std::forward_as_tuple()); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + } + + return mKeyVals[idxAndState.first].getSecond(); + } + + template + void insert(Iter first, Iter last) { + for (; first != last; ++first) { + // value_type ctor needed because this might be called with std::pair's + insert(value_type(*first)); + } + } + + void insert(std::initializer_list ilist) { + for (auto&& vt : ilist) { + insert(std::move(vt)); + } + } + + template + std::pair emplace(Args&&... args) { + ROBIN_HOOD_TRACE(this) + Node n{*this, std::forward(args)...}; + auto idxAndState = insertKeyPrepareEmptySpot(getFirstConst(n)); + switch (idxAndState.second) { + case InsertionState::key_found: + n.destroy(*this); + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node(*this, std::move(n)); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = std::move(n); + break; + + case InsertionState::overflow_error: + n.destroy(*this); + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + template + iterator emplace_hint(const_iterator position, Args&&... args) { + (void)position; + return emplace(std::forward(args)...).first; + } + + template + std::pair try_emplace(const key_type& key, Args&&... args) { + return try_emplace_impl(key, std::forward(args)...); + } + + template + std::pair try_emplace(key_type&& key, Args&&... args) { + return try_emplace_impl(std::move(key), std::forward(args)...); + } + + template + iterator try_emplace(const_iterator hint, const key_type& key, Args&&... args) { + (void)hint; + return try_emplace_impl(key, std::forward(args)...).first; + } + + template + iterator try_emplace(const_iterator hint, key_type&& key, Args&&... args) { + (void)hint; + return try_emplace_impl(std::move(key), std::forward(args)...).first; + } + + template + std::pair insert_or_assign(const key_type& key, Mapped&& obj) { + return insertOrAssignImpl(key, std::forward(obj)); + } + + template + std::pair insert_or_assign(key_type&& key, Mapped&& obj) { + return insertOrAssignImpl(std::move(key), std::forward(obj)); + } + + template + iterator insert_or_assign(const_iterator hint, const key_type& key, Mapped&& obj) { + (void)hint; + return insertOrAssignImpl(key, std::forward(obj)).first; + } + + template + iterator insert_or_assign(const_iterator hint, key_type&& key, Mapped&& obj) { + (void)hint; + return insertOrAssignImpl(std::move(key), std::forward(obj)).first; + } + + std::pair insert(const value_type& keyval) { + ROBIN_HOOD_TRACE(this) + return emplace(keyval); + } + + iterator insert(const_iterator hint, const value_type& keyval) { + (void)hint; + return emplace(keyval).first; + } + + std::pair insert(value_type&& keyval) { + return emplace(std::move(keyval)); + } + + iterator insert(const_iterator hint, value_type&& keyval) { + (void)hint; + return emplace(std::move(keyval)).first; + } + + // Returns 1 if key is found, 0 otherwise. + size_t count(const key_type& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { + return 1; + } + return 0; + } + + template + typename std::enable_if::type count(const OtherKey& key) const { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { + return 1; + } + return 0; + } + + bool contains(const key_type& key) const { // NOLINT + return 1U == count(key); + } + + template + typename std::enable_if::type contains(const OtherKey& key) const { + return 1U == count(key); + } + + // Returns a reference to the value found for key. + // Throws std::out_of_range if element cannot be found + template + typename std::enable_if::value, Q&>::type at(key_type const& key) { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { + doThrow("key not found"); + } + return kv->getSecond(); + } + + // Returns a reference to the value found for key. + // Throws std::out_of_range if element cannot be found + template + typename std::enable_if::value, Q const&>::type at(key_type const& key) const { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { + doThrow("key not found"); + } + return kv->getSecond(); + } + + const_iterator find(const key_type& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + template + const_iterator find(const OtherKey& key, is_transparent_tag /*unused*/) const { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + template + typename std::enable_if::type // NOLINT + find(const OtherKey& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + iterator find(const key_type& key) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + template + iterator find(const OtherKey& key, is_transparent_tag/*unused*/) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + template + typename std::enable_if::type find(const OtherKey& key) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + iterator begin() { + ROBIN_HOOD_TRACE(this) + if (empty()) { + return end(); + } + return iterator(mKeyVals, mInfo, fast_forward_tag{}); + } + const_iterator begin() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return cbegin(); + } + const_iterator cbegin() const { // NOLINT + ROBIN_HOOD_TRACE(this) + if (empty()) { + return cend(); + } + return const_iterator(mKeyVals, mInfo, fast_forward_tag{}); + } + + iterator end() { + ROBIN_HOOD_TRACE(this) + // no need to supply valid info pointer: end() must not be dereferenced, and only node + // pointer is compared. + return iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; + } + const_iterator end() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return cend(); + } + const_iterator cend() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return const_iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; + } + + iterator erase(const_iterator pos) { + ROBIN_HOOD_TRACE(this) + // its safe to perform const cast here + return erase(iterator{const_cast(pos.mKeyVals), const_cast(pos.mInfo)}); + } + + // Erases element at pos, returns iterator to the next element. + iterator erase(iterator pos) { + ROBIN_HOOD_TRACE(this) + // we assume that pos always points to a valid entry, and not end(). + auto const idx = static_cast(pos.mKeyVals - mKeyVals); + + shiftDown(idx); + --mNumElements; + + if (*pos.mInfo) { + // we've backward shifted, return this again + return pos; + } + + // no backward shift, return next element + return ++pos; + } + + size_t erase(const key_type& key) { + ROBIN_HOOD_TRACE(this) + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + + // check while info matches with the source idx + do { + if (info == mInfo[idx] && WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { + shiftDown(idx); + --mNumElements; + return 1; + } + next(&info, &idx); + } while (info <= mInfo[idx]); + + // nothing found to delete + return 0; + } + + // reserves space for the specified number of elements. Makes sure the old data fits. + // exactly the same as reserve(c). + void rehash(size_t c) { + // forces a reserve + reserve(c, true); + } + + // reserves space for the specified number of elements. Makes sure the old data fits. + // Exactly the same as rehash(c). Use rehash(0) to shrink to fit. + void reserve(size_t c) { + // reserve, but don't force rehash + reserve(c, false); + } + + // If possible reallocates the map to a smaller one. This frees the underlying table. + // Does not do anything if load_factor is too large for decreasing the table's size. + void compact() { + ROBIN_HOOD_TRACE(this) + auto newSize = InitialNumElements; + while (calcMaxNumElementsAllowed(newSize) < mNumElements && newSize != 0) { + newSize *= 2; + } + if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { + throwOverflowError(); + } + + ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") + + // only actually do anything when the new size is bigger than the old one. This prevents to + // continuously allocate for each reserve() call. + if (newSize < mMask + 1) { + rehashPowerOfTwo(newSize, true); + } + } + + size_type size() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return mNumElements; + } + + size_type max_size() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return static_cast(-1); + } + + ROBIN_HOOD(NODISCARD) bool empty() const noexcept { + ROBIN_HOOD_TRACE(this) + return 0 == mNumElements; + } + + float max_load_factor() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return MaxLoadFactor100 / 100.0F; + } + + // Average number of elements per bucket. Since we allow only 1 per bucket + float load_factor() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return static_cast(size()) / static_cast(mMask + 1); + } + + ROBIN_HOOD(NODISCARD) size_t mask() const noexcept { + ROBIN_HOOD_TRACE(this) + return mMask; + } + + ROBIN_HOOD(NODISCARD) size_t calcMaxNumElementsAllowed(size_t maxElements) const noexcept { + if (ROBIN_HOOD_LIKELY(maxElements <= (std::numeric_limits::max)() / 100)) { + return maxElements * MaxLoadFactor100 / 100; + } + + // we might be a bit imprecise, but since maxElements is quite large that doesn't matter + return (maxElements / 100) * MaxLoadFactor100; + } + + ROBIN_HOOD(NODISCARD) size_t calcNumBytesInfo(size_t numElements) const noexcept { + // we add a uint64_t, which houses the sentinel (first byte) and padding so we can load + // 64bit types. + return numElements + sizeof(uint64_t); + } + + ROBIN_HOOD(NODISCARD) + size_t calcNumElementsWithBuffer(size_t numElements) const noexcept { + auto maxNumElementsAllowed = calcMaxNumElementsAllowed(numElements); + return numElements + (std::min)(maxNumElementsAllowed, (static_cast(0xFF))); + } + + // calculation only allowed for 2^n values + ROBIN_HOOD(NODISCARD) size_t calcNumBytesTotal(size_t numElements) const { +#if ROBIN_HOOD(BITNESS) == 64 + return numElements * sizeof(Node) + calcNumBytesInfo(numElements); +#else + // make sure we're doing 64bit operations, so we are at least safe against 32bit overflows. + auto const ne = static_cast(numElements); + auto const s = static_cast(sizeof(Node)); + auto const infos = static_cast(calcNumBytesInfo(numElements)); + + auto const total64 = ne * s + infos; + auto const total = static_cast(total64); + + if (ROBIN_HOOD_UNLIKELY(static_cast(total) != total64)) { + throwOverflowError(); + } + return total; +#endif + } + +private: + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, bool>::type has(const value_type& e) const { + ROBIN_HOOD_TRACE(this) + auto it = find(e.first); + return it != end() && it->second == e.second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, bool>::type has(const value_type& e) const { + ROBIN_HOOD_TRACE(this) + return find(e) != end(); + } + + void reserve(size_t c, bool forceRehash) { + ROBIN_HOOD_TRACE(this) + auto const minElementsAllowed = (std::max)(c, mNumElements); + auto newSize = InitialNumElements; + while (calcMaxNumElementsAllowed(newSize) < minElementsAllowed && newSize != 0) { + newSize *= 2; + } + if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { + throwOverflowError(); + } + + ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") + + // only actually do anything when the new size is bigger than the old one. This prevents to + // continuously allocate for each reserve() call. + if (forceRehash || newSize > mMask + 1) { + rehashPowerOfTwo(newSize, false); + } + } + + // reserves space for at least the specified number of elements. + // only works if numBuckets if power of two + // True on success, false otherwise + void rehashPowerOfTwo(size_t numBuckets, bool forceFree) { + ROBIN_HOOD_TRACE(this) + + Node* const oldKeyVals = mKeyVals; + uint8_t const* const oldInfo = mInfo; + + const size_t oldMaxElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + + // resize operation: move stuff + initData(numBuckets); + if (oldMaxElementsWithBuffer > 1) { + for (size_t i = 0; i < oldMaxElementsWithBuffer; ++i) { + if (oldInfo[i] != 0) { + // might throw an exception, which is really bad since we are in the middle of + // moving stuff. + insert_move(std::move(oldKeyVals[i])); + // destroy the node but DON'T destroy the data. + oldKeyVals[i].~Node(); + } + } + + // this check is not necessary as it's guarded by the previous if, but it helps + // silence g++'s overeager "attempt to free a non-heap object 'map' + // [-Werror=free-nonheap-object]" warning. + if (oldKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { + // don't destroy old data: put it into the pool instead + if (forceFree) { + std::free(oldKeyVals); + } else { + DataPool::addOrFree(oldKeyVals, calcNumBytesTotal(oldMaxElementsWithBuffer)); + } + } + } + } + + ROBIN_HOOD(NOINLINE) void throwOverflowError() const { +#if ROBIN_HOOD(HAS_EXCEPTIONS) + throw std::overflow_error("robin_hood::map overflow"); +#else + abort(); +#endif + } + + template + std::pair try_emplace_impl(OtherKey&& key, Args&&... args) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node( + *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(args)...)); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(args)...)); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + template + std::pair insertOrAssignImpl(OtherKey&& key, Mapped&& obj) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + mKeyVals[idxAndState.first].getSecond() = std::forward(obj); + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node( + *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(obj))); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(obj))); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + void initData(size_t max_elements) { + mNumElements = 0; + mMask = max_elements - 1; + mMaxNumElementsAllowed = calcMaxNumElementsAllowed(max_elements); + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(max_elements); + + // malloc & zero mInfo. Faster than calloc everything. + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + ROBIN_HOOD_LOG("std::calloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mKeyVals = reinterpret_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + std::memset(mInfo, 0, numBytesTotal - numElementsWithBuffer * sizeof(Node)); + + // set sentinel + mInfo[numElementsWithBuffer] = 1; + + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + enum class InsertionState { overflow_error, key_found, new_node, overwrite_node }; + + // Finds key, and if not already present prepares a spot where to pot the key & value. + // This potentially shifts nodes out of the way, updates mInfo and number of inserted + // elements, so the only operation left to do is create/assign a new node at that spot. + template + std::pair insertKeyPrepareEmptySpot(OtherKey&& key) { + for (int i = 0; i < 256; ++i) { + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + nextWhileLess(&info, &idx); + + // while we potentially have a match + while (info == mInfo[idx]) { + if (WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { + // key already exists, do NOT insert. + // see http://en.cppreference.com/w/cpp/container/unordered_map/insert + return std::make_pair(idx, InsertionState::key_found); + } + next(&info, &idx); + } + + // unlikely that this evaluates to true + if (ROBIN_HOOD_UNLIKELY(mNumElements >= mMaxNumElementsAllowed)) { + if (!increase_size()) { + return std::make_pair(size_t(0), InsertionState::overflow_error); + } + continue; + } + + // key not found, so we are now exactly where we want to insert it. + auto const insertion_idx = idx; + auto const insertion_info = info; + if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + + // find an empty spot + while (0 != mInfo[idx]) { + next(&info, &idx); + } + + if (idx != insertion_idx) { + shiftUp(idx, insertion_idx); + } + // put at empty spot + mInfo[insertion_idx] = static_cast(insertion_info); + ++mNumElements; + return std::make_pair(insertion_idx, idx == insertion_idx + ? InsertionState::new_node + : InsertionState::overwrite_node); + } + + // enough attempts failed, so finally give up. + return std::make_pair(size_t(0), InsertionState::overflow_error); + } + + bool try_increase_info() { + ROBIN_HOOD_LOG("mInfoInc=" << mInfoInc << ", numElements=" << mNumElements + << ", maxNumElementsAllowed=" + << calcMaxNumElementsAllowed(mMask + 1)) + if (mInfoInc <= 2) { + // need to be > 2 so that shift works (otherwise undefined behavior!) + return false; + } + // we got space left, try to make info smaller + mInfoInc = static_cast(mInfoInc >> 1U); + + // remove one bit of the hash, leaving more space for the distance info. + // This is extremely fast because we can operate on 8 bytes at once. + ++mInfoHashShift; + auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + + for (size_t i = 0; i < numElementsWithBuffer; i += 8) { + auto val = unaligned_load(mInfo + i); + val = (val >> 1U) & UINT64_C(0x7f7f7f7f7f7f7f7f); + std::memcpy(mInfo + i, &val, sizeof(val)); + } + // update sentinel, which might have been cleared out! + mInfo[numElementsWithBuffer] = 1; + + mMaxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); + return true; + } + + // True if resize was possible, false otherwise + bool increase_size() { + // nothing allocated yet? just allocate InitialNumElements + if (0 == mMask) { + initData(InitialNumElements); + return true; + } + + auto const maxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); + if (mNumElements < maxNumElementsAllowed && try_increase_info()) { + return true; + } + + ROBIN_HOOD_LOG("mNumElements=" << mNumElements << ", maxNumElementsAllowed=" + << maxNumElementsAllowed << ", load=" + << (static_cast(mNumElements) * 100.0 / + (static_cast(mMask) + 1))) + + if (mNumElements * 2 < calcMaxNumElementsAllowed(mMask + 1)) { + // we have to resize, even though there would still be plenty of space left! + // Try to rehash instead. Delete freed memory so we don't steadyily increase mem in case + // we have to rehash a few times + nextHashMultiplier(); + rehashPowerOfTwo(mMask + 1, true); + } else { + // we've reached the capacity of the map, so the hash seems to work nice. Keep using it. + rehashPowerOfTwo((mMask + 1) * 2, false); + } + return true; + } + + void nextHashMultiplier() { + // adding an *even* number, so that the multiplier will always stay odd. This is necessary + // so that the hash stays a mixing function (and thus doesn't have any information loss). + mHashMultiplier += UINT64_C(0xc4ceb9fe1a85ec54); + } + + void destroy() { + if (0 == mMask) { + // don't deallocate! + return; + } + + Destroyer::value>{} + .nodesDoNotDeallocate(*this); + + // This protection against not deleting mMask shouldn't be needed as it's sufficiently + // protected with the 0==mMask check, but I have this anyways because g++ 7 otherwise + // reports a compile error: attempt to free a non-heap object 'fm' + // [-Werror=free-nonheap-object] + if (mKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { + ROBIN_HOOD_LOG("std::free") + std::free(mKeyVals); + } + } + + void init() noexcept { + mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); + mInfo = reinterpret_cast(&mMask); + mNumElements = 0; + mMask = 0; + mMaxNumElementsAllowed = 0; + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + // members are sorted so no padding occurs + uint64_t mHashMultiplier = UINT64_C(0xc4ceb9fe1a85ec53); // 8 byte 8 + Node* mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); // 8 byte 16 + uint8_t* mInfo = reinterpret_cast(&mMask); // 8 byte 24 + size_t mNumElements = 0; // 8 byte 32 + size_t mMask = 0; // 8 byte 40 + size_t mMaxNumElementsAllowed = 0; // 8 byte 48 + InfoType mInfoInc = InitialInfoInc; // 4 byte 52 + InfoType mInfoHashShift = InitialInfoHashShift; // 4 byte 56 + // 16 byte 56 if NodeAllocator +}; + +} // namespace detail + +// map + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_flat_map = detail::Table; + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_node_map = detail::Table; + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_map = + detail::Table) <= sizeof(size_t) * 6 && + std::is_nothrow_move_constructible>::value && + std::is_nothrow_move_assignable>::value, + MaxLoadFactor100, Key, T, Hash, KeyEqual>; + +// set + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_flat_set = detail::Table; + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_node_set = detail::Table; + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_set = detail::Table < sizeof(Key) <= sizeof(size_t) * 6 && + std::is_nothrow_move_constructible::value && + std::is_nothrow_move_assignable::value, + MaxLoadFactor100, Key, void, Hash, KeyEqual>; + +} // namespace robin_hood +/* *INDENT-ON* */ + +#endif // NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp new file mode 100644 index 00000000..f8aa72ff --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp @@ -0,0 +1,257 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__TYPES_HPP_ +#define ATHENA_SMAC_PLANNER__TYPES_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace athena_smac_planner +{ + +typedef std::pair NodeHeuristicPair; +typedef std::vector LookupTable; +typedef std::pair TrigValues; + +/** + * @struct athena_smac_planner::SearchInfo + * @brief Search properties and penalties + */ +struct SearchInfo +{ + float minimum_turning_radius{8.0}; + float non_straight_penalty{1.05}; + float change_penalty{0.0}; + float reverse_penalty{2.0}; + float cost_penalty{2.0}; + float retrospective_penalty{0.015}; + float rotation_penalty{5.0}; + float analytic_expansion_ratio{3.5}; + float analytic_expansion_max_length{60.0}; + float analytic_expansion_max_cost{200.0}; + bool analytic_expansion_max_cost_override{false}; + std::string lattice_filepath; + bool cache_obstacle_heuristic{false}; + bool allow_reverse_expansion{false}; + bool allow_primitive_interpolation{false}; + bool downsample_obstacle_heuristic{true}; + bool use_quadratic_cost_penalty{false}; +}; + +/** + * @struct athena_smac_planner::SmootherParams + * @brief Parameters for the smoother + */ +struct SmootherParams +{ + /** + * @brief A constructor for athena_smac_planner::SmootherParams + */ + SmootherParams() + : holonomic_(false) + { + } + + /** + * @brief Get params from ROS parameter + * @param node Ptr to node + * @param name Name of plugin + */ + void get(rclcpp::Node * node, const std::string & name) + { + std::string prefix = name + ".smoother."; + auto dp = [node, &prefix](const std::string & p, auto def) { + if (!node->has_parameter(prefix + p)) { + node->declare_parameter(prefix + p, def); + } + return node->get_parameter(prefix + p).get_value(); + }; + tolerance_ = dp("tolerance", 1e-10); + max_its_ = dp("max_iterations", 1000); + w_data_ = dp("w_data", 0.2); + w_smooth_ = dp("w_smooth", 0.3); + do_refinement_ = dp("do_refinement", true); + refinement_num_ = dp("refinement_num", 2); + } + + double tolerance_; + int max_its_; + double w_data_; + double w_smooth_; + bool holonomic_; + bool do_refinement_; + int refinement_num_; +}; + +/** + * @struct athena_smac_planner::TurnDirection + * @brief A struct with the motion primitive's direction embedded + */ +enum class TurnDirection +{ + UNKNOWN = 0, + FORWARD = 1, + LEFT = 2, + RIGHT = 3, + REVERSE = 4, + REV_LEFT = 5, + REV_RIGHT = 6 +}; + +/** + * @struct athena_smac_planner::MotionPose + * @brief A struct for poses in motion primitives + */ +struct MotionPose +{ + /** + * @brief A constructor for athena_smac_planner::MotionPose + */ + MotionPose() {} + + /** + * @brief A constructor for athena_smac_planner::MotionPose + * @param x X pose + * @param y Y pose + * @param theta Angle of pose + * @param TurnDirection Direction of the primitive's turn + */ + MotionPose(const float & x, const float & y, const float & theta, const TurnDirection & turn_dir) + : _x(x), _y(y), _theta(theta), _turn_dir(turn_dir) + {} + + MotionPose operator-(const MotionPose & p2) + { + return MotionPose( + this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta, TurnDirection::UNKNOWN); + } + + float _x; + float _y; + float _theta; + TurnDirection _turn_dir; +}; + +typedef std::vector MotionPoses; + +/** + * @struct athena_smac_planner::LatticeMetadata + * @brief A struct of all lattice metadata + */ +struct LatticeMetadata +{ + float min_turning_radius; + float grid_resolution; + unsigned int number_of_headings; + std::vector heading_angles; + unsigned int number_of_trajectories; + std::string motion_model; +}; + +/** + * @struct athena_smac_planner::MotionPrimitive + * @brief A struct of all motion primitive data + */ +struct MotionPrimitive +{ + unsigned int trajectory_id; + float start_angle; + float end_angle; + float turning_radius; + float trajectory_length; + float arc_length; + float straight_length; + bool left_turn; + MotionPoses poses; +}; + +/** + * @struct athena_smac_planner::GoalState + * @brief A struct to store the goal state + */ +template +struct GoalState +{ + NodeT * goal = nullptr; + bool is_valid = true; +}; + +typedef std::vector MotionPrimitives; +typedef std::vector MotionPrimitivePtrs; + +/** + * @class athena_smac_planner::Coordinates + * @brief Implementation of coordinate2d structure + */ +struct Coordinates2D +{ + Coordinates2D() {} + Coordinates2D(const float & x_in, const float & y_in) + : x(x_in), y(y_in) + {} + + inline bool operator==(const Coordinates2D & rhs) const + { + return this->x == rhs.x && this->y == rhs.y; + } + + inline bool operator!=(const Coordinates2D & rhs) const + { + return !(*this == rhs); + } + + float x, y; +}; + +/** + * @class athena_smac_planner::Coordinates + * @brief Implementation of coordinate structure + */ +struct Coordinates +{ + /** + * @brief A constructor for athena_smac_planner::NodeHybrid::Coordinates + */ + Coordinates() {} + + /** + * @brief A constructor for athena_smac_planner::NodeHybrid::Coordinates + * @param x_in X coordinate + * @param y_in Y coordinate + * @param theta_in Theta coordinate + */ + Coordinates(const float & x_in, const float & y_in, const float & theta_in) + : x(x_in), y(y_in), theta(theta_in) + {} + + inline bool operator==(const Coordinates & rhs) const + { + return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; + } + + inline bool operator!=(const Coordinates & rhs) const + { + return !(*this == rhs); + } + + float x, y, theta; +}; +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__TYPES_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp new file mode 100644 index 00000000..494f68d4 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp @@ -0,0 +1,231 @@ +// Copyright (c) 2021, Samsung Research America +// Copyright (c) 2023, 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__UTILS_HPP_ +#define ATHENA_SMAC_PLANNER__UTILS_HPP_ + +#include +#include +#include + +#include "nlohmann/json.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "tf2/utils.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "athena_smac_planner/types.hpp" +#include + +namespace athena_smac_planner +{ + +/** +* @brief Create an Eigen Vector2D of world poses from continuous map coords +* @param mx float of map X coordinate +* @param my float of map Y coordinate +* @param costmap Costmap pointer +* @return Eigen::Vector2d eigen vector of the generated path +*/ +inline geometry_msgs::msg::Pose getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) +{ + geometry_msgs::msg::Pose msg; + msg.position.x = + static_cast(costmap->getOriginX()) + mx * costmap->getResolution(); + msg.position.y = + static_cast(costmap->getOriginY()) + my * costmap->getResolution(); + return msg; +} + +/** +* @brief Create quaternion from radians +* @param theta continuous bin coordinates angle +* @return quaternion orientation in map frame +*/ +inline geometry_msgs::msg::Quaternion getWorldOrientation( + const float & theta) +{ + // theta is in radians already + tf2::Quaternion q; + q.setEuler(0.0, 0.0, theta); + return tf2::toMsg(q); +} + +/** +* @brief Find the min cost of the inflation decay function for which the robot MAY be +* in collision in any orientation +* @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) +* @return double circumscribed cost, any higher than this and need to do full footprint collision checking +* since some element of the robot could be in collision +*/ +inline double findCircumscribedCost(std::shared_ptr costmap) +{ + double result = -1.0; + std::vector>::iterator layer; + + // check if the costmap has an inflation layer + std::shared_ptr inflation_layer = nullptr; + for (auto & layer : *costmap->getLayeredCostmap()->getPlugins()) { + auto candidate = std::dynamic_pointer_cast(layer); + if (candidate) { + inflation_layer = candidate; + break; + } + } + if (inflation_layer != nullptr) { + double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + double resolution = costmap->getCostmap()->getResolution(); + result = static_cast(inflation_layer->computeCost(circum_radius / resolution)); + } else { + RCLCPP_WARN( + rclcpp::get_logger("computeCircumscribedCost"), + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!"); + } + + return result; +} + +/** + * @brief convert json to lattice metadata + * @param[in] json json object + * @param[out] lattice meta data + */ +inline void fromJsonToMetaData(const nlohmann::json & json, LatticeMetadata & lattice_metadata) +{ + json.at("turning_radius").get_to(lattice_metadata.min_turning_radius); + json.at("grid_resolution").get_to(lattice_metadata.grid_resolution); + json.at("num_of_headings").get_to(lattice_metadata.number_of_headings); + json.at("heading_angles").get_to(lattice_metadata.heading_angles); + json.at("number_of_trajectories").get_to(lattice_metadata.number_of_trajectories); + json.at("motion_model").get_to(lattice_metadata.motion_model); +} + +/** + * @brief convert json to pose + * @param[in] json json object + * @param[out] pose + */ +inline void fromJsonToPose(const nlohmann::json & json, MotionPose & pose) +{ + pose._x = json[0]; + pose._y = json[1]; + pose._theta = json[2]; +} + +/** + * @brief convert json to motion primitive + * @param[in] json json object + * @param[out] motion primitive + */ +inline void fromJsonToMotionPrimitive( + const nlohmann::json & json, MotionPrimitive & motion_primitive) +{ + json.at("trajectory_id").get_to(motion_primitive.trajectory_id); + json.at("start_angle_index").get_to(motion_primitive.start_angle); + json.at("end_angle_index").get_to(motion_primitive.end_angle); + json.at("trajectory_radius").get_to(motion_primitive.turning_radius); + json.at("trajectory_length").get_to(motion_primitive.trajectory_length); + json.at("arc_length").get_to(motion_primitive.arc_length); + json.at("straight_length").get_to(motion_primitive.straight_length); + json.at("left_turn").get_to(motion_primitive.left_turn); + + for (unsigned int i = 0; i < json["poses"].size(); i++) { + MotionPose pose; + fromJsonToPose(json["poses"][i], pose); + motion_primitive.poses.push_back(pose); + } +} + +/** + * @brief transform footprint into edges + * @param[in] robot position , orientation and footprint + * @param[out] robot footprint edges + */ +inline std::vector transformFootprintToEdges( + const geometry_msgs::msg::Pose & pose, + const std::vector & footprint) +{ + const double & x = pose.position.x; + const double & y = pose.position.y; + const double & yaw = tf2::getYaw(pose.orientation); + const double sin_yaw = sin(yaw); + const double cos_yaw = cos(yaw); + + std::vector out_footprint; + out_footprint.resize(2 * footprint.size()); + for (unsigned int i = 0; i < footprint.size(); i++) { + out_footprint[2 * i].x = x + cos_yaw * footprint[i].x - sin_yaw * footprint[i].y; + out_footprint[2 * i].y = y + sin_yaw * footprint[i].x + cos_yaw * footprint[i].y; + if (i == 0) { + out_footprint.back().x = out_footprint[i].x; + out_footprint.back().y = out_footprint[i].y; + } else { + out_footprint[2 * i - 1].x = out_footprint[2 * i].x; + out_footprint[2 * i - 1].y = out_footprint[2 * i].y; + } + } + return out_footprint; +} + +/** + * @brief initializes marker to visualize shape of linestring + * @param edge edge to mark of footprint + * @param i marker ID + * @param frame_id frame of the marker + * @param timestamp timestamp of the marker + * @return marker populated + */ +inline visualization_msgs::msg::Marker createMarker( + const std::vector edge, + unsigned int i, const std::string & frame_id, const rclcpp::Time & timestamp) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.frame_locked = false; + marker.ns = "planned_footprint"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.lifetime = rclcpp::Duration(0, 0); + + marker.id = i; + for (auto & point : edge) { + marker.points.push_back(point); + } + + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.05; + marker.scale.y = 0.05; + marker.scale.z = 0.05; + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + marker.color.a = 1.3f; + return marker; +} + + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__UTILS_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/package.xml b/src/subsystems/minimal_navigation/athena_smac_planner/package.xml new file mode 100644 index 00000000..fbae2a06 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/package.xml @@ -0,0 +1,31 @@ + + + + athena_smac_planner + 1.4.0 + Vendored Hybrid-A* planner (SMAC) for the Athena navigation stack + UMD Loop + Apache-2.0 + + ament_cmake + nav2_common + + angles + eigen + geometry_msgs + msgs + nav2_core + nav2_costmap_2d + nav_msgs + nlohmann-json-dev + ompl + rclcpp + rclcpp_lifecycle + tf2_ros + tf2 + visualization_msgs + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp new file mode 100644 index 00000000..9f2ce25f --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp @@ -0,0 +1,526 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "athena_smac_planner/a_star.hpp" +using namespace std::chrono; // NOLINT + +namespace athena_smac_planner +{ + +template +AStarAlgorithm::AStarAlgorithm( + const MotionModel & motion_model, + const SearchInfo & search_info) +: _traverse_unknown(true), + _is_initialized(false), + _max_iterations(0), + _terminal_checking_interval(5000), + _max_planning_time(0), + _x_size(0), + _y_size(0), + _search_info(search_info), + _start(nullptr), + _goal_manager(GoalManagerT()), + _motion_model(motion_model) +{ + _graph.reserve(100000); +} + +template +AStarAlgorithm::~AStarAlgorithm() +{ +} + +template +void AStarAlgorithm::initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations, + const int & terminal_checking_interval, + const double & max_planning_time, + const float & lookup_table_size, + const unsigned int & dim_3_size) +{ + _traverse_unknown = allow_unknown; + _max_iterations = max_iterations; + _max_on_approach_iterations = max_on_approach_iterations; + _terminal_checking_interval = terminal_checking_interval; + _max_planning_time = max_planning_time; + if (!_is_initialized) { + _shared_ctx = std::make_shared(); + _shared_ctx->distance_heuristic->precomputeDistanceHeuristic(lookup_table_size, _motion_model, + dim_3_size, + _search_info, _shared_ctx->motion_table); + } + _is_initialized = true; + _dim3_size = dim_3_size; + _expander = std::make_unique>( + _motion_model, _search_info, _traverse_unknown, _dim3_size); +} + +template +void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision_checker) +{ + _collision_checker = collision_checker; + _costmap = collision_checker->getCostmap(); + unsigned int x_size = _costmap->getSizeInCellsX(); + unsigned int y_size = _costmap->getSizeInCellsY(); + + clearGraph(); + + if (getSizeX() != x_size || getSizeY() != y_size) { + _x_size = x_size; + _y_size = y_size; + } + + // Always refresh the motion model so dynamic penalty parameters take effect immediately + NodeT::initMotionModel(_shared_ctx.get(), _motion_model, _x_size, _y_size, _dim3_size, + _search_info); + + // Always set context pointers to ensure newly allocated objects get their contexts restored + _goal_manager.setContext(_shared_ctx.get()); + _expander->setContext(_shared_ctx.get()); + _expander->setCollisionChecker(_collision_checker); +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( + const uint64_t & index) +{ + auto iter = _graph.find(index); + if (iter != _graph.end()) { + return &(iter->second); + } + + return &(_graph.emplace(index, NodeT(index, _shared_ctx.get())).first->second); +} + +template +void AStarAlgorithm::setStart( + const float & mx, + const float & my, + const unsigned int & dim_3) +{ + _start = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + _start->setPose(Coordinates(mx, my, dim_3)); +} + +template +void AStarAlgorithm::populateExpansionsLog( + const NodePtr & node, + std::vector> * expansions_log) +{ + typename NodeT::Coordinates coords = node->pose; + expansions_log->emplace_back( + _costmap->getOriginX() + ((coords.x + 0.5) * _costmap->getResolution()), + _costmap->getOriginY() + ((coords.y + 0.5) * _costmap->getResolution()), + _shared_ctx->motion_table.getAngleFromBin(coords.theta)); +} + +template +void AStarAlgorithm::setGoal( + const float & mx, + const float & my, + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode, + const int & coarse_search_resolution) +{ + // Default to minimal resolution unless overridden for ALL_DIRECTION + _coarse_search_resolution = 1; + + _goal_manager.clear(); + Coordinates ref_goal_coord(mx, my, static_cast(dim_3)); + + if (!_search_info.cache_obstacle_heuristic || + _goal_manager.hasGoalChanged(ref_goal_coord)) + { + if (!_start) { + throw std::runtime_error("Start must be set before goal."); + } + + _shared_ctx->obstacle_heuristic->resetObstacleHeuristic( + _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my, + _shared_ctx->motion_table.downsample_obstacle_heuristic); + } + + _goal_manager.setRefGoalCoordinates(ref_goal_coord); + + unsigned int num_bins = _shared_ctx->motion_table.num_angle_quantization; + // set goal based on heading mode + switch (goal_heading_mode) { + case GoalHeadingMode::DEFAULT: { + // add a single goal node with single heading + auto goal = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); + break; + } + + case GoalHeadingMode::BIDIRECTIONAL: { + // Add two goals, one for each direction + // add goal in original direction + auto goal = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); + + // Add goal node in opposite (180°) direction + unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; + auto opposite_goal = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + opposite_heading)); + opposite_goal->setPose( + typename NodeT::Coordinates(mx, my, static_cast(opposite_heading))); + _goal_manager.addGoal(opposite_goal); + break; + } + + case GoalHeadingMode::ALL_DIRECTION: { + // Set the coarse search resolution only for all direction + _coarse_search_resolution = coarse_search_resolution; + + // Add goal nodes for all headings + for (unsigned int i = 0; i < num_bins; ++i) { + auto goal = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + i)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(i))); + _goal_manager.addGoal(goal); + } + break; + } + case GoalHeadingMode::UNKNOWN: + throw std::runtime_error("Goal heading is UNKNOWN."); + } +} + +template +bool AStarAlgorithm::areInputsValid() +{ + // Check if graph was filled in + if (_graph.empty()) { + throw std::runtime_error("Failed to compute path, no costmap given."); + } + + // Check if points were filled in + if (!_start || _goal_manager.goalsIsEmpty()) { + throw std::runtime_error("Failed to compute path, no valid start or goal given."); + } + + // remove invalid goals + _goal_manager.removeInvalidGoals(getToleranceHeuristic(), _collision_checker, _traverse_unknown); + + // Check if ending point is valid + if (_goal_manager.getGoalsSet().empty()) { + throw nav2_core::PlannerException("Goal was in lethal cost"); + } + + // Note: We do not check the if the start is valid because it is cleared + return true; +} + +template +bool AStarAlgorithm::getClosestPathWithinTolerance(CoordinateVector & path) +{ + if (_best_heuristic_node.first < getToleranceHeuristic()) { + _graph.at(_best_heuristic_node.second).backtracePath(path); + return true; + } + + return false; +} + +template +bool AStarAlgorithm::createPath( + CoordinateVector & path, int & iterations, + const float & tolerance, + std::function cancel_checker, + std::vector> * expansions_log) +{ + steady_clock::time_point start_time = steady_clock::now(); + _tolerance = tolerance; + _best_heuristic_node = {std::numeric_limits::max(), 0}; + clearQueue(); + + if (!areInputsValid()) { + return false; + } + + NodeVector coarse_check_goals, fine_check_goals; + _goal_manager.prepareGoalsForAnalyticExpansion( + coarse_check_goals, fine_check_goals, + _coarse_search_resolution); + + // 0) Add starting point to the open set + addNode(0.0, getStart()); + getStart()->setAccumulatedCost(0.0); + + // Optimization: preallocate all variables + NodePtr current_node = nullptr; + NodePtr neighbor = nullptr; + NodePtr expansion_result = nullptr; + float g_cost = 0.0; + NodeVector neighbors; + int approach_iterations = 0; + NeighborIterator neighbor_iterator; + int analytic_iterations = 0; + int closest_distance = std::numeric_limits::max(); + + // Given an index, return a node ptr reference if its collision-free and valid + const uint64_t max_index = static_cast(getSizeX()) * + static_cast(getSizeY()) * + static_cast(getSizeDim3()); + NodeGetter neighborGetter = + [&, this](const uint64_t & index, NodePtr & neighbor_rtn) -> bool + { + if (index >= max_index) { + return false; + } + + neighbor_rtn = addToGraph(index); + return true; + }; + + while (iterations < getMaxIterations() && !_queue.empty()) { + // Check for planning timeout and cancel only on every Nth iteration + if (iterations % _terminal_checking_interval == 0) { + if (cancel_checker()) { + throw nav2_core::PlannerException("Planner was cancelled"); + } + std::chrono::duration planning_duration = + std::chrono::duration_cast>(steady_clock::now() - start_time); + if (static_cast(planning_duration.count()) >= _max_planning_time) { + // In case of timeout, return the path that is closest, if within tolerance. + return getClosestPathWithinTolerance(path); + } + } + + // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue + current_node = getNextNode(); + + // Save current node coordinates for debug + if (expansions_log) { + populateExpansionsLog(current_node, expansions_log); + } + + // We allow for nodes to be queued multiple times in case + // shorter paths result in it, but we can visit only once + // Also a chance to perform last-checks necessary. + if (onVisitationCheckNode(current_node)) { + continue; + } + + iterations++; + + // 2) Mark Nbest as visited + current_node->visited(); + + // 2.1) Use an analytic expansion (if available) to generate a path + expansion_result = nullptr; + expansion_result = _expander->tryAnalyticExpansion( + current_node, coarse_check_goals, fine_check_goals, + _goal_manager.getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); + if (expansion_result != nullptr) { + current_node = expansion_result; + } + + // 3) Check if we're at the goal, backtrace if required + if (_goal_manager.isGoal(current_node)) { + return current_node->backtracePath(path); + } else if (_best_heuristic_node.first < getToleranceHeuristic()) { + // Optimization: Let us find when in tolerance and refine within reason + approach_iterations++; + if (approach_iterations >= getOnApproachMaxIterations()) { + return _graph.at(_best_heuristic_node.second).backtracePath(path); + } + } + + // 4) Expand neighbors of Nbest not visited + neighbors.clear(); + current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors); + + for (neighbor_iterator = neighbors.begin(); + neighbor_iterator != neighbors.end(); ++neighbor_iterator) + { + neighbor = *neighbor_iterator; + + // 4.1) Compute the cost to go to this node + g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor); + + // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach + if (g_cost < neighbor->getAccumulatedCost()) { + neighbor->setAccumulatedCost(g_cost); + neighbor->parent = current_node; + + // 4.3) Add to queue with heuristic cost + addNode(g_cost + getHeuristicCost(neighbor), neighbor); + } + } + } + + // If we run out of search options, return the path that is closest, if within tolerance. + return getClosestPathWithinTolerance(path); +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() +{ + return _start; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +{ + NodeBasic node = _queue.top().second; + _queue.pop(); + node.processSearchNode(); + return node.graph_node_ptr; +} + +template +void AStarAlgorithm::addNode(const float & cost, NodePtr & node) +{ + NodeBasic queued_node(node->getIndex()); + queued_node.populateSearchNode(node); + _queue.emplace(cost, queued_node); +} + +template +float AStarAlgorithm::getHeuristicCost(const NodePtr & node) +{ + const Coordinates node_coords = + NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); + float heuristic = node->getHeuristicCost(node_coords, _goal_manager.getGoalsCoordinates()); + if (heuristic < _best_heuristic_node.first) { + _best_heuristic_node = {heuristic, node->getIndex()}; + } + + return heuristic; +} + +template +bool AStarAlgorithm::onVisitationCheckNode(const NodePtr & current_node) +{ + return current_node->wasVisited(); +} + +template +void AStarAlgorithm::clearQueue() +{ + NodeQueue q; + std::swap(_queue, q); +} + +template +void AStarAlgorithm::clearGraph() +{ + Graph g; + std::swap(_graph, g); + _graph.reserve(100000); +} + +template +uint64_t AStarAlgorithm::getIndex( + const unsigned int & x, const unsigned int & y, + const unsigned int & dim_3) +{ + return NodeT::getIndex(x, y, dim_3, _shared_ctx->motion_table.size_x, + _shared_ctx->motion_table.num_angle_quantization); +} + +template +int & AStarAlgorithm::getMaxIterations() +{ + return _max_iterations; +} + +template +int & AStarAlgorithm::getOnApproachMaxIterations() +{ + return _max_on_approach_iterations; +} + +template +float & AStarAlgorithm::getToleranceHeuristic() +{ + return _tolerance; +} + +template +unsigned int & AStarAlgorithm::getSizeX() +{ + return _x_size; +} + +template +unsigned int & AStarAlgorithm::getSizeY() +{ + return _y_size; +} + +template +unsigned int & AStarAlgorithm::getSizeDim3() +{ + return _dim3_size; +} + +template +unsigned int AStarAlgorithm::getCoarseSearchResolution() +{ + return _coarse_search_resolution; +} + +template +typename AStarAlgorithm::GoalManagerT AStarAlgorithm::getGoalManager() +{ + return _goal_manager; +} + +template +typename AStarAlgorithm::NodeContext * AStarAlgorithm::getContext() +{ + return _shared_ctx.get(); +} + +// Instantiate algorithm for the supported template types +template class AStarAlgorithm; + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp new file mode 100644 index 00000000..c8c057a1 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp @@ -0,0 +1,449 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#include +#include +#include + +#include "athena_smac_planner/analytic_expansion.hpp" + +namespace athena_smac_planner +{ + +template +AnalyticExpansion::AnalyticExpansion( + const MotionModel & motion_model, + const SearchInfo & search_info, + const bool & traverse_unknown, + const unsigned int & dim_3_size) +: _motion_model(motion_model), + _search_info(search_info), + _traverse_unknown(traverse_unknown), + _dim_3_size(dim_3_size), + _collision_checker(nullptr) +{ +} + +template +void AnalyticExpansion::setCollisionChecker( + GridCollisionChecker * collision_checker) +{ + _collision_checker = collision_checker; +} + +template +void AnalyticExpansion::setContext(NodeContext * ctx) +{ + _ctx = ctx; +} + +template +typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( + const NodePtr & current_node, + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, + const NodeGetter & getter, int & analytic_iterations, + int & closest_distance) +{ + // This must be a valid motion model for analytic expansion to be attempted + if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP || + _motion_model == MotionModel::STATE_LATTICE) + { + // See if we are closer and should be expanding more often + const Coordinates node_coords = + NodeT::getCoords( + current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); + + AnalyticExpansionNodes current_best_analytic_nodes; + NodePtr current_best_goal = nullptr; + NodePtr current_best_node = nullptr; + float current_best_score = std::numeric_limits::max(); + + closest_distance = std::min( + closest_distance, + static_cast(current_node->getHeuristicCost(node_coords, goals_coords))); + // We want to expand at a rate of d/expansion_ratio, + // but check to see if we are so close that we would be expanding every iteration + // If so, limit it to the expansion ratio (rounded up) + int desired_iterations = std::max( + static_cast(closest_distance / _search_info.analytic_expansion_ratio), + static_cast(std::ceil(_search_info.analytic_expansion_ratio))); + + // If we are closer now, we should update the target number of iterations to go + analytic_iterations = + std::min(analytic_iterations, desired_iterations); + + // Always run the expansion on the first run in case there is a + // trivial path to be found + if (analytic_iterations <= 0) { + // Reset the counter and try the analytic path expansion + analytic_iterations = desired_iterations; + bool found_valid_expansion = false; + + // First check the coarse search resolution goals + for (auto & current_goal_node : coarse_check_goals) { + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, current_goal_node, getter, + _ctx->motion_table.state_space); + if (!analytic_nodes.nodes.empty()) { + found_valid_expansion = true; + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); + // Update the best score if we found a better path + if (score < current_best_score) { + current_best_analytic_nodes = analytic_nodes; + current_best_goal = current_goal_node; + current_best_score = score; + current_best_node = node; + } + } + } + + // perform a final search if we found a goal + if (found_valid_expansion) { + for (auto & current_goal_node : fine_check_goals) { + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, current_goal_node, getter, + _ctx->motion_table.state_space); + if (!analytic_nodes.nodes.empty()) { + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); + // Update the best score if we found a better path + if (score < current_best_score) { + current_best_analytic_nodes = analytic_nodes; + current_best_goal = current_goal_node; + current_best_score = score; + current_best_node = node; + } + } + } + } + } + + if (!current_best_analytic_nodes.nodes.empty()) { + return setAnalyticPath( + current_best_node, current_best_goal, + current_best_analytic_nodes); + } + analytic_iterations--; + } + + // No valid motion model - return nullptr + return NodePtr(nullptr); +} + +template +int AnalyticExpansion::countDirectionChanges( + const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path) +{ + const double * lengths = path.length_; + int changes = 0; + int last_dir = 0; + for (int i = 0; i < 5; ++i) { + if (lengths[i] == 0.0) { + continue; + } + + int currentDirection = (lengths[i] > 0.0) ? 1 : -1; + if (last_dir != 0 && currentDirection != last_dir) { + ++changes; + } + last_dir = currentDirection; + } + + return changes; +} + +template +typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( + const NodePtr & node, + const NodePtr & goal, + const NodeGetter & node_getter, + const ompl::base::StateSpacePtr & state_space) +{ + ompl::base::ScopedState<> from(state_space), to(state_space), s(state_space); + from[0] = node->pose.x; + from[1] = node->pose.y; + from[2] = _ctx->motion_table.getAngleFromBin(node->pose.theta); + to[0] = goal->pose.x; + to[1] = goal->pose.y; + to[2] = _ctx->motion_table.getAngleFromBin(goal->pose.theta); + + float d = state_space->distance(from(), to()); + + auto rs_state_space = dynamic_cast(state_space.get()); + int direction_changes = 0; + if (rs_state_space) { + direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get())); + } + + // A move of sqrt(2) is guaranteed to be in a new cell + static const float sqrt_2 = sqrtf(2.0f); + + // If the length is too far, exit. This prevents unsafe shortcutting of paths + // into higher cost areas far out from the goal itself, let search to the work of getting + // close before the analytic expansion brings it home. This should never be smaller than + // 4-5x the minimum turning radius being used, or planning times will begin to spike. + if (d > _search_info.analytic_expansion_max_length || d < sqrt_2) { + return AnalyticExpansionNodes(); + } + + unsigned int num_intervals = static_cast(std::floor(d / sqrt_2)); + + AnalyticExpansionNodes possible_nodes; + // When "from" and "to" are zero or one cell away, + // num_intervals == 0 + possible_nodes.nodes.reserve(num_intervals); // We won't store this node or the goal + std::vector reals; + double theta; + + // Pre-allocate + NodePtr prev(node); + uint64_t index = 0; + NodePtr next(nullptr); + float angle = 0.0; + Coordinates proposed_coordinates; + bool failure = false; + std::vector node_costs; + node_costs.reserve(num_intervals); + + // Check intermediary poses (non-goal, non-start) + for (float i = 1; i <= num_intervals; i++) { + state_space->interpolate(from(), to(), i / num_intervals, s()); + reals = s.reals(); + // Make sure in range [0, 2PI) + theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; + theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; + angle = _ctx->motion_table.getAngle(theta); + + // Turn the pose into a node, and check if it is valid + index = NodeT::getIndex( + static_cast(reals[0]), + static_cast(reals[1]), + static_cast(angle), + _ctx->motion_table.size_x, + _ctx->motion_table.num_angle_quantization); + // Get the node from the graph + if (node_getter(index, next)) { + Coordinates initial_node_coords = next->pose; + proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; + next->setPose(proposed_coordinates); + if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { + // Save the node, and its previous coordinates in case we need to abort + possible_nodes.add(next, initial_node_coords, proposed_coordinates); + node_costs.emplace_back(next->getCost()); + prev = next; + } else { + // Abort + next->setPose(initial_node_coords); + failure = true; + break; + } + } else { + // Abort + failure = true; + break; + } + } + + if (!failure) { + // We found 'a' valid expansion. Now to tell if its a quality option... + const float max_cost = _search_info.analytic_expansion_max_cost; + auto max_cost_it = std::max_element(node_costs.begin(), node_costs.end()); + if (max_cost_it != node_costs.end() && *max_cost_it > max_cost) { + // If any element is above the comfortable cost limit, check edge cases: + // (1) Check if goal is in greater than max_cost space requiring + // entering it, but only entering it on final approach, not in-and-out + // (2) Checks if goal is in normal space, but enters costed space unnecessarily + // mid-way through, skirting obstacle or in non-globally confined space + bool cost_exit_high_cost_region = false; + for (auto iter = node_costs.rbegin(); iter != node_costs.rend(); ++iter) { + const float & curr_cost = *iter; + if (curr_cost <= max_cost) { + cost_exit_high_cost_region = true; + } else if (curr_cost > max_cost && cost_exit_high_cost_region) { + failure = true; + break; + } + } + + // (3) Handle exception: there may be no other option close to goal + // if max cost is set too low (optional) + if (failure) { + if (d < 2.0f * M_PI * _ctx->motion_table.min_turning_radius && + _search_info.analytic_expansion_max_cost_override) + { + failure = false; + } + } + } + } + + // Reset to initial poses to not impact future searches + for (const auto & node_pose : possible_nodes.nodes) { + const auto & n = node_pose.node; + n->setPose(node_pose.initial_coords); + } + + if (failure) { + return AnalyticExpansionNodes(); + } + + possible_nodes.setDirectionChanges(direction_changes); + return possible_nodes; +} + +template +float AnalyticExpansion::refineAnalyticPath( + NodePtr & node, + const NodePtr & goal_node, + const NodeGetter & getter, + AnalyticExpansionNodes & analytic_nodes) +{ + NodePtr test_node = node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && + test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + // print the goals pose + refined_analytic_nodes = + getAnalyticPath( + test_node, goal_node, getter, + _ctx->motion_table.state_space); + if (refined_analytic_nodes.nodes.empty()) { + break; + } + if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) { + // If the direction changes are worse, we don't want to use this path + continue; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; + } + } + + // The analytic expansion can short-cut near obstacles when closer to a goal + // So, we can attempt to refine it more by increasing the possible radius + // higher than the minimum turning radius and use the best solution based on + // a scoring function similar to that used in traversal cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.nodes.size() < 2) { + return std::numeric_limits::max(); + } + + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x, + expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y); + const float & weight = _ctx->motion_table.cost_penalty; + for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; + + float original_score = scoringFn(analytic_nodes); + float best_score = original_score; + float score = std::numeric_limits::max(); + float min_turn_rad = _ctx->motion_table.min_turning_radius; + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (_ctx->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); + } + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + score = scoringFn(refined_analytic_nodes); + + // Normal scoring: prioritize lower cost as long as not more directional changes + if (score <= best_score && + refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes) + { + analytic_nodes = refined_analytic_nodes; + best_score = score; + continue; + } + + // Special case: If we have a better score than original (only) and less directional changes + // the path quality is still better than the original and is less operationally complex + if (score <= original_score && + refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes) + { + analytic_nodes = refined_analytic_nodes; + best_score = score; + } + } + + return best_score; +} + +template +typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( + const NodePtr & node, + const NodePtr & goal_node, + const AnalyticExpansionNodes & expanded_nodes) +{ + _detached_nodes.clear(); + // Legitimate final path - set the parent relationships, states, and poses + NodePtr prev = node; + for (const auto & node_pose : expanded_nodes.nodes) { + auto n = node_pose.node; + cleanNode(n); + if (n->getIndex() != goal_node->getIndex()) { + if (n->wasVisited()) { + _detached_nodes.push_back(std::make_unique(-1, _ctx)); + n = _detached_nodes.back().get(); + } + n->parent = prev; + n->pose = node_pose.proposed_coords; + n->visited(); + prev = n; + } + } + if (goal_node != prev) { + goal_node->parent = prev; + cleanNode(goal_node); + goal_node->visited(); + } + return goal_node; +} + +template +void AnalyticExpansion::cleanNode(const NodePtr & /*expanded_nodes*/) +{ +} + +template class AnalyticExpansion; + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp new file mode 100644 index 00000000..24c8baf7 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp @@ -0,0 +1,196 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#include "athena_smac_planner/collision_checker.hpp" + +namespace athena_smac_planner +{ + +GridCollisionChecker::GridCollisionChecker( + std::shared_ptr costmap_ros, + unsigned int num_quantizations, + rclcpp::Node::SharedPtr node) +: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr) +{ + if (node) { + clock_ = node->get_clock(); + logger_ = node->get_logger(); + } + + if (costmap_ros) { + costmap_ros_ = costmap_ros; + } + + // Convert number of regular bins into angles + float bin_size = 2 * M_PI / static_cast(num_quantizations); + angles_.reserve(num_quantizations); + for (unsigned int i = 0; i != num_quantizations; i++) { + angles_.push_back(bin_size * i); + } +} + +// GridCollisionChecker::GridCollisionChecker( +// nav2_costmap_2d::Costmap2D * costmap, +// std::vector & angles) +// : FootprintCollisionChecker(costmap), +// angles_(angles) +// { +// } + +void GridCollisionChecker::setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_collision_cost) +{ + possible_collision_cost_ = static_cast(possible_collision_cost); + if (possible_collision_cost_ <= 0.0f) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/athena_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + footprint_is_radius_ = radius; + + // Use radius, no caching required + if (radius) { + return; + } + + // No change, no updates required + if (footprint == unoriented_footprint_) { + return; + } + + oriented_footprints_.clear(); + oriented_footprints_.reserve(angles_.size()); + double sin_th, cos_th; + geometry_msgs::msg::Point new_pt; + const unsigned int footprint_size = footprint.size(); + + // Precompute the orientation bins for checking to use + for (unsigned int i = 0; i != angles_.size(); i++) { + sin_th = sin(angles_[i]); + cos_th = cos(angles_[i]); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint_size); + + for (unsigned int j = 0; j < footprint_size; j++) { + new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; + new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; + oriented_footprint.push_back(new_pt); + } + + oriented_footprints_.push_back(oriented_footprint); + } + + unoriented_footprint_ = footprint; +} + +bool GridCollisionChecker::inCollision( + const float & x, + const float & y, + const float & angle_bin, + const bool & traverse_unknown) +{ + // Check to make sure cell is inside the map + if (outsideRange(costmap_->getSizeInCellsX(), x) || + outsideRange(costmap_->getSizeInCellsY(), y)) + { + return true; + } + + // Assumes setFootprint already set + center_cost_ = static_cast(costmap_->getCost( + static_cast(x + 0.5f), static_cast(y + 0.5f))); + + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points, but first see + // if the robot is even potentially in an inscribed collision + if (center_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { + return false; + } + + // If its inscribed, in collision, or unknown in the middle, + // no need to even check the footprint, its invalid + if (center_cost_ == UNKNOWN_COST && !traverse_unknown) { + return true; + } + + if (center_cost_ == INSCRIBED_COST || center_cost_ == OCCUPIED_COST) { + return true; + } + + // if possible inscribed, need to check actual footprint pose. + // Use precomputed oriented footprints are done on initialization, + // offset by translation value to collision check + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + geometry_msgs::msg::Point new_pt; + const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { + new_pt.x = wx + oriented_footprint[i].x; + new_pt.y = wy + oriented_footprint[i].y; + current_footprint.push_back(new_pt); + } + + float footprint_cost = static_cast(footprintCost(current_footprint)); + + if (footprint_cost == UNKNOWN_COST && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost >= OCCUPIED_COST; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + if (center_cost_ == UNKNOWN_COST && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return center_cost_ >= INSCRIBED_COST; + } +} + +bool GridCollisionChecker::inCollision( + const unsigned int & i, + const bool & traverse_unknown) +{ + center_cost_ = costmap_->getCost(i); + if (center_cost_ == UNKNOWN_COST && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return center_cost_ >= INSCRIBED_COST; +} + +float GridCollisionChecker::getCost() +{ + // Assumes inCollision called prior + return static_cast(center_cost_); +} + +bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value) +{ + return value < 0.0f || value > max; +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp new file mode 100644 index 00000000..4cfac234 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp @@ -0,0 +1,127 @@ +// Copyright (c) 2020, Carlos Luis +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#include "athena_smac_planner/costmap_downsampler.hpp" + +#include +#include +#include + +namespace athena_smac_planner +{ + +CostmapDownsampler::CostmapDownsampler() +: _costmap(nullptr), + _downsampled_costmap(nullptr) +{ +} + +CostmapDownsampler::~CostmapDownsampler() +{ +} + +void CostmapDownsampler::on_configure( + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor, + const bool & use_min_cost_neighbor) +{ + _costmap = costmap; + _downsampling_factor = downsampling_factor; + _use_min_cost_neighbor = use_min_cost_neighbor; + updateCostmapSize(); + + _downsampled_costmap = std::make_unique( + _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, + _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN_COST); +} + +void CostmapDownsampler::on_cleanup() +{ + _costmap = nullptr; + _downsampled_costmap.reset(); +} + +nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( + const unsigned int & downsampling_factor) +{ + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + // Adjust costmap size if needed + if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || + _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || + _downsampled_costmap->getResolution() != _downsampled_resolution) + { + resizeCostmap(); + } + + // Assign costs + for (unsigned int i = 0; i < _downsampled_size_x; ++i) { + for (unsigned int j = 0; j < _downsampled_size_y; ++j) { + setCostOfCell(i, j); + } + } + + return _downsampled_costmap.get(); +} + +void CostmapDownsampler::updateCostmapSize() +{ + _size_x = _costmap->getSizeInCellsX(); + _size_y = _costmap->getSizeInCellsY(); + _downsampled_size_x = ceil(static_cast(_size_x) / _downsampling_factor); + _downsampled_size_y = ceil(static_cast(_size_y) / _downsampling_factor); + _downsampled_resolution = _downsampling_factor * _costmap->getResolution(); +} + +void CostmapDownsampler::resizeCostmap() +{ + _downsampled_costmap->resizeMap( + _downsampled_size_x, + _downsampled_size_y, + _downsampled_resolution, + _costmap->getOriginX(), + _costmap->getOriginY()); +} + +void CostmapDownsampler::setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my) +{ + unsigned int mx, my; + unsigned char cost = _use_min_cost_neighbor ? 255 : 0; + unsigned int x_offset = new_mx * _downsampling_factor; + unsigned int y_offset = new_my * _downsampling_factor; + + for (unsigned int i = 0; i < _downsampling_factor; ++i) { + mx = x_offset + i; + if (mx >= _size_x) { + continue; + } + for (unsigned int j = 0; j < _downsampling_factor; ++j) { + my = y_offset + j; + if (my >= _size_y) { + continue; + } + cost = _use_min_cost_neighbor ? + std::min(cost, _costmap->getCost(mx, my)) : + std::max(cost, _costmap->getCost(mx, my)); + } + } + + _downsampled_costmap->setCost(new_mx, new_my, cost); +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp new file mode 100644 index 00000000..1b16307e --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2026, 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. Reserved. + +#include "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "ompl/base/spaces/ReedsSheppStateSpace.h" +#include "athena_smac_planner/distance_heuristic.hpp" +#include "athena_smac_planner/node_hybrid.hpp" + +namespace athena_smac_planner +{ + +template<> +template +void DistanceHeuristic::precomputeDistanceHeuristic( + const float & lookup_table_dim, + const MotionModel & motion_model, + const unsigned int & dim_3_size, + const SearchInfo & search_info, + MotionTableT & motion_table) +{ + // Dubin or Reeds-Shepp shortest distances + if (motion_model == MotionModel::DUBIN) { + motion_table.state_space = std::make_shared( + search_info.minimum_turning_radius); + } else if (motion_model == MotionModel::REEDS_SHEPP) { + motion_table.state_space = std::make_shared( + search_info.minimum_turning_radius); + } else { + throw std::runtime_error( + "Node attempted to precompute distance heuristics " + "with invalid motion model!"); + } + + ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + to[0] = 0.0; + to[1] = 0.0; + to[2] = 0.0; + size_lookup_ = lookup_table_dim; + float motion_heuristic = 0.0; + unsigned int index = 0; + int dim_3_size_int = static_cast(dim_3_size); + float angular_bin_size = 2 * M_PI / static_cast(dim_3_size); + + // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal + // to help drive the search towards admissible approaches. Deu to symmetries in the + // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror + // around the X axis any relative node lookup. This reduces memory overhead and increases + // the size of a window a platform can store in memory. + dist_heuristic_lookup_table_.resize(size_lookup_ * ceil(size_lookup_ / 2.0) * dim_3_size_int); + for (float x = ceil(-size_lookup_ / 2.0); x <= floor(size_lookup_ / 2.0); x += 1.0) { + for (float y = 0.0; y <= floor(size_lookup_ / 2.0); y += 1.0) { + for (int heading = 0; heading != dim_3_size_int; heading++) { + from[0] = x; + from[1] = y; + from[2] = heading * angular_bin_size; + motion_heuristic = motion_table.state_space->distance(from(), to()); + dist_heuristic_lookup_table_[index] = motion_heuristic; + index++; + } + } + } +} + +template +template +float DistanceHeuristic::getDistanceHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const float & obstacle_heuristic, + MotionTableT & motion_table) +{ + // rotate and translate node_coords such that goal_coords relative is (0,0,0) + // Due to the rounding involved in exact cell increments for caching, + // this is not an exact replica of a live heuristic, but has bounded error. + // (Usually less than 1 cell length) + + // This angle is negative since we are de-rotating the current node + // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th) + const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta]; + const float cos_th = trig_vals.first; + const float sin_th = -trig_vals.second; + const float dx = node_coords.x - goal_coords.x; + const float dy = node_coords.y - goal_coords.y; + + double dtheta_bin = node_coords.theta - goal_coords.theta; + if (dtheta_bin < 0) { + dtheta_bin += motion_table.num_angle_quantization; + } + if (dtheta_bin > motion_table.num_angle_quantization) { + dtheta_bin -= motion_table.num_angle_quantization; + } + + Coordinates node_coords_relative( + round(dx * cos_th - dy * sin_th), + round(dx * sin_th + dy * cos_th), + round(dtheta_bin)); + + // Check if the relative node coordinate is within the localized window around the goal + // to apply the distance heuristic. Since the lookup table is contains only the positive + // X axis, we mirror the Y and theta values across the X axis to find the heuristic values. + float motion_heuristic = 0.0; + const int floored_size = floor(size_lookup_ / 2.0); + const int ceiling_size = ceil(size_lookup_ / 2.0); + const float mirrored_relative_y = abs(node_coords_relative.y); + if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) { + // Need to mirror angle if Y coordinate was mirrored + int theta_pos; + if (node_coords_relative.y < 0.0) { + theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta; + } else { + theta_pos = node_coords_relative.theta; + } + const int x_pos = node_coords_relative.x + floored_size; + const int y_pos = static_cast(mirrored_relative_y); + const int index = + x_pos * ceiling_size * motion_table.num_angle_quantization + + y_pos * motion_table.num_angle_quantization + + theta_pos; + motion_heuristic = dist_heuristic_lookup_table_[index]; + } else if (obstacle_heuristic <= 0.0) { + ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + to[0] = goal_coords.x; + to[1] = goal_coords.y; + from[0] = node_coords.x; + from[1] = node_coords.y; + if constexpr (std::is_same_v) { + to[2] = goal_coords.theta * motion_table.num_angle_quantization; + from[2] = node_coords.theta * motion_table.num_angle_quantization; + } else { + to[2] = motion_table.getAngleFromBin(goal_coords.theta); + from[2] = motion_table.getAngleFromBin(node_coords.theta); + } + motion_heuristic = motion_table.state_space->distance(from(), to()); + } + + return motion_heuristic; +} + +// Instantiate algorithm for the supported template types +template void DistanceHeuristic::precomputeDistanceHeuristic( + const float &, const MotionModel &, const unsigned int &, const SearchInfo &, + HybridMotionTable &); +template float DistanceHeuristic::getDistanceHeuristic( + const Coordinates &, const Coordinates &, const float &, HybridMotionTable &); +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp new file mode 100644 index 00000000..76c3b95b --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp @@ -0,0 +1,159 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "athena_smac_planner/smac_planner_hybrid.hpp" +#include "msgs/msg/planner_event.hpp" + +class GlobalPlannerNode : public rclcpp::Node +{ +public: + explicit GlobalPlannerNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("global_planner", options) + { + declare_parameter("use_sim_time", false); + } + + void configure() + { + costmap_ros_ = std::make_shared( + "global_costmap", + get_namespace(), + "global_costmap"); + costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", get_parameter("use_sim_time").as_bool())); + costmap_ros_->configure(); + costmap_ros_->activate(); + + planner_ = std::make_shared( + shared_from_this(), costmap_ros_); + + path_pub_ = create_publisher( + "/global_path", rclcpp::QoS(1).transient_local()); + event_pub_ = create_publisher("/planner_event", 10); + + robot_pose_sub_ = create_subscription( + "/robot_pose", 10, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + std::optional pending; + { + std::lock_guard lk(mutex_); + robot_pose_ = *msg; + // If a goal arrived before robot_pose was ready, plan now + if (pending_goal_.has_value()) { + pending = *pending_goal_; + pending_goal_.reset(); + } + } + if (pending.has_value()) { + RCLCPP_INFO(get_logger(), "[global_planner] robot_pose received — planning pending goal"); + plan(*pending); + } + }); + + goal_pose_sub_ = create_subscription( + "/goal_pose", 10, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { onGoal(msg); }); + + RCLCPP_INFO(get_logger(), "GlobalPlannerNode configured."); + } + + std::shared_ptr getCostmapROS() + { + return costmap_ros_; + } + +private: + void publishEvent(uint8_t event_type) + { + msgs::msg::PlannerEvent ev; + ev.event = event_type; + event_pub_->publish(ev); + } + + void onGoal(const geometry_msgs::msg::PoseStamped::SharedPtr goal) + { + publishEvent(msgs::msg::PlannerEvent::NEW_GOAL); + + { + std::lock_guard lk(mutex_); + if (!robot_pose_.has_value()) { + RCLCPP_WARN(get_logger(), + "[global_planner] goal received but no robot_pose yet — waiting"); + pending_goal_ = *goal; // cache; plan() called once robot_pose arrives + return; + } + // Overwrite any stale pending goal — the new goal supersedes it + pending_goal_.reset(); + } + + plan(*goal); + } + + void plan(const geometry_msgs::msg::PoseStamped & goal) + { + geometry_msgs::msg::PoseStamped start; + { + std::lock_guard lk(mutex_); + start = robot_pose_.value(); + } + + publishEvent(msgs::msg::PlannerEvent::PLANNING); + + try { + auto path = planner_->createPlan(start, goal, [] { return false; }); + path_pub_->publish(path); + publishEvent(msgs::msg::PlannerEvent::PLAN_SUCCEEDED); + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Planning failed: %s", ex.what()); + publishEvent(msgs::msg::PlannerEvent::PLAN_FAILED); + } + } + + std::shared_ptr costmap_ros_; + std::shared_ptr planner_; + + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr event_pub_; + rclcpp::Subscription::SharedPtr robot_pose_sub_; + rclcpp::Subscription::SharedPtr goal_pose_sub_; + + std::mutex mutex_; + std::optional robot_pose_; + std::optional pending_goal_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + node->configure(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.add_node(node->getCostmapROS()->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp new file mode 100644 index 00000000..ee1dc183 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp @@ -0,0 +1,54 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#include "athena_smac_planner/node_basic.hpp" + +namespace athena_smac_planner +{ + +template +void NodeBasic::processSearchNode() +{ +} + +template<> +void NodeBasic::processSearchNode() +{ + // We only want to override the node's pose if it has not yet been visited + // to prevent the case that a node has been queued multiple times and + // a new branch is overriding one of lower cost already visited. + if (!this->graph_node_ptr->wasVisited()) { + this->graph_node_ptr->pose = this->pose; + this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index, this->turn_dir); + } +} + +template +void NodeBasic::populateSearchNode(NodeT * & node) +{ + this->graph_node_ptr = node; +} + +template<> +void NodeBasic::populateSearchNode(NodeHybrid * & node) +{ + this->pose = node->pose; + this->graph_node_ptr = node; + this->motion_index = node->getMotionPrimitiveIndex(); + this->turn_dir = node->getTurnDirection(); +} + +template class NodeBasic; + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp new file mode 100644 index 00000000..0c71a622 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp @@ -0,0 +1,540 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// Copyright (c) 2023, 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. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "ompl/base/spaces/ReedsSheppStateSpace.h" + +#include "athena_smac_planner/node_hybrid.hpp" + +using namespace std::chrono; // NOLINT + +namespace athena_smac_planner +{ + +// Each of these tables are the projected motion models through +// time and space applied to the search on the current node in +// continuous map-coordinates (e.g. not meters but partial map cells) +// Currently, these are set to project *at minimum* into a neighboring +// cell. Though this could be later modified to project a certain +// amount of time or particular distance forward. + +// http://planning.cs.uiuc.edu/planning/node821.html +// Model for ackermann style vehicle with minimum radius restriction +void HybridMotionTable::initDubin( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + travel_distance_reward = 1.0f - search_info.retrospective_penalty; + downsample_obstacle_heuristic = search_info.downsample_obstacle_heuristic; + use_quadratic_cost_penalty = search_info.use_quadratic_cost_penalty; + + // if nothing changed, no need to re-compute primitives + if (num_angle_quantization_in == num_angle_quantization && + min_turning_radius == search_info.minimum_turning_radius && + allow_primitive_interpolation == search_info.allow_primitive_interpolation && + motion_model == MotionModel::DUBIN) + { + return; + } + + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + min_turning_radius = search_info.minimum_turning_radius; + allow_primitive_interpolation = search_info.allow_primitive_interpolation; + motion_model = MotionModel::DUBIN; + + // angle must meet 3 requirements: + // 1) be increment of quantized bin size + // 2) chord length must be greater than sqrt(2) to leave current cell + // 3) maximum curvature must be respected, represented by minimum turning angle + // Thusly: + // On circle of radius minimum turning angle, we need select motion primitives + // with chord length > sqrt(2) and be an increment of our bin size + // + // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size + // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) + float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); + // Now make sure angle is an increment of the quantized bin size + // And since its based on the minimum chord, we need to make sure its always larger + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + // Search dimensions are clean multiples of quantization - this prevents + // paths with loops in them + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + // find deflections + // If we make a right triangle out of the chord in circle of radius + // min turning angle, we can see that delta X = R * sin (angle) + const float delta_x = min_turning_radius * sin(angle); + // Using that same right triangle, we can see that the complement + // to delta Y is R * cos (angle). If we subtract R, we get the actual value + const float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); + const float delta_dist = hypotf(delta_x, delta_y); + + projections.clear(); + projections.reserve(3); + projections.emplace_back(delta_dist, 0.0, 0.0, TurnDirection::FORWARD); // Forward + projections.emplace_back(delta_x, delta_y, increments, TurnDirection::LEFT); // Left + projections.emplace_back(delta_x, -delta_y, -increments, TurnDirection::RIGHT); // Right + + if (search_info.allow_primitive_interpolation && increments > 1.0f) { + // Create primitives that are +/- N to fill in search space to use all set angular quantizations + // Allows us to create N many primitives so that each search iteration can expand into any angle + // bin possible with the minimum turning radius constraint, not just the most extreme turns. + projections.reserve(3 + (2 * (increments - 1))); + for (unsigned int i = 1; i < static_cast(increments); i++) { + const float angle_n = static_cast(i) * bin_size; + const float turning_rad_n = delta_dist / (2.0f * sin(angle_n / 2.0f)); + const float delta_x_n = turning_rad_n * sin(angle_n); + const float delta_y_n = turning_rad_n - (turning_rad_n * cos(angle_n)); + projections.emplace_back( + delta_x_n, delta_y_n, static_cast(i), TurnDirection::LEFT); // Left + projections.emplace_back( + delta_x_n, -delta_y_n, -static_cast(i), TurnDirection::RIGHT); // Right + } + } + + // Create the correct OMPL state space + state_space = std::make_shared(min_turning_radius); + + // Precompute projection deltas + delta_xs.resize(projections.size()); + delta_ys.resize(projections.size()); + trig_values.resize(num_angle_quantization); + + for (unsigned int i = 0; i != projections.size(); i++) { + delta_xs[i].resize(num_angle_quantization); + delta_ys[i].resize(num_angle_quantization); + + for (unsigned int j = 0; j != num_angle_quantization; j++) { + double cos_theta = cos(bin_size * j); + double sin_theta = sin(bin_size * j); + if (i == 0) { + // if first iteration, cache the trig values for later + trig_values[j] = {cos_theta, sin_theta}; + } + delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; + delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; + } + } + + // Precompute travel costs for each motion primitive + travel_costs.resize(projections.size()); + for (unsigned int i = 0; i != projections.size(); i++) { + const TurnDirection turn_dir = projections[i]._turn_dir; + if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { + // Turning, so length is the arc length + const float arc_angle = projections[i]._theta * bin_size; + const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); + travel_costs[i] = turning_rad * arc_angle; + } else { + travel_costs[i] = delta_dist; + } + } +} + +// http://planning.cs.uiuc.edu/planning/node822.html +// Same as Dubin model but now reverse is valid +// See notes in Dubin for explanation +void HybridMotionTable::initReedsShepp( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + travel_distance_reward = 1.0f - search_info.retrospective_penalty; + downsample_obstacle_heuristic = search_info.downsample_obstacle_heuristic; + use_quadratic_cost_penalty = search_info.use_quadratic_cost_penalty; + + // if nothing changed, no need to re-compute primitives + if (num_angle_quantization_in == num_angle_quantization && + min_turning_radius == search_info.minimum_turning_radius && + allow_primitive_interpolation == search_info.allow_primitive_interpolation && + motion_model == MotionModel::REEDS_SHEPP) + { + return; + } + + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + min_turning_radius = search_info.minimum_turning_radius; + allow_primitive_interpolation = search_info.allow_primitive_interpolation; + motion_model = MotionModel::REEDS_SHEPP; + + float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + const float delta_x = min_turning_radius * sin(angle); + const float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); + const float delta_dist = hypotf(delta_x, delta_y); + + projections.clear(); + projections.reserve(6); + projections.emplace_back(delta_dist, 0.0, 0.0, TurnDirection::FORWARD); // Forward + projections.emplace_back( + delta_x, delta_y, increments, TurnDirection::LEFT); // Forward + Left + projections.emplace_back( + delta_x, -delta_y, -increments, TurnDirection::RIGHT); // Forward + Right + projections.emplace_back(-delta_dist, 0.0, 0.0, TurnDirection::REVERSE); // Backward + projections.emplace_back( + -delta_x, delta_y, -increments, TurnDirection::REV_LEFT); // Backward + Left + projections.emplace_back( + -delta_x, -delta_y, increments, TurnDirection::REV_RIGHT); // Backward + Right + + if (search_info.allow_primitive_interpolation && increments > 1.0f) { + // Create primitives that are +/- N to fill in search space to use all set angular quantizations + // Allows us to create N many primitives so that each search iteration can expand into any angle + // bin possible with the minimum turning radius constraint, not just the most extreme turns. + projections.reserve(6 + (4 * (increments - 1))); + for (unsigned int i = 1; i < static_cast(increments); i++) { + const float angle_n = static_cast(i) * bin_size; + const float turning_rad_n = delta_dist / (2.0f * sin(angle_n / 2.0f)); + const float delta_x_n = turning_rad_n * sin(angle_n); + const float delta_y_n = turning_rad_n - (turning_rad_n * cos(angle_n)); + projections.emplace_back( + delta_x_n, delta_y_n, static_cast(i), TurnDirection::LEFT); // Forward + Left + projections.emplace_back( + delta_x_n, -delta_y_n, -static_cast(i), TurnDirection::RIGHT); // Forward + Right + projections.emplace_back( + -delta_x_n, delta_y_n, -static_cast(i), + TurnDirection::REV_LEFT); // Backward + Left + projections.emplace_back( + -delta_x_n, -delta_y_n, static_cast(i), + TurnDirection::REV_RIGHT); // Backward + Right + } + } + + // Create the correct OMPL state space + state_space = std::make_shared(min_turning_radius); + + // Precompute projection deltas + delta_xs.resize(projections.size()); + delta_ys.resize(projections.size()); + trig_values.resize(num_angle_quantization); + + for (unsigned int i = 0; i != projections.size(); i++) { + delta_xs[i].resize(num_angle_quantization); + delta_ys[i].resize(num_angle_quantization); + + for (unsigned int j = 0; j != num_angle_quantization; j++) { + double cos_theta = cos(bin_size * j); + double sin_theta = sin(bin_size * j); + if (i == 0) { + // if first iteration, cache the trig values for later + trig_values[j] = {cos_theta, sin_theta}; + } + delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; + delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; + } + } + + // Precompute travel costs for each motion primitive + travel_costs.resize(projections.size()); + for (unsigned int i = 0; i != projections.size(); i++) { + const TurnDirection turn_dir = projections[i]._turn_dir; + if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { + // Turning, so length is the arc length + const float arc_angle = projections[i]._theta * bin_size; + const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); + travel_costs[i] = turning_rad * arc_angle; + } else { + travel_costs[i] = delta_dist; + } + } +} + +MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) +{ + MotionPoses projection_list; + projection_list.reserve(projections.size()); + + for (unsigned int i = 0; i != projections.size(); i++) { + const MotionPose & proj_motion_model = projections[i]; + + // normalize theta, I know its overkill, but I've been burned before... + const float & node_heading = node->pose.theta; + float new_heading = node_heading + proj_motion_model._theta; + + if (new_heading < 0.0) { + new_heading += num_angle_quantization_float; + } + + if (new_heading >= num_angle_quantization_float) { + new_heading -= num_angle_quantization_float; + } + + projection_list.emplace_back( + delta_xs[i][node_heading] + node->pose.x, + delta_ys[i][node_heading] + node->pose.y, + new_heading, proj_motion_model._turn_dir); + } + + return projection_list; +} + +unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) +{ + auto bin = static_cast(round(static_cast(theta) / bin_size)); + return bin < num_angle_quantization ? bin : 0u; +} + +float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) +{ + return bin_idx * bin_size; +} + +double HybridMotionTable::getAngle(const double & theta) +{ + return theta / bin_size; +} + +NodeHybrid::NodeHybrid(const uint64_t index, NodeContext * ctx) +: parent(nullptr), + pose(0.0f, 0.0f, 0.0f), + _cell_cost(std::numeric_limits::quiet_NaN()), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _motion_primitive_index(std::numeric_limits::max()), + _is_node_valid(false), + _ctx(ctx) +{ +} + +NodeHybrid::~NodeHybrid() +{ + parent = nullptr; +} + +void NodeHybrid::reset() +{ + parent = nullptr; + _cell_cost = std::numeric_limits::quiet_NaN(); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _motion_primitive_index = std::numeric_limits::max(); + pose.x = 0.0f; + pose.y = 0.0f; + pose.theta = 0.0f; + _is_node_valid = false; +} + +bool NodeHybrid::isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker * collision_checker) +{ + // Already found, we can return the result + if (!std::isnan(_cell_cost)) { + return _is_node_valid; + } + + _is_node_valid = !collision_checker->inCollision( + this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown); + _cell_cost = collision_checker->getCost(); + return _is_node_valid; +} + +float NodeHybrid::getTraversalCost(const NodePtr & child) +{ + const float normalized_cost = child->getCost() / 252.0f; + if (std::isnan(normalized_cost)) { + throw std::runtime_error( + "Node attempted to get traversal " + "cost without a known SE2 collision cost!"); + } + + const TurnDirection & child_turn_dir = child->getTurnDirection(); + float travel_cost_raw = _ctx->motion_table.travel_costs[child->getMotionPrimitiveIndex()]; + float travel_cost = 0.0; + + if (_ctx->motion_table.use_quadratic_cost_penalty) { + travel_cost_raw *= + (_ctx->motion_table.travel_distance_reward + + (_ctx->motion_table.cost_penalty * normalized_cost * normalized_cost)); + } else { + travel_cost_raw *= + (_ctx->motion_table.travel_distance_reward + _ctx->motion_table.cost_penalty * + normalized_cost); + } + + if (child_turn_dir == TurnDirection::FORWARD || child_turn_dir == TurnDirection::REVERSE || + getMotionPrimitiveIndex() == std::numeric_limits::max()) + { + // New motion is a straight motion, no additional costs to be applied + travel_cost = travel_cost_raw; + } else { + if (getTurnDirection() == child_turn_dir) { + // Turning motion but keeps in same direction: encourages to commit to turning if starting it + travel_cost = travel_cost_raw * _ctx->motion_table.non_straight_penalty; + } else { + // Turning motion and changing direction: penalizes wiggling + travel_cost = travel_cost_raw * + (_ctx->motion_table.non_straight_penalty + _ctx->motion_table.change_penalty); + } + } + + if (child_turn_dir == TurnDirection::REV_RIGHT || + child_turn_dir == TurnDirection::REV_LEFT || + child_turn_dir == TurnDirection::REVERSE) + { + // reverse direction + travel_cost *= _ctx->motion_table.reverse_penalty; + } + + return travel_cost; +} + +float NodeHybrid::getHeuristicCost( + const Coordinates & node_coords, + const CoordinateVector & goals_coords) +{ + // obstacle heuristic does not depend on goal heading + const float obstacle_heuristic = + _ctx->obstacle_heuristic->getObstacleHeuristic(node_coords, _ctx->motion_table.cost_penalty, + _ctx->motion_table.use_quadratic_cost_penalty, + _ctx->motion_table.downsample_obstacle_heuristic); + float distance_heuristic = std::numeric_limits::max(); + for (unsigned int i = 0; i < goals_coords.size(); i++) { + distance_heuristic = std::min( + distance_heuristic, + _ctx->distance_heuristic->getDistanceHeuristic(node_coords, goals_coords[i], + obstacle_heuristic, _ctx->motion_table)); + } + return std::max(obstacle_heuristic, distance_heuristic); +} + +void NodeHybrid::initMotionModel( + NodeContext * ctx, + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & num_angle_quantization, + SearchInfo & search_info) +{ + // find the motion model selected + switch (motion_model) { + case MotionModel::DUBIN: + ctx->motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); + break; + case MotionModel::REEDS_SHEPP: + ctx->motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); + break; + default: + throw std::runtime_error( + "Invalid motion model for Hybrid A*. Please select between" + " Dubin (Ackermann forward only)," + " Reeds-Shepp (Ackermann forward and back)."); + } +} + +void NodeHybrid::getNeighbors( + std::function & NeighborGetter, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) +{ + uint64_t index = 0; + NodePtr neighbor = nullptr; + Coordinates initial_node_coords; + const MotionPoses motion_projections = _ctx->motion_table.getProjections(this); + + for (unsigned int i = 0; i != motion_projections.size(); i++) { + index = NodeHybrid::getIndex( + static_cast(motion_projections[i]._x), + static_cast(motion_projections[i]._y), + static_cast(motion_projections[i]._theta), + _ctx->motion_table.size_x, _ctx->motion_table.num_angle_quantization); + + if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { + // Cache the initial pose in case it was visited but valid + // don't want to disrupt continuous coordinate expansion + initial_node_coords = neighbor->pose; + neighbor->setPose( + Coordinates( + motion_projections[i]._x, + motion_projections[i]._y, + motion_projections[i]._theta)); + if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { + neighbor->setMotionPrimitiveIndex(i, motion_projections[i]._turn_dir); + neighbors.push_back(neighbor); + } else { + neighbor->setPose(initial_node_coords); + } + } + } +} + +bool NodeHybrid::backtracePath(CoordinateVector & path) +{ + if (!this->parent) { + return false; + } + + NodePtr current_node = this; + + while (current_node->parent) { + path.push_back(current_node->pose); + // Convert angle to radians + path.back().theta = _ctx->motion_table.getAngleFromBin(path.back().theta); + current_node = current_node->parent; + } + + // add the start pose + path.push_back(current_node->pose); + // Convert angle to radians + path.back().theta = _ctx->motion_table.getAngleFromBin(path.back().theta); + + return true; +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp new file mode 100644 index 00000000..74715329 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp @@ -0,0 +1,230 @@ +// Copyright (c) 2026, 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. Reserved. + +#include "athena_smac_planner/obstacle_heuristic.hpp" + +namespace athena_smac_planner +{ + +void ObstacleHeuristic::resetObstacleHeuristic( + std::shared_ptr costmap_ros_i, + const unsigned int & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y, + const bool downsample_obstacle_heuristic) +{ + // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up + // the planner considerably to search through 75% less cells with no detectable + // erosion of path quality after even modest smoothing. The error would be no more + // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality + costmap_ros = costmap_ros_i; + auto costmap = costmap_ros->getCostmap(); + + // Clear lookup table + unsigned int size = 0u; + unsigned int size_x = 0u; + if (downsample_obstacle_heuristic) { + size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); + size = size_x * + ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); + } else { + size_x = costmap->getSizeInCellsX(); + size = size_x * costmap->getSizeInCellsY(); + } + + if (obstacle_heuristic_lookup_table_.size() == size) { + // must reset all values + std::fill( + obstacle_heuristic_lookup_table_.begin(), + obstacle_heuristic_lookup_table_.end(), 0.0f); + } else { + unsigned int obstacle_size = obstacle_heuristic_lookup_table_.size(); + obstacle_heuristic_lookup_table_.resize(size, 0.0f); + // must reset values for non-constructed indices + std::fill_n( + obstacle_heuristic_lookup_table_.begin(), obstacle_size, 0.0f); + } + + obstacle_heuristic_queue_.clear(); + obstacle_heuristic_queue_.reserve(size); + + // Set initial goal point to queue from. Divided by 2 due to downsampled costmap. + unsigned int goal_index; + if (downsample_obstacle_heuristic) { + goal_index = floor(goal_y / 2.0f) * size_x + floor(goal_x / 2.0f); + } else { + goal_index = floor(goal_y) * size_x + floor(goal_x); + } + + obstacle_heuristic_queue_.emplace_back( + distanceHeuristic2D(goal_index, size_x, start_x, start_y), goal_index); + + // initialize goal cell with a very small value to differentiate it from 0.0 (~uninitialized) + // the negative value means the cell is in the open set + obstacle_heuristic_lookup_table_[goal_index] = -0.00001f; +} + +float ObstacleHeuristic::getObstacleHeuristic( + const Coordinates & node_coords, + const float & cost_penalty, + const bool use_quadratic_cost_penalty, + const bool downsample_obstacle_heuristic) +{ + // If already expanded, return the cost + auto costmap = costmap_ros->getCostmap(); + unsigned int size_x = 0u; + unsigned int size_y = 0u; + if (downsample_obstacle_heuristic) { + size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); + size_y = ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); + } else { + size_x = costmap->getSizeInCellsX(); + size_y = costmap->getSizeInCellsY(); + } + + // Divided by 2 due to downsampled costmap. + unsigned int start_y, start_x; + if (downsample_obstacle_heuristic) { + start_y = floor(node_coords.y / 2.0f); + start_x = floor(node_coords.x / 2.0f); + } else { + start_y = floor(node_coords.y); + start_x = floor(node_coords.x); + } + + const unsigned int start_index = start_y * size_x + start_x; + const float & requested_node_cost = obstacle_heuristic_lookup_table_[start_index]; + if (requested_node_cost > 0.0f) { + // costs are doubled due to downsampling + return downsample_obstacle_heuristic ? 2.0f * requested_node_cost : requested_node_cost; + } + + // If not, expand until it is included. This dynamic programming ensures that + // we only expand the MINIMUM spanning set of the costmap per planning request. + // Rather than naively expanding the entire (potentially massive) map for a limited + // path, we only expand to the extent required for the furthest expansion in the + // search-planning request that dynamically updates during search as needed. + + // start_x and start_y have changed since last call + // we need to recompute 2D distance heuristic and reprioritize queue + for (auto & n : obstacle_heuristic_queue_) { + n.first = -obstacle_heuristic_lookup_table_[n.second] + + distanceHeuristic2D(n.second, size_x, start_x, start_y); + } + std::make_heap( + obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), + ObstacleHeuristicComparator{}); + + const int size_x_int = static_cast(size_x); + const float sqrt2 = sqrtf(2.0f); + float c_cost, cost, travel_cost, new_cost, existing_cost; + unsigned int mx, my; + unsigned int idx, new_idx = 0; + + const std::vector neighborhood = {1, -1, // left right + size_x_int, -size_x_int, // up down + size_x_int + 1, size_x_int - 1, // upper diagonals + -size_x_int + 1, -size_x_int - 1}; // lower diagonals + + while (!obstacle_heuristic_queue_.empty()) { + idx = obstacle_heuristic_queue_.front().second; + std::pop_heap( + obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), + ObstacleHeuristicComparator{}); + obstacle_heuristic_queue_.pop_back(); + c_cost = obstacle_heuristic_lookup_table_[idx]; + if (c_cost > 0.0f) { + // cell has been processed and closed, no further cost improvements + // are mathematically possible thanks to euclidean distance heuristic consistency + continue; + } + c_cost = -c_cost; + obstacle_heuristic_lookup_table_[idx] = c_cost; // set a positive value to close the cell + + // find neighbors + for (unsigned int i = 0; i != neighborhood.size(); i++) { + new_idx = static_cast(static_cast(idx) + neighborhood[i]); + + // if neighbor path is better and non-lethal, set new cost and add to queue + if (new_idx < size_x * size_y) { + if (downsample_obstacle_heuristic) { + // Get costmap values as if downsampled + unsigned int y_offset = (new_idx / size_x) * 2; + unsigned int x_offset = (new_idx - ((new_idx / size_x) * size_x)) * 2; + cost = costmap->getCost(x_offset, y_offset); + for (unsigned int k = 0; k < 2u; ++k) { + unsigned int mxd = x_offset + k; + if (mxd >= costmap->getSizeInCellsX()) { + continue; + } + for (unsigned int j = 0; j < 2u; ++j) { + unsigned int myd = y_offset + j; + if (myd >= costmap->getSizeInCellsY()) { + continue; + } + if (k == 0 && j == 0) { + continue; + } + cost = std::min(cost, static_cast(costmap->getCost(mxd, myd))); + } + } + } else { + cost = static_cast(costmap->getCost(new_idx)); + } + + if (cost >= INSCRIBED_COST) { + continue; + } + + my = new_idx / size_x; + mx = new_idx - (my * size_x); + + if (mx >= size_x - 3 || mx <= 3) { + continue; + } + if (my >= size_y - 3 || my <= 3) { + continue; + } + + existing_cost = obstacle_heuristic_lookup_table_[new_idx]; + if (existing_cost <= 0.0f) { + if (use_quadratic_cost_penalty) { + travel_cost = + (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 63504.0f)); // 252^2 + } else { + travel_cost = + ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 252.0f)); + } + + new_cost = c_cost + travel_cost; + if (existing_cost == 0.0f || -existing_cost > new_cost) { + // the negative value means the cell is in the open set + obstacle_heuristic_lookup_table_[new_idx] = -new_cost; + obstacle_heuristic_queue_.emplace_back( + new_cost + distanceHeuristic2D(new_idx, size_x, start_x, start_y), new_idx); + std::push_heap( + obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), + ObstacleHeuristicComparator{}); + } + } + } + } + + if (idx == start_index) { + break; + } + } + return downsample_obstacle_heuristic ? 2.0f * requested_node_cost : requested_node_cost; +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp new file mode 100644 index 00000000..bfe1879d --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp @@ -0,0 +1,478 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2023, 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. Reserved. + +#include +#include +#include +#include +#include +#include + +#include "athena_smac_planner/smac_planner_hybrid.hpp" + +// #define BENCHMARK_TESTING + +namespace athena_smac_planner +{ + +using namespace std::chrono; // NOLINT + +SmacPlannerHybrid::SmacPlannerHybrid( + rclcpp::Node::SharedPtr node, + std::shared_ptr costmap_ros, + const std::string & name) +: _a_star(nullptr), + _collision_checker(nullptr, 1, nullptr), + _smoother(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr) +{ + _name = name; + _logger = node->get_logger(); + _clock = node->get_clock(); + _costmap = costmap_ros->getCostmap(); + _costmap_ros = costmap_ros; + _global_frame = costmap_ros->getGlobalFrameID(); + + RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerHybrid", _name.c_str()); + + // Helper: declare param if absent then get its value + auto p = [&node, &name](const std::string & param, auto def) { + if (!node->has_parameter(name + "." + param)) { + node->declare_parameter(name + "." + param, def); + } + return node->get_parameter(name + "." + param).get_value(); + }; + + int angle_quantizations; + double analytic_expansion_max_length_m; + bool smooth_path; + + // General planner params + _downsample_costmap = p("downsample_costmap", false); + _downsampling_factor = p("downsampling_factor", 1); + + angle_quantizations = p("angle_quantization_bins", 72); + _angle_bin_size = 2.0 * M_PI / angle_quantizations; + _angle_quantizations = static_cast(angle_quantizations); + + _tolerance = static_cast(p("tolerance", 0.25)); + _allow_unknown = p("allow_unknown", true); + _max_iterations = p("max_iterations", 1000000); + _max_on_approach_iterations = p("max_on_approach_iterations", 1000); + _terminal_checking_interval = p("terminal_checking_interval", 5000); + smooth_path = p("smooth_path", true); + + _minimum_turning_radius_global_coords = p("minimum_turning_radius", 0.4); + _search_info.allow_primitive_interpolation = p("allow_primitive_interpolation", false); + _search_info.cache_obstacle_heuristic = p("cache_obstacle_heuristic", false); + _search_info.reverse_penalty = p("reverse_penalty", 2.0); + _search_info.change_penalty = p("change_penalty", 0.0); + _search_info.non_straight_penalty = p("non_straight_penalty", 1.2); + _search_info.cost_penalty = p("cost_penalty", 2.0); + _search_info.retrospective_penalty = p("retrospective_penalty", 0.015); + _search_info.analytic_expansion_ratio = p("analytic_expansion_ratio", 3.5); + _search_info.analytic_expansion_max_cost = p("analytic_expansion_max_cost", 200.0); + _search_info.analytic_expansion_max_cost_override = + p("analytic_expansion_max_cost_override", false); + _search_info.use_quadratic_cost_penalty = p("use_quadratic_cost_penalty", false); + _search_info.downsample_obstacle_heuristic = p("downsample_obstacle_heuristic", true); + + analytic_expansion_max_length_m = p("analytic_expansion_max_length", 3.0); + _search_info.analytic_expansion_max_length = + analytic_expansion_max_length_m / _costmap->getResolution(); + + _max_planning_time = p("max_planning_time", 5.0); + _lookup_table_size = p("lookup_table_size", 20.0); + _debug_visualizations = p("debug_visualizations", false); + _motion_model_for_search = p("motion_model_for_search", std::string("DUBIN")); + + std::string goal_heading_type = p("goal_heading_mode", std::string("DEFAULT")); + _goal_heading_mode = fromStringToGH(goal_heading_type); + + _coarse_search_resolution = p("coarse_search_resolution", 1); + + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { + throw std::runtime_error( + "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION."); + } + + _motion_model = fromString(_motion_model_for_search); + + if (_motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.", + _motion_model_for_search.c_str()); + } + + if (_max_on_approach_iterations <= 0) { + RCLCPP_WARN( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + + if (_max_iterations <= 0) { + RCLCPP_WARN( + _logger, "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0, " + "disabling coarse iteration resolution search for goal heading"); + _coarse_search_resolution = 1; + } + + if (_angle_quantizations % _coarse_search_resolution != 0) { + throw std::runtime_error( + "coarse iteration should be an increment of the number of angular bins configured"); + } + + if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) { + RCLCPP_WARN( + _logger, "Min turning radius cannot be less than the search grid cell resolution!"); + _minimum_turning_radius_global_coords = _costmap->getResolution() * _downsampling_factor; + } + + // Convert to grid coordinates + if (!_downsample_costmap) { + _downsampling_factor = 1; + } + _search_info.minimum_turning_radius = + _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); + _lookup_table_dim = + static_cast(_lookup_table_size) / + static_cast(_costmap->getResolution() * _downsampling_factor); + + // Make sure its a whole number + _lookup_table_dim = static_cast(static_cast(_lookup_table_dim)); + + // Make sure its an odd number + if (static_cast(_lookup_table_dim) % 2 == 0) { + RCLCPP_INFO( + _logger, + "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", + _lookup_table_dim); + _lookup_table_dim += 1.0; + } + + // Initialize collision checker + _collision_checker = GridCollisionChecker( + _costmap_ros, _angle_quantizations, node); + _collision_checker.setFootprint( + costmap_ros->getRobotFootprint(), + costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); + + // Initialize A* template + _a_star = std::make_unique>(_motion_model, _search_info); + _a_star->initialize( + _allow_unknown, + _max_iterations, + _max_on_approach_iterations, + _terminal_checking_interval, + _max_planning_time, + _lookup_table_dim, + _angle_quantizations); + + // Initialize path smoother + if (smooth_path) { + SmootherParams smoother_params; + smoother_params.get(node.get(), _name); + _smoother = std::make_unique(smoother_params); + _smoother->initialize(_minimum_turning_radius_global_coords); + } + + // Initialize costmap downsampler + if (_downsample_costmap && _downsampling_factor > 1) { + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure(_costmap, _downsampling_factor); + } + + // Create publishers + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + + if (_debug_visualizations) { + _expansions_publisher = + node->create_publisher("expansions", 1); + _planned_footprints_publisher = + node->create_publisher("planned_footprints", 1); + _smoothed_footprints_publisher = + node->create_publisher("smoothed_footprints", 1); + } + + RCLCPP_INFO( + _logger, "Configured %s of type SmacPlannerHybrid with " + "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f." + "Using motion model: %s.", + _name.c_str(), _max_iterations, _max_on_approach_iterations, + _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + _tolerance, toString(_motion_model).c_str()); +} + +SmacPlannerHybrid::~SmacPlannerHybrid() +{ + RCLCPP_INFO(_logger, "Destroying %s of type SmacPlannerHybrid", _name.c_str()); + _a_star.reset(); + _smoother.reset(); + if (_costmap_downsampler) { + _costmap_downsampler->on_cleanup(); + _costmap_downsampler.reset(); + } +} + +nav_msgs::msg::Path SmacPlannerHybrid::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) +{ + std::lock_guard lock_reinit(_mutex); + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_downsample_costmap && _downsampling_factor > 1) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + _collision_checker.setCostmap(costmap); + } + + // Set collision checker and costmap information + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); + _a_star->setCollisionChecker(&_collision_checker); + + // Set starting point, in A* bin search coordinates + float mx_start, my_start, mx_goal, my_goal; + { + unsigned int umx, umy; + if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, umx, umy)) { + throw std::runtime_error( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } + mx_start = static_cast(umx); + my_start = static_cast(umy); + } + + double start_orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); + while (start_orientation_bin < 0.0) { + start_orientation_bin += static_cast(_angle_quantizations); + } + // This is needed to handle precision issues + if (start_orientation_bin >= static_cast(_angle_quantizations)) { + start_orientation_bin -= static_cast(_angle_quantizations); + } + unsigned int start_orientation_bin_int = + static_cast(start_orientation_bin); + _a_star->setStart(mx_start, my_start, start_orientation_bin_int); + + // Set goal point, in A* bin search coordinates + { + unsigned int umx, umy; + if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, umx, umy)) { + throw std::runtime_error( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } + mx_goal = static_cast(umx); + my_goal = static_cast(umy); + } + double goal_orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); + while (goal_orientation_bin < 0.0) { + goal_orientation_bin += static_cast(_angle_quantizations); + } + // This is needed to handle precision issues + if (goal_orientation_bin >= static_cast(_angle_quantizations)) { + goal_orientation_bin -= static_cast(_angle_quantizations); + } + unsigned int goal_orientation_bin_int = + static_cast(goal_orientation_bin); + _a_star->setGoal( + mx_goal, my_goal, static_cast(goal_orientation_bin_int), + _goal_heading_mode, _coarse_search_resolution); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _clock->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + // Corner case of start and goal being on the same cell + if (std::floor(mx_start) == std::floor(mx_goal) && + std::floor(my_start) == std::floor(my_goal) && + start_orientation_bin_int == goal_orientation_bin_int) + { + pose.pose = start.pose; + pose.pose.orientation = goal.pose.orientation; + plan.poses.push_back(pose); + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + auto msg = std::make_unique(plan); + _raw_plan_publisher->publish(std::move(msg)); + } + + return plan; + } + + // Compute plan + NodeHybrid::CoordinateVector path; + int num_iterations = 0; + std::string error; + std::unique_ptr>> expansions = nullptr; + if (_debug_visualizations) { + expansions = std::make_unique>>(); + } + + if (!_a_star->createPath( + path, num_iterations, + _tolerance / static_cast(costmap->getResolution()), cancel_checker, expansions.get())) + { + if (_debug_visualizations) { + auto msg = std::make_unique(); + geometry_msgs::msg::Pose msg_pose; + msg->header.stamp = _clock->now(); + msg->header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg->poses.push_back(msg_pose); + } + _expansions_publisher->publish(std::move(msg)); + } + + if (num_iterations == 1) { + throw std::runtime_error("Start occupied"); + } + + if (num_iterations < _a_star->getMaxIterations()) { + throw std::runtime_error("No valid path could be found"); + } else { + throw std::runtime_error("Exceeded maximum iterations"); + } + } + + // Convert to world coordinates + plan.poses.reserve(path.size()); + for (int i = path.size() - 1; i >= 0; --i) { + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); + pose.pose.orientation = getWorldOrientation(path[i].theta); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + auto msg = std::make_unique(plan); + _raw_plan_publisher->publish(std::move(msg)); + } + + if (_debug_visualizations) { + // Publish expansions for debug + auto now = _clock->now(); + auto msg = std::make_unique(); + geometry_msgs::msg::Pose msg_pose; + msg->header.stamp = now; + msg->header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg->poses.push_back(msg_pose); + } + _expansions_publisher->publish(std::move(msg)); + + if (_planned_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _planned_footprints_publisher->publish(std::move(marker_array)); + + // Publish planned footprints for debug + marker_array = std::make_unique(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); + } + _planned_footprints_publisher->publish(std::move(marker_array)); + } + } + + // Find how much time we have left to do smoothing + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + double time_remaining = _max_planning_time - static_cast(time_span.count()); + +#ifdef BENCHMARK_TESTING + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + + // Smooth plan + if (_smoother && num_iterations > 1) { + _smoother->smooth(plan, costmap, time_remaining); + } + +#ifdef BENCHMARK_TESTING + steady_clock::time_point c = steady_clock::now(); + duration time_span2 = duration_cast>(c - b); + std::cout << "It took " << time_span2.count() * 1000 << + " milliseconds to smooth path." << std::endl; +#endif + + if (_debug_visualizations) { + if (_smoothed_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _smoothed_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); + auto now = _clock->now(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); + } + _smoothed_footprints_publisher->publish(std::move(marker_array)); + } + } + + return plan; +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp new file mode 100644 index 00000000..7438c3d8 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp @@ -0,0 +1,436 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#include +#include + +#include +#include +#include + +#include "angles/angles.h" + +#include "tf2/utils.hpp" + +#include "athena_smac_planner/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" + +namespace athena_smac_planner +{ +using namespace nav2_util::geometry_utils; // NOLINT +using namespace std::chrono; // NOLINT +using smoother_utils::PathSegment; + +Smoother::Smoother(const SmootherParams & params) +{ + tolerance_ = params.tolerance_; + max_its_ = params.max_its_; + data_w_ = params.w_data_; + smooth_w_ = params.w_smooth_; + is_holonomic_ = params.holonomic_; + do_refinement_ = params.do_refinement_; + refinement_num_ = params.refinement_num_; +} + +void Smoother::initialize(const double & min_turning_radius) +{ + min_turning_rad_ = min_turning_radius; + state_space_ = std::make_unique(min_turning_rad_); +} + +bool Smoother::smooth( + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time) +{ + // by-pass path orientations approximation when skipping smac smoother + if (max_its_ == 0) { + return false; + } + + steady_clock::time_point start = steady_clock::now(); + double time_remaining = max_time; + bool success = true, reversing_segment; + nav_msgs::msg::Path curr_path_segment; + curr_path_segment.header = path.header; + std::vector path_segments = smoother_utils::findDirectionalPathSegments(path); + + for (unsigned int i = 0; i != path_segments.size(); i++) { + if (path_segments[i].end - path_segments[i].start > 10) { + // Populate path segment + curr_path_segment.poses.clear(); + std::copy( + path.poses.begin() + path_segments[i].start, + path.poses.begin() + path_segments[i].end + 1, + std::back_inserter(curr_path_segment.poses)); + + // Make sure we're still able to smooth with time remaining + steady_clock::time_point now = steady_clock::now(); + time_remaining = max_time - duration_cast>(now - start).count(); + refinement_ctr_ = 0; + + // Smooth path segment naively + const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose; + const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back().pose; + bool local_success = + smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining); + success = success && local_success; + + // Enforce boundary conditions + if (!is_holonomic_ && local_success) { + enforceStartBoundaryConditions(start_pose, curr_path_segment, costmap, reversing_segment); + enforceEndBoundaryConditions(goal_pose, curr_path_segment, costmap, reversing_segment); + } + + // Assemble the path changes to the main path + std::copy( + curr_path_segment.poses.begin(), + curr_path_segment.poses.end(), + path.poses.begin() + path_segments[i].start); + } + } + + return success; +} + +bool Smoother::smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time) +{ + steady_clock::time_point a = steady_clock::now(); + rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time); + + int its = 0; + double change = tolerance_; + const unsigned int & path_size = path.poses.size(); + double x_i, y_i, y_m1, y_ip1, y_i_org; + unsigned int mx, my; + + nav_msgs::msg::Path new_path = path; + nav_msgs::msg::Path last_path = path; + + while (change >= tolerance_) { + its += 1; + change = 0.0; + + // Make sure the smoothing function will converge + if (its >= max_its_) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Number of iterations has exceeded limit of %i.", max_its_); + path = last_path; + smoother_utils::updateApproximatePathOrientations(path, reversing_segment); + return false; + } + + // Make sure still have time left to process + steady_clock::time_point b = steady_clock::now(); + rclcpp::Duration timespan(duration_cast>(b - a)); + if (timespan > max_dur) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing time exceeded allowed duration of %0.2f.", max_time); + path = last_path; + smoother_utils::updateApproximatePathOrientations(path, reversing_segment); + return false; + } + + for (unsigned int i = 1; i != path_size - 1; i++) { + for (unsigned int j = 0; j != 2; j++) { + x_i = getFieldByDim(path.poses[i], j); + y_i = getFieldByDim(new_path.poses[i], j); + y_m1 = getFieldByDim(new_path.poses[i - 1], j); + y_ip1 = getFieldByDim(new_path.poses[i + 1], j); + y_i_org = y_i; + + // Smooth based on local 3 point neighborhood and original data locations + y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i)); + setFieldByDim(new_path.poses[i], j, y_i); + change += abs(y_i - y_i_org); + } + + // validate update is admissible, only checks cost if a valid costmap pointer is provided + float cost = 0.0; + if (costmap) { + costmap->worldToMap( + getFieldByDim(new_path.poses[i], 0), + getFieldByDim(new_path.poses[i], 1), + mx, my); + cost = static_cast(costmap->getCost(mx, my)); + } + + if (cost > MAX_NON_OBSTACLE_COST && cost != UNKNOWN_COST) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing process resulted in an infeasible collision. " + "Returning the last path before the infeasibility was introduced."); + path = last_path; + smoother_utils::updateApproximatePathOrientations(path, reversing_segment); + return false; + } + } + + last_path = new_path; + } + + // Let's do additional refinement, it shouldn't take more than a couple milliseconds + // but really puts the path quality over the top. + if (do_refinement_ && refinement_ctr_ < refinement_num_) { + refinement_ctr_++; + smoothImpl(new_path, reversing_segment, costmap, max_time); + } + + smoother_utils::updateApproximatePathOrientations(new_path, reversing_segment); + path = new_path; + return true; +} + +double Smoother::getFieldByDim( + const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim) +{ + if (dim == 0) { + return msg.pose.position.x; + } else if (dim == 1) { + return msg.pose.position.y; + } else { + return msg.pose.position.z; + } +} + +void Smoother::setFieldByDim( + geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, + const double & value) +{ + if (dim == 0) { + msg.pose.position.x = value; + } else if (dim == 1) { + msg.pose.position.y = value; + } else { + msg.pose.position.z = value; + } +} + +unsigned int Smoother::findShortestBoundaryExpansionIdx( + const BoundaryExpansions & boundary_expansions) +{ + // Check which is valid with the minimum integrated length such that + // shorter end-points away that are infeasible to achieve without + // a loop-de-loop are punished + double min_length = 1e9; + int shortest_boundary_expansion_idx = 1e9; + for (unsigned int idx = 0; idx != boundary_expansions.size(); idx++) { + if (boundary_expansions[idx].expansion_path_length0.0 && + boundary_expansions[idx].expansion_path_length > 0.0) + { + min_length = boundary_expansions[idx].expansion_path_length; + shortest_boundary_expansion_idx = idx; + } + } + + return shortest_boundary_expansion_idx; +} + +void Smoother::findBoundaryExpansion( + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & end, + BoundaryExpansion & expansion, + const nav2_costmap_2d::Costmap2D * costmap) +{ + ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_); + + from[0] = start.position.x; + from[1] = start.position.y; + from[2] = tf2::getYaw(start.orientation); + to[0] = end.position.x; + to[1] = end.position.y; + to[2] = tf2::getYaw(end.orientation); + + double d = state_space_->distance(from(), to()); + // If this path is too long compared to the original, then this is probably + // a loop-de-loop, treat as invalid as to not deviate too far from the original path. + // 2.0 selected from prinicipled choice of boundary test points + // r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be + // approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI. + // For all but the last backup test point, a loop would be approximately + // 2x greater than any of the selections. + if (d > 2.0 * expansion.original_path_length) { + return; + } + + std::vector reals; + double theta(0.0), x(0.0), y(0.0); + double x_m = start.position.x; + double y_m = start.position.y; + + // Get intermediary poses + for (double i = 0; i <= expansion.path_end_idx; i++) { + state_space_->interpolate(from(), to(), i / expansion.path_end_idx, s()); + reals = s.reals(); + // Make sure in range [0, 2PI) + theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; + theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; + x = reals[0]; + y = reals[1]; + + // Check for collision + unsigned int mx, my; + costmap->worldToMap(x, y, mx, my); + if (static_cast(costmap->getCost(mx, my)) >= INSCRIBED_COST) { + expansion.in_collision = true; + } + + // Integrate path length + expansion.expansion_path_length += hypot(x - x_m, y - y_m); + x_m = x; + y_m = y; + + // Store point + expansion.pts.emplace_back(x, y, theta); + } +} + +template +BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, IteratorT end) +{ + std::vector distances = { + min_turning_rad_, // Radius + 2.0 * min_turning_rad_, // Diameter + M_PI * min_turning_rad_, // 50% Circumference + 2.0 * M_PI * min_turning_rad_ // Circumference + }; + + BoundaryExpansions boundary_expansions; + boundary_expansions.resize(distances.size()); + double curr_dist = 0.0; + double x_last = start->pose.position.x; + double y_last = start->pose.position.y; + geometry_msgs::msg::Point pt; + unsigned int curr_dist_idx = 0; + + for (IteratorT iter = start; iter != end; iter++) { + pt = iter->pose.position; + curr_dist += hypot(pt.x - x_last, pt.y - y_last); + x_last = pt.x; + y_last = pt.y; + + if (curr_dist >= distances[curr_dist_idx]) { + boundary_expansions[curr_dist_idx].path_end_idx = iter - start; + boundary_expansions[curr_dist_idx].original_path_length = curr_dist; + curr_dist_idx++; + } + + if (curr_dist_idx == boundary_expansions.size()) { + break; + } + } + + return boundary_expansions; +} + +void Smoother::enforceStartBoundaryConditions( + const geometry_msgs::msg::Pose & start_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment) +{ + // Find range of points for testing + BoundaryExpansions boundary_expansions = + generateBoundaryExpansionPoints(path.poses.begin(), path.poses.end()); + + // Generate the motion model and metadata from start -> test points + for (unsigned int i = 0; i != boundary_expansions.size(); i++) { + BoundaryExpansion & expansion = boundary_expansions[i]; + if (expansion.path_end_idx == 0.0) { + continue; + } + + if (!reversing_segment) { + findBoundaryExpansion( + start_pose, path.poses[expansion.path_end_idx].pose, expansion, + costmap); + } else { + findBoundaryExpansion( + path.poses[expansion.path_end_idx].pose, start_pose, expansion, + costmap); + } + } + + // Find the shortest kinematically feasible boundary expansion + unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); + if (best_expansion_idx > boundary_expansions.size()) { + return; + } + + // Override values to match curve + BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; + if (reversing_segment) { + std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); + } + for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { + path.poses[i].pose.position.x = best_expansion.pts[i].x; + path.poses[i].pose.position.y = best_expansion.pts[i].y; + path.poses[i].pose.orientation = orientationAroundZAxis(best_expansion.pts[i].theta); + } +} + +void Smoother::enforceEndBoundaryConditions( + const geometry_msgs::msg::Pose & end_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment) +{ + // Find range of points for testing + BoundaryExpansions boundary_expansions = + generateBoundaryExpansionPoints(path.poses.rbegin(), path.poses.rend()); + + // Generate the motion model and metadata from start -> test points + unsigned int expansion_starting_idx; + for (unsigned int i = 0; i != boundary_expansions.size(); i++) { + BoundaryExpansion & expansion = boundary_expansions[i]; + if (expansion.path_end_idx == 0.0) { + continue; + } + expansion_starting_idx = path.poses.size() - expansion.path_end_idx - 1; + if (!reversing_segment) { + findBoundaryExpansion(path.poses[expansion_starting_idx].pose, end_pose, expansion, costmap); + } else { + findBoundaryExpansion(end_pose, path.poses[expansion_starting_idx].pose, expansion, costmap); + } + } + + // Find the shortest kinematically feasible boundary expansion + unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); + if (best_expansion_idx > boundary_expansions.size()) { + return; + } + + // Override values to match curve + BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; + if (reversing_segment) { + std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); + } + expansion_starting_idx = path.poses.size() - best_expansion.path_end_idx - 1; + for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { + path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x; + path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y; + path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis( + best_expansion.pts[i].theta); + } +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt b/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt new file mode 100644 index 00000000..8b0d98b2 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.8) +project(dem_costmap) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED) + +set(dependencies + rclcpp + nav_msgs + sensor_msgs + cv_bridge + OpenCV +) + +add_executable(map_node src/map_node.cpp) +target_include_directories(map_node PUBLIC + $ + $ + ${OpenCV_INCLUDE_DIRS}) + +target_compile_features(map_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + map_node SYSTEM + ${dependencies} +) + +target_link_libraries(map_node ${OpenCV_LIBRARIES}) + +install(TARGETS map_node + DESTINATION lib/${PROJECT_NAME}) + +# Install maps directory +install(DIRECTORY maps/ + DESTINATION share/${PROJECT_NAME}/maps) + +# Install launch directory +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch) + +# Install config directory +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/subsystems/minimal_navigation/dem_costmap/LICENSE b/src/subsystems/minimal_navigation/dem_costmap/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml b/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml new file mode 100644 index 00000000..60bafdb3 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml @@ -0,0 +1,8 @@ +dem_costmap_converter: + ros__parameters: + dem_file_path: "" + map_resolution: 1.0 + max_passable_slope_degrees: 15.0 + output_frame: map + origin_x: -400.0 + origin_y: -400.0 \ No newline at end of file diff --git a/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py b/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py new file mode 100644 index 00000000..8b03a15d --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + pkg_dir = get_package_share_directory('dem_costmap') + config_file = os.path.join(pkg_dir, 'config', 'dem_costmap.yaml') + dem_file = os.path.join(pkg_dir, 'maps', 'north_site800m.tif') + + dem_node = Node( + package='dem_costmap', + executable='map_node', + name='dem_costmap_converter', + output='screen', + parameters=[ + config_file, + { + 'dem_file_path': dem_file + } + ] + ) + + return LaunchDescription([ + dem_node + ]) diff --git a/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif b/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif new file mode 100644 index 00000000..ec4e9fca --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:122777571140e3b8484bc334cdbdf3247eacf3a9c06a6624ec7cdc340476a3a3 +size 2559156 diff --git a/src/subsystems/minimal_navigation/dem_costmap/package.xml b/src/subsystems/minimal_navigation/dem_costmap/package.xml new file mode 100644 index 00000000..00fe8e66 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/package.xml @@ -0,0 +1,22 @@ + + + + dem_costmap + 0.0.0 + DEM to Costmap converter for terrain-based navigation + mdurrani + Apache-2.0 + + ament_cmake + rclcpp + nav_msgs + sensor_msgs + cv_bridge + libopencv-dev + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp b/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp new file mode 100644 index 00000000..637faf71 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp @@ -0,0 +1,236 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +class DEMCostmapConverter : public rclcpp::Node +{ +public: + DEMCostmapConverter() : Node("dem_costmap_converter") + { + // Declare parameters + this->declare_parameter("dem_file_path", ""); + this->declare_parameter("map_resolution", 1.0); // meters per pixel + this->declare_parameter("max_passable_slope_degrees", 15.0); + this->declare_parameter("output_frame", "map"); + this->declare_parameter("origin_x", 0.0); + this->declare_parameter("origin_y", 0.0); + + // Get parameters + std::string dem_file_path = this->get_parameter("dem_file_path").as_string(); + map_resolution_ = this->get_parameter("map_resolution").as_double(); + max_passable_slope_degrees_ = this->get_parameter("max_passable_slope_degrees").as_double(); + output_frame_ = this->get_parameter("output_frame").as_string(); + origin_x_ = this->get_parameter("origin_x").as_double(); + origin_y_ = this->get_parameter("origin_y").as_double(); + + // Create publisher with transient_local QoS for static maps + costmap_publisher_ = this->create_publisher( + "map", rclcpp::QoS(1).transient_local()); + + RCLCPP_INFO(this->get_logger(), "DEM Costmap Converter initialized"); + RCLCPP_INFO(this->get_logger(), "DEM file: %s", dem_file_path.c_str()); + RCLCPP_INFO(this->get_logger(), "Resolution: %.2f m/pixel", map_resolution_); + RCLCPP_INFO(this->get_logger(), "Max passable slope: %.1f degrees", max_passable_slope_degrees_); + + // Load and process DEM + costmap_ready_ = false; + if (!dem_file_path.empty()) { + loadAndProcessDEM(dem_file_path); + } else { + RCLCPP_ERROR(this->get_logger(), "No DEM file path provided. Use param 'dem_file_path'"); + RCLCPP_ERROR(this->get_logger(), "Node will not publish costmap without valid DEM file"); + } + + // Publish at 1Hz + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&DEMCostmapConverter::publishCostmap, this)); + } + +private: + void loadAndProcessDEM(const std::string& file_path) + { + RCLCPP_INFO(this->get_logger(), "Loading DEM from: %s", file_path.c_str()); + + // Check if file exists + if (!std::filesystem::exists(file_path)) { + RCLCPP_ERROR(this->get_logger(), "DEM file does not exist: %s", file_path.c_str()); + return; + } + + // Load DEM using OpenCV (supports TIFF) + cv::Mat dem = cv::imread(file_path, cv::IMREAD_ANYDEPTH | cv::IMREAD_GRAYSCALE); + + if (dem.empty()) { + RCLCPP_ERROR(this->get_logger(), "Failed to load DEM file: %s", file_path.c_str()); + return; + } + + RCLCPP_INFO(this->get_logger(), "DEM loaded successfully: %dx%d pixels", dem.cols, dem.rows); + + // Convert to float for calculations + cv::Mat dem_float; + dem.convertTo(dem_float, CV_32F); + + // Calculate slope map + cv::Mat slope_map = calculateSlope(dem_float); + + // Convert slope to costmap + costmap_msg_ = createCostmapFromSlope(slope_map, dem.cols, dem.rows); + costmap_ready_ = true; + + RCLCPP_INFO(this->get_logger(), "Costmap generated successfully"); + } + + cv::Mat calculateSlope(const cv::Mat& dem) + { + RCLCPP_INFO(this->get_logger(), "Calculating slope map..."); + + cv::Mat grad_x, grad_y; + cv::Mat slope_radians = cv::Mat::zeros(dem.size(), CV_32F); + + // Calculate gradients using Sobel operator + cv::Sobel(dem, grad_x, CV_32F, 1, 0, 3); + cv::Sobel(dem, grad_y, CV_32F, 0, 1, 3); + + // Scale gradients by resolution to get proper slope + grad_x = grad_x / (8.0 * map_resolution_); // Sobel scale factor is 8 + grad_y = grad_y / (8.0 * map_resolution_); + + // Calculate slope magnitude + for (int i = 0; i < dem.rows; ++i) { + for (int j = 0; j < dem.cols; ++j) { + float dx = grad_x.at(i, j); + float dy = grad_y.at(i, j); + float slope_rad = std::atan(std::sqrt(dx*dx + dy*dy)); + slope_radians.at(i, j) = slope_rad; + } + } + + // Convert to degrees + cv::Mat slope_degrees; + slope_radians *= (180.0 / M_PI); + slope_degrees = slope_radians; + + return slope_degrees; + } + + nav_msgs::msg::OccupancyGrid createCostmapFromSlope(const cv::Mat& slope_map, int width, int height) + { + RCLCPP_INFO(this->get_logger(), "Converting slope to costmap..."); + + nav_msgs::msg::OccupancyGrid costmap; + costmap.header.frame_id = output_frame_; + costmap.header.stamp = this->get_clock()->now(); + + // Set map info + costmap.info.resolution = map_resolution_; + costmap.info.width = width; + costmap.info.height = height; + costmap.info.origin.position.x = origin_x_; + costmap.info.origin.position.y = origin_y_; + costmap.info.origin.position.z = 0.0; + costmap.info.origin.orientation.w = 1.0; + + // Calculate and log costmap bounds + double costmap_max_x = origin_x_ + (width * map_resolution_); + double costmap_max_y = origin_y_ + (height * map_resolution_); + + RCLCPP_INFO(this->get_logger(), "Costmap bounds:"); + RCLCPP_INFO(this->get_logger(), " Origin: (%.2f, %.2f)", origin_x_, origin_y_); + RCLCPP_INFO(this->get_logger(), " Size: %dx%d pixels", width, height); + RCLCPP_INFO(this->get_logger(), " Resolution: %.2f m/pixel", map_resolution_); + RCLCPP_INFO(this->get_logger(), " Bounds: X[%.2f, %.2f], Y[%.2f, %.2f]", + origin_x_, costmap_max_x, origin_y_, costmap_max_y); + RCLCPP_INFO(this->get_logger(), " Total coverage: %.2f x %.2f meters", + width * map_resolution_, height * map_resolution_); + + // Resize data array + costmap.data.resize(width * height); + + // Convert slope to cost values following nav2 costmap2d architecture + int lethal_count = 0; + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + float slope_deg = slope_map.at(i, j); + int cost = slopeToCost(slope_deg); + + // OpenCV uses (row, col) but OccupancyGrid uses (x, y) + // Need to flip Y coordinate for ROS convention + int ros_i = height - 1 - i; + costmap.data[ros_i * width + j] = cost; + + if (cost >= 99) lethal_count++; + } + } + + RCLCPP_INFO(this->get_logger(), + "Costmap conversion complete. Lethal/high-cost cells: %d/%d (%.1f%%)", + lethal_count, width * height, + 100.0 * lethal_count / (width * height)); + + return costmap; + } + + int slopeToCost(float slope_degrees) + { + // Nav2 costmap2d cost values: + // 0: Free space + // 1-252: Scaled costs + // 253: Possibly circumscribed + // 254: Lethal obstacle (impassable) + // 255: No information / unknown + + if (slope_degrees >= max_passable_slope_degrees_) { + return 254; // Lethal - impassable + } else if (slope_degrees >= 10.0) { + // High cost: linear scale from 150-252 for 10-15 degrees + float ratio = (slope_degrees - 10.0) / (max_passable_slope_degrees_ - 10.0); + return static_cast(150 + ratio * 102); + } else if (slope_degrees >= 5.0) { + // Medium cost: linear scale from 50-149 for 5-10 degrees + float ratio = (slope_degrees - 5.0) / 5.0; + return static_cast(50 + ratio * 99); + } else { + // Low cost: linear scale from 0-49 for 0-5 degrees + float ratio = slope_degrees / 5.0; + return static_cast(ratio * 49); + } + } + + + void publishCostmap() + { + if (costmap_ready_) { + costmap_msg_.header.stamp = this->get_clock()->now(); + costmap_publisher_->publish(costmap_msg_); + } else { + RCLCPP_DEBUG(this->get_logger(), "Costmap not ready, skipping publish"); + } + } + + // Member variables + rclcpp::Publisher::SharedPtr costmap_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + nav_msgs::msg::OccupancyGrid costmap_msg_; + bool costmap_ready_; + + double map_resolution_; + double max_passable_slope_degrees_; + std::string output_frame_; + double origin_x_; + double origin_y_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt b/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt new file mode 100644 index 00000000..209d45e6 --- /dev/null +++ b/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(gps_pose_publisher) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(msgs REQUIRED) +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") +find_package(GeographicLib REQUIRED) + +add_executable(gps_pose_publisher_node src/gps_pose_publisher_node.cpp) +target_include_directories(gps_pose_publisher_node PRIVATE ${GeographicLib_INCLUDE_DIRS}) +target_link_libraries(gps_pose_publisher_node + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${std_msgs_TARGETS} + tf2_ros::tf2_ros + tf2::tf2 + ${msgs_TARGETS} + ${GeographicLib_LIBRARIES} +) + +install(TARGETS gps_pose_publisher_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml b/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml new file mode 100644 index 00000000..9442c5f1 --- /dev/null +++ b/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml @@ -0,0 +1,24 @@ + + + + gps_pose_publisher + 0.1.0 + Converts GPS fix + heading into a map-frame PoseStamped and TF broadcast + UMD Loop + Apache-2.0 + + ament_cmake + + rclcpp + geometry_msgs + sensor_msgs + std_msgs + tf2_ros + tf2 + msgs + geographiclib + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp b/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp new file mode 100644 index 00000000..10d0291c --- /dev/null +++ b/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp @@ -0,0 +1,192 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "std_msgs/msg/string.hpp" +#include "msgs/msg/heading.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "msgs/srv/lat_lon_to_enu.hpp" + +#include + +class GpsPosePublisher : public rclcpp::Node +{ +public: + explicit GpsPosePublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("gps_pose_publisher", options) + { + declare_parameter("heading_topic", std::string("/gps/heading")); + declare_parameter("use_start_gate_ref", false); + declare_parameter("origin_lat", std::numeric_limits::quiet_NaN()); + declare_parameter("origin_lon", std::numeric_limits::quiet_NaN()); + declare_parameter("origin_alt", 0.0); + + tf_broadcaster_ = std::make_shared(*this); + + pose_pub_ = create_publisher("/robot_pose", 10); + status_pub_ = create_publisher("/gps_status", 10); + + fix_sub_ = create_subscription( + "/gps/fix", rclcpp::SensorDataQoS(), + [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { onFix(msg); }); + + std::string heading_topic; + get_parameter("heading_topic", heading_topic); + heading_sub_ = create_subscription( + heading_topic, rclcpp::SensorDataQoS(), + [this](const msgs::msg::Heading::SharedPtr msg) { + heading_enu_rad_ = (90.0 - msg->compass_bearing) * M_PI / 180.0; + has_heading_ = true; + }); + + // Pre-set origin from params if provided (non-NaN overrides first-fix logic) + const double param_lat = get_parameter("origin_lat").as_double(); + const double param_lon = get_parameter("origin_lon").as_double(); + const double param_alt = get_parameter("origin_alt").as_double(); + if (!std::isnan(param_lat) && !std::isnan(param_lon)) { + setOrigin(param_lat, param_lon, param_alt); + } + + bool use_start_gate_ref; + get_parameter("use_start_gate_ref", use_start_gate_ref); + if (use_start_gate_ref) { + start_gate_sub_ = create_subscription( + "/start_gate_ref", 1, + [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + if (!origin_set_) { + setOrigin(msg->latitude, msg->longitude, msg->altitude); + } + }); + } + + latlon_srv_ = create_service( + "~/latlon_to_enu", + [this]( + const msgs::srv::LatLonToENU::Request::SharedPtr req, + msgs::srv::LatLonToENU::Response::SharedPtr res) + { + if (!origin_set_) { + RCLCPP_ERROR(get_logger(), "latlon_to_enu called before origin is set"); + res->x = 0.0; res->y = 0.0; res->z = 0.0; + return; + } + double x, y, z; + projection_.Forward(req->lat, req->lon, 0.0, x, y, z); + res->x = x; res->y = y; res->z = z; + }); + } + +private: + void setOrigin(double lat, double lon, double alt) + { + projection_.Reset(lat, lon, alt); + origin_set_ = true; + RCLCPP_INFO(get_logger(), "GPS origin set: lat=%.8f lon=%.8f alt=%.3f", lat, lon, alt); + } + + void onFix(const sensor_msgs::msg::NavSatFix::SharedPtr msg) + { + if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + std_msgs::msg::String s; + s.data = "NO_FIX"; + status_pub_->publish(s); + return; + } + + if (!origin_set_) { + bool use_start_gate_ref; + get_parameter("use_start_gate_ref", use_start_gate_ref); + if (!use_start_gate_ref) { + setOrigin(msg->latitude, msg->longitude, msg->altitude); + } else { + return; + } + } + + double x, y, z; + projection_.Forward(msg->latitude, msg->longitude, msg->altitude, x, y, z); + + // Publish fix quality + heading source + std::string quality; + switch (msg->status.status) { + case sensor_msgs::msg::NavSatStatus::STATUS_FIX: quality = "GPS"; break; + case sensor_msgs::msg::NavSatStatus::STATUS_SBAS_FIX: quality = "SBAS"; break; + case sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX: quality = "GBAS"; break; + default: quality = "FIX"; break; + } + std_msgs::msg::String s; + s.data = "FIX quality=" + quality + " heading=" + (has_heading_ ? "compass" : "none"); + status_pub_->publish(s); + + if (!has_heading_) { + return; + } + + geometry_msgs::msg::PoseStamped pose; + pose.header.stamp = msg->header.stamp; + pose.header.frame_id = "map"; + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = z; + + const double half = heading_enu_rad_ * 0.5; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = std::sin(half); + pose.pose.orientation.w = std::cos(half); + + pose_pub_->publish(pose); + + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = msg->header.stamp; + tf.header.frame_id = "map"; + tf.child_frame_id = "base_link"; + tf.transform.translation.x = x; + tf.transform.translation.y = y; + tf.transform.translation.z = z; + tf.transform.rotation = pose.pose.orientation; + tf_broadcaster_->sendTransform(tf); + } + + std::shared_ptr tf_broadcaster_; + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::Subscription::SharedPtr fix_sub_; + rclcpp::Subscription::SharedPtr heading_sub_; + rclcpp::Subscription::SharedPtr start_gate_sub_; + rclcpp::Service::SharedPtr latlon_srv_; + + GeographicLib::LocalCartesian projection_; + bool origin_set_{false}; + bool has_heading_{false}; + double heading_enu_rad_{0.0}; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt b/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt new file mode 100644 index 00000000..57406adb --- /dev/null +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.8) +project(minimal_nav_bringup) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml new file mode 100644 index 00000000..0227280c --- /dev/null +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml @@ -0,0 +1,92 @@ +# nav_params.yaml — single parameter file passed to all navigation nodes. +# See PLAN.md §7 for full documentation. + +# ── Global ────────────────────────────────────────────────────────────────── +use_sim_time: false + +# ── gps_pose_publisher ─────────────────────────────────────────────────────── +gps_pose_publisher: + ros__parameters: + use_sim_time: false + heading_topic: "/gps/heading" + origin_lat: .nan # auto-set from first fix when NaN + origin_lon: .nan + origin_alt: 0.0 + use_start_gate_ref: true # wait for /start_gate_ref NavSatFix to set origin + +# ── mission_executive ──────────────────────────────────────────────────────── +mission_executive: + ros__parameters: + use_sim_time: false + velocity_zero_threshold: 0.05 # m/s — applied to /odom twist (ZED SDK) + arrival_hold_time: 1.0 # seconds of continuous zero velocity + replan_distance_m: 3.0 # cross-track error threshold to trigger replan + latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" + +# ── global_planner (athena_smac_planner) ───────────────────────────────────── +global_planner: + ros__parameters: + use_sim_time: false + geotiff_path: "" + grid_size_m: 500.0 + grid_resolution_m: 1.0 + obstacle_threshold: 50 + min_turning_r: 1.2 # must match mppi_runner + allow_reverse: false + +global_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 1.0 + publish_frequency: 0.5 + transform_tolerance: 1.0 # generous — GPS-only localization has jitter + rolling_window: false + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_topic: /map + subscribe_to_updates: true + map_subscribe_transient_local: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 1.0 # >= robot_radius (0.45 m) + +# ── mppi_runner (ackermann_mppi) ───────────────────────────────────────────── +mppi_runner: + ros__parameters: + use_sim_time: false + controller_frequency: 20.0 + motion_model: "Ackermann" + min_turning_r: 1.2 + vx_max: 3.0 + vx_min: 0.0 + wz_max: 1.0 + time_steps: 56 + batch_size: 1000 + model_dt: 0.05 + critics: + - ConstraintCritic + - GoalCritic + - GoalAngleCritic + - PathFollowCritic + - PathAlignCritic + - PreferForwardCritic + # Local costmap — owned by mppi_runner as a child Costmap2DROS node + local_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 1.0 + rolling_window: true + width: 20 + height: 20 + resolution: 0.1 + plugins: [] # Phase 2a: add ObstacleLayer + InflationLayer diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py b/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py new file mode 100644 index 00000000..1edbd225 --- /dev/null +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +# Copyright (c) 2025 UMD Loop +# +# 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. + +"""nav.launch.py — Full Athena navigation stack (GPS-only, Nav2-free). + +Launch order / node summary +─────────────────────────── + athena_gps : Pixhawk GPS + heading bridge (from athena_gps/gps_launch.py) + gps_pose_publisher : WGS84→ENU, /robot_pose, map→base_link TF + map_node : DEM GeoTIFF → nav_msgs/OccupancyGrid /map + global_planner : Hybrid-A* planner, /goal_pose → /global_path + ackermann_mppi : MPPI local controller, /global_path → /cmd_vel_nav + mission_executive : State machine, action/service operator interface + cmd_vel_stamper : TwistStamped bridge → /rear_ackermann_controller/reference +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # ── Arguments ──────────────────────────────────────────────────────────── + sim_arg = DeclareLaunchArgument( + 'sim', + default_value='false', + choices=['true', 'false'], + description='Use simulation GPS bridge instead of real hardware', + ) + + sim = LaunchConfiguration('sim') + + # ── Config ─────────────────────────────────────────────────────────────── + nav_bringup_dir = get_package_share_directory('minimal_nav_bringup') + nav_params_file = os.path.join(nav_bringup_dir, 'config', 'nav_params.yaml') + + athena_map_dir = get_package_share_directory('dem_costmap') + dem_file = os.path.join(athena_map_dir, 'maps', 'north_site800m.tif') + + # ── GPS hardware / sim bridge ───────────────────────────────────────────── + gps_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory('athena_gps'), + 'launch', + 'gps_launch.py', + ) + ), + launch_arguments={'sim': sim}.items(), + ) + + # ── gps_pose_publisher ──────────────────────────────────────────────────── + gps_pose_publisher_node = Node( + package='gps_pose_publisher', + executable='gps_pose_publisher_node', + name='gps_pose_publisher', + output='screen', + parameters=[nav_params_file], + ) + + # ── map_node (DEM → OccupancyGrid) ──────────────────────────────────────── + map_node = Node( + package='dem_costmap', + executable='map_node', + name='map_node', + output='screen', + parameters=[ + nav_params_file, + {'dem_file_path': dem_file}, + ], + ) + + # ── global_planner (Hybrid-A* via athena_smac_planner) ─────────────────── + global_planner_node = Node( + package='athena_smac_planner', + executable='global_planner_node', + name='global_planner', + output='screen', + parameters=[nav_params_file], + ) + + # ── ackermann_mppi (MPPI local controller) ──────────────────────────────── + # /odom remapped to /odom/ground_truth (ZED SDK visual-inertial odometry) + ackermann_mppi_node = Node( + package='ackermann_mppi', + executable='ackermann_mppi_node', + name='mppi_runner', + output='screen', + parameters=[nav_params_file], + remappings=[ + ('/odom', '/odom/ground_truth'), + ], + ) + + # ── mission_executive (state machine) ──────────────────────────────────── + # /odom remapped to /odom/ground_truth for stop detection + mission_executive_node = Node( + package='mission_executive', + executable='mission_executive_node', + name='mission_executive', + output='screen', + parameters=[nav_params_file], + remappings=[ + ('/odom', '/odom/ground_truth'), + ], + ) + + # ── twist_stamper: /cmd_vel_nav → /rear_ackermann_controller/reference ──── + twist_stamper_node = Node( + package='twist_stamper', + executable='twist_stamper', + name='cmd_vel_stamper', + output='screen', + parameters=[{'use_sim_time': sim}], + remappings=[ + ('cmd_vel_in', '/cmd_vel_nav'), + ('cmd_vel_out', '/rear_ackermann_controller/reference'), + ], + ) + + return LaunchDescription([ + sim_arg, + gps_launch, + gps_pose_publisher_node, + map_node, + global_planner_node, + ackermann_mppi_node, + mission_executive_node, + twist_stamper_node, + ]) diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml b/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml new file mode 100644 index 00000000..71d3ccc1 --- /dev/null +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml @@ -0,0 +1,23 @@ + + + + minimal_nav_bringup + 0.1.0 + Launch and configuration files for the full Athena navigation stack + UMD Loop + Apache-2.0 + + ament_cmake + + gps_pose_publisher + dem_costmap + athena_smac_planner + ackermann_mppi + mission_executive + athena_gps + twist_stamper + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt b/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt new file mode 100644 index 00000000..d3f37c8b --- /dev/null +++ b/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(mission_executive) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(msgs REQUIRED) + +add_executable(mission_executive_node src/mission_executive_node.cpp) +target_compile_features(mission_executive_node PUBLIC cxx_std_17) +target_link_libraries(mission_executive_node + rclcpp::rclcpp + rclcpp_action::rclcpp_action + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_msgs_TARGETS} + ${std_srvs_TARGETS} + ${msgs_TARGETS} +) + +install(TARGETS mission_executive_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/subsystems/minimal_navigation/mission_executive/package.xml b/src/subsystems/minimal_navigation/mission_executive/package.xml new file mode 100644 index 00000000..49fb99be --- /dev/null +++ b/src/subsystems/minimal_navigation/mission_executive/package.xml @@ -0,0 +1,23 @@ + + + + mission_executive + 0.1.0 + State-machine node that manages target sequencing, abort/return, teleop mode, and arrival detection for the Athena navigation stack + UMD Loop + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + nav_msgs + std_msgs + std_srvs + msgs + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp b/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp new file mode 100644 index 00000000..97c8e998 --- /dev/null +++ b/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp @@ -0,0 +1,784 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/string.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include "msgs/action/navigate_to_target.hpp" +#include "msgs/msg/active_target.hpp" +#include "msgs/msg/nav_status.hpp" +#include "msgs/msg/planner_event.hpp" +#include "msgs/srv/lat_lon_to_enu.hpp" +#include "msgs/srv/set_target.hpp" + +using namespace std::chrono_literals; + +// ── State ────────────────────────────────────────────────────────────────── + +enum class State +{ + IDLE, + NAVIGATING, + ARRIVING, + STOPPED_AT_TARGET, + ABORTING, + RETURNING, + STOPPED_AT_RETURN, + TELEOP +}; + +static std::string stateToStr(State s) +{ + switch (s) { + case State::IDLE: return "IDLE"; + case State::NAVIGATING: return "NAVIGATING"; + case State::ARRIVING: return "ARRIVING"; + case State::STOPPED_AT_TARGET: return "STOPPED_AT_TARGET"; + case State::ABORTING: return "ABORTING"; + case State::RETURNING: return "RETURNING"; + case State::STOPPED_AT_RETURN: return "STOPPED_AT_RETURN"; + case State::TELEOP: return "TELEOP"; + default: return "UNKNOWN"; + } +} + +// ── Node ─────────────────────────────────────────────────────────────────── + +class MissionExecutive : public rclcpp::Node +{ +public: + using NavAction = msgs::action::NavigateToTarget; + using GoalHandle = rclcpp_action::ServerGoalHandle; + + // Target entry stored in the registry and as the active target + struct TargetEntry + { + std::string id; + double x_m{0.0}; + double y_m{0.0}; + uint8_t target_type{0}; + uint8_t goal_source{0}; // GPS=0, METER=1 + double tolerance_m{3.0}; + bool visited{false}; + geometry_msgs::msg::PoseStamped goal_enu; + }; + + explicit MissionExecutive(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("mission_executive", options) + { + declare_parameter("velocity_zero_threshold", 0.05); + declare_parameter("arrival_hold_time", 1.0); + declare_parameter("replan_distance_m", 3.0); + declare_parameter("latlon_to_enu_service", + std::string("/gps_pose_publisher/latlon_to_enu")); + // §8: launch file should remap to /odom/ground_truth when using ZED ground-truth odometry + declare_parameter("odom_topic", std::string("/odom")); + + velocity_zero_threshold_ = get_parameter("velocity_zero_threshold").as_double(); + arrival_hold_time_ = get_parameter("arrival_hold_time").as_double(); + replan_distance_m_ = get_parameter("replan_distance_m").as_double(); + const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); + const auto odom_topic = get_parameter("odom_topic").as_string(); + + // Reentrant group — allows the action server / service client to block + // inside handleAccepted() while other callbacks still run on other threads. + reentrant_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + // ── Publishers ────────────────────────────────────────────────────────── + goal_pub_ = create_publisher("/goal_pose", 10); + nav_enabled_pub_ = create_publisher( + "/nav_enabled", rclcpp::QoS(1).reliable()); + nav_mode_pub_ = create_publisher("/nav_mode", 10); + active_target_pub_ = create_publisher("/active_target", 10); + nav_status_pub_ = create_publisher( + "/nav_status", rclcpp::QoS(1).reliable()); + + // ── Service client ────────────────────────────────────────────────────── + latlon_client_ = create_client( + latlon_svc, + rmw_qos_profile_services_default, + reentrant_group_); + + // ── Action server ─────────────────────────────────────────────────────── + action_server_ = rclcpp_action::create_server( + this, + "~/navigate_to_target", + [this](const rclcpp_action::GoalUUID &, std::shared_ptr goal) { + return handleGoal(goal); + }, + [this](std::shared_ptr gh) { + return handleCancel(gh); + }, + [this](std::shared_ptr gh) { + handleAccepted(gh); + }, + rcl_action_server_get_default_options(), + reentrant_group_); + + // ── Services ──────────────────────────────────────────────────────────── + abort_srv_ = create_service( + "~/abort", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr res) + { + std::lock_guard lk(mutex_); + if (state_ == State::IDLE || state_ == State::TELEOP || + state_ == State::ABORTING || + state_ == State::STOPPED_AT_TARGET || state_ == State::STOPPED_AT_RETURN) + { + res->success = false; + res->message = "Nothing to abort in state " + stateToStr(state_); + return; + } + transition(State::ABORTING, "~/abort service called"); + res->success = true; + res->message = "Aborting"; + }); + + set_target_srv_ = create_service( + "~/set_target", + [this]( + const msgs::srv::SetTarget::Request::SharedPtr req, + msgs::srv::SetTarget::Response::SharedPtr res) + { + onSetTarget(req, res); + }, + rmw_qos_profile_services_default, + reentrant_group_); + + teleop_srv_ = create_service( + "~/teleop", + [this]( + const std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res) + { + std::lock_guard lk(mutex_); + if (req->data) { + // TELEOP_ON — valid from any state + transition(State::TELEOP, "teleop enabled"); + res->success = true; + res->message = "Teleop ON"; + } else { + // TELEOP_OFF — only from TELEOP + if (state_ == State::TELEOP) { + transition(State::IDLE, "teleop disabled"); + res->success = true; + res->message = "Teleop OFF"; + } else { + res->success = false; + res->message = "Not in TELEOP state"; + } + } + }); + + // ── Subscriptions ──────────────────────────────────────────────────────── + robot_pose_sub_ = create_subscription( + "/robot_pose", 10, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + std::lock_guard lk(mutex_); + robot_pose_ = *msg; + // Arrival check first: once we transition to ARRIVING, + // checkCrossTrackError's state guard prevents a spurious replan. + checkArrival(); + checkCrossTrackError(); + }); + + odom_sub_ = create_subscription( + odom_topic, 10, + [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lk(mutex_); + const auto & lin = msg->twist.twist.linear; + robot_speed_ = std::hypot(lin.x, lin.y); + checkStopDetection(); + }); + + planner_event_sub_ = create_subscription( + "/planner_event", 10, + [this](const msgs::msg::PlannerEvent::SharedPtr msg) { + std::lock_guard lk(mutex_); + last_planner_event_ = msg->event; + if (msg->event == msgs::msg::PlannerEvent::PLAN_FAILED) { + onPlanFailed(); + } + }); + + auto transient_qos = rclcpp::QoS(1).transient_local(); + global_path_sub_ = create_subscription( + "/global_path", transient_qos, + [this](const nav_msgs::msg::Path::SharedPtr msg) { + std::lock_guard lk(mutex_); + global_path_ = *msg; + }); + + // ── 2 Hz status timer ──────────────────────────────────────────────────── + status_timer_ = create_wall_timer( + 500ms, + [this]() { + std::lock_guard lk(mutex_); + publishStatus(); + checkActionResult(); + }); + + // Publish initial safe state + publishNavEnabled(false); + publishNavMode(); + + RCLCPP_INFO(get_logger(), "MissionExecutive ready — state: IDLE"); + } + +private: + // ── State transitions ───────────────────────────────────────────────────── + + // Must be called with mutex_ held. + void transition(State new_state, const std::string & reason) + { + RCLCPP_INFO(get_logger(), "[mission_executive] %s → %s: %s", + stateToStr(state_).c_str(), stateToStr(new_state).c_str(), reason.c_str()); + state_ = new_state; + + const bool nav_active = + state_ == State::NAVIGATING || + state_ == State::ARRIVING || + state_ == State::RETURNING; + publishNavEnabled(nav_active); + publishNavMode(); + publishStatus(); + + if (state_ == State::TELEOP || !nav_active) { + // Clear stop-detection tracking on any non-navigating transition + low_speed_tracking_ = false; + } + } + + // ── Internal publish helpers (no lock needed — caller holds mutex) ───────── + + void publishNavEnabled(bool enabled) + { + std_msgs::msg::Bool msg; + msg.data = enabled; + nav_enabled_pub_->publish(msg); + } + + void publishNavMode() + { + std_msgs::msg::String msg; + switch (state_) { + case State::TELEOP: + msg.data = "teleop"; break; + case State::NAVIGATING: + case State::ARRIVING: + case State::RETURNING: + msg.data = "autonomous"; break; + default: + msg.data = "stopped"; break; + } + nav_mode_pub_->publish(msg); + } + + void publishActiveTarget() + { + if (!active_target_.has_value()) return; + msgs::msg::ActiveTarget at; + at.target_id = active_target_->id; + at.target_type = active_target_->target_type; + at.tolerance_m = active_target_->tolerance_m; + at.goal_enu = active_target_->goal_enu; + at.goal_source = active_target_->goal_source; + at.status = stateToStr(state_); + active_target_pub_->publish(at); + } + + void publishStatus() + { + msgs::msg::NavStatus s; + s.state = stateToStr(state_); + if (active_target_.has_value()) { + s.active_target_id = active_target_->id; + s.active_target_type = active_target_->target_type; + s.goal_source = active_target_->goal_source; + } + s.distance_to_goal_m = distToGoal(); + s.cross_track_error_m = computeCrossTrackError(); + s.heading_error_rad = computeHeadingError(); + s.robot_speed_mps = robot_speed_; + s.is_return = is_return_; + s.last_planner_event = last_planner_event_; + nav_status_pub_->publish(s); + } + + // ── Geometry helpers ────────────────────────────────────────────────────── + + // Both return -1.0 if data is unavailable. Called with mutex_ held. + + static double quaternionToYaw(const geometry_msgs::msg::Quaternion & q) + { + const double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); + const double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); + return std::atan2(siny_cosp, cosy_cosp); + } + + double computeHeadingError() const + { + if (!robot_pose_.has_value() || !active_target_.has_value()) return 0.0; + const double yaw = quaternionToYaw(robot_pose_->pose.orientation); + const double dx = active_target_->goal_enu.pose.position.x - robot_pose_->pose.position.x; + const double dy = active_target_->goal_enu.pose.position.y - robot_pose_->pose.position.y; + const double bearing = std::atan2(dy, dx); + double err = bearing - yaw; + while (err > M_PI) err -= 2.0 * M_PI; + while (err < -M_PI) err += 2.0 * M_PI; + return err; + } + + double distToGoal() const + { + if (!robot_pose_.has_value() || !active_target_.has_value()) return -1.0; + const double dx = + robot_pose_->pose.position.x - active_target_->goal_enu.pose.position.x; + const double dy = + robot_pose_->pose.position.y - active_target_->goal_enu.pose.position.y; + return std::hypot(dx, dy); + } + + double computeCrossTrackError() const + { + if (!robot_pose_.has_value() || !global_path_.has_value()) return -1.0; + const auto & poses = global_path_->poses; + if (poses.size() < 2) return -1.0; + + const double rx = robot_pose_->pose.position.x; + const double ry = robot_pose_->pose.position.y; + double min_dist = std::numeric_limits::max(); + + for (size_t i = 0; i < poses.size() - 1; ++i) { + const double ax = poses[i].pose.position.x, ay = poses[i].pose.position.y; + const double bx = poses[i+1].pose.position.x, by = poses[i+1].pose.position.y; + const double dx = bx - ax, dy = by - ay; + const double len2 = dx*dx + dy*dy; + double dist; + if (len2 < 1e-10) { + dist = std::hypot(rx - ax, ry - ay); + } else { + const double t = std::clamp(((rx-ax)*dx + (ry-ay)*dy) / len2, 0.0, 1.0); + dist = std::hypot(rx - (ax + t*dx), ry - (ay + t*dy)); + } + min_dist = std::min(min_dist, dist); + } + return min_dist; + } + + // ── Subscription callbacks (mutex_ already held by caller) ──────────────── + + void checkCrossTrackError() + { + if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; + if (!active_target_.has_value()) return; + + const double xte = computeCrossTrackError(); + if (xte < 0.0 || xte <= replan_distance_m_) return; + + RCLCPP_INFO(get_logger(), + "[mission_executive] replan triggered: xtrack=%.2fm", xte); + goal_pub_->publish(active_target_->goal_enu); + } + + void checkArrival() + { + // Transition NAVIGATING/RETURNING → ARRIVING when within tolerance + if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; + if (!active_target_.has_value()) return; + + const double d = distToGoal(); + if (d < 0.0 || d >= active_target_->tolerance_m) return; + + transition(State::ARRIVING, + "within tolerance (d=" + std::to_string(d) + "m)"); + } + + void checkStopDetection() + { + // Transition ARRIVING → STOPPED_AT_TARGET / STOPPED_AT_RETURN + if (state_ != State::ARRIVING) return; + + if (robot_speed_ < velocity_zero_threshold_) { + if (!low_speed_tracking_) { + low_speed_start_ = now(); + low_speed_tracking_ = true; + } else { + const double held = (now() - low_speed_start_).seconds(); + if (held >= arrival_hold_time_) { + low_speed_tracking_ = false; + if (is_return_) { + transition(State::STOPPED_AT_RETURN, "velocity held < threshold"); + } else { + // Mark target as visited so RETURN is permitted later + if (active_target_.has_value()) { + auto it = target_registry_.find(active_target_->id); + if (it != target_registry_.end()) { + it->second.visited = true; + } + } + transition(State::STOPPED_AT_TARGET, "velocity held < threshold"); + } + } + } + } else { + low_speed_tracking_ = false; + } + } + + void onPlanFailed() + { + // Called with mutex_ held + if (state_ == State::NAVIGATING || state_ == State::RETURNING) { + RCLCPP_WARN(get_logger(), + "[mission_executive] PLAN_FAILED — transitioning to ABORTING"); + transition(State::ABORTING, "PLAN_FAILED received"); + } + } + + // ── Action result dispatch (called from the 2 Hz status timer) ──────────── + // Must be called with mutex_ held. + + void checkActionResult() + { + if (!active_goal_handle_) return; + + auto result = std::make_shared(); + + // Cancel requested externally — result already sent, go straight to IDLE. + // (ABORTING is only for the case where the result hasn't been sent yet.) + if (active_goal_handle_->is_canceling()) { + result->success = false; + result->message = "Cancelled"; + active_goal_handle_->canceled(result); + active_goal_handle_ = nullptr; + transition(State::IDLE, "action cancelled"); + return; + } + + switch (state_) { + case State::STOPPED_AT_TARGET: + case State::STOPPED_AT_RETURN: + result->success = true; + result->message = "Arrived at target"; + active_goal_handle_->succeed(result); + active_goal_handle_ = nullptr; + return; + + case State::ABORTING: + result->success = false; + result->message = "Aborted"; + active_goal_handle_->abort(result); + active_goal_handle_ = nullptr; + if (queued_goal_handle_) { + // Promote queued goal — go directly to NAVIGATING/RETURNING (§3.2.2) + active_target_ = queued_entry_; + is_return_ = queued_is_return_; + active_goal_handle_ = queued_goal_handle_; + queued_goal_handle_ = nullptr; + transition(is_return_ ? State::RETURNING : State::NAVIGATING, + "queued goal dequeued after abort"); + goal_pub_->publish(active_target_->goal_enu); + publishActiveTarget(); + } else { + transition(State::IDLE, "abort complete"); + } + return; + + default: + break; + } + + // Still executing — push feedback + auto fb = std::make_shared(); + fb->distance_to_goal_m = distToGoal(); + fb->cross_track_error_m = computeCrossTrackError(); + fb->state = stateToStr(state_); + active_goal_handle_->publish_feedback(fb); + } + + // ── Action server callbacks ─────────────────────────────────────────────── + + rclcpp_action::GoalResponse handleGoal( + std::shared_ptr /*goal*/) + { + // Accept all goals; teleop/state guards are applied in handleAccepted. + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handleCancel(std::shared_ptr /*gh*/) + { + return rclcpp_action::CancelResponse::ACCEPT; + } + + // Runs on a thread from the MultiThreadedExecutor (reentrant group). + // May block briefly for the latlon_to_enu service call. + void handleAccepted(std::shared_ptr goal_handle) + { + const auto goal = goal_handle->get_goal(); + + // ── Resolve ENU coordinates ──────────────────────────────────────────── + TargetEntry entry; + const bool is_ret = goal->is_return; + + if (!goal->target_id.empty()) { + // Look up pre-registered target + std::lock_guard lk(mutex_); + auto it = target_registry_.find(goal->target_id); + if (it == target_registry_.end()) { + auto res = std::make_shared(); + res->success = false; + res->message = "Unknown target_id: " + goal->target_id; + goal_handle->abort(res); + return; + } + if (is_ret && !it->second.visited) { + auto res = std::make_shared(); + res->success = false; + res->message = "Target not yet visited — cannot RETURN"; + goal_handle->abort(res); + return; + } + entry = it->second; + } else { + // Inline goal — no target_id + entry.id = ""; + entry.target_type = goal->target_type; + entry.tolerance_m = goal->tolerance_m; + entry.goal_source = goal->goal_type; + + if (goal->goal_type == NavAction::Goal::GPS) { + if (!latlon_client_->wait_for_service(3s)) { + auto res = std::make_shared(); + res->success = false; + res->message = "latlon_to_enu service not available"; + goal_handle->abort(res); + return; + } + auto req = std::make_shared(); + req->lat = goal->lat; + req->lon = goal->lon; + auto future = latlon_client_->async_send_request(req); + // Blocking wait is safe here: MultiThreadedExecutor + reentrant group + // means other callbacks continue on other threads. + if (future.wait_for(5s) != std::future_status::ready) { + auto res = std::make_shared(); + res->success = false; + res->message = "latlon_to_enu service timeout"; + goal_handle->abort(res); + return; + } + auto resp = future.get(); + entry.x_m = resp->x; + entry.y_m = resp->y; + } else { + entry.x_m = goal->x_m; + entry.y_m = goal->y_m; + } + + entry.goal_enu.header.frame_id = "map"; + entry.goal_enu.header.stamp = now(); + entry.goal_enu.pose.position.x = entry.x_m; + entry.goal_enu.pose.position.y = entry.y_m; + entry.goal_enu.pose.orientation.w = 1.0; + } + + // ── Apply state transition ───────────────────────────────────────────── + { + std::lock_guard lk(mutex_); + + if (state_ == State::TELEOP) { + auto res = std::make_shared(); + res->success = false; + res->message = "Cannot navigate — currently in TELEOP mode"; + goal_handle->abort(res); + return; + } + + // §3.2.2: During ABORTING, queue the goal rather than preempting the abort + if (state_ == State::ABORTING) { + // Preempt any previously queued goal + if (queued_goal_handle_ && queued_goal_handle_->is_active()) { + auto old_res = std::make_shared(); + old_res->success = false; + old_res->message = "Preempted by newer queued goal"; + queued_goal_handle_->abort(old_res); + } + queued_goal_handle_ = goal_handle; + queued_entry_ = entry; + queued_is_return_ = is_ret; + RCLCPP_INFO(get_logger(), + "[mission_executive] goal queued — currently ABORTING"); + return; + } + + // Preempt any existing active goal (guard: abort() throws if not active) + if (active_goal_handle_ && active_goal_handle_->is_active()) { + auto old_res = std::make_shared(); + old_res->success = false; + old_res->message = "Preempted by new goal"; + active_goal_handle_->abort(old_res); + active_goal_handle_ = nullptr; + } + + active_target_ = entry; + is_return_ = is_ret; + active_goal_handle_ = goal_handle; + + transition(is_ret ? State::RETURNING : State::NAVIGATING, + is_ret ? "RETURN action accepted" : "GO_TO action accepted"); + + // Trigger global planner + goal_pub_->publish(active_target_->goal_enu); + publishActiveTarget(); + } + } + + // ── SetTarget service ───────────────────────────────────────────────────── + + void onSetTarget( + const msgs::srv::SetTarget::Request::SharedPtr req, + msgs::srv::SetTarget::Response::SharedPtr res) + { + TargetEntry entry; + entry.id = req->target_id; + entry.target_type = req->target_type; + entry.tolerance_m = req->tolerance_m; + entry.goal_source = req->goal_type; + + if (req->goal_type == msgs::srv::SetTarget::Request::GPS) { + if (!latlon_client_->wait_for_service(3s)) { + res->success = false; + res->message = "latlon_to_enu service not available"; + return; + } + auto conv = std::make_shared(); + conv->lat = req->lat; + conv->lon = req->lon; + auto future = latlon_client_->async_send_request(conv); + if (future.wait_for(5s) != std::future_status::ready) { + res->success = false; + res->message = "latlon_to_enu service timeout"; + return; + } + auto resp = future.get(); + entry.x_m = resp->x; + entry.y_m = resp->y; + } else { + entry.x_m = req->x_m; + entry.y_m = req->y_m; + } + + entry.goal_enu.header.frame_id = "map"; + entry.goal_enu.header.stamp = now(); + entry.goal_enu.pose.position.x = entry.x_m; + entry.goal_enu.pose.position.y = entry.y_m; + entry.goal_enu.pose.orientation.w = 1.0; + + { + std::lock_guard lk(mutex_); + target_registry_[entry.id] = entry; + } + + res->success = true; + res->message = "Target '" + entry.id + "' registered"; + RCLCPP_INFO(get_logger(), + "Target '%s' registered: (%.2f, %.2f) tol=%.2fm", + entry.id.c_str(), entry.x_m, entry.y_m, entry.tolerance_m); + } + + // ── Member data ─────────────────────────────────────────────────────────── + + std::mutex mutex_; + State state_{State::IDLE}; + + std::unordered_map target_registry_; + std::optional active_target_; + bool is_return_{false}; + + // Goal queued while ABORTING (§3.2.2 — GO_TO/RETURN during ABORTING are "queued") + std::shared_ptr queued_goal_handle_; + TargetEntry queued_entry_; + bool queued_is_return_{false}; + + // Sensor cache + std::optional robot_pose_; + std::optional global_path_; + double robot_speed_{0.0}; + rclcpp::Time low_speed_start_{0, 0, RCL_ROS_TIME}; + bool low_speed_tracking_{false}; + uint8_t last_planner_event_{0}; + + // Parameters + double velocity_zero_threshold_{0.05}; + double arrival_hold_time_{1.0}; + double replan_distance_m_{3.0}; + + // ROS interfaces + rclcpp::CallbackGroup::SharedPtr reentrant_group_; + + rclcpp::Publisher::SharedPtr goal_pub_; + rclcpp::Publisher::SharedPtr nav_enabled_pub_; + rclcpp::Publisher::SharedPtr nav_mode_pub_; + rclcpp::Publisher::SharedPtr active_target_pub_; + rclcpp::Publisher::SharedPtr nav_status_pub_; + + rclcpp_action::Server::SharedPtr action_server_; + std::shared_ptr active_goal_handle_; + + rclcpp::Service::SharedPtr abort_srv_; + rclcpp::Service::SharedPtr set_target_srv_; + rclcpp::Service::SharedPtr teleop_srv_; + + rclcpp::Subscription::SharedPtr robot_pose_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr planner_event_sub_; + rclcpp::Subscription::SharedPtr global_path_sub_; + + rclcpp::Client::SharedPtr latlon_client_; + + rclcpp::TimerBase::SharedPtr status_timer_; +}; + +// ── main ────────────────────────────────────────────────────────────────── + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/minimal_navigation/todo.md b/src/subsystems/minimal_navigation/todo.md new file mode 100644 index 00000000..284b0975 --- /dev/null +++ b/src/subsystems/minimal_navigation/todo.md @@ -0,0 +1,69 @@ +# Navigation Stack — Implementation Progress + +## Legend +- [x] Done +- [ ] Not started + +--- + +## Plan 1 — `gps_pose_publisher` ✅ +- [x] New package: `gps_pose_publisher/` +- [x] WGS84 → ENU via `GeographicLib::LocalCartesian` +- [x] Publishes `/robot_pose` (PoseStamped, frame: map) +- [x] Broadcasts `map → base_link` TF +- [x] Publishes `/gps_status` (String) +- [x] Service `~/latlon_to_enu` (msgs/srv/LatLonToENU) +- [x] `use_start_gate_ref` param — waits for `/start_gate_ref` NavSatFix if true +- [x] Heading conversion: compass deg → ENU rad (`(90 - deg) * π/180`) +- [x] Only publishes once both GPS fix AND heading are available + +## Plan 2 — `ackermann_mppi` modifications ✅ +- [x] Input topic renamed: `/plan` → `/global_path` (transient_local QoS) +- [x] `nav_enabled_topic` param (default `/nav_enabled`) +- [x] Subscribe `std_msgs/Bool` on nav_enabled topic +- [x] `controlLoop()` short-circuits (zero cmd_vel, DEBUG log) when nav disabled +- [x] `std::atomic nav_enabled_` — thread-safe with MultiThreadedExecutor + +## Plan 3 — `athena_smac_planner` + `global_planner` ✅ +- [x] Package rename: `nav2_smac_planner` → `athena_smac_planner` (package.xml + CMakeLists.txt) +- [x] Library rename: `nav2_smac_planner` → `athena_smac_planner` +- [x] Removed nonexistent plugin XML exports from package.xml +- [x] `global_planner_node.cpp` — subscribes `/goal_pose`, `/robot_pose` +- [x] Owns `Costmap2DROS` under `"global_costmap"` namespace (StaticLayer from `/map`) +- [x] Uses `SmacPlannerHybrid::createPlan()` directly (no lifecycle) +- [x] Publishes `/global_path` (transient_local) and `/planner_event` +- [x] Emits `PLAN_FAILED` event on exception; `PLAN_SUCCEEDED` on success +- [x] WARN if `/goal_pose` arrives before `/robot_pose` +- [x] MultiThreadedExecutor with node + costmap node + +--- + +## Plan 4 — `mission_executive` ✅ +- [x] Package: `mission_executive/` (package.xml, CMakeLists.txt) +- [x] Action server `~/navigate_to_target` (NavigateToTarget) +- [x] Services: `~/abort` (Trigger), `~/set_target` (SetTarget), `~/teleop` (SetBool) +- [x] Publishes: `/goal_pose`, `/nav_enabled`, `/nav_mode`, `/active_target`, `/nav_status` +- [x] Subscribes: `/robot_pose`, `/odom`, `/planner_event`, `/global_path` +- [x] States: IDLE, NAVIGATING, ARRIVING, STOPPED_AT_TARGET, ABORTING, RETURNING, STOPPED_AT_RETURN, TELEOP +- [x] Full transition table from PLAN.md §3.2.2 +- [x] Calls `gps_pose_publisher/latlon_to_enu` for coordinate conversion +- [x] MultiThreadedExecutor + reentrant callback group (deadlock-safe) +- [x] Cross-track error replanning: replan when xtrack > `replan_distance_m` +- [x] Stop detection: `|twist.linear| < velocity_zero_threshold` held for `arrival_hold_time` seconds +- [x] Target registry via `~/set_target`; RETURN requires visited target +- [x] Preemption: new GO_TO aborts current active goal + +## Plan 5 — `nav.launch.py` + unified build ✅ +- [x] Package: `nav_bringup/` (package.xml, CMakeLists.txt) +- [x] `config/nav_params.yaml` — single param file for all nodes (from PLAN.md §7) +- [x] `launch/nav.launch.py` — includes `athena_gps/launch/gps_launch.py` +- [x] `/odom` → `/odom/ground_truth` remapped for `mission_executive` + `ackermann_mppi` +- [x] twist_stamper node: `cmd_vel_nav` → `/rear_ackermann_controller/reference` +- [x] Launches: `gps_pose_publisher`, `map_node`, `global_planner`, `ackermann_mppi`, `mission_executive` +- [x] Single `nav_params.yaml` passed to all nodes + +--- + +## Notes +- `msgs` package (at `src/msgs/`) already contains all required message/service/action definitions +- Phase 2 extensions (obstacle layers, ArUco, object detection) are deferred per PLAN.md §5 From 4e946ada583c82f71beb5bc81139bf8e71829d5a Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Mon, 30 Mar 2026 01:15:06 -0400 Subject: [PATCH 02/21] no valid path --- src/simulation/launch/control.launch.py | 2 +- src/simulation/scripts/heading_publisher.py | 4 +- .../ackermann_mppi/CMakeLists.txt | 1 + .../src/ackermann_mppi_node.cpp | 48 +++- .../src/global_planner_node.cpp | 5 +- .../config/nav_params.yaml | 123 ++++---- .../minimal_nav_bringup/config/nav_view.rviz | 270 ++++++++++++++++++ .../minimal_nav_bringup/launch/nav.launch.py | 17 +- .../nav_visualizer/nav_visualizer/__init__.py | 0 .../nav_visualizer/nav_visualizer_node.py | 154 ++++++++++ .../nav_visualizer/package.xml | 26 ++ .../nav_visualizer/resource/nav_visualizer | 0 .../nav_visualizer/setup.cfg | 4 + .../nav_visualizer/setup.py | 26 ++ .../nav_visualizer/test/test_copyright.py | 25 ++ .../nav_visualizer/test/test_flake8.py | 25 ++ .../nav_visualizer/test/test_pep257.py | 23 ++ 17 files changed, 658 insertions(+), 95 deletions(-) mode change 100644 => 100755 src/simulation/scripts/heading_publisher.py create mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_view.rviz create mode 100644 src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/__init__.py create mode 100644 src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/nav_visualizer_node.py create mode 100644 src/subsystems/minimal_navigation/nav_visualizer/package.xml create mode 100644 src/subsystems/minimal_navigation/nav_visualizer/resource/nav_visualizer create mode 100644 src/subsystems/minimal_navigation/nav_visualizer/setup.cfg create mode 100644 src/subsystems/minimal_navigation/nav_visualizer/setup.py create mode 100644 src/subsystems/minimal_navigation/nav_visualizer/test/test_copyright.py create mode 100644 src/subsystems/minimal_navigation/nav_visualizer/test/test_flake8.py create mode 100644 src/subsystems/minimal_navigation/nav_visualizer/test/test_pep257.py diff --git a/src/simulation/launch/control.launch.py b/src/simulation/launch/control.launch.py index 4baaa9cb..e4d69616 100644 --- a/src/simulation/launch/control.launch.py +++ b/src/simulation/launch/control.launch.py @@ -14,7 +14,7 @@ def generate_launch_description(): ackermann_controller_spawner = Node( package='controller_manager', executable='spawner', - arguments=['rear_ackermann_controller'], + arguments=['front_ackermann_controller'], output='screen', remappings=[ ("/ackermann_steering_controller/tf_odometry", "/tf"), diff --git a/src/simulation/scripts/heading_publisher.py b/src/simulation/scripts/heading_publisher.py old mode 100644 new mode 100755 index 7c5df039..c46eaa70 --- a/src/simulation/scripts/heading_publisher.py +++ b/src/simulation/scripts/heading_publisher.py @@ -4,7 +4,7 @@ import rclpy from rclpy.node import Node -from rclpy.qos import SensorDataQoS +from rclpy.qos import QoSPresetProfiles from sensor_msgs.msg import MagneticField from msgs.msg import Heading @@ -27,7 +27,7 @@ def __init__(self): MagneticField, mag_topic, self.on_mag, - SensorDataQoS(), + QoSPresetProfiles.SENSOR_DATA.value, ) self.get_logger().info( diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt b/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt index 8b73412f..d0668b92 100644 --- a/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt +++ b/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt @@ -88,6 +88,7 @@ target_link_libraries(ackermann_mppi_node nav2_costmap_2d::nav2_costmap_2d_core ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} ) # --- Install --- diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp index 730e1081..fed24ea7 100644 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp @@ -24,7 +24,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/path.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -70,7 +70,7 @@ class AckermannMPPINode : public rclcpp::Node declare_parameter("odom_topic", std::string("/odom")); declare_parameter("plan_topic", std::string("/global_path")); declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); - declare_parameter("cmd_vel_topic", std::string("/cmd_vel")); + declare_parameter("cmd_vel_topic", std::string("/rear_ackermann_controller/reference")); declare_parameter("robot_base_frame", std::string("base_link")); declare_parameter("global_frame", std::string("map")); } @@ -101,7 +101,13 @@ class AckermannMPPINode : public rclcpp::Node "local_costmap" // local namespace ); // Transfer the TF buffer so the costmap shares our TF instance - costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", get_parameter("use_sim_time").as_bool())); + bool use_sim_time = false; + get_parameter_or("use_sim_time", use_sim_time, false); + costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", use_sim_time)); + // Empty plugin list: rcl_yaml_param_parser cannot represent an empty YAML + // sequence (plugins: []) — it produces a null rcl_variant_s that crashes + // NodeParameters. Set it programmatically instead. + costmap_ros_->set_parameter(rclcpp::Parameter("plugins", std::vector{})); costmap_ros_->configure(); costmap_ros_->activate(); @@ -133,6 +139,9 @@ class AckermannMPPINode : public rclcpp::Node std::lock_guard lock(plan_mutex_); current_plan_ = *msg; last_plan_time_ = now(); + RCLCPP_INFO(get_logger(), "[mppi] plan received: %zu poses", msg->poses.size()); + } else { + RCLCPP_WARN(get_logger(), "[mppi] received empty plan, ignoring"); } }); @@ -140,12 +149,13 @@ class AckermannMPPINode : public rclcpp::Node nav_enabled_topic, 1, [this](const std_msgs::msg::Bool::SharedPtr msg) { nav_enabled_.store(msg->data); + RCLCPP_INFO(get_logger(), "[mppi] nav_enabled changed → %s", msg->data ? "TRUE" : "FALSE"); }); // Publisher std::string cmd_vel_topic; get_parameter("cmd_vel_topic", cmd_vel_topic); - cmd_vel_pub_ = create_publisher(cmd_vel_topic, 1); + cmd_vel_pub_ = create_publisher(cmd_vel_topic, 1); // Control loop timer const auto period = std::chrono::duration(1.0 / controller_frequency); @@ -175,7 +185,10 @@ class AckermannMPPINode : public rclcpp::Node { if (!nav_enabled_.load()) { RCLCPP_DEBUG(get_logger(), "Navigation disabled — sending zero velocity."); - cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); + geometry_msgs::msg::TwistStamped zero; + zero.header.stamp = now(); + zero.header.frame_id = base_frame_; + cmd_vel_pub_->publish(zero); return; } @@ -186,7 +199,8 @@ class AckermannMPPINode : public rclcpp::Node { std::lock_guard lock(plan_mutex_); if (current_plan_.poses.empty()) { - return; // No plan received yet + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "[mppi] nav enabled but no plan yet"); + return; } plan = current_plan_; plan_time = last_plan_time_; @@ -203,7 +217,10 @@ class AckermannMPPINode : public rclcpp::Node get_logger(), *get_clock(), 2000, "Plan is stale (%.1f s old, timeout %.1f s) — sending zero velocity.", plan_age, plan_timeout_s_); - cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); + geometry_msgs::msg::TwistStamped zero; + zero.header.stamp = now(); + zero.header.frame_id = base_frame_; + cmd_vel_pub_->publish(zero); optimizer_.reset(); return; } @@ -237,13 +254,24 @@ class AckermannMPPINode : public rclcpp::Node // Use the last pose of the plan as the goal const auto & goal = plan.poses.back().pose; + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, + "[mppi] running evalControl — pose=(%.2f, %.2f) plan_poses=%zu", + robot_pose.pose.position.x, robot_pose.pose.position.y, plan.poses.size()); + try { auto [cmd, optimal_traj] = optimizer_.evalControl( robot_pose, robot_speed, plan, goal); - cmd_vel_pub_->publish(cmd.twist); + geometry_msgs::msg::TwistStamped stamped; + stamped.header.stamp = now(); + stamped.header.frame_id = base_frame_; + stamped.twist = cmd.twist; + cmd_vel_pub_->publish(stamped); } catch (const std::runtime_error & ex) { RCLCPP_ERROR(get_logger(), "MPPI failed: %s — sending zero velocity.", ex.what()); - cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); + geometry_msgs::msg::TwistStamped zero; + zero.header.stamp = now(); + zero.header.frame_id = base_frame_; + cmd_vel_pub_->publish(zero); optimizer_.reset(); } } @@ -262,7 +290,7 @@ class AckermannMPPINode : public rclcpp::Node rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr plan_sub_; rclcpp::Subscription::SharedPtr nav_enabled_sub_; - rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; rclcpp::TimerBase::SharedPtr control_timer_; std::atomic nav_enabled_{true}; diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp index 76c3b95b..259ec178 100644 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp @@ -31,7 +31,6 @@ class GlobalPlannerNode : public rclcpp::Node explicit GlobalPlannerNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("global_planner", options) { - declare_parameter("use_sim_time", false); } void configure() @@ -41,6 +40,10 @@ class GlobalPlannerNode : public rclcpp::Node get_namespace(), "global_costmap"); costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", get_parameter("use_sim_time").as_bool())); + // Empty plugin list: rcl_yaml_param_parser cannot represent an empty YAML + // sequence (plugins: []) — it produces a null rcl_variant_s that crashes + // NodeParameters. Set it programmatically instead. + costmap_ros_->set_parameter(rclcpp::Parameter("plugins", std::vector{})); costmap_ros_->configure(); costmap_ros_->activate(); diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml index 0227280c..f58c3009 100644 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml @@ -1,23 +1,18 @@ # nav_params.yaml — single parameter file passed to all navigation nodes. # See PLAN.md §7 for full documentation. -# ── Global ────────────────────────────────────────────────────────────────── -use_sim_time: false - # ── gps_pose_publisher ─────────────────────────────────────────────────────── gps_pose_publisher: ros__parameters: - use_sim_time: false + use_sim_time: true heading_topic: "/gps/heading" - origin_lat: .nan # auto-set from first fix when NaN - origin_lon: .nan origin_alt: 0.0 - use_start_gate_ref: true # wait for /start_gate_ref NavSatFix to set origin + use_start_gate_ref: false # wait for /start_gate_ref NavSatFix to set origin # ── mission_executive ──────────────────────────────────────────────────────── mission_executive: ros__parameters: - use_sim_time: false + use_sim_time: true velocity_zero_threshold: 0.05 # m/s — applied to /odom twist (ZED SDK) arrival_hold_time: 1.0 # seconds of continuous zero velocity replan_distance_m: 3.0 # cross-track error threshold to trigger replan @@ -26,67 +21,65 @@ mission_executive: # ── global_planner (athena_smac_planner) ───────────────────────────────────── global_planner: ros__parameters: - use_sim_time: false - geotiff_path: "" - grid_size_m: 500.0 - grid_resolution_m: 1.0 - obstacle_threshold: 50 - min_turning_r: 1.2 # must match mppi_runner - allow_reverse: false + use_sim_time: true + # SmacPlannerHybrid params (read under "SmacPlannerHybrid.*" prefix by the planner) + SmacPlannerHybrid: + minimum_turning_radius: 1.2 # must match mppi_runner + angle_quantization_bins: 36 + allow_unknown: true + max_iterations: 10000000 + max_on_approach_iterations: 10000 + smooth_path: false # skip smoother for now +# ── global_costmap (owned by global_planner) ───────────────────────────────── +# Costmap2DROS creates the node at /global_costmap/global_costmap (nav2 double-naming) global_costmap: - ros__parameters: - use_sim_time: false - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 1.0 - publish_frequency: 0.5 - transform_tolerance: 1.0 # generous — GPS-only localization has jitter - rolling_window: false - plugins: ["static_layer", "inflation_layer"] - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_topic: /map - subscribe_to_updates: true - map_subscribe_transient_local: true - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 1.0 # >= robot_radius (0.45 m) + global_costmap: + ros__parameters: + use_sim_time: true + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 1.0 + publish_frequency: 0.5 + transform_tolerance: 1.0 # generous — GPS-only localization has jitter + rolling_window: false + width: 1000 + height: 1000 + resolution: 1.0 + origin_x: -500.0 + origin_y: -500.0 + # plugins set programmatically in global_planner_node.cpp (empty [] crashes rcl_yaml_param_parser) + +# ── local_costmap (owned by mppi_runner) ───────────────────────────────────── +# Costmap2DROS creates the node at /local_costmap/local_costmap (nav2 double-naming) +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: true + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 1.0 + rolling_window: true + width: 20 + height: 20 + resolution: 0.1 + # plugins omitted — set to [] programmatically in AckermannMPPINode before configure() # ── mppi_runner (ackermann_mppi) ───────────────────────────────────────────── mppi_runner: ros__parameters: - use_sim_time: false + use_sim_time: true controller_frequency: 20.0 - motion_model: "Ackermann" - min_turning_r: 1.2 - vx_max: 3.0 - vx_min: 0.0 - wz_max: 1.0 - time_steps: 56 - batch_size: 1000 - model_dt: 0.05 - critics: - - ConstraintCritic - - GoalCritic - - GoalAngleCritic - - PathFollowCritic - - PathAlignCritic - - PreferForwardCritic - # Local costmap — owned by mppi_runner as a child Costmap2DROS node - local_costmap: - ros__parameters: - use_sim_time: false - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 5.0 - publish_frequency: 2.0 - transform_tolerance: 1.0 - rolling_window: true - width: 20 - height: 20 - resolution: 0.1 - plugins: [] # Phase 2a: add ObstacleLayer + InflationLayer + mppi: + model_dt: 0.05 + time_steps: 56 + batch_size: 1000 + vx_max: 3.0 + vx_min: 0.0 + wz_max: 1.0 + AckermannConstraints: + min_turning_r: 1.2 diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_view.rviz b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_view.rviz new file mode 100644 index 00000000..01d04502 --- /dev/null +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_view.rviz @@ -0,0 +1,270 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + - /Global Plan1 + - /Local Plan1 + - /Sampled Paths1 + - /Local Costmap1 + Splitter Ratio: 0.5 + Tree Height: 576 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Update Interval: 0 + Value: false + - Alpha: 0.7 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Global Map + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.05 + Name: Global Plan + Offset: + X: 0 + Y: 0 + Z: 0.1 + Pose Color: 255; 255; 255 + Pose Style: None + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_path + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.1 + Name: Local Plan (MPPI) + Offset: + X: 0 + Y: 0 + Z: 0.2 + Pose Color: 255; 255; 255 + Pose Style: None + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mppi_runner/optimal_path + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Sampled Paths + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mppi_runner/sampled_paths + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: false + Line Style: Lines + Line Width: 0.03 + Name: Unsmoothed Plan + Offset: + X: 0 + Y: 0 + Z: 0.05 + Pose Color: 255; 255; 255 + Pose Style: None + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_planner/unsmoothed_plan + Value: false + - Class: rviz_default_plugins/PoseArray + Color: 255; 255; 255 + Enabled: false + Head Length: 0.1 + Head Radius: 0.03 + Name: Search Expansions + Shaft Length: 0.2 + Shaft Radius: 0.01 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_planner/expansions + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Planned Footprints + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_planner/planned_footprints + Value: false + - Alpha: 0.7 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/local_costmap/costmap + Update Interval: 0 + Value: true + - Alpha: 0.7 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: false + Name: Global Costmap + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/global_costmap/costmap + Update Interval: 0 + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Topic: + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Pitch: 1.57 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 0 + Saved: ~ diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py b/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py index 1edbd225..b817e200 100644 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py @@ -21,9 +21,8 @@ gps_pose_publisher : WGS84→ENU, /robot_pose, map→base_link TF map_node : DEM GeoTIFF → nav_msgs/OccupancyGrid /map global_planner : Hybrid-A* planner, /goal_pose → /global_path - ackermann_mppi : MPPI local controller, /global_path → /cmd_vel_nav + ackermann_mppi : MPPI local controller, /global_path → /rear_ackermann_controller/reference mission_executive : State machine, action/service operator interface - cmd_vel_stamper : TwistStamped bridge → /rear_ackermann_controller/reference """ import os @@ -122,19 +121,6 @@ def generate_launch_description(): ], ) - # ── twist_stamper: /cmd_vel_nav → /rear_ackermann_controller/reference ──── - twist_stamper_node = Node( - package='twist_stamper', - executable='twist_stamper', - name='cmd_vel_stamper', - output='screen', - parameters=[{'use_sim_time': sim}], - remappings=[ - ('cmd_vel_in', '/cmd_vel_nav'), - ('cmd_vel_out', '/rear_ackermann_controller/reference'), - ], - ) - return LaunchDescription([ sim_arg, gps_launch, @@ -143,5 +129,4 @@ def generate_launch_description(): global_planner_node, ackermann_mppi_node, mission_executive_node, - twist_stamper_node, ]) diff --git a/src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/__init__.py b/src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/nav_visualizer_node.py b/src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/nav_visualizer_node.py new file mode 100644 index 00000000..d6dc0d1e --- /dev/null +++ b/src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/nav_visualizer_node.py @@ -0,0 +1,154 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Path +from msgs.msg import NavStatus +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +import numpy as np +import threading +import math + +def quat_to_yaw(q): + # q is geometry_msgs/Quaternion + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z) + return math.atan2(siny_cosp, cosy_cosp) + +class NavVisualizer(Node): + def __init__(self): + super().__init__('nav_visualizer') + + # Data storage + self.robot_pose = None + self.global_path = None + self.mppi_path = None + self.goal_pose = None + self.nav_status = None + + # Subscriptions + self.create_subscription(PoseStamped, '/robot_pose', self.pose_cb, 10) + self.create_subscription(Path, '/global_path', self.global_path_cb, 10) + self.create_subscription(Path, '/optimal_path', self.mppi_path_cb, 10) + self.create_subscription(PoseStamped, '/goal_pose', self.goal_cb, 10) + self.create_subscription(NavStatus, '/nav_status', self.status_cb, 10) + + self.get_logger().info("Nav Visualizer Node Started") + + # Lock for thread safety between ROS and Matplotlib + self.lock = threading.Lock() + + def pose_cb(self, msg): + with self.lock: + self.robot_pose = msg + + def global_path_cb(self, msg): + with self.lock: + self.global_path = msg + + def mppi_path_cb(self, msg): + with self.lock: + self.mppi_path = msg + + def goal_cb(self, msg): + with self.lock: + self.goal_pose = msg + + def status_cb(self, msg): + with self.lock: + self.nav_status = msg + +class VisualizerPlot: + def __init__(self, node): + self.node = node + self.fig, self.ax = plt.subplots(figsize=(10, 8)) + + # Plot elements + self.robot_marker, = self.ax.plot([], [], 'ro', label='Robot', markersize=10) + self.heading_line, = self.ax.plot([], [], 'r-', linewidth=2) + self.global_path_line, = self.ax.plot([], [], 'b--', label='Global Path') + self.mppi_path_line, = self.ax.plot([], [], 'g-', label='MPPI Path', linewidth=2) + self.goal_marker, = self.ax.plot([], [], 'kx', label='Goal', markersize=12, markeredgewidth=2) + + self.text_status = self.ax.text(0.02, 0.95, '', transform=self.ax.transAxes, + bbox=dict(facecolor='white', alpha=0.7)) + + self.ax.set_aspect('equal') + self.ax.grid(True) + self.ax.legend(loc='upper right') + self.ax.set_xlabel('East (m)') + self.ax.set_ylabel('North (m)') + self.ax.set_title('Athena 2D Navigation Visualizer') + + def init_plot(self): + return self.robot_marker, self.heading_line, self.global_path_line, self.mppi_path_line, self.goal_marker, self.text_status + + def update(self, frame): + with self.node.lock: + # Update Robot + if self.node.robot_pose: + x = self.node.robot_pose.pose.position.x + y = self.node.robot_pose.pose.position.y + yaw = quat_to_yaw(self.node.robot_pose.pose.orientation) + self.robot_marker.set_data([x], [y]) + + # Heading arrow (0.5m long) + arrow_len = 1.0 + self.heading_line.set_data([x, x + arrow_len * math.cos(yaw)], + [y, y + arrow_len * math.sin(yaw)]) + + # Update Goal + if self.node.goal_pose: + self.goal_marker.set_data([self.node.goal_pose.pose.position.x], + [self.node.goal_pose.pose.position.y]) + + # Update Global Path + if self.node.global_path: + px = [p.pose.position.x for p in self.node.global_path.poses] + py = [p.pose.position.y for p in self.node.global_path.poses] + self.global_path_line.set_data(px, py) + + # Update MPPI Path + if self.node.mppi_path: + mx = [p.pose.position.x for p in self.node.mppi_path.poses] + my = [p.pose.position.y for p in self.node.mppi_path.poses] + self.mppi_path_line.set_data(mx, my) + + # Update Status Text + if self.node.nav_status: + status_text = (f"State: {self.node.nav_status.state}\n" + f"Target: {self.node.nav_status.active_target_id}\n" + f"Dist to Goal: {self.node.nav_status.distance_to_goal_m:.2f}m\n" + f"X-Track Error: {self.node.nav_status.cross_track_error_m:.2f}m\n" + f"Speed: {self.node.nav_status.robot_speed_mps:.2f} m/s") + self.text_status.set_text(status_text) + + # Auto-center view on robot + if self.node.robot_pose: + cx = self.node.robot_pose.pose.position.x + cy = self.node.robot_pose.pose.position.y + self.ax.set_xlim(cx - 15, cx + 15) + self.ax.set_ylim(cy - 15, cy + 15) + + return self.robot_marker, self.heading_line, self.global_path_line, self.mppi_path_line, self.goal_marker, self.text_status + +def main(args=None): + rclpy.init(args=args) + node = NavVisualizer() + + # Run ROS spin in a separate thread + thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + thread.start() + + # Setup Matplotlib + viz = VisualizerPlot(node) + ani = FuncAnimation(viz.fig, viz.update, frames=None, init_func=viz.init_plot, + blit=True, interval=100, cache_frame_data=False) + + plt.show() + + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/subsystems/minimal_navigation/nav_visualizer/package.xml b/src/subsystems/minimal_navigation/nav_visualizer/package.xml new file mode 100644 index 00000000..f09aae2e --- /dev/null +++ b/src/subsystems/minimal_navigation/nav_visualizer/package.xml @@ -0,0 +1,26 @@ + + + + nav_visualizer + 0.0.0 + 2D Navigation Visualizer for Athena + ros + MIT + + rclpy + nav_msgs + geometry_msgs + visualization_msgs + msgs + python3-matplotlib + python3-numpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/subsystems/minimal_navigation/nav_visualizer/resource/nav_visualizer b/src/subsystems/minimal_navigation/nav_visualizer/resource/nav_visualizer new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/minimal_navigation/nav_visualizer/setup.cfg b/src/subsystems/minimal_navigation/nav_visualizer/setup.cfg new file mode 100644 index 00000000..bd1b4600 --- /dev/null +++ b/src/subsystems/minimal_navigation/nav_visualizer/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/nav_visualizer +[install] +install_scripts=$base/lib/nav_visualizer diff --git a/src/subsystems/minimal_navigation/nav_visualizer/setup.py b/src/subsystems/minimal_navigation/nav_visualizer/setup.py new file mode 100644 index 00000000..e684ed1d --- /dev/null +++ b/src/subsystems/minimal_navigation/nav_visualizer/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'nav_visualizer' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools', 'matplotlib', 'numpy'], + zip_safe=True, + maintainer='ros', + maintainer_email='mdurrani808@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'nav_visualizer_node = nav_visualizer.nav_visualizer_node:main' + ], + }, +) diff --git a/src/subsystems/minimal_navigation/nav_visualizer/test/test_copyright.py b/src/subsystems/minimal_navigation/nav_visualizer/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/subsystems/minimal_navigation/nav_visualizer/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/subsystems/minimal_navigation/nav_visualizer/test/test_flake8.py b/src/subsystems/minimal_navigation/nav_visualizer/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/subsystems/minimal_navigation/nav_visualizer/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/subsystems/minimal_navigation/nav_visualizer/test/test_pep257.py b/src/subsystems/minimal_navigation/nav_visualizer/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/subsystems/minimal_navigation/nav_visualizer/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From a3c172e34885a012e8098fc4458b783bac8e62d9 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Mon, 30 Mar 2026 17:27:50 -0400 Subject: [PATCH 03/21] workign --- src/simulation/scripts/heading_publisher.py | 31 +- .../src/front_ackermann_controller.cpp | 8 +- .../dem_costmap/CMakeLists.txt | 0 .../dem_costmap/LICENSE | 0 .../dem_costmap/config/dem_costmap.yaml | 0 .../dem_costmap/launch/dem_costmap.launch.py | 0 .../dem_costmap/maps/north_site800m.tif | 0 .../dem_costmap/package.xml | 0 .../dem_costmap/src/map_node.cpp | 0 .../emmn_visualizer}/__init__.py | 0 .../emmn_visualizer/emmn_visualizer_node.py | 651 +++++ .../emmn_visualizer}/package.xml | 9 +- .../emmn_visualizer/resource/emmn_visualizer} | 0 .../emmn_visualizer/setup.cfg | 4 + .../emmn_visualizer}/setup.py | 8 +- .../global_planner/CMakeLists.txt | 29 + .../global_planner/package.xml | 25 + .../src/global_planner_node.cpp | 330 +++ .../gps_pose_publisher/CMakeLists.txt | 0 .../gps_pose_publisher/package.xml | 0 .../src/gps_pose_publisher_node.cpp | 17 +- .../minimal_nav_bringup/CMakeLists.txt | 0 .../config/nav_params.yaml | 67 + .../minimal_nav_bringup/launch/nav.launch.py | 63 +- .../minimal_nav_bringup/package.xml | 7 +- .../mission_executive/CMakeLists.txt | 6 + .../mission_executive/package.xml | 3 + .../src/mission_executive_node.cpp | 122 +- src/subsystems/even_more_minimal_nav/plan.md | 101 + .../vector_field_planner/CMakeLists.txt | 42 + .../vector_field_planner/package.xml | 28 + .../src/vector_field_planner_node.cpp | 433 +++ .../test/test_pure_pursuit_sim.cpp | 660 +++++ src/subsystems/minimal_navigation/PLAN.md | 508 ---- .../ackermann_mppi/CMakeLists.txt | 122 - .../include/ackermann_mppi/critic_data.hpp | 85 - .../ackermann_mppi/critic_function.hpp | 120 - .../critics/constraint_critic.hpp | 46 - .../ackermann_mppi/critics/goal_critic.hpp | 39 - .../critics/obstacles_critic.hpp | 61 - .../critics/path_align_critic.hpp | 43 - .../critics/path_follow_critic.hpp | 40 - .../ackermann_mppi/models/constraints.hpp | 47 - .../models/control_sequence.hpp | 50 - .../models/optimizer_settings.hpp | 47 - .../include/ackermann_mppi/models/path.hpp | 46 - .../include/ackermann_mppi/models/state.hpp | 55 - .../ackermann_mppi/models/trajectories.hpp | 46 - .../include/ackermann_mppi/motion_models.hpp | 133 - .../include/ackermann_mppi/optimizer.hpp | 238 -- .../ackermann_mppi/tools/noise_generator.hpp | 97 - .../include/ackermann_mppi/tools/utils.hpp | 387 --- .../ackermann_mppi/package.xml | 36 - .../src/ackermann_mppi_node.cpp | 329 --- .../src/critics/constraint_critic.cpp | 65 - .../src/critics/goal_critic.cpp | 55 - .../src/critics/obstacles_critic.cpp | 214 -- .../src/critics/path_align_critic.cpp | 141 - .../src/critics/path_follow_critic.cpp | 69 - .../ackermann_mppi/src/noise_generator.cpp | 103 - .../ackermann_mppi/src/optimizer.cpp | 486 ---- .../athena_smac_planner/CMakeLists.txt | 114 - .../include/athena_smac_planner/a_star.hpp | 323 --- .../analytic_expansion.hpp | 200 -- .../athena_smac_planner/collision_checker.hpp | 139 - .../include/athena_smac_planner/constants.hpp | 105 - .../costmap_downsampler.hpp | 99 - .../distance_heuristic.hpp | 77 - .../athena_smac_planner/goal_manager.hpp | 289 -- .../athena_smac_planner/node_basic.hpp | 69 - .../athena_smac_planner/node_hybrid.hpp | 386 --- .../obstacle_heuristic.hpp | 96 - .../smac_planner_hybrid.hpp | 111 - .../include/athena_smac_planner/smoother.hpp | 208 -- .../thirdparty/robin_hood.h | 2539 ----------------- .../include/athena_smac_planner/types.hpp | 257 -- .../include/athena_smac_planner/utils.hpp | 231 -- .../athena_smac_planner/package.xml | 31 - .../athena_smac_planner/src/a_star.cpp | 526 ---- .../src/analytic_expansion.cpp | 449 --- .../src/collision_checker.cpp | 196 -- .../src/costmap_downsampler.cpp | 127 - .../src/distance_heuristic.cpp | 157 - .../src/global_planner_node.cpp | 162 -- .../athena_smac_planner/src/node_basic.cpp | 54 - .../athena_smac_planner/src/node_hybrid.cpp | 540 ---- .../src/obstacle_heuristic.cpp | 230 -- .../src/smac_planner_hybrid.cpp | 478 ---- .../athena_smac_planner/src/smoother.cpp | 436 --- .../config/nav_params.yaml | 85 - .../minimal_nav_bringup/config/nav_view.rviz | 270 -- .../nav_visualizer/nav_visualizer_node.py | 154 - .../nav_visualizer/setup.cfg | 4 - .../nav_visualizer/test/test_copyright.py | 25 - .../nav_visualizer/test/test_flake8.py | 25 - .../nav_visualizer/test/test_pep257.py | 23 - src/subsystems/minimal_navigation/todo.md | 69 - 97 files changed, 2549 insertions(+), 13087 deletions(-) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/dem_costmap/CMakeLists.txt (100%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/dem_costmap/LICENSE (100%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/dem_costmap/config/dem_costmap.yaml (100%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/dem_costmap/launch/dem_costmap.launch.py (100%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/dem_costmap/maps/north_site800m.tif (100%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/dem_costmap/package.xml (100%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/dem_costmap/src/map_node.cpp (100%) rename src/subsystems/{minimal_navigation/nav_visualizer/nav_visualizer => even_more_minimal_nav/emmn_visualizer/emmn_visualizer}/__init__.py (100%) create mode 100644 src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py rename src/subsystems/{minimal_navigation/nav_visualizer => even_more_minimal_nav/emmn_visualizer}/package.xml (78%) rename src/subsystems/{minimal_navigation/nav_visualizer/resource/nav_visualizer => even_more_minimal_nav/emmn_visualizer/resource/emmn_visualizer} (100%) create mode 100644 src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.cfg rename src/subsystems/{minimal_navigation/nav_visualizer => even_more_minimal_nav/emmn_visualizer}/setup.py (73%) create mode 100644 src/subsystems/even_more_minimal_nav/global_planner/CMakeLists.txt create mode 100644 src/subsystems/even_more_minimal_nav/global_planner/package.xml create mode 100644 src/subsystems/even_more_minimal_nav/global_planner/src/global_planner_node.cpp rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/gps_pose_publisher/CMakeLists.txt (100%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/gps_pose_publisher/package.xml (100%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/gps_pose_publisher/src/gps_pose_publisher_node.cpp (90%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/minimal_nav_bringup/CMakeLists.txt (100%) create mode 100644 src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/minimal_nav_bringup/launch/nav.launch.py (68%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/minimal_nav_bringup/package.xml (72%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/mission_executive/CMakeLists.txt (85%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/mission_executive/package.xml (90%) rename src/subsystems/{minimal_navigation => even_more_minimal_nav}/mission_executive/src/mission_executive_node.cpp (86%) create mode 100644 src/subsystems/even_more_minimal_nav/plan.md create mode 100644 src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt create mode 100644 src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml create mode 100644 src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp create mode 100644 src/subsystems/even_more_minimal_nav/vector_field_planner/test/test_pure_pursuit_sim.cpp delete mode 100644 src/subsystems/minimal_navigation/PLAN.md delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/package.xml delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/package.xml delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp delete mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml delete mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_view.rviz delete mode 100644 src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/nav_visualizer_node.py delete mode 100644 src/subsystems/minimal_navigation/nav_visualizer/setup.cfg delete mode 100644 src/subsystems/minimal_navigation/nav_visualizer/test/test_copyright.py delete mode 100644 src/subsystems/minimal_navigation/nav_visualizer/test/test_flake8.py delete mode 100644 src/subsystems/minimal_navigation/nav_visualizer/test/test_pep257.py delete mode 100644 src/subsystems/minimal_navigation/todo.md diff --git a/src/simulation/scripts/heading_publisher.py b/src/simulation/scripts/heading_publisher.py index c46eaa70..292c366a 100755 --- a/src/simulation/scripts/heading_publisher.py +++ b/src/simulation/scripts/heading_publisher.py @@ -22,6 +22,9 @@ def __init__(self): self.heading_acc_ = self.get_parameter('heading_acc').get_parameter_value().double_value self.pub_ = self.create_publisher(Heading, heading_topic, 10) + + # Always cache the first heading to start at 0 degrees + self.initial_heading_enu = None self.sub_ = self.create_subscription( MagneticField, @@ -31,18 +34,34 @@ def __init__(self): ) self.get_logger().info( - f'Heading publisher: {mag_topic} -> {heading_topic}' + f'Heading publisher: {mag_topic} -> {heading_topic} (Force Initial Offset: Enabled)' ) def on_mag(self, msg: MagneticField): bx = msg.magnetic_field.x by = msg.magnetic_field.y - # compass bearing: degrees clockwise from north - compass_bearing = (math.degrees(math.atan2(-by, bx)) + 360.0) % 360.0 - - # ENU heading: degrees CCW from east - heading_enu = (90.0 - compass_bearing + 360.0) % 360.0 + # In Gazebo's magnetometer the field is reported in the robot body frame + # with the simulated Earth field pointing North (+Y world). For a robot + # with x=forward, y=left this gives: + # bx = H * sin(yaw_enu) (North projected onto forward axis) + # by = H * cos(yaw_enu) (North projected onto left axis) + # So atan2(bx, by) = atan2(sin θ, cos θ) = θ, the correct CCW-from-East + # ENU heading. Using atan2(by, bx) would give a CW compass bearing + # instead, inverting the sign of every turn. + heading_enu_raw = math.degrees(math.atan2(bx, by)) + + if self.initial_heading_enu is None: + self.initial_heading_enu = heading_enu_raw + self.get_logger().info(f'Initial raw heading cached: {self.initial_heading_enu:.2f} deg. Starting at 0.00 deg.') + + # Apply offset so we start at 0 (East) + # ENU heading: degrees CCW from East + heading_enu = (heading_enu_raw - self.initial_heading_enu + 360.0) % 360.0 + + # Compass bearing: degrees clockwise from North + # North is 90 deg ENU. + compass_bearing = (90.0 - heading_enu + 360.0) % 360.0 out = Heading() out.header = msg.header diff --git a/src/subsystems/drive/drive_controllers/src/front_ackermann_controller.cpp b/src/subsystems/drive/drive_controllers/src/front_ackermann_controller.cpp index 5b2c28f1..5516835c 100644 --- a/src/subsystems/drive/drive_controllers/src/front_ackermann_controller.cpp +++ b/src/subsystems/drive/drive_controllers/src/front_ackermann_controller.cpp @@ -223,8 +223,8 @@ controller_interface::return_type FrontAckermannController::update( // Assign angles and velocities based on the hardware's actual behavior if (steer_cmd > 0.0) { // LEFT TURN: left wheel is INNER - front_left_steer_angle = -inner_angle; - front_right_steer_angle = -outer_angle; + front_left_steer_angle = inner_angle; + front_right_steer_angle = outer_angle; front_left_vel = inner_front_vel; front_right_vel = outer_front_vel; @@ -232,8 +232,8 @@ controller_interface::return_type FrontAckermannController::update( rear_right_vel = outer_rear_vel; } else { // RIGHT TURN: right wheel is INNER - front_left_steer_angle = outer_angle; - front_right_steer_angle = inner_angle; + front_left_steer_angle = -outer_angle; + front_right_steer_angle = -inner_angle; front_left_vel = outer_front_vel; front_right_vel = inner_front_vel; diff --git a/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/dem_costmap/CMakeLists.txt similarity index 100% rename from src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt rename to src/subsystems/even_more_minimal_nav/dem_costmap/CMakeLists.txt diff --git a/src/subsystems/minimal_navigation/dem_costmap/LICENSE b/src/subsystems/even_more_minimal_nav/dem_costmap/LICENSE similarity index 100% rename from src/subsystems/minimal_navigation/dem_costmap/LICENSE rename to src/subsystems/even_more_minimal_nav/dem_costmap/LICENSE diff --git a/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml b/src/subsystems/even_more_minimal_nav/dem_costmap/config/dem_costmap.yaml similarity index 100% rename from src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml rename to src/subsystems/even_more_minimal_nav/dem_costmap/config/dem_costmap.yaml diff --git a/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py b/src/subsystems/even_more_minimal_nav/dem_costmap/launch/dem_costmap.launch.py similarity index 100% rename from src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py rename to src/subsystems/even_more_minimal_nav/dem_costmap/launch/dem_costmap.launch.py diff --git a/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif b/src/subsystems/even_more_minimal_nav/dem_costmap/maps/north_site800m.tif similarity index 100% rename from src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif rename to src/subsystems/even_more_minimal_nav/dem_costmap/maps/north_site800m.tif diff --git a/src/subsystems/minimal_navigation/dem_costmap/package.xml b/src/subsystems/even_more_minimal_nav/dem_costmap/package.xml similarity index 100% rename from src/subsystems/minimal_navigation/dem_costmap/package.xml rename to src/subsystems/even_more_minimal_nav/dem_costmap/package.xml diff --git a/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp b/src/subsystems/even_more_minimal_nav/dem_costmap/src/map_node.cpp similarity index 100% rename from src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp rename to src/subsystems/even_more_minimal_nav/dem_costmap/src/map_node.cpp diff --git a/src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/__init__.py b/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/__init__.py similarity index 100% rename from src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/__init__.py rename to src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/__init__.py diff --git a/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py b/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py new file mode 100644 index 00000000..3e24dd18 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py @@ -0,0 +1,651 @@ +import math +import threading +from collections import deque + +import matplotlib +matplotlib.use('TkAgg') +import matplotlib.pyplot as plt +import matplotlib.gridspec as gridspec +from matplotlib.animation import FuncAnimation +from matplotlib.widgets import CheckButtons +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Path, OccupancyGrid +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import Bool, String + +from msgs.msg import NavStatus, Heading, PlannerEvent + +# ─── Constants ──────────────────────────────────────────────────────────────── + +STATE_COLORS = { + 'IDLE': '#888888', + 'NAVIGATING': '#00cc55', + 'ARRIVING': '#ffaa00', + 'STOPPED_AT_TARGET': '#44aaff', + 'STOPPED_AT_RETURN': '#44aaff', + 'ABORTING': '#ff3333', + 'RETURNING': '#aa44ff', + 'TELEOP': '#ff8800', +} +DEFAULT_STATE_COLOR = '#444444' + +PLANNER_EVENT_NAMES = { + PlannerEvent.NEW_GOAL: 'NEW_GOAL', + PlannerEvent.PLANNING: 'PLANNING', + PlannerEvent.PLAN_SUCCEEDED: 'PLAN_OK', + PlannerEvent.PLAN_FAILED: 'PLAN_FAILED', +} +PLAN_FAILED_COLOR = '#ff4444' +PLAN_OK_COLOR = '#44ff88' + +# ─── Coordinate utilities ────────────────────────────────────────────────────── + +def quat_to_yaw(q): + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z) + return math.atan2(siny_cosp, cosy_cosp) + + +class CoordinateConverter: + """WGS-84 ENU ↔ LLA conversion for hover coordinate display.""" + + def __init__(self): + self.origin_lat = None + self.a = 6378137.0 + self.f = 1 / 298.257223563 + self.b = self.a * (1 - self.f) + self.e2 = (self.a**2 - self.b**2) / self.a**2 + + def set_origin(self, lat, lon, alt): + self.origin_lat = lat + lr, lo = math.radians(lat), math.radians(lon) + self.origin_lat_rad, self.origin_lon_rad = lr, lo + self.x0, self.y0, self.z0 = self._lla_to_ecef(lat, lon, alt) + self.s_lat, self.c_lat = math.sin(lr), math.cos(lr) + self.s_lon, self.c_lon = math.sin(lo), math.cos(lo) + + def _lla_to_ecef(self, lat, lon, alt): + lr, lo = math.radians(lat), math.radians(lon) + N = self.a / math.sqrt(1 - self.e2 * math.sin(lr)**2) + return ( + (N + alt) * math.cos(lr) * math.cos(lo), + (N + alt) * math.cos(lr) * math.sin(lo), + (N * (1 - self.e2) + alt) * math.sin(lr), + ) + + def enu_to_lla(self, e, n, u=0.0): + if self.origin_lat is None: + return None, None + dx = -self.s_lon * e - self.c_lon * self.s_lat * n + self.c_lon * self.c_lat * u + dy = self.c_lon * e - self.s_lon * self.s_lat * n + self.s_lon * self.c_lat * u + dz = self.c_lat * n + self.s_lat * u + x, y, z = dx + self.x0, dy + self.y0, dz + self.z0 + lon = math.atan2(y, x) + p = math.sqrt(x**2 + y**2) + lat = math.atan2(z, p * (1 - self.e2)) + for _ in range(5): + N = self.a / math.sqrt(1 - self.e2 * math.sin(lat)**2) + lat = math.atan2(z + self.e2 * N * math.sin(lat), p) + return math.degrees(lat), math.degrees(lon) + + +# ─── ROS node (data collection only) ───────────────────────────────────────── + +class EMMNVisualizerNode(Node): + def __init__(self): + super().__init__('emmn_visualizer') + + self.lock = threading.Lock() + + # Data store + self.robot_pose = None + self.global_path = None + self.goal_pose = None + self.nav_status = None + self.nav_enabled = False + self.nav_mode = 'stopped' + self.heading_msg = None + self.costmap = None + self.converter = CoordinateConverter() + self.event_log = deque(maxlen=10) # (timestamp_str, event_name, is_failure) + self.new_path_received = False + self.new_costmap_received = False + + # QoS profiles + transient_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ) + reliable_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + ) + + self.create_subscription(PoseStamped, '/robot_pose', self._pose_cb, 10) + self.create_subscription(Path, '/global_path', self._path_cb, transient_qos) + self.create_subscription(OccupancyGrid, '/map', self._map_cb, transient_qos) + self.create_subscription(PoseStamped, '/goal_pose', self._goal_cb, 10) + self.create_subscription(NavStatus, '/nav_status', self._status_cb, 10) + self.create_subscription(Bool, '/nav_enabled', self._enabled_cb, reliable_qos) + self.create_subscription(String, '/nav_mode', self._mode_cb, 10) + self.create_subscription(PlannerEvent, '/planner_event', self._event_cb, 10) + self.create_subscription(NavSatFix, '/gps/fix', self._gps_cb, 10) + self.create_subscription(Heading, '/gps/heading', self._heading_cb, 10) + + # ── Callbacks ────────────────────────────────────────────────────────────── + + def _pose_cb(self, msg): + with self.lock: + self.robot_pose = msg + + def _path_cb(self, msg): + with self.lock: + self.global_path = msg + self.new_path_received = True + + def _map_cb(self, msg): + with self.lock: + self.costmap = msg + self.new_costmap_received = True + + def _goal_cb(self, msg): + with self.lock: + self.goal_pose = msg + + def _status_cb(self, msg): + with self.lock: + self.nav_status = msg + + def _enabled_cb(self, msg): + with self.lock: + self.nav_enabled = msg.data + + def _mode_cb(self, msg): + with self.lock: + self.nav_mode = msg.data + + def _event_cb(self, msg): + now = self.get_clock().now().to_msg() + ts = f'{now.sec % 86400 // 3600:02d}:{now.sec % 3600 // 60:02d}:{now.sec % 60:02d}' + name = PLANNER_EVENT_NAMES.get(msg.event, f'EVENT_{msg.event}') + failed = (msg.event == PlannerEvent.PLAN_FAILED) + with self.lock: + self.event_log.append((ts, name, failed)) + + def _gps_cb(self, msg): + with self.lock: + if self.converter.origin_lat is None and msg.status.status >= 0: + self.converter.set_origin(msg.latitude, msg.longitude, msg.altitude) + + def _heading_cb(self, msg): + with self.lock: + self.heading_msg = msg + + +# ─── Plotting ───────────────────────────────────────────────────────────────── + +def _find_lookahead(path_poses, rx, ry, lookahead_m): + """Approximate the VFP lookahead point: closest path index + walk forward.""" + if not path_poses: + return None + best_i, best_d2 = 0, float('inf') + for i, p in enumerate(path_poses): + dx, dy = p.pose.position.x - rx, p.pose.position.y - ry + d2 = dx*dx + dy*dy + if d2 < best_d2: + best_d2, best_i = d2, i + for i in range(best_i, len(path_poses)): + dx = path_poses[i].pose.position.x - rx + dy = path_poses[i].pose.position.y - ry + if math.hypot(dx, dy) >= lookahead_m: + return path_poses[i].pose.position.x, path_poses[i].pose.position.y + last = path_poses[-1].pose.position + return last.x, last.y + + +def _closest_path_point(path_poses, rx, ry): + """Return the closest point on the path (segment-level) and its distance.""" + if not path_poses or len(path_poses) < 2: + return None + min_dist, best_pt = float('inf'), None + for i in range(len(path_poses) - 1): + ax, ay = path_poses[i].pose.position.x, path_poses[i].pose.position.y + bx, by = path_poses[i+1].pose.position.x, path_poses[i+1].pose.position.y + dx, dy = bx - ax, by - ay + len2 = dx*dx + dy*dy + if len2 < 1e-10: + cx, cy = ax, ay + else: + t = max(0.0, min(1.0, ((rx-ax)*dx + (ry-ay)*dy) / len2)) + cx, cy = ax + t*dx, ay + t*dy + d = math.hypot(rx - cx, ry - cy) + if d < min_dist: + min_dist, best_pt = d, (cx, cy) + return best_pt + + +class VisualizerPlot: + LOOKAHEAD_M = 3.0 # default — matches nav_params.yaml lookahead_dist_m + + def __init__(self, node: EMMNVisualizerNode): + self.node = node + + # ── Figure layout ────────────────────────────────────────────────────── + self.fig = plt.figure(figsize=(15, 9)) + self.fig.patch.set_facecolor('#1a1a2e') + + gs = gridspec.GridSpec( + 1, 2, + width_ratios=[3, 1], + left=0.04, right=0.98, + top=0.96, bottom=0.10, + wspace=0.04, + ) + + self.ax_map = self.fig.add_subplot(gs[0, 0]) + self.ax_status = self.fig.add_subplot(gs[0, 1]) + + self._style_map_axes() + self._style_status_axes() + + # ── Map elements ──────────────────────────────────────────────────────── + # Costmap (imshow) + self.costmap_img = None # created lazily on first costmap + + # Global path + self.path_line, = self.ax_map.plot([], [], '--', color='#5588ff', + linewidth=1.8, label='Global Path', zorder=2) + + # Cross-track error: line from robot to closest path point + self.xte_line, = self.ax_map.plot([], [], '-', color='#ffff00', + linewidth=1.5, label='XTE', zorder=5, + linestyle='dashed') + self.xte_dot, = self.ax_map.plot([], [], 'o', color='#ffff00', + markersize=6, zorder=6) + + # Lookahead circle + line + point + self.lookahead_circle = plt.Circle((0, 0), self.LOOKAHEAD_M, + color='#88ccff', fill=False, linewidth=1.2, + linestyle='dotted', zorder=4) + self.ax_map.add_patch(self.lookahead_circle) + self.lookahead_line, = self.ax_map.plot([], [], '-', color='#88ccff', + linewidth=1.2, zorder=5, linestyle='dotted') + self.lookahead_pt, = self.ax_map.plot([], [], 'D', color='#88ccff', + markersize=7, zorder=7, label='Lookahead') + + # Robot marker + heading arrow + self.robot_dot, = self.ax_map.plot([], [], 'o', color='#ff4444', + markersize=10, zorder=10, label='Robot') + self.heading_line,= self.ax_map.plot([], [], '-', color='#ffaa00', + linewidth=2.5, zorder=11) + + # Goal marker + self.goal_marker, = self.ax_map.plot([], [], 'x', color='white', + markersize=12, markeredgewidth=2.5, + zorder=9, label='Goal') + + self.ax_map.legend(loc='lower left', framealpha=0.6, + facecolor='#1a1a2e', labelcolor='white', + fontsize=9) + + # Hover text + self.hover_text = self.ax_map.text( + 0.02, 0.02, '', transform=self.ax_map.transAxes, + fontsize=8, color='#cccccc', + bbox=dict(facecolor='#1a1a2e', alpha=0.8, edgecolor='#555555'), + verticalalignment='bottom', + ) + + # ── Status panel elements ─────────────────────────────────────────────── + self.status_text = self.ax_status.text( + 0.05, 0.98, 'Waiting for data...', + transform=self.ax_status.transAxes, + fontsize=9, color='white', family='monospace', + verticalalignment='top', + bbox=dict(facecolor='#0d0d1a', alpha=0.0, edgecolor='none'), + ) + + # ── Checkboxes (bottom strip) ─────────────────────────────────────────── + cb_labels = ['Auto-Fit', 'Show Costmap', 'Show Lookahead', 'Show XTE'] + cb_states = [True, True, True, True] + rax = self.fig.add_axes([0.04, 0.01, 0.30, 0.07]) + rax.set_facecolor('#1a1a2e') + self.check = CheckButtons(rax, cb_labels, cb_states) + for txt in self.check.labels: + txt.set_color('white') + txt.set_fontsize(9) + self.auto_fit = True + self.show_costmap = True + self.show_lookahead = True + self.show_xte = True + self.check.on_clicked(self._on_toggle) + + # ── Events ───────────────────────────────────────────────────────────── + self.fig.canvas.mpl_connect('motion_notify_event', self._on_hover) + + # ── Axis styling ─────────────────────────────────────────────────────────── + + def _style_map_axes(self): + ax = self.ax_map + ax.set_facecolor('#0d0d1a') + ax.tick_params(colors='#888888', labelsize=8) + for spine in ax.spines.values(): + spine.set_edgecolor('#333355') + ax.grid(True, linestyle=':', alpha=0.3, color='#444466') + ax.set_aspect('equal') + ax.set_xlabel('East (m)', color='#888888', fontsize=9) + ax.set_ylabel('North (m)', color='#888888', fontsize=9) + ax.set_title('Even More Minimal Nav — Live Visualizer', color='#aaaacc', fontsize=11) + + def _style_status_axes(self): + ax = self.ax_status + ax.set_facecolor('#0d0d1a') + ax.set_xlim(0, 1) + ax.set_ylim(0, 1) + ax.axis('off') + for spine in ax.spines.values(): + spine.set_edgecolor('#333355') + + # ── Toggles ──────────────────────────────────────────────────────────────── + + def _on_toggle(self, label): + if label == 'Auto-Fit': + self.auto_fit = not self.auto_fit + if self.auto_fit: + with self.node.lock: + self.node.new_path_received = True + elif label == 'Show Costmap': + self.show_costmap = not self.show_costmap + if self.costmap_img is not None: + self.costmap_img.set_visible(self.show_costmap) + elif label == 'Show Lookahead': + self.show_lookahead = not self.show_lookahead + elif label == 'Show XTE': + self.show_xte = not self.show_xte + + # ── Hover ────────────────────────────────────────────────────────────────── + + def _on_hover(self, event): + if event.inaxes != self.ax_map or event.xdata is None: + return + with self.node.lock: + lat, lon = self.node.converter.enu_to_lla(event.xdata, event.ydata) + if lat is not None: + self.hover_text.set_text( + f'E:{event.xdata:+.1f}m N:{event.ydata:+.1f}m\n' + f'Lat:{lat:.6f} Lon:{lon:.6f}' + ) + else: + self.hover_text.set_text(f'E:{event.xdata:+.1f}m N:{event.ydata:+.1f}m') + + # ── Main update (called at ~10 Hz by FuncAnimation) ─────────────────────── + + def update(self, _frame): + with self.node.lock: + robot_pose = self.node.robot_pose + global_path = self.node.global_path + goal_pose = self.node.goal_pose + nav_status = self.node.nav_status + nav_enabled = self.node.nav_enabled + nav_mode = self.node.nav_mode + heading_msg = self.node.heading_msg + costmap = self.node.costmap + event_log = list(self.node.event_log) + new_path = self.node.new_path_received + new_costmap = self.node.new_costmap_received + + if new_costmap: + self.node.new_costmap_received = False + + path_poses = global_path.poses if global_path else [] + + rx = ry = yaw = None + if robot_pose: + rx, ry = robot_pose.pose.position.x, robot_pose.pose.position.y + if heading_msg: + yaw = math.radians(heading_msg.heading) + else: + yaw = quat_to_yaw(robot_pose.pose.orientation) + + # ── Costmap ───────────────────────────────────────────────────────────── + if new_costmap and costmap is not None: + self._render_costmap(costmap) + + # ── Global path ───────────────────────────────────────────────────────── + if path_poses: + xs = [p.pose.position.x for p in path_poses] + ys = [p.pose.position.y for p in path_poses] + self.path_line.set_data(xs, ys) + else: + self.path_line.set_data([], []) + + # ── Robot + heading ───────────────────────────────────────────────────── + if rx is not None: + self.robot_dot.set_data([rx], [ry]) + L = 2.5 + self.heading_line.set_data( + [rx, rx + L * math.cos(yaw)], + [ry, ry + L * math.sin(yaw)], + ) + self.lookahead_circle.center = (rx, ry) + else: + self.robot_dot.set_data([], []) + self.heading_line.set_data([], []) + + # ── Goal ──────────────────────────────────────────────────────────────── + if goal_pose: + self.goal_marker.set_data( + [goal_pose.pose.position.x], [goal_pose.pose.position.y]) + else: + self.goal_marker.set_data([], []) + + # ── XTE indicator ─────────────────────────────────────────────────────── + if self.show_xte and rx is not None and len(path_poses) >= 2: + cp = _closest_path_point(path_poses, rx, ry) + if cp: + self.xte_line.set_data([rx, cp[0]], [ry, cp[1]]) + self.xte_dot.set_data([cp[0]], [cp[1]]) + else: + self.xte_line.set_data([], []) + self.xte_dot.set_data([], []) + else: + self.xte_line.set_data([], []) + self.xte_dot.set_data([], []) + + # ── Lookahead ──────────────────────────────────────────────────────────── + lp = None + if self.show_lookahead and rx is not None and path_poses: + lp = _find_lookahead(path_poses, rx, ry, self.LOOKAHEAD_M) + self.lookahead_circle.set_visible(self.show_lookahead and rx is not None) + if lp and self.show_lookahead: + self.lookahead_pt.set_data([lp[0]], [lp[1]]) + self.lookahead_line.set_data([rx, lp[0]], [ry, lp[1]]) + else: + self.lookahead_pt.set_data([], []) + self.lookahead_line.set_data([], []) + + # ── Viewport ───────────────────────────────────────────────────────────── + if self.auto_fit and new_path: + self._fit_viewport(path_poses, rx, ry) + with self.node.lock: + self.node.new_path_received = False + + # ── Status panel ───────────────────────────────────────────────────────── + self._update_status(nav_status, nav_enabled, nav_mode, event_log, + heading_msg, costmap, path_poses, rx, ry) + + # ── Costmap renderer ───────────────────────────────────────────────────── + + def _render_costmap(self, costmap): + info = costmap.info + W, H = info.width, info.height + res = info.resolution + ox = info.origin.position.x + oy = info.origin.position.y + + arr = np.array(costmap.data, dtype=np.int16).reshape(H, W).astype(np.float32) + arr[arr < 0] = np.nan # unknown cells → transparent + arr = np.clip(arr, 0, 254) / 254.0 + + extent = [ox, ox + W * res, oy, oy + H * res] + cmap = plt.cm.RdYlGn_r.copy() + cmap.set_bad(color='none') + + if self.costmap_img is None: + self.costmap_img = self.ax_map.imshow( + arr, origin='lower', extent=extent, + cmap=cmap, vmin=0.0, vmax=1.0, + alpha=0.45, zorder=1, + interpolation='nearest', + ) + self.fig.colorbar( + self.costmap_img, ax=self.ax_map, + fraction=0.025, pad=0.01, + label='Slope Cost (0=safe, 1=lethal)', + ).ax.yaxis.label.set_color('#888888') + else: + self.costmap_img.set_data(arr) + self.costmap_img.set_extent(extent) + + self.costmap_img.set_visible(self.show_costmap) + + # ── Viewport fit ──────────────────────────────────────────────────────────── + + def _fit_viewport(self, path_poses, rx, ry): + pts_x, pts_y = [], [] + for p in path_poses: + pts_x.append(p.pose.position.x) + pts_y.append(p.pose.position.y) + if rx is not None: + pts_x.append(rx) + pts_y.append(ry) + if not pts_x: + return + mn_x, mx_x = min(pts_x), max(pts_x) + mn_y, mx_y = min(pts_y), max(pts_y) + dx = max(mx_x - mn_x, 15.0) + dy = max(mx_y - mn_y, 15.0) + cx = (mn_x + mx_x) / 2.0 + cy = (mn_y + mx_y) / 2.0 + pad = 0.65 + self.ax_map.set_xlim(cx - dx * pad, cx + dx * pad) + self.ax_map.set_ylim(cy - dy * pad, cy + dy * pad) + + # ── Status panel text ──────────────────────────────────────────────────────── + + def _update_status(self, nav_status, nav_enabled, nav_mode, event_log, + heading_msg, costmap, path_poses, rx, ry): + state = nav_status.state if nav_status else 'NO DATA' + color = STATE_COLORS.get(state, DEFAULT_STATE_COLOR) + + lines = [] + lines.append(f'{"═"*26}') + lines.append(f' {state:^24s}') + lines.append(f'{"═"*26}') + + # Target info + if nav_status and nav_status.active_target_id: + ttype = nav_status.active_target_type + lines.append(f' TARGET : {nav_status.active_target_id}') + lines.append(f' Type : {ttype}') + lines.append(f' Return : {"Yes" if nav_status.is_return else "No"}') + else: + lines.append(' TARGET : (none)') + lines.append('') + + # Metrics + lines.append('── Metrics ──────────────────') + if nav_status: + dist = nav_status.distance_to_goal_m + xte = nav_status.cross_track_error_m + herr = math.degrees(nav_status.heading_error_rad) + avel = nav_status.robot_speed_mps # actually imu angular vel + dist_str = f'{dist:.1f}m' if dist >= 0 else 'N/A' + xte_str = f'{xte:.2f}m' if xte >= 0 else 'N/A' + lines.append(f' Dist to Goal : {dist_str:>8s}') + lines.append(f' Cross-Track : {xte_str:>8s}') + lines.append(f' Heading Err : {herr:>+7.1f}°') + lines.append(f' AngVel (IMU) : {avel:>7.3f} r/s') + else: + lines.append(' (no nav_status)') + lines.append('') + + # Heading + lines.append('── Heading ──────────────────') + if heading_msg: + lines.append(f' ENU heading : {heading_msg.heading:>+7.1f}°') + lines.append(f' Compass : {heading_msg.compass_bearing:>+7.1f}°') + else: + lines.append(' (no heading msg)') + lines.append('') + + # System state + lines.append('── System ───────────────────') + enabled_str = 'YES' if nav_enabled else 'NO' + lines.append(f' Nav Enabled : {enabled_str}') + lines.append(f' Nav Mode : {nav_mode}') + if costmap is not None: + W = costmap.info.width + H = costmap.info.height + lines.append(f' Costmap : {W}×{H}') + else: + lines.append(' Costmap : not received') + lines.append(f' Path poses : {len(path_poses)}') + lines.append('') + + # Robot position + lines.append('── Robot Position ───────────') + if rx is not None: + lat, lon = self.node.converter.enu_to_lla(rx, ry) + lines.append(f' E: {rx:>+8.2f}m') + lines.append(f' N: {ry:>+8.2f}m') + if lat is not None: + lines.append(f' Lat: {lat:.6f}') + lines.append(f' Lon: {lon:.6f}') + else: + lines.append(' (no robot pose)') + lines.append('') + + # Event log + lines.append('── Planner Events ───────────') + if event_log: + for ts, name, failed in reversed(event_log): + marker = '!!' if failed else ' ' + lines.append(f' {marker}[{ts}] {name}') + else: + lines.append(' (none yet)') + + text = '\n'.join(lines) + self.status_text.set_text(text) + + # Color the state line background dynamically via the text bbox + self.status_text.set_bbox(dict( + facecolor=color + '33', # hex color + 20% alpha + edgecolor=color, + linewidth=0.8, + )) + + +# ─── Entry point ────────────────────────────────────────────────────────────── + +def main(args=None): + rclpy.init(args=args) + node = EMMNVisualizerNode() + + spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + spin_thread.start() + + viz = VisualizerPlot(node) + ani = FuncAnimation(viz.fig, viz.update, interval=100, blit=False, cache_frame_data=False) + + plt.show() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/subsystems/minimal_navigation/nav_visualizer/package.xml b/src/subsystems/even_more_minimal_nav/emmn_visualizer/package.xml similarity index 78% rename from src/subsystems/minimal_navigation/nav_visualizer/package.xml rename to src/subsystems/even_more_minimal_nav/emmn_visualizer/package.xml index f09aae2e..70a5d095 100644 --- a/src/subsystems/minimal_navigation/nav_visualizer/package.xml +++ b/src/subsystems/even_more_minimal_nav/emmn_visualizer/package.xml @@ -1,16 +1,17 @@ - nav_visualizer + emmn_visualizer 0.0.0 - 2D Navigation Visualizer for Athena + Debug visualizer for even_more_minimal_nav ros - MIT + Apache-2.0 rclpy nav_msgs geometry_msgs - visualization_msgs + sensor_msgs + std_msgs msgs python3-matplotlib python3-numpy diff --git a/src/subsystems/minimal_navigation/nav_visualizer/resource/nav_visualizer b/src/subsystems/even_more_minimal_nav/emmn_visualizer/resource/emmn_visualizer similarity index 100% rename from src/subsystems/minimal_navigation/nav_visualizer/resource/nav_visualizer rename to src/subsystems/even_more_minimal_nav/emmn_visualizer/resource/emmn_visualizer diff --git a/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.cfg b/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.cfg new file mode 100644 index 00000000..234583bb --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/emmn_visualizer +[install] +install_scripts=$base/lib/emmn_visualizer diff --git a/src/subsystems/minimal_navigation/nav_visualizer/setup.py b/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.py similarity index 73% rename from src/subsystems/minimal_navigation/nav_visualizer/setup.py rename to src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.py index e684ed1d..aa39ff14 100644 --- a/src/subsystems/minimal_navigation/nav_visualizer/setup.py +++ b/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.py @@ -1,6 +1,6 @@ from setuptools import find_packages, setup -package_name = 'nav_visualizer' +package_name = 'emmn_visualizer' setup( name=package_name, @@ -15,12 +15,12 @@ zip_safe=True, maintainer='ros', maintainer_email='mdurrani808@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', + description='Debug visualizer for even_more_minimal_nav', + license='Apache-2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'nav_visualizer_node = nav_visualizer.nav_visualizer_node:main' + 'emmn_visualizer_node = emmn_visualizer.emmn_visualizer_node:main', ], }, ) diff --git a/src/subsystems/even_more_minimal_nav/global_planner/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/global_planner/CMakeLists.txt new file mode 100644 index 00000000..b0834978 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/global_planner/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(global_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) + +add_executable(global_planner_node src/global_planner_node.cpp) +target_compile_features(global_planner_node PUBLIC cxx_std_17) +target_link_libraries(global_planner_node + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + tf2::tf2 +) + +install(TARGETS global_planner_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/subsystems/even_more_minimal_nav/global_planner/package.xml b/src/subsystems/even_more_minimal_nav/global_planner/package.xml new file mode 100644 index 00000000..5cbb7242 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/global_planner/package.xml @@ -0,0 +1,25 @@ + + + + global_planner + 0.1.0 + + Global path planner for the Athena navigation stack. + Phase 1: straight-line interpolation from current pose to goal. + Phase 2: slope-weighted A* on the OccupancyGrid from dem_costmap_converter. + + UMD Loop + Apache-2.0 + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + tf2_ros + tf2 + + + ament_cmake + + diff --git a/src/subsystems/even_more_minimal_nav/global_planner/src/global_planner_node.cpp b/src/subsystems/even_more_minimal_nav/global_planner/src/global_planner_node.cpp new file mode 100644 index 00000000..3baed4fd --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/global_planner/src/global_planner_node.cpp @@ -0,0 +1,330 @@ +// Copyright (c) 2025 UMD Loop +// +// 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. + +// global_planner_node.cpp +// +// Converts a /goal_pose into a /global_path. +// +// Phase 1 (use_costmap: false): +// Straight-line interpolation from current TF position to goal. +// Runs immediately on every new goal with no external dependencies. +// +// Phase 2 (use_costmap: true, costmap received): +// A* on the OccupancyGrid from dem_costmap_converter. +// Cost to enter a cell: dist_to_neighbor + slope_weight * (grid_value / 254). +// Cells with grid_value >= 254 are lethal and impassable. +// Falls back to straight-line if A* cannot find a path. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/path.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/exceptions.h" + +class GlobalPlanner : public rclcpp::Node +{ +public: + GlobalPlanner() : Node("global_planner") + { + declare_parameter("path_resolution_m", 1.0); + declare_parameter("use_costmap", false); + declare_parameter("slope_weight", 10.0); + + path_resolution_m_ = get_parameter("path_resolution_m").as_double(); + use_costmap_ = get_parameter("use_costmap").as_bool(); + slope_weight_ = get_parameter("slope_weight").as_double(); + + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + path_pub_ = create_publisher( + "/global_path", rclcpp::QoS(1).transient_local()); + + goal_sub_ = create_subscription( + "/goal_pose", 10, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { onGoal(msg); }); + + // Costmap is optional — published by dem_costmap_converter with transient_local QoS. + costmap_sub_ = create_subscription( + "/map", rclcpp::QoS(1).transient_local(), + [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + costmap_ = *msg; + }); + + RCLCPP_INFO(get_logger(), "GlobalPlanner ready (use_costmap=%s)", + use_costmap_ ? "true" : "false"); + } + +private: + // ── Goal callback ────────────────────────────────────────────────────────── + + void onGoal(const geometry_msgs::msg::PoseStamped::SharedPtr goal) + { + // Look up current robot position in map frame + geometry_msgs::msg::TransformStamped tf; + try { + tf = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "TF map→base_link unavailable: %s — cannot plan", ex.what()); + return; + } + + const double sx = tf.transform.translation.x; + const double sy = tf.transform.translation.y; + const double gx = goal->pose.position.x; + const double gy = goal->pose.position.y; + + RCLCPP_INFO(get_logger(), "Planning (%.2f,%.2f) → (%.2f,%.2f)", sx, sy, gx, gy); + + // Phase 2: A* if enabled and costmap is available + if (use_costmap_ && costmap_.has_value()) { + auto maybe_path = planAstar(sx, sy, gx, gy); + if (maybe_path.has_value()) { + path_pub_->publish(*maybe_path); + RCLCPP_INFO(get_logger(), "A* path published (%zu poses)", maybe_path->poses.size()); + return; + } + RCLCPP_WARN(get_logger(), "A* failed — falling back to straight-line"); + } + + // Phase 1: straight-line + auto path = planStraightLine(sx, sy, gx, gy); + path_pub_->publish(path); + RCLCPP_INFO(get_logger(), "Straight-line path published (%zu poses)", path.poses.size()); + } + + // ── Phase 1: straight-line interpolation ────────────────────────────────── + + nav_msgs::msg::Path planStraightLine( + double sx, double sy, double gx, double gy) const + { + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.header.stamp = now(); + + const double dist = std::hypot(gx - sx, gy - sy); + + if (dist < 1e-6) { + // Already at goal — single-pose path + geometry_msgs::msg::PoseStamped p; + p.header = path.header; + p.pose.position.x = gx; + p.pose.position.y = gy; + p.pose.orientation.w = 1.0; + path.poses.push_back(p); + return path; + } + + // Number of samples including both endpoints + const int n = std::max(2, static_cast(std::ceil(dist / path_resolution_m_)) + 1); + + // Heading quaternion: constant along the line + const double yaw = std::atan2(gy - sy, gx - sx); + const double half = yaw * 0.5; + const double qz = std::sin(half); + const double qw = std::cos(half); + + path.poses.reserve(n); + for (int i = 0; i < n; ++i) { + const double t = static_cast(i) / (n - 1); + geometry_msgs::msg::PoseStamped p; + p.header = path.header; + p.pose.position.x = sx + t * (gx - sx); + p.pose.position.y = sy + t * (gy - sy); + p.pose.orientation.z = qz; + p.pose.orientation.w = qw; + path.poses.push_back(p); + } + + return path; + } + + // ── Phase 2: A* on OccupancyGrid ────────────────────────────────────────── + // + // g(cell) = dist_to_neighbor + slope_weight_ * (grid_value / 254) + // + // OccupancyGrid.data is int8[], but dem_costmap_converter writes values in + // the nav2 range 0–254. Cast to uint8_t before comparing to avoid sign bugs + // (254 as int8_t = -2, which would incorrectly pass a < 254 guard). + + std::optional planAstar( + double sx, double sy, double gx, double gy) const + { + const auto & info = costmap_->info; + const int W = static_cast(info.width); + const int H = static_cast(info.height); + const double res = info.resolution; + const double ox = info.origin.position.x; + const double oy = info.origin.position.y; + + // World → grid cell (col, row) + auto toGrid = [&](double wx, double wy, int & col, int & row) -> bool { + col = static_cast((wx - ox) / res); + row = static_cast((wy - oy) / res); + return col >= 0 && col < W && row >= 0 && row < H; + }; + + // Grid cell center → world + auto toWorld = [&](int col, int row, double & wx, double & wy) { + wx = ox + (col + 0.5) * res; + wy = oy + (row + 0.5) * res; + }; + + int sc, sr, gc, gr; + if (!toGrid(sx, sy, sc, sr)) { + RCLCPP_WARN(get_logger(), "Start (%.2f,%.2f) is outside costmap bounds", sx, sy); + return std::nullopt; + } + if (!toGrid(gx, gy, gc, gr)) { + RCLCPP_WARN(get_logger(), "Goal (%.2f,%.2f) is outside costmap bounds", gx, gy); + return std::nullopt; + } + + // Reject lethal goal cell + const auto goal_val = static_cast(costmap_->data[gr * W + gc]); + if (goal_val >= 254) { + RCLCPP_WARN(get_logger(), "Goal cell is lethal (cost=%u) — A* cannot plan", goal_val); + return std::nullopt; + } + + // A* with 8-connected grid + // Open set: (f_cost, col, row) + using State = std::tuple; + std::priority_queue, std::greater> open; + + std::vector g_cost(W * H, std::numeric_limits::infinity()); + std::vector parent(W * H, -1); + + const int s_idx = sr * W + sc; + g_cost[s_idx] = 0.0; + open.emplace(0.0, sc, sr); + + // 8-directional moves: (dcol, drow, euclidean_distance) + constexpr int dcol[8] = { 1, -1, 0, 0, 1, 1, -1, -1}; + constexpr int drow[8] = { 0, 0, 1, -1, 1, -1, 1, -1}; + constexpr double step[8] = { 1, 1, 1, 1, M_SQRT2, M_SQRT2, M_SQRT2, M_SQRT2}; + + bool found = false; + while (!open.empty()) { + auto [f, cx, cy] = open.top(); open.pop(); + + const int idx = cy * W + cx; + if (f > g_cost[idx] + 1e-9) continue; // stale entry + if (cx == gc && cy == gr) { found = true; break; } + + for (int d = 0; d < 8; ++d) { + const int nx = cx + dcol[d]; + const int ny = cy + drow[d]; + if (nx < 0 || nx >= W || ny < 0 || ny >= H) continue; + + const auto cell_val = static_cast(costmap_->data[ny * W + nx]); + if (cell_val >= 254) continue; // lethal — cannot enter + + const double move_cost = + step[d] * res + slope_weight_ * static_cast(cell_val) / 254.0; + + const double ng = g_cost[idx] + move_cost; + const int n_idx = ny * W + nx; + if (ng < g_cost[n_idx]) { + g_cost[n_idx] = ng; + parent[n_idx] = idx; + // Euclidean heuristic (admissible) + const double h = std::hypot(nx - gc, ny - gr) * res; + open.emplace(ng + h, nx, ny); + } + } + } + + if (!found) { + RCLCPP_WARN(get_logger(), "A* exhausted open set — no path found"); + return std::nullopt; + } + + // Trace path from goal back to start, then reverse + std::vector> cells; // (col, row) + for (int cur = gr * W + gc; cur != -1; cur = parent[cur]) { + cells.emplace_back(cur % W, cur / W); + } + std::reverse(cells.begin(), cells.end()); + + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.header.stamp = now(); + path.poses.reserve(cells.size()); + + for (size_t i = 0; i < cells.size(); ++i) { + double wx, wy; + toWorld(cells[i].first, cells[i].second, wx, wy); + + geometry_msgs::msg::PoseStamped p; + p.header = path.header; + p.pose.position.x = wx; + p.pose.position.y = wy; + + // Point toward the next cell; last pose inherits previous heading + if (i + 1 < cells.size()) { + double nwx, nwy; + toWorld(cells[i + 1].first, cells[i + 1].second, nwx, nwy); + const double yaw = std::atan2(nwy - wy, nwx - wx); + const double half = yaw * 0.5; + p.pose.orientation.z = std::sin(half); + p.pose.orientation.w = std::cos(half); + } else if (!path.poses.empty()) { + p.pose.orientation = path.poses.back().pose.orientation; + } else { + p.pose.orientation.w = 1.0; + } + + path.poses.push_back(p); + } + + return path; + } + + // ── Parameters ───────────────────────────────────────────────────────────── + double path_resolution_m_{1.0}; + bool use_costmap_{false}; + double slope_weight_{10.0}; + + // ── TF ───────────────────────────────────────────────────────────────────── + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // ── ROS interfaces ────────────────────────────────────────────────────────── + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr costmap_sub_; + + // ── State ─────────────────────────────────────────────────────────────────── + std::optional costmap_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/gps_pose_publisher/CMakeLists.txt similarity index 100% rename from src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt rename to src/subsystems/even_more_minimal_nav/gps_pose_publisher/CMakeLists.txt diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml b/src/subsystems/even_more_minimal_nav/gps_pose_publisher/package.xml similarity index 100% rename from src/subsystems/minimal_navigation/gps_pose_publisher/package.xml rename to src/subsystems/even_more_minimal_nav/gps_pose_publisher/package.xml diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp b/src/subsystems/even_more_minimal_nav/gps_pose_publisher/src/gps_pose_publisher_node.cpp similarity index 90% rename from src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp rename to src/subsystems/even_more_minimal_nav/gps_pose_publisher/src/gps_pose_publisher_node.cpp index 10d0291c..80633d6c 100644 --- a/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp +++ b/src/subsystems/even_more_minimal_nav/gps_pose_publisher/src/gps_pose_publisher_node.cpp @@ -40,6 +40,10 @@ class GpsPosePublisher : public rclcpp::Node declare_parameter("origin_lat", std::numeric_limits::quiet_NaN()); declare_parameter("origin_lon", std::numeric_limits::quiet_NaN()); declare_parameter("origin_alt", 0.0); + // Additive offset applied after converting heading to ENU radians. + // Use 180.0 if the heading sensor is mounted backwards on the rover, + // or (90.0 - compass_deg) conversion is needed for compass-convention heading sources. + declare_parameter("heading_offset_deg", 0.0); tf_broadcaster_ = std::make_shared(*this); @@ -52,10 +56,12 @@ class GpsPosePublisher : public rclcpp::Node std::string heading_topic; get_parameter("heading_topic", heading_topic); + get_parameter("heading_offset_deg", heading_offset_deg_); + heading_sub_ = create_subscription( heading_topic, rclcpp::SensorDataQoS(), [this](const msgs::msg::Heading::SharedPtr msg) { - heading_enu_rad_ = (90.0 - msg->compass_bearing) * M_PI / 180.0; + heading_enu_rad_ = (msg->heading + heading_offset_deg_) * M_PI / 180.0; has_heading_ = true; }); @@ -135,12 +141,12 @@ class GpsPosePublisher : public rclcpp::Node default: quality = "FIX"; break; } std_msgs::msg::String s; - s.data = "FIX quality=" + quality + " heading=" + (has_heading_ ? "compass" : "none"); + s.data = "FIX quality=" + quality + " heading=" + (has_heading_ ? "compass" : "none(using 0)"); status_pub_->publish(s); - if (!has_heading_) { - return; - } + // Always publish pose/TF so the navigation stack has position feedback even + // before a heading source is available. heading_enu_rad_ defaults to 0.0 + // (facing East) and updates when the first heading message arrives. geometry_msgs::msg::PoseStamped pose; pose.header.stamp = msg->header.stamp; @@ -180,6 +186,7 @@ class GpsPosePublisher : public rclcpp::Node bool origin_set_{false}; bool has_heading_{false}; double heading_enu_rad_{0.0}; + double heading_offset_deg_{0.0}; }; int main(int argc, char ** argv) diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt similarity index 100% rename from src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt rename to src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml new file mode 100644 index 00000000..b0c00234 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml @@ -0,0 +1,67 @@ +# nav_params.yaml — single parameter file passed to all navigation nodes. + +# ── gps_pose_publisher ─────────────────────────────────────────────────────── +gps_pose_publisher: + ros__parameters: + use_sim_time: true + heading_topic: "/gps/heading" + heading_offset_deg: 0.0 # set to 180 if sensor is mounted backwards + use_start_gate_ref: false # if true, waits for /start_gate_ref NavSatFix to set origin + origin_lat: .nan # set to a fixed lat/lon to skip first-fix origin logic + origin_lon: .nan + origin_alt: 0.0 + +# ── dem_costmap_converter ──────────────────────────────────────────────────── +dem_costmap_converter: + ros__parameters: + use_sim_time: true + dem_file_path: "" # set by launch file at runtime + map_resolution: 1.0 # meters per pixel + max_passable_slope_degrees: 15.0 + output_frame: map + origin_x: -400.0 + origin_y: -400.0 + +# ── global_planner ─────────────────────────────────────────────────────────── +global_planner: + ros__parameters: + use_sim_time: true + path_resolution_m: 1.0 # spacing between path poses + use_costmap: false # set to true to enable A* on dem_costmap_converter output + slope_weight: 10.0 # Phase 2: alpha in g = dist + alpha * (cell_cost / 254) + +# ── mission_executive ──────────────────────────────────────────────────────── +mission_executive: + ros__parameters: + use_sim_time: true + stop_angular_vel_threshold: 0.05 # rad/s — IMU gyro magnitude below which robot is "stopped" + arrival_hold_time: 1.0 # seconds gyro must stay below threshold to confirm arrival + replan_distance_m: 5.0 # cross-track error (m) that triggers a replan + latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" + # Input topics + imu_topic: "/imu" + planner_event_topic: "/planner_event" + global_path_topic: "/global_path" + # Output topics + goal_pose_topic: "/goal_pose" + nav_enabled_topic: "/nav_enabled" + nav_mode_topic: "/nav_mode" + active_target_topic: "/active_target" + nav_status_topic: "/nav_status" + +# ── vector_field_planner ────────────────────────────────────────────────────── +vector_field_planner: + ros__parameters: + use_sim_time: true + map_frame: map + base_frame: base_link + cmd_vel_topic: "/front_ackermann_controller/reference" + tf_timeout_s: 0.1 + max_speed_mps: 0.8 + max_steering_angle_rad: 0.5 # radians — clamp on angular.z output + lookahead_dist_m: 3.0 # pure-pursuit lookahead distance + k_p_steering: 1.5 # proportional gain: angular.z = k_p * heading_error + repulsion_gain: 0.0 # Phase 2: obstacle repulsion magnitude (0 = disabled) + repulsion_cutoff_m: 3.0 # Phase 2: distance at which repulsion activates + goal_tolerance_m: 1.5 # distance to path end that counts as "arrived" + publish_debug_markers: true diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py similarity index 68% rename from src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py rename to src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py index b817e200..7cb292f3 100644 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py +++ b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py @@ -13,16 +13,24 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""nav.launch.py — Full Athena navigation stack (GPS-only, Nav2-free). - -Launch order / node summary -─────────────────────────── - athena_gps : Pixhawk GPS + heading bridge (from athena_gps/gps_launch.py) - gps_pose_publisher : WGS84→ENU, /robot_pose, map→base_link TF - map_node : DEM GeoTIFF → nav_msgs/OccupancyGrid /map - global_planner : Hybrid-A* planner, /goal_pose → /global_path - ackermann_mppi : MPPI local controller, /global_path → /rear_ackermann_controller/reference - mission_executive : State machine, action/service operator interface +"""nav.launch.py — Athena navigation stack (GPS-only, Nav2-free). + +Node summary +──────────── + athena_gps : Pixhawk GPS + heading bridge + gps_pose_publisher : WGS84→ENU, /robot_pose, map→base_link TF + dem_costmap_converter : DEM GeoTIFF → nav_msgs/OccupancyGrid /map + global_planner : /goal_pose → /global_path (straight-line or A*) + vector_field_planner : /global_path → /cmd_vel (pure-pursuit) + mission_executive : state machine, action/service operator interface + +Topic graph +─────────── + gps_pose_publisher → TF map→base_link + dem_costmap_converter → /map + mission_executive → /nav_enabled, /goal_pose + global_planner → /global_path + vector_field_planner → /cmd_vel, ~/debug_markers """ import os @@ -50,8 +58,8 @@ def generate_launch_description(): nav_bringup_dir = get_package_share_directory('minimal_nav_bringup') nav_params_file = os.path.join(nav_bringup_dir, 'config', 'nav_params.yaml') - athena_map_dir = get_package_share_directory('dem_costmap') - dem_file = os.path.join(athena_map_dir, 'maps', 'north_site800m.tif') + dem_costmap_dir = get_package_share_directory('dem_costmap') + dem_file = os.path.join(dem_costmap_dir, 'maps', 'north_site800m.tif') # ── GPS hardware / sim bridge ───────────────────────────────────────────── gps_launch = IncludeLaunchDescription( @@ -74,11 +82,12 @@ def generate_launch_description(): parameters=[nav_params_file], ) - # ── map_node (DEM → OccupancyGrid) ──────────────────────────────────────── - map_node = Node( + # ── dem_costmap_converter (DEM GeoTIFF → OccupancyGrid /map) ───────────── + # dem_file_path is passed inline so the YAML can leave it blank by default. + dem_costmap_converter_node = Node( package='dem_costmap', executable='map_node', - name='map_node', + name='dem_costmap_converter', output='screen', parameters=[ nav_params_file, @@ -86,47 +95,43 @@ def generate_launch_description(): ], ) - # ── global_planner (Hybrid-A* via athena_smac_planner) ─────────────────── + # ── global_planner (/goal_pose → /global_path) ──────────────────────────── global_planner_node = Node( - package='athena_smac_planner', + package='global_planner', executable='global_planner_node', name='global_planner', output='screen', parameters=[nav_params_file], ) - # ── ackermann_mppi (MPPI local controller) ──────────────────────────────── - # /odom remapped to /odom/ground_truth (ZED SDK visual-inertial odometry) - ackermann_mppi_node = Node( - package='ackermann_mppi', - executable='ackermann_mppi_node', - name='mppi_runner', + # ── vector_field_planner (/global_path → /cmd_vel) ──────────────────────── + vector_field_planner_node = Node( + package='vector_field_planner', + executable='vector_field_planner_node', + name='vector_field_planner', output='screen', parameters=[nav_params_file], + #arguments=['--ros-args', '--log-level', 'debug'], remappings=[ ('/odom', '/odom/ground_truth'), ], ) # ── mission_executive (state machine) ──────────────────────────────────── - # /odom remapped to /odom/ground_truth for stop detection mission_executive_node = Node( package='mission_executive', executable='mission_executive_node', name='mission_executive', output='screen', parameters=[nav_params_file], - remappings=[ - ('/odom', '/odom/ground_truth'), - ], ) return LaunchDescription([ sim_arg, gps_launch, gps_pose_publisher_node, - map_node, + dem_costmap_converter_node, global_planner_node, - ackermann_mppi_node, + vector_field_planner_node, mission_executive_node, ]) diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml similarity index 72% rename from src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml rename to src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml index 71d3ccc1..052734d8 100644 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml +++ b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml @@ -3,7 +3,7 @@ minimal_nav_bringup 0.1.0 - Launch and configuration files for the full Athena navigation stack + Launch and configuration files for the Athena navigation stack (GPS-only, Nav2-free) UMD Loop Apache-2.0 @@ -11,11 +11,10 @@ gps_pose_publisher dem_costmap - athena_smac_planner - ackermann_mppi + global_planner + vector_field_planner mission_executive athena_gps - twist_stamper ament_cmake diff --git a/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt similarity index 85% rename from src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt rename to src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt index d3f37c8b..166a36af 100644 --- a/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt +++ b/src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt @@ -10,8 +10,11 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) find_package(msgs REQUIRED) add_executable(mission_executive_node src/mission_executive_node.cpp) @@ -21,8 +24,11 @@ target_link_libraries(mission_executive_node rclcpp_action::rclcpp_action ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} ${std_msgs_TARGETS} ${std_srvs_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros ${msgs_TARGETS} ) diff --git a/src/subsystems/minimal_navigation/mission_executive/package.xml b/src/subsystems/even_more_minimal_nav/mission_executive/package.xml similarity index 90% rename from src/subsystems/minimal_navigation/mission_executive/package.xml rename to src/subsystems/even_more_minimal_nav/mission_executive/package.xml index 49fb99be..358a2e7f 100644 --- a/src/subsystems/minimal_navigation/mission_executive/package.xml +++ b/src/subsystems/even_more_minimal_nav/mission_executive/package.xml @@ -13,8 +13,11 @@ rclcpp_action geometry_msgs nav_msgs + sensor_msgs std_msgs std_srvs + tf2 + tf2_ros msgs diff --git a/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp b/src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp similarity index 86% rename from src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp rename to src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp index 97c8e998..9f9594be 100644 --- a/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp +++ b/src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp @@ -25,8 +25,11 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "tf2/exceptions.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/string.hpp" #include "std_srvs/srv/set_bool.hpp" @@ -94,32 +97,55 @@ class MissionExecutive : public rclcpp::Node explicit MissionExecutive(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("mission_executive", options) { - declare_parameter("velocity_zero_threshold", 0.05); - declare_parameter("arrival_hold_time", 1.0); - declare_parameter("replan_distance_m", 3.0); + // Behaviour parameters + declare_parameter("stop_angular_vel_threshold", 0.05); // rad/s — IMU gyro magnitude + declare_parameter("arrival_hold_time", 1.0); + declare_parameter("replan_distance_m", 3.0); declare_parameter("latlon_to_enu_service", std::string("/gps_pose_publisher/latlon_to_enu")); - // §8: launch file should remap to /odom/ground_truth when using ZED ground-truth odometry - declare_parameter("odom_topic", std::string("/odom")); - velocity_zero_threshold_ = get_parameter("velocity_zero_threshold").as_double(); - arrival_hold_time_ = get_parameter("arrival_hold_time").as_double(); - replan_distance_m_ = get_parameter("replan_distance_m").as_double(); - const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); - const auto odom_topic = get_parameter("odom_topic").as_string(); + // Input topics + declare_parameter("imu_topic", std::string("/imu")); + declare_parameter("planner_event_topic", std::string("/planner_event")); + declare_parameter("global_path_topic", std::string("/global_path")); + + // Output topics + declare_parameter("goal_pose_topic", std::string("/goal_pose")); + declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); + declare_parameter("nav_mode_topic", std::string("/nav_mode")); + declare_parameter("active_target_topic", std::string("/active_target")); + declare_parameter("nav_status_topic", std::string("/nav_status")); + + stop_angular_vel_threshold_ = get_parameter("stop_angular_vel_threshold").as_double(); + arrival_hold_time_ = get_parameter("arrival_hold_time").as_double(); + replan_distance_m_ = get_parameter("replan_distance_m").as_double(); + const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); + + const auto imu_topic = get_parameter("imu_topic").as_string(); + const auto planner_event_topic = get_parameter("planner_event_topic").as_string(); + const auto global_path_topic = get_parameter("global_path_topic").as_string(); + const auto goal_pose_topic = get_parameter("goal_pose_topic").as_string(); + const auto nav_enabled_topic = get_parameter("nav_enabled_topic").as_string(); + const auto nav_mode_topic = get_parameter("nav_mode_topic").as_string(); + const auto active_target_topic = get_parameter("active_target_topic").as_string(); + const auto nav_status_topic = get_parameter("nav_status_topic").as_string(); // Reentrant group — allows the action server / service client to block // inside handleAccepted() while other callbacks still run on other threads. reentrant_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + // TF — used to look up map→base_link for arrival and cross-track checks + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + // ── Publishers ────────────────────────────────────────────────────────── - goal_pub_ = create_publisher("/goal_pose", 10); + goal_pub_ = create_publisher(goal_pose_topic, 10); nav_enabled_pub_ = create_publisher( - "/nav_enabled", rclcpp::QoS(1).reliable()); - nav_mode_pub_ = create_publisher("/nav_mode", 10); - active_target_pub_ = create_publisher("/active_target", 10); + nav_enabled_topic, rclcpp::QoS(1).reliable()); + nav_mode_pub_ = create_publisher(nav_mode_topic, 10); + active_target_pub_ = create_publisher(active_target_topic, 10); nav_status_pub_ = create_publisher( - "/nav_status", rclcpp::QoS(1).reliable()); + nav_status_topic, rclcpp::QoS(1).reliable()); // ── Service client ────────────────────────────────────────────────────── latlon_client_ = create_client( @@ -201,28 +227,21 @@ class MissionExecutive : public rclcpp::Node }); // ── Subscriptions ──────────────────────────────────────────────────────── - robot_pose_sub_ = create_subscription( - "/robot_pose", 10, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - std::lock_guard lk(mutex_); - robot_pose_ = *msg; - // Arrival check first: once we transition to ARRIVING, - // checkCrossTrackError's state guard prevents a spurious replan. - checkArrival(); - checkCrossTrackError(); - }); - odom_sub_ = create_subscription( - odom_topic, 10, - [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + // IMU-based stop detection: use angular velocity magnitude from the gyro. + // A stopped rover has near-zero rotation rate; stop_angular_vel_threshold (rad/s) + // controls how small it must be to count as "stopped". + imu_sub_ = create_subscription( + imu_topic, rclcpp::SensorDataQoS(), + [this](const sensor_msgs::msg::Imu::SharedPtr msg) { std::lock_guard lk(mutex_); - const auto & lin = msg->twist.twist.linear; - robot_speed_ = std::hypot(lin.x, lin.y); + const auto & w = msg->angular_velocity; + imu_angular_vel_ = std::sqrt(w.x * w.x + w.y * w.y + w.z * w.z); checkStopDetection(); }); planner_event_sub_ = create_subscription( - "/planner_event", 10, + planner_event_topic, 10, [this](const msgs::msg::PlannerEvent::SharedPtr msg) { std::lock_guard lk(mutex_); last_planner_event_ = msg->event; @@ -233,17 +252,21 @@ class MissionExecutive : public rclcpp::Node auto transient_qos = rclcpp::QoS(1).transient_local(); global_path_sub_ = create_subscription( - "/global_path", transient_qos, + global_path_topic, transient_qos, [this](const nav_msgs::msg::Path::SharedPtr msg) { std::lock_guard lk(mutex_); global_path_ = *msg; }); // ── 2 Hz status timer ──────────────────────────────────────────────────── + // Also drives arrival detection and replan checks; 2 Hz is fine for both. status_timer_ = create_wall_timer( 500ms, [this]() { std::lock_guard lk(mutex_); + refreshPoseFromTF(); // updates robot_pose_ for all checks below + checkArrival(); + checkCrossTrackError(); publishStatus(); checkActionResult(); }); @@ -329,7 +352,7 @@ class MissionExecutive : public rclcpp::Node s.distance_to_goal_m = distToGoal(); s.cross_track_error_m = computeCrossTrackError(); s.heading_error_rad = computeHeadingError(); - s.robot_speed_mps = robot_speed_; + s.robot_speed_mps = imu_angular_vel_; // IMU angular velocity magnitude (rad/s) s.is_return = is_return_; s.last_planner_event = last_planner_event_; nav_status_pub_->publish(s); @@ -398,6 +421,25 @@ class MissionExecutive : public rclcpp::Node // ── Subscription callbacks (mutex_ already held by caller) ──────────────── + // Look up map→base_link and cache the result in robot_pose_. + // Called from the status timer; silently does nothing if the TF is not yet available. + void refreshPoseFromTF() + { + try { + const auto tf = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero); + geometry_msgs::msg::PoseStamped p; + p.header.frame_id = "map"; + p.header.stamp = tf.header.stamp; + p.pose.position.x = tf.transform.translation.x; + p.pose.position.y = tf.transform.translation.y; + p.pose.position.z = tf.transform.translation.z; + p.pose.orientation = tf.transform.rotation; + robot_pose_ = p; + } catch (const tf2::TransformException &) { + // TF not yet available — robot_pose_ stays as-is (nullopt at startup) + } + } + void checkCrossTrackError() { if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; @@ -429,7 +471,7 @@ class MissionExecutive : public rclcpp::Node // Transition ARRIVING → STOPPED_AT_TARGET / STOPPED_AT_RETURN if (state_ != State::ARRIVING) return; - if (robot_speed_ < velocity_zero_threshold_) { + if (imu_angular_vel_ < stop_angular_vel_threshold_) { if (!low_speed_tracking_) { low_speed_start_ = now(); low_speed_tracking_ = true; @@ -734,13 +776,13 @@ class MissionExecutive : public rclcpp::Node // Sensor cache std::optional robot_pose_; std::optional global_path_; - double robot_speed_{0.0}; + double imu_angular_vel_{0.0}; // rad/s rclcpp::Time low_speed_start_{0, 0, RCL_ROS_TIME}; bool low_speed_tracking_{false}; uint8_t last_planner_event_{0}; // Parameters - double velocity_zero_threshold_{0.05}; + double stop_angular_vel_threshold_{0.05}; double arrival_hold_time_{1.0}; double replan_distance_m_{3.0}; @@ -760,8 +802,10 @@ class MissionExecutive : public rclcpp::Node rclcpp::Service::SharedPtr set_target_srv_; rclcpp::Service::SharedPtr teleop_srv_; - rclcpp::Subscription::SharedPtr robot_pose_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr planner_event_sub_; rclcpp::Subscription::SharedPtr global_path_sub_; diff --git a/src/subsystems/even_more_minimal_nav/plan.md b/src/subsystems/even_more_minimal_nav/plan.md new file mode 100644 index 00000000..c0bbdc7b --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/plan.md @@ -0,0 +1,101 @@ +### Nodes (updated) + +**`gps_pose_publisher`** — unchanged +**`mission_executive`** — unchanged +**`dem_costmap_converter`** — exists, no changes needed +**`global_planner`** — new, described below +**`vector_field_planner`** — unchanged + +--- + +## `dem_costmap_converter` role + +Runs independently at startup. Loads the GeoTIFF, computes a slope map via Sobel gradients, converts slope degrees to nav2-convention cost values (0–254), and republishes the `OccupancyGrid` at 1 Hz with transient_local QoS. The `global_planner` subscribes to this and treats it as a static cost lookup table — nothing else changes in `dem_costmap_converter`. + +--- + +## `global_planner` node + +**Inputs:** +- `/goal_pose` (`geometry_msgs/PoseStamped`) — from `mission_executive` +- `map → base_link` TF — start position +- `/map` (`nav_msgs/OccupancyGrid`, transient_local) — from `dem_costmap_converter`, optional + +**Output:** +- `/global_path` (`nav_msgs/Path`, transient_local) + +### Phase 1 (now): straight-line interpolation + +When a goal arrives and either `use_costmap` is false or no costmap has been received yet, sample evenly-spaced poses along the straight line from robot to goal at `path_resolution_m` spacing. Publish immediately. + +### Phase 2 (later): costmap-weighted A\* + +When `use_costmap: true` and a costmap has been received, run A\* on the occupancy grid. The cost to enter a cell is read directly from the grid value published by `dem_costmap_converter` — cells at 254 are treated as impassable walls, everything below scales traversal cost. A `slope_weight` parameter scales how much the cost value penalizes the path versus raw distance, giving field-tunable behavior between "shortest path" and "flattest path": + +$$g(cell) = \text{dist\_to\_neighbor} + \alpha \cdot \frac{\text{grid\_value}(cell)}{254}$$ + +where $\alpha$ is `slope_weight`. + +The fallback is always available: if A\* fails to find a path (e.g. goal is in a lethal cell), the planner logs a warning and falls back to straight-line, which `mission_executive` will eventually replan anyway via its cross-track error monitor. + +### Transition between phases + +The planner checks `use_costmap` and costmap availability on every new `/goal_pose`. Switching from Phase 1 to Phase 2 in the field requires only: +1. Setting `use_costmap: true` in the YAML +2. Setting `dem_file_path` and launching `dem_costmap_converter` + +No code changes, no interface changes. + +--- + +## Updated `nav_params.yaml` + +``` +dem_costmap_converter: dem_file_path, map_resolution, + max_passable_slope_degrees, + output_frame, origin_x, origin_y + +gps_pose_publisher: heading_topic, heading_offset_deg, + use_start_gate_ref, origin_lat/lon/alt + +mission_executive: velocity_zero_threshold, arrival_hold_time, + replan_distance_m, latlon_to_enu_service, odom_topic + +global_planner: path_resolution_m, + use_costmap, + slope_weight + +vector_field_planner: map_frame, base_frame, tf_timeout_s, + max_speed_mps, max_steering_angle_rad, + lookahead_dist_m, k_p_steering, + repulsion_gain, repulsion_cutoff_m, + goal_tolerance_m, publish_debug_markers +``` + +`elevation_map_path` is gone — that's now entirely owned by `dem_costmap_converter` via `dem_file_path`. The `global_planner` never touches the GeoTIFF directly. + +--- + +## Updated topic graph + +| Publisher | Topic | Subscriber(s) | +|---|---|---| +| `gps_pose_publisher` | TF `map→base_link` | `global_planner`, `vector_field_planner`, `mission_executive` | +| `dem_costmap_converter` | `/map` | `global_planner` | +| `mission_executive` | `/nav_enabled` | `vector_field_planner` | +| `mission_executive` | `/goal_pose` | `global_planner` | +| `global_planner` | `/global_path` | `mission_executive`, `vector_field_planner` | +| `vector_field_planner` | `/cmd_vel` | chassis driver | +| `vector_field_planner` | `~/debug_markers` | RViz | + +--- + +## What changes when moving to Phase 2 + +1. Launch `dem_costmap_converter` with `dem_file_path` pointing at your GeoTIFF +2. Set `use_costmap: true` and tune `slope_weight` in the YAML +3. Nothing else — same topics, same interfaces, `mission_executive` and `vector_field_planner` are completely unaware of the change + + +minimal_nav_bringup is an old package that oyu will have to update the params and launch file for, but a similar pattern should be followed. + diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt new file mode 100644 index 00000000..81a6864a --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.8) +project(vector_field_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) + +add_executable(vector_field_planner_node src/vector_field_planner_node.cpp) +target_compile_features(vector_field_planner_node PUBLIC cxx_std_17) +target_link_libraries(vector_field_planner_node + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_msgs_TARGETS} + ${visualization_msgs_TARGETS} + tf2_ros::tf2_ros + tf2::tf2 +) + +install(TARGETS vector_field_planner_node + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_pure_pursuit_sim + test/test_pure_pursuit_sim.cpp + ) + target_compile_features(test_pure_pursuit_sim PUBLIC cxx_std_17) +endif() + +ament_package() diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml b/src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml new file mode 100644 index 00000000..70bfa788 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml @@ -0,0 +1,28 @@ + + + + vector_field_planner + 0.1.0 + + Pure-pursuit path follower for the Athena navigation stack. + Subscribes to /global_path and /nav_enabled, publishes /cmd_vel. + + UMD Loop + Apache-2.0 + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + std_msgs + visualization_msgs + tf2_ros + tf2 + + ament_cmake_gtest + + + ament_cmake + + diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp b/src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp new file mode 100644 index 00000000..37c3c2a5 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp @@ -0,0 +1,433 @@ +// Copyright (c) 2025 UMD Loop +// +// 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. + +// vector_field_planner_node.cpp +// +// Pure-pursuit path follower for an Ackermann rover. +// +// Inputs: +// /global_path (nav_msgs/Path, transient_local) — from global_planner +// /nav_enabled (std_msgs/Bool, reliable) — from mission_executive +// TF map→base_link — current robot pose +// +// Outputs: +// /cmd_vel (geometry_msgs/Twist) — linear.x + angular.z +// ~/debug_markers (visualization_msgs/MarkerArray) — RViz visualization +// +// Algorithm: +// On each 20 Hz tick, if nav_enabled and a path is loaded: +// 1. Look up current pose via TF. +// 2. Stop if within goal_tolerance_m of the last path pose. +// 3. Find the lookahead point: walk forward from the closest path pose +// until a point is >= lookahead_dist_m from the robot. +// 4. Compute heading error from current yaw to the lookahead direction. +// 5. angular.z = k_p_steering * heading_error, clamped to +-max_steering_angle_rad. +// 6. linear.x = max_speed_mps (constant; slow-down near goal is left for a later pass). +// +// repulsion_gain / repulsion_cutoff_m are declared for completeness but not +// yet implemented — they will be used in a future phase when a local costmap +// or laser scan is available. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/bool.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/exceptions.h" + +class VectorFieldPlanner : public rclcpp::Node +{ +public: + VectorFieldPlanner() : Node("vector_field_planner") + { + declare_parameter("map_frame", std::string("map")); + declare_parameter("base_frame", std::string("base_link")); + declare_parameter("cmd_vel_topic", std::string("/front_ackermann_controller/reference")); + declare_parameter("tf_timeout_s", 0.1); + declare_parameter("max_speed_mps", 1.5); + declare_parameter("max_steering_angle_rad", 0.5); + declare_parameter("lookahead_dist_m", 3.0); + declare_parameter("k_p_steering", 1.5); + declare_parameter("repulsion_gain", 0.0); // reserved — Phase 2 + declare_parameter("repulsion_cutoff_m", 3.0); // reserved — Phase 2 + declare_parameter("goal_tolerance_m", 1.5); + declare_parameter("publish_debug_markers", true); + + map_frame_ = get_parameter("map_frame").as_string(); + base_frame_ = get_parameter("base_frame").as_string(); + cmd_vel_topic_ = get_parameter("cmd_vel_topic").as_string(); + tf_timeout_s_ = get_parameter("tf_timeout_s").as_double(); + max_speed_mps_ = get_parameter("max_speed_mps").as_double(); + max_steering_angle_rad_= get_parameter("max_steering_angle_rad").as_double(); + lookahead_dist_m_ = get_parameter("lookahead_dist_m").as_double(); + k_p_steering_ = get_parameter("k_p_steering").as_double(); + goal_tolerance_m_ = get_parameter("goal_tolerance_m").as_double(); + publish_debug_markers_ = get_parameter("publish_debug_markers").as_bool(); + + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + cmd_pub_ = create_publisher(cmd_vel_topic_, 10); + + if (publish_debug_markers_) { + marker_pub_ = create_publisher( + "~/debug_markers", 10); + } + + path_sub_ = create_subscription( + "/global_path", rclcpp::QoS(1).transient_local(), + [this](const nav_msgs::msg::Path::SharedPtr msg) { + path_ = msg; + last_closest_idx_ = std::numeric_limits::max(); + tick_count_ = 0; + consecutive_clamped_ = 0; + + // Log path quality — sparse paths (spacing > lookahead_dist_m) trigger the + // findLookahead-behind-robot bug documented in the unit tests. + double total_len = 0.0; + double max_gap = 0.0; + for (size_t i = 1; i < msg->poses.size(); ++i) { + const double gap = std::hypot( + msg->poses[i].pose.position.x - msg->poses[i-1].pose.position.x, + msg->poses[i].pose.position.y - msg->poses[i-1].pose.position.y); + total_len += gap; + max_gap = std::max(max_gap, gap); + } + RCLCPP_INFO(get_logger(), + "[PATH_RECV] poses=%zu total_len=%.1fm max_gap=%.2fm lookahead=%.1fm goal=(%.2f,%.2f)", + msg->poses.size(), total_len, max_gap, lookahead_dist_m_, + msg->poses.back().pose.position.x, msg->poses.back().pose.position.y); + if (max_gap > lookahead_dist_m_) { + RCLCPP_WARN(get_logger(), + "[PATH_RECV] max_gap %.2fm > lookahead %.1fm — findLookahead will return " + "behind-robot points once robot passes a sparse waypoint. " + "Reduce path_resolution_m or increase lookahead_dist_m.", + max_gap, lookahead_dist_m_); + } + }); + + nav_enabled_sub_ = create_subscription( + "/nav_enabled", rclcpp::QoS(1).reliable(), + [this](const std_msgs::msg::Bool::SharedPtr msg) { + const bool was_enabled = nav_enabled_; + nav_enabled_ = msg->data; + if (was_enabled && !nav_enabled_) { + RCLCPP_INFO(get_logger(), "[NAV_DISABLED] stopping after %u ticks", tick_count_); + tick_count_ = 0; + consecutive_clamped_ = 0; + stuck_ticks_ = 0; + publishStop(); + } else if (!was_enabled && nav_enabled_) { + RCLCPP_INFO(get_logger(), "[NAV_ENABLED] starting navigation"); + } + }); + + // 20 Hz control loop + timer_ = create_wall_timer( + std::chrono::milliseconds(50), + [this]() { controlLoop(); }); + + RCLCPP_INFO(get_logger(), "VectorFieldPlanner ready"); + } + +private: + // ── Control loop (20 Hz) ─────────────────────────────────────────────────── + + void controlLoop() + { + if (!nav_enabled_ || !path_ || path_->poses.empty()) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000, + "[NAV_IDLE] nav_enabled=%d has_path=%d", + static_cast(nav_enabled_), static_cast(path_ && !path_->poses.empty())); + return; + } + + ++tick_count_; + + // Look up current robot pose + geometry_msgs::msg::TransformStamped tf; + try { + tf = tf_buffer_->lookupTransform( + map_frame_, base_frame_, + tf2::TimePointZero, + tf2::durationFromSec(tf_timeout_s_)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "[TF_FAIL] %s→%s: %s (tick=%u)", + map_frame_.c_str(), base_frame_.c_str(), ex.what(), tick_count_); + return; + } + + const double rx = tf.transform.translation.x; + const double ry = tf.transform.translation.y; + + // Yaw from quaternion (z-rotation only — planar robot) + const auto & q = tf.transform.rotation; + const double yaw = std::atan2( + 2.0 * (q.w * q.z + q.x * q.y), + 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); + + // Check arrival at the last pose in the path + const auto & goal_pos = path_->poses.back().pose.position; + const double dist_to_goal = std::hypot(goal_pos.x - rx, goal_pos.y - ry); + if (dist_to_goal < goal_tolerance_m_) { + RCLCPP_INFO(get_logger(), + "[GOAL_REACHED] dist=%.3fm tol=%.3fm ticks=%u", + dist_to_goal, goal_tolerance_m_, tick_count_); + publishStop(); + return; + } + + // Find lookahead point + const size_t closest_idx = findClosestIndex(rx, ry); + const auto [lx, ly] = findLookahead(rx, ry, closest_idx); + + const double lookahead_dist = std::hypot(lx - rx, ly - ry); + + // Detect lookahead-behind-robot: dot(robot_forward, robot→lookahead) < 0 + const double fwd_dot = (lx - rx) * std::cos(yaw) + (ly - ry) * std::sin(yaw); + const bool lk_behind = fwd_dot < 0.0; + + if (lk_behind) { + RCLCPP_WARN(get_logger(), + "[LK_BEHIND] lookahead=(%.2f,%.2f) is BEHIND robot at (%.2f,%.2f) yaw=%.1fd " + "fwd_dot=%.3f closest_idx=%zu — robot will steer backward. " + "Likely cause: path spacing (%.2fm) > lookahead_dist (%.2fm).", + lx, ly, rx, ry, yaw * 180.0 / M_PI, fwd_dot, closest_idx, + [&]() -> double { + if (closest_idx + 1 < path_->poses.size()) { + return std::hypot( + path_->poses[closest_idx+1].pose.position.x - path_->poses[closest_idx].pose.position.x, + path_->poses[closest_idx+1].pose.position.y - path_->poses[closest_idx].pose.position.y); + } + return 0.0; + }(), + lookahead_dist_m_); + } + + // Warn if closest_idx has not advanced for many ticks (robot stuck or looping) + if (closest_idx == last_closest_idx_ && tick_count_ > 1) { + ++stuck_ticks_; + if (stuck_ticks_ == 20) { // 1 second at 20 Hz + RCLCPP_WARN(get_logger(), + "[STUCK] closest_idx=%zu has not advanced for %u ticks. " + "pos=(%.2f,%.2f) goal_dist=%.2fm. Robot may be off-path or looping.", + closest_idx, stuck_ticks_, rx, ry, dist_to_goal); + } + } else { + stuck_ticks_ = 0; + } + + // Heading error to the lookahead + double heading_err = std::atan2(ly - ry, lx - rx) - yaw; + // Normalise to [-pi, pi] + while (heading_err > M_PI) heading_err -= 2.0 * M_PI; + while (heading_err < -M_PI) heading_err += 2.0 * M_PI; + + // Proportional steering, clamped to physical limits + const double steering_unclamped = k_p_steering_ * heading_err; + const double steering = std::clamp( + steering_unclamped, + -max_steering_angle_rad_, max_steering_angle_rad_); + + const bool clamped = std::abs(steering_unclamped) > max_steering_angle_rad_; + if (clamped) { + ++consecutive_clamped_; + if (consecutive_clamped_ == 40) { // 2 seconds saturated + RCLCPP_WARN(get_logger(), + "[STEER_SAT] steering saturated for %u consecutive ticks (%.1f s). " + "err=%.1fd unclamped=%.3f max=%.3f. " + "Consider increasing max_steering_angle_rad or k_p_steering.", + consecutive_clamped_, + consecutive_clamped_ / 20.0, + heading_err * 180.0 / M_PI, + steering_unclamped, max_steering_angle_rad_); + } + } else { + consecutive_clamped_ = 0; + } + + // ── Single structured log line (4 Hz) — paste directly into an LLM ────── + // Fields: tick pos yaw goal_dist path_idx lookahead lk_fwd_dot heading_err steer flags + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 250, + "[NAV t=%u pos=(%.2f,%.2f) yaw=%.1fd goal=%.2fm idx=%zu/%zu " + "lk=(%.2f,%.2f) lk_dist=%.2fm fwd=%.2f err=%.1fd steer=%.3f%s%s]", + tick_count_, rx, ry, yaw * 180.0 / M_PI, dist_to_goal, + closest_idx, path_->poses.size() - 1, + lx, ly, lookahead_dist, fwd_dot, + heading_err * 180.0 / M_PI, steering, + clamped ? " CLAMPED" : "", + lk_behind ? " LK_BEHIND" : ""); + + geometry_msgs::msg::TwistStamped cmd; + cmd.header.stamp = now(); + cmd.header.frame_id = base_frame_; + cmd.twist.linear.x = max_speed_mps_; + cmd.twist.angular.z = steering; + cmd_pub_->publish(cmd); + + if (publish_debug_markers_) { + publishDebugMarkers(rx, ry, lx, ly); + } + } + + // ── Helpers ──────────────────────────────────────────────────────────────── + + // Index of the path pose closest to (rx, ry). + size_t findClosestIndex(double rx, double ry) + { + size_t best = 0; + double best_dist2 = std::numeric_limits::infinity(); + + for (size_t i = 0; i < path_->poses.size(); ++i) { + const double dx = path_->poses[i].pose.position.x - rx; + const double dy = path_->poses[i].pose.position.y - ry; + const double d2 = dx * dx + dy * dy; + if (d2 < best_dist2) { + best_dist2 = d2; + best = i; + } + } + + // Warn if the closest index jumped backwards — a common cause of + // oscillation on turns when the robot is equidistant to path segments + // before and after the apex. + if (best < last_closest_idx_ && last_closest_idx_ != std::numeric_limits::max()) { + RCLCPP_WARN(get_logger(), + "[closest_idx] BACKWARDS JUMP: %zu → %zu (dist=%.3f m) — " + "robot may be closer to an earlier path segment", + last_closest_idx_, best, std::sqrt(best_dist2)); + } + last_closest_idx_ = best; + return best; + } + + // Starting from closest_idx, walk forward along the path until a pose is + // >= lookahead_dist_m from the robot. If the path is shorter, return its + // last pose so the robot drives toward the goal even at close range. + std::pair findLookahead( + double rx, double ry, size_t closest_idx) const + { + for (size_t i = closest_idx; i < path_->poses.size(); ++i) { + const double dx = path_->poses[i].pose.position.x - rx; + const double dy = path_->poses[i].pose.position.y - ry; + if (std::hypot(dx, dy) >= lookahead_dist_m_) { + return {path_->poses[i].pose.position.x, + path_->poses[i].pose.position.y}; + } + } + const auto & last = path_->poses.back().pose.position; + return {last.x, last.y}; + } + + // Zero-velocity command — always safe to call. + void publishStop() + { + geometry_msgs::msg::TwistStamped stop_cmd; + stop_cmd.header.stamp = now(); + stop_cmd.header.frame_id = base_frame_; + cmd_pub_->publish(stop_cmd); + } + + // Two RViz markers: a sphere at the lookahead point, and a line from the + // robot to that point. + void publishDebugMarkers(double rx, double ry, double lx, double ly) + { + visualization_msgs::msg::MarkerArray arr; + const auto stamp = now(); + + // Lookahead sphere + visualization_msgs::msg::Marker sphere; + sphere.header.frame_id = map_frame_; + sphere.header.stamp = stamp; + sphere.ns = "vector_field_planner"; + sphere.id = 0; + sphere.type = visualization_msgs::msg::Marker::SPHERE; + sphere.action = visualization_msgs::msg::Marker::ADD; + sphere.pose.position.x = lx; + sphere.pose.position.y = ly; + sphere.pose.orientation.w = 1.0; + sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.5; + sphere.color.r = 1.0f; + sphere.color.g = 0.5f; + sphere.color.a = 1.0f; + arr.markers.push_back(sphere); + + // Line from robot to lookahead + visualization_msgs::msg::Marker line; + line.header = sphere.header; + line.ns = "vector_field_planner"; + line.id = 1; + line.type = visualization_msgs::msg::Marker::LINE_STRIP; + line.action = visualization_msgs::msg::Marker::ADD; + line.scale.x = 0.1; + line.color.r = 1.0f; + line.color.a = 1.0f; + geometry_msgs::msg::Point p1, p2; + p1.x = rx; p1.y = ry; + p2.x = lx; p2.y = ly; + line.points = {p1, p2}; + arr.markers.push_back(line); + + marker_pub_->publish(arr); + } + + // ── Parameters ───────────────────────────────────────────────────────────── + std::string map_frame_; + std::string base_frame_; + std::string cmd_vel_topic_; + double tf_timeout_s_; + double max_speed_mps_; + double max_steering_angle_rad_; + double lookahead_dist_m_; + double k_p_steering_; + double goal_tolerance_m_; + bool publish_debug_markers_; + + // ── TF ───────────────────────────────────────────────────────────────────── + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // ── ROS interfaces ────────────────────────────────────────────────────────── + rclcpp::Publisher::SharedPtr cmd_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Subscription::SharedPtr path_sub_; + rclcpp::Subscription::SharedPtr nav_enabled_sub_; + rclcpp::TimerBase::SharedPtr timer_; + + // ── State ─────────────────────────────────────────────────────────────────── + nav_msgs::msg::Path::SharedPtr path_; + bool nav_enabled_{false}; + size_t last_closest_idx_{std::numeric_limits::max()}; + unsigned int tick_count_{0}; + unsigned int consecutive_clamped_{0}; + unsigned int stuck_ticks_{0}; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/test/test_pure_pursuit_sim.cpp b/src/subsystems/even_more_minimal_nav/vector_field_planner/test/test_pure_pursuit_sim.cpp new file mode 100644 index 00000000..644ace6a --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/vector_field_planner/test/test_pure_pursuit_sim.cpp @@ -0,0 +1,660 @@ +// Copyright (c) 2025 UMD Loop +// +// 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. + +// test_pure_pursuit_sim.cpp +// +// Closed-loop simulation tests for the VectorFieldPlanner + FrontAckermannController pipeline. +// No ROS runtime required — all logic is replicated inline from the production sources: +// src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp +// src/subsystems/drive/drive_controllers/src/front_ackermann_controller.cpp +// +// Each replica mirrors the production code exactly. If you change the production code, +// update the corresponding replica here so the simulation stays faithful. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +// ============================================================================ +// Replica of VectorFieldPlanner logic +// ============================================================================ + +struct Pose2D { double x, y; }; + +struct SimPath { std::vector poses; }; + +/// Mirrors VectorFieldPlanner::findClosestIndex — full-scan, no index caching. +static size_t findClosestIndex(const SimPath & path, double rx, double ry) +{ + size_t best = 0; + double best_dist2 = std::numeric_limits::infinity(); + for (size_t i = 0; i < path.poses.size(); ++i) { + const double dx = path.poses[i].x - rx; + const double dy = path.poses[i].y - ry; + const double d2 = dx * dx + dy * dy; + if (d2 < best_dist2) { best_dist2 = d2; best = i; } + } + return best; +} + +/// Mirrors VectorFieldPlanner::findLookahead — scans forward from closest_idx. +static std::pair findLookahead( + const SimPath & path, double rx, double ry, + size_t closest_idx, double lookahead_dist_m) +{ + for (size_t i = closest_idx; i < path.poses.size(); ++i) { + const double dx = path.poses[i].x - rx; + const double dy = path.poses[i].y - ry; + if (std::hypot(dx, dy) >= lookahead_dist_m) { + return {path.poses[i].x, path.poses[i].y}; + } + } + return {path.poses.back().x, path.poses.back().y}; +} + +struct PlannerParams { + double lookahead_dist_m = 3.0; + double k_p_steering = 1.5; + double max_steering_angle_rad = 0.5; + double max_speed_mps = 0.8; + double goal_tolerance_m = 1.5; +}; + +/// Mirrors VectorFieldPlanner::controlLoop (pure math portion — no TF). +/// Returns {linear_x, angular_z} that would be published as TwistStamped. +static std::pair plannerStep( + const SimPath & path, double rx, double ry, double yaw, + const PlannerParams & p, bool & goal_reached) +{ + const auto & goal_pos = path.poses.back(); + const double dist_to_goal = std::hypot(goal_pos.x - rx, goal_pos.y - ry); + if (dist_to_goal < p.goal_tolerance_m) { + goal_reached = true; + return {0.0, 0.0}; + } + goal_reached = false; + + const size_t closest_idx = findClosestIndex(path, rx, ry); + const auto [lx, ly] = findLookahead(path, rx, ry, closest_idx, p.lookahead_dist_m); + + double heading_err = std::atan2(ly - ry, lx - rx) - yaw; + while (heading_err > M_PI) heading_err -= 2.0 * M_PI; + while (heading_err < -M_PI) heading_err += 2.0 * M_PI; + + const double steering = std::clamp( + p.k_p_steering * heading_err, + -p.max_steering_angle_rad, p.max_steering_angle_rad); + + return {p.max_speed_mps, steering}; +} + +// ============================================================================ +// Replica of FrontAckermannController logic +// ============================================================================ + +struct AckermannParams { + double wheelbase = 0.8382; // from front_ackermann_controller.yaml + double track_width = 0.6604; + double wheel_radius = 0.254; + double max_speed = 1.27; + double max_steer_angle = 0.785; +}; + +struct WheelState { + double fl_steer = 0.0, fr_steer = 0.0; + double fl_vel = 0.0, fr_vel = 0.0, rl_vel = 0.0, rr_vel = 0.0; + // Derived robot-level velocities (open-loop odom) + double linear_vel = 0.0; + double angular_vel = 0.0; +}; + +/// Mirrors FrontAckermannController::update (the wheel command + odom portion). +/// linear_vel_cmd = twist.linear.x, angular_z_cmd = twist.angular.z. +/// +/// KEY: angular_z_cmd is treated by the controller as omega (rad/s) in the +/// bicycle model: steer_angle = atan(omega * L / v) +/// The planner, however, outputs angular.z = k_p * heading_err intending it +/// to represent a desired steering angle in radians. +static WheelState controllerStep( + double linear_vel_cmd, double angular_z_cmd, + const AckermannParams & p) +{ + WheelState ws; + + linear_vel_cmd = std::clamp(linear_vel_cmd, -p.max_speed, p.max_speed); + + double steer_cmd = 0.0; + if (std::abs(linear_vel_cmd) > 1e-4) { + // Controller interprets angular_z as omega, converts to steer angle + steer_cmd = std::atan(angular_z_cmd * p.wheelbase / linear_vel_cmd); + } + steer_cmd = std::clamp(steer_cmd, -p.max_steer_angle, p.max_steer_angle); + + ws.fl_vel = ws.fr_vel = ws.rl_vel = ws.rr_vel = linear_vel_cmd; + + if (std::abs(steer_cmd) > 1e-4) { + const double turn_radius = p.wheelbase / std::tan(steer_cmd); + double angular_vel = std::abs(linear_vel_cmd) / std::abs(turn_radius); + if (linear_vel_cmd < 0.0) angular_vel = -angular_vel; + + const double inner_angle = std::atan(p.wheelbase / (std::abs(turn_radius) - p.track_width / 2.0)); + const double outer_angle = std::atan(p.wheelbase / (std::abs(turn_radius) + p.track_width / 2.0)); + + const double inner_rear = angular_vel * (std::abs(turn_radius) - p.track_width / 2.0); + const double outer_rear = angular_vel * (std::abs(turn_radius) + p.track_width / 2.0); + const double inner_front = angular_vel * std::sqrt( + p.wheelbase * p.wheelbase + + std::pow(std::abs(turn_radius) - p.track_width / 2.0, 2)); + const double outer_front = angular_vel * std::sqrt( + p.wheelbase * p.wheelbase + + std::pow(std::abs(turn_radius) + p.track_width / 2.0, 2)); + + if (steer_cmd > 0.0) { // LEFT TURN: left wheel is inner + ws.fl_steer = inner_angle; ws.fr_steer = outer_angle; + ws.fl_vel = inner_front; ws.fr_vel = outer_front; + ws.rl_vel = inner_rear; ws.rr_vel = outer_rear; + } else { // RIGHT TURN: right wheel is inner + ws.fl_steer = -outer_angle; ws.fr_steer = -inner_angle; + ws.fl_vel = outer_front; ws.fr_vel = inner_front; + ws.rl_vel = outer_rear; ws.rr_vel = inner_rear; + } + } + + // Open-loop odometry (mirrors the open_loop branch in the controller) + ws.linear_vel = (ws.rl_vel + ws.rr_vel) / 2.0; + const double avg_steer = (ws.fl_steer + ws.fr_steer) / 2.0; + if (std::abs(p.wheelbase) > 1e-6) { + ws.angular_vel = ws.linear_vel * std::tan(avg_steer) / p.wheelbase; + } + + return ws; +} + +// ============================================================================ +// Path generator +// ============================================================================ + +/// Straight-line path with poses spaced resolution_m apart. +static SimPath makeStraightPath( + double x0, double y0, double x1, double y1, double resolution_m = 1.0) +{ + SimPath path; + const double dx = x1 - x0, dy = y1 - y0; + const double dist = std::hypot(dx, dy); + const int n = static_cast(std::ceil(dist / resolution_m)) + 1; + for (int i = 0; i < n; ++i) { + const double t = std::min(1.0, (i * resolution_m) / dist); + path.poses.push_back({x0 + t * dx, y0 + t * dy}); + } + path.poses.back() = {x1, y1}; // ensure exact endpoint + return path; +} + +// ============================================================================ +// Helper: run full closed-loop simulation and capture a trajectory +// ============================================================================ + +struct SimResult { + bool goal_reached = false; + int steps = 0; + double final_x = 0.0; + double final_y = 0.0; + double final_yaw = 0.0; + double final_dist_to_goal = 0.0; + std::vector> trajectory; // (x, y, yaw) +}; + +static SimResult runSimulation( + const SimPath & path, double start_x, double start_y, double start_yaw, + const PlannerParams & planner, const AckermannParams & ackermann, + double dt = 0.05, int max_steps = 2000) +{ + SimResult result; + double x = start_x, y = start_y, yaw = start_yaw; + + for (int step = 0; step < max_steps; ++step) { + result.trajectory.emplace_back(x, y, yaw); + + bool goal_reached = false; + const auto [lin_x, ang_z] = plannerStep(path, x, y, yaw, planner, goal_reached); + + if (goal_reached) { + result.goal_reached = true; + result.steps = step; + break; + } + + // Controller converts (lin_x, ang_z) → wheel commands + const WheelState ws = controllerStep(lin_x, ang_z, ackermann); + + // Integrate pose with midpoint method (mirrors controller odom) + const double heading_mid = yaw + ws.angular_vel * dt / 2.0; + x += ws.linear_vel * std::cos(heading_mid) * dt; + y += ws.linear_vel * std::sin(heading_mid) * dt; + yaw += ws.angular_vel * dt; + while (yaw > M_PI) yaw -= 2.0 * M_PI; + while (yaw < -M_PI) yaw += 2.0 * M_PI; + + result.steps = step + 1; + } + + result.final_x = x; + result.final_y = y; + result.final_yaw = yaw; + result.final_dist_to_goal = std::hypot( + path.poses.back().x - x, path.poses.back().y - y); + return result; +} + +// ============================================================================ +// Unit tests — planner logic +// ============================================================================ + +TEST(FindClosestIndex, StartOfPath) +{ + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + EXPECT_EQ(findClosestIndex(path, 0.0, 0.0), 0u); +} + +TEST(FindClosestIndex, AdvancesAsRobotMoves) +{ + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + // Path direction: (10,-10)/sqrt(200) = (0.707,-0.707) + // A robot at (3.5, -3.5) should map to the closest waypoint index ~5 + const size_t idx = findClosestIndex(path, 3.5, -3.5); + EXPECT_GE(idx, 4u); + EXPECT_LE(idx, 6u); +} + +TEST(FindClosestIndex, NearGoal) +{ + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const size_t last = path.poses.size() - 1; + EXPECT_EQ(findClosestIndex(path, 10.0, -10.0), last); +} + +// --------------------------------------------------------------------------- + +TEST(FindLookahead, ReturnsPointAtLeastLookaheadAway) +{ + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const double lookahead = 3.0; + const auto [lx, ly] = findLookahead(path, 0.0, 0.0, 0, lookahead); + EXPECT_GE(std::hypot(lx, ly), lookahead) + << "Lookahead point must be at least " << lookahead << " m from robot"; +} + +TEST(FindLookahead, DirectionIsSoutheast) +{ + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const auto [lx, ly] = findLookahead(path, 0.0, 0.0, 0, 3.0); + EXPECT_GT(lx, 0.0) << "Lookahead x should be positive (east)"; + EXPECT_LT(ly, 0.0) << "Lookahead y should be negative (south)"; +} + +TEST(FindLookahead, FallsBackToGoalWhenPathShorterThanLookahead) +{ + // Robot very close to the goal — remaining path < lookahead distance + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const size_t near_end = path.poses.size() - 2; + const double rx = path.poses[near_end].x; + const double ry = path.poses[near_end].y; + + const auto [lx, ly] = findLookahead(path, rx, ry, near_end, 3.0); + EXPECT_NEAR(lx, 10.0, 1e-6) << "Should fall back to goal x"; + EXPECT_NEAR(ly, -10.0, 1e-6) << "Should fall back to goal y"; +} + +TEST(FindLookahead, DoesNotSearchBeforeClosestIndex) +{ + // If closest_idx is near the end, the lookahead must not wrap around + // to the beginning of the path. + const SimPath path = makeStraightPath(0.0, 0.0, 20.0, 0.0, 1.0); // east-only path + // Robot is at x=18, y=0 — very close to end + const size_t closest = findClosestIndex(path, 18.0, 0.0); + const auto [lx, ly] = findLookahead(path, 18.0, 0.0, closest, 3.0); + EXPECT_GE(lx, 18.0) << "Lookahead must be ahead of the robot, not behind"; +} + +// BUG DOCUMENTED: findLookahead returns a point BEHIND the robot when path spacing +// exceeds lookahead_dist_m. +// +// Root cause: findLookahead scans from closest_idx and returns the first pose that is +// >= lookahead_dist_m from the robot. When path spacing > lookahead_dist_m (e.g. a +// 2-pose path), closest_idx=0 may be the START pose that the robot has already passed. +// If the robot is now > lookahead_dist_m from pose[0], the scan immediately returns +// pose[0] — which is behind the robot. The robot then steers backward, circles, +// and never reaches the goal. +// +// This does NOT trigger with the production path (path_resolution_m=1.0, lookahead=3.0) +// because the 1 m-spaced poses ensure closest_idx quickly advances past the robot. +// It WOULD trigger if the global planner ever emits a coarse path (spacing > 3 m) +// or if lookahead_dist_m is reduced below the path spacing. +TEST(FindLookahead, BugSparsePathReturnsPointBehindRobot) +{ + // Two-pose path: start=(0,0), goal=(10,-10). Spacing ≈ 14.1 m >> lookahead=3.0 m. + SimPath path; + path.poses.push_back({0.0, 0.0}); + path.poses.push_back({10.0, -10.0}); + + // Robot has moved ~4 m along the path direction (past lookahead radius from pose[0]). + const double rx = 4.0 * 0.7071, ry = 4.0 * -0.7071; // (2.83, -2.83) + + // closest_idx is 0 (robot is still in the first half of the path) + const size_t closest = findClosestIndex(path, rx, ry); + EXPECT_EQ(closest, 0u); + + // findLookahead starts at i=0 (pose (0,0)). + // dist from robot (2.83,-2.83) to pose[0] (0,0) = 4.0 m >= lookahead 3.0 m. + // So it immediately returns (0,0) — the point the robot already passed. + const auto [lx, ly] = findLookahead(path, rx, ry, closest, 3.0); + + // This assertion DOCUMENTS the bug: the returned lookahead is pose[0] = (0,0), + // which is BEHIND the robot. + EXPECT_NEAR(lx, 0.0, 1e-9) << "BUG: findLookahead returned the already-passed start pose"; + EXPECT_NEAR(ly, 0.0, 1e-9) << "BUG: findLookahead returned the already-passed start pose"; + + // Consequence: the heading error points backward, causing the robot to u-turn. + const double yaw = std::atan2(-1.0, 1.0); // robot facing SE (correct heading) + double heading_err = std::atan2(ly - ry, lx - rx) - yaw; + while (heading_err > M_PI) heading_err -= 2.0 * M_PI; + while (heading_err < -M_PI) heading_err += 2.0 * M_PI; + + // heading_err should be ~±π (opposite direction) — the robot steers away from goal + EXPECT_GT(std::abs(heading_err), M_PI / 2.0) + << "BUG consequence: heading error is > 90 deg because lookahead is behind the robot"; +} + +// --------------------------------------------------------------------------- + +TEST(HeadingError, DiagonalGoalFromEastFacing) +{ + // Robot at origin facing east (yaw=0); lookahead at (1,-1) → bearing = -45° + const double rx = 0.0, ry = 0.0, yaw = 0.0; + const double lx = 1.0, ly = -1.0; + double err = std::atan2(ly - ry, lx - rx) - yaw; + while (err > M_PI) err -= 2 * M_PI; + while (err < -M_PI) err += 2 * M_PI; + EXPECT_NEAR(err, -M_PI / 4.0, 1e-9); +} + +TEST(HeadingError, NormalisedThroughPi) +{ + // Facing west (-π), lookahead directly east → raw error ≈ +2π, must wrap to 0 + const double yaw = M_PI; // facing west + const double lx = 1.0, ly = 0.0; // lookahead due east → bearing = 0 + double err = std::atan2(ly, lx) - yaw; + while (err > M_PI) err -= 2 * M_PI; + while (err < -M_PI) err += 2 * M_PI; + EXPECT_NEAR(std::abs(err), M_PI, 1e-9); // ±π, both are correct +} + +// ============================================================================ +// Unit tests — controller steer conversion +// ============================================================================ + +// This test documents the planner/controller interface mismatch. +// The planner sends angular.z = k_p * heading_err, intending it as a steering angle. +// The controller converts it via: steer = atan(angular_z * L / v), as if it's omega. +// The two interpretations diverge for non-trivial angular_z values. +TEST(AckermannController, AngularZInterpretationMismatch) +{ + const AckermannParams p; + const double linear_v = 0.8; // planner max_speed_mps from nav_params.yaml + + // At max steering command from the planner (angular_z = 0.5 rad) + const double angular_z_max = 0.5; // planner clamps here + // Controller will compute: + const double steer_from_controller = std::atan(angular_z_max * p.wheelbase / linear_v); + + // If angular_z were truly a steer angle, the controller should just pass it through. + // Instead, atan(0.5 * 0.8382 / 0.8) ≈ 0.484 rad — not 0.5. + // The discrepancy grows at higher speeds (lower ratio L/v shrinks the effective steer). + EXPECT_NE(steer_from_controller, angular_z_max) + << "Controller does not pass angular_z through as a steer angle"; + + // Document the actual value so regressions are caught + EXPECT_NEAR(steer_from_controller, std::atan(0.5 * 0.8382 / 0.8), 1e-9); +} + +TEST(AckermannController, HighSpeedReducesEffectiveSteering) +{ + // At high speed the atan(omega*L/v) formula yields a smaller steer angle, + // so the robot under-steers relative to what the planner intended. + const AckermannParams p; + const double angular_z = 0.5; + + const double steer_slow = std::atan(angular_z * p.wheelbase / 0.3); // slow + const double steer_fast = std::atan(angular_z * p.wheelbase / 1.2); // fast + + EXPECT_GT(steer_slow, steer_fast) + << "Effective steering should decrease as speed increases for the same angular_z"; +} + +TEST(AckermannController, SteerSignConvention) +{ + // Negative angular_z → right turn → negative wheel angles + const AckermannParams p; + const WheelState ws = controllerStep(0.8, -0.5, p); + EXPECT_LT(ws.fl_steer, 0.0) << "Front-left steer should be negative for right turn"; + EXPECT_LT(ws.fr_steer, 0.0) << "Front-right steer should be negative for right turn"; + EXPECT_LT(ws.angular_vel, 0.0) << "Robot angular velocity should be negative for right turn"; +} + +TEST(AckermannController, ZeroAngularZGoeseStraight) +{ + const AckermannParams p; + const WheelState ws = controllerStep(0.8, 0.0, p); + EXPECT_NEAR(ws.fl_steer, 0.0, 1e-9); + EXPECT_NEAR(ws.fr_steer, 0.0, 1e-9); + EXPECT_NEAR(ws.angular_vel, 0.0, 1e-9); + EXPECT_NEAR(ws.linear_vel, 0.8, 1e-9); +} + +// ============================================================================ +// Closed-loop simulation: (0,0) → (10,-10) +// ============================================================================ + +// Default parameters, drawn directly from nav_params.yaml and front_ackermann_controller.yaml +static PlannerParams defaultPlannerParams() +{ + PlannerParams p; + p.lookahead_dist_m = 3.0; + p.k_p_steering = 1.5; + p.max_steering_angle_rad = 0.5; + p.max_speed_mps = 0.8; + p.goal_tolerance_m = 1.5; + return p; +} + +static AckermannParams defaultAckermannParams() +{ + return AckermannParams{}; // all defaults from struct definition +} + +TEST(ClosedLoopSim, GoalIsReached) +{ + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const SimResult result = runSimulation( + path, 0.0, 0.0, 0.0, + defaultPlannerParams(), defaultAckermannParams()); + + EXPECT_TRUE(result.goal_reached) + << "Goal (10,-10) was never reached after " << result.steps << " steps.\n" + << "Final position: (" << result.final_x << ", " << result.final_y << ")\n" + << "Distance to goal: " << result.final_dist_to_goal << " m\n" + << "Goal tolerance: " << defaultPlannerParams().goal_tolerance_m << " m"; +} + +TEST(ClosedLoopSim, GoalReachedWithinReasonableTime) +{ + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const SimResult result = runSimulation( + path, 0.0, 0.0, 0.0, + defaultPlannerParams(), defaultAckermannParams()); + + // At 0.8 m/s the Euclidean distance is ~14.1 m → ideal time ~17.7 s → ~354 steps. + // Allow 3x margin for curvature, but not infinite spinning. + const int reasonable_steps = 1100; // ~55 s at 20 Hz + EXPECT_LE(result.steps, reasonable_steps) + << "Simulation used " << result.steps << " steps (>" << reasonable_steps + << "). Robot may be oscillating or not progressing."; +} + +TEST(ClosedLoopSim, RobotSteerRightInitially) +{ + // (0,0) → (10,-10): the robot starts facing east and must turn right (south-east). + // Verify the very first command steers the robot in the correct direction. + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const PlannerParams p = defaultPlannerParams(); + + bool goal_reached = false; + const auto [lin_x, ang_z] = plannerStep(path, 0.0, 0.0, 0.0, p, goal_reached); + + EXPECT_FALSE(goal_reached); + EXPECT_GT(lin_x, 0.0) << "Speed should be positive"; + EXPECT_LT(ang_z, 0.0) << "angular.z should be negative (right turn toward (10,-10))"; + + // Verify the controller also produces a right-turning angular velocity + const WheelState ws = controllerStep(lin_x, ang_z, defaultAckermannParams()); + EXPECT_LT(ws.angular_vel, 0.0) + << "Controller must produce negative angular velocity for initial right turn"; +} + +TEST(ClosedLoopSim, LookaheadPointIsAlwaysForward) +{ + // During the entire trajectory, the lookahead point should always be ahead of + // (or at) the robot's current path progress — never behind. + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const PlannerParams p = defaultPlannerParams(); + const AckermannParams a = defaultAckermannParams(); + const SimResult result = runSimulation(path, 0.0, 0.0, 0.0, p, a); + + // Replay trajectory and verify the lookahead is never further BEHIND the path + // than the closest index. + size_t prev_closest = 0; + for (const auto & [x, y, yaw] : result.trajectory) { + const size_t closest = findClosestIndex(path, x, y); + const auto [lx, ly] = findLookahead(path, x, y, closest, p.lookahead_dist_m); + + // The lookahead's closest index must be >= the robot's current closest index + const size_t lookahead_path_idx = findClosestIndex(path, lx, ly); + EXPECT_GE(lookahead_path_idx, closest) + << "Lookahead point is behind the robot's current closest path index at (" + << x << ", " << y << ")"; + + prev_closest = closest; + } + (void)prev_closest; +} + +TEST(ClosedLoopSim, DistanceToGoalMonotonicallyDecreasesOnAverage) +{ + // The robot must make net progress toward the goal. We check that the + // distance to goal at the midpoint of the simulation is less than at the start, + // and at the end is less than at the midpoint. + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const SimResult result = runSimulation( + path, 0.0, 0.0, 0.0, + defaultPlannerParams(), defaultAckermannParams()); + + if (result.trajectory.size() < 3) { + GTEST_SKIP() << "Trajectory too short to test monotonicity"; + } + + const auto distToGoal = [&](size_t idx) { + const auto & [x, y, yaw] = result.trajectory[idx]; + return std::hypot(10.0 - x, -10.0 - y); + }; + + const size_t quarter = result.trajectory.size() / 4; + const size_t mid = result.trajectory.size() / 2; + const size_t thr = 3 * result.trajectory.size() / 4; + + EXPECT_LT(distToGoal(quarter), distToGoal(0)) + << "Robot should be closer to goal at 25% of trajectory than at start"; + EXPECT_LT(distToGoal(mid), distToGoal(quarter)) + << "Robot should be closer to goal at 50% than at 25%"; + EXPECT_LT(distToGoal(thr), distToGoal(mid)) + << "Robot should be closer to goal at 75% than at 50%"; +} + +// ============================================================================ +// Edge cases +// ============================================================================ + +TEST(EdgeCase, AlreadyAtGoal) +{ + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const PlannerParams p = defaultPlannerParams(); + + bool goal_reached = false; + const auto [lin_x, ang_z] = plannerStep(path, 10.0, -10.0, 0.0, p, goal_reached); + EXPECT_TRUE(goal_reached); + EXPECT_NEAR(lin_x, 0.0, 1e-9); + EXPECT_NEAR(ang_z, 0.0, 1e-9); +} + +TEST(EdgeCase, SinglePosePath) +{ + SimPath path; + path.poses.push_back({5.0, -5.0}); + + const PlannerParams p = defaultPlannerParams(); + bool goal_reached = false; + // Robot well outside tolerance + const auto [lin_x, ang_z] = plannerStep(path, 0.0, 0.0, 0.0, p, goal_reached); + EXPECT_FALSE(goal_reached); + EXPECT_GT(lin_x, 0.0); +} + +// BUG REGRESSION: a 2-pose path causes the robot to loop and never reach the goal. +// See FindLookahead.BugSparsePathReturnsPointBehindRobot for the root cause. +// Once the planner bug is fixed this test should be inverted to EXPECT true. +TEST(EdgeCase, PathWithOnlyTwoPosesFailsDueToLookaheadBug) +{ + SimPath path; + path.poses.push_back({0.0, 0.0}); + path.poses.push_back({10.0, -10.0}); + + const SimResult result = runSimulation( + path, 0.0, 0.0, 0.0, + defaultPlannerParams(), defaultAckermannParams()); + + // This currently FAILS to reach the goal because findLookahead returns the + // already-passed start pose once the robot is > lookahead_dist_m from it. + // When this is fixed, flip the expectation to EXPECT_TRUE. + EXPECT_FALSE(result.goal_reached) + << "BUG FIXED: update this test to EXPECT_TRUE(goal_reached)"; +} + +TEST(EdgeCase, RobotStartsFacingAwayFromGoal) +{ + // Robot faces west (yaw = ±π); goal is at (10,-10) (south-east). + // The controller must still steer the robot around and eventually reach the goal. + const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + const SimResult result = runSimulation( + path, 0.0, 0.0, M_PI, // facing west + defaultPlannerParams(), defaultAckermannParams()); + + EXPECT_TRUE(result.goal_reached) + << "Goal not reached when starting backward. Dist=" << result.final_dist_to_goal; +} diff --git a/src/subsystems/minimal_navigation/PLAN.md b/src/subsystems/minimal_navigation/PLAN.md deleted file mode 100644 index 09d8b107..00000000 --- a/src/subsystems/minimal_navigation/PLAN.md +++ /dev/null @@ -1,508 +0,0 @@ -# Athena Navigation Stack — Implementation Spec - -A GPS-only, Nav2-free navigation system in a single ROS 2 package. No IMU-GNSS fusion — localization is GPS + compass heading only. - - ---- - -## 1. Architecture Overview - -Four nodes form the runtime pipeline: - -``` -gps_pose_publisher → mission_executive → global_planner → mppi_runner - │ │ │ │ - map→base_link TF state machine Hybrid-A* MPPI control - /robot_pose /goal_pose /global_path /cmd_vel -``` - -**TF tree:** `map → base_link` (no `odom` frame — see §1.2). - -### 1.1 Existing Code - -| Source | Role | Required Changes | -|--------|------|------------------| -| `ackermann_mppi/` | Local planner / path follower | Rename `/plan` → `/global_path`; add `/nav_enabled` guard; add costmap debug publisher | -| `smac/` | Global planner (Hybrid-A\*) | Rename package to `athena_smac_planner`; integrate `athena_map` costmap input | -| `athena_map/` | Publishes `nav_msgs/OccupancyGrid` on `/map` | Ensure `frame_id: "map"`, `transient_local` QoS, and resolution matches `global_costmap` (0.5 m). We control this node — adapt as needed for clean integration. | -| `athena_planner/` | BT nodes (ArUco pose extraction, etc.) | Reuse in Phase 2 | -| `aruco_detection/` | ArUco refinement (~80% complete) | Wire in Phase 2c | - -**Do not use `localizer/`** — IMU-GNSS fusion is explicitly excluded from the localization path. - -### 1.2 TF Convention — Intentional `odom` Omission - -REP-105 expects `map → odom → base_link`. This stack uses `map → base_link` directly because there is no fused odometry source suitable for the `odom` frame. `gps_pose_publisher` broadcasts this transform. - -The ZED SDK provides `/odom` (`nav_msgs/Odometry`) with visual-inertial velocity estimates. This is used **only** for measured velocity in stop detection (§3.2.4) — it does not participate in localization or TF. - -### 1.3 Package Rename: `smac/` → `athena_smac_planner` - -The vendored `smac/` package declared `nav2_smac_planner` in `package.xml`, colliding with the system-installed `ros-$ROS_DISTRO-nav2-smac-planner`. Fix: rename to `athena_smac_planner` in both `package.xml` and `CMakeLists.txt`. - ---- - -## 2. Operator Interface - -All operator commands use ROS 2 services and actions (the legacy `OperatorCmd.msg` topic is retired). - -| Command | Interface | Type | Rationale | -|---------|-----------|------|-----------| -| `GO_TO` | Action | `NavigateToTarget` | Long-running; preemptable; provides distance feedback | -| `RETURN` | Action | `NavigateToTarget` | Same semantics as `GO_TO` (with `is_return=true`) | -| `ABORT` | Service | `std_srvs/Trigger` | Instant; needs confirmation | -| `SET_TARGET` | Service | `SetTarget` | Pre-register a target by ID for later navigation | -| `TELEOP_ON` / `TELEOP_OFF` | Service | `std_srvs/SetBool` | Needs ack; returns success + optional message | - -**Intended workflow:** Use `SetTarget` to pre-load named targets (with coordinates, type, and tolerance) into `mission_executive`'s target registry. Then use `NavigateToTarget` with just a `target_id` (+ `is_return` flag) to begin navigation. `NavigateToTarget` also accepts inline coordinates for ad-hoc goals that don't need pre-registration — when `target_id` is empty, the coordinates in the goal message are used directly. - -All custom types live in the `navigation_msgs` package (see §6). - ---- - -## 3. Node Specifications - -### 3.1 `gps_pose_publisher` - -Converts raw GPS + heading into a `map`-frame pose and broadcasts `map → base_link`. - -**Subscriptions:** - -| Topic | Type | Notes | -|-------|------|-------| -| `/gps/fix` | `sensor_msgs/NavSatFix` | | -| `/gps/heading` (configurable) | `std_msgs/Float64` | Input always in degrees. Internally converted to ENU radians (0 = East, CCW+). | - -**Publications:** - -| Topic | Type | Notes | -|-------|------|-------| -| `/robot_pose` | `geometry_msgs/PoseStamped` | Frame: `map` | -| `/gps_status` | `std_msgs/String` | Fix quality + heading source each update | -| TF: `map → base_link` | | | - -**Services:** - -| Service | Type | Purpose | -|---------|------|---------| -| `~/latlon_to_enu` | `LatLonToENU` | Centralizes WGS84 → ENU projection | - -**Core logic:** - -- WGS84 → ENU via `GeographicLib::LocalCartesian`: - ```cpp - GeographicLib::LocalCartesian proj(lat0, lon0, alt0); - proj.Forward(lat, lon, alt, e, n, u); - ``` -- Origin set from first fix, or from `/start_gate_ref` if `use_start_gate_ref: true`. -- Heading conversion: input degrees → ENU radians, then `tf2::Quaternion::setRPY(0, 0, heading_rad)`. - ---- - -### 3.2 `mission_executive` - -Operator-driven state machine managing target sequencing, abort/return, mode switching, and arrival detection. - -#### 3.2.1 States - -`IDLE`, `NAVIGATING`, `ARRIVING`, `STOPPED_AT_TARGET`, `ABORTING`, `RETURNING`, `STOPPED_AT_RETURN`, `TELEOP`. - -`TELEOP_ON` from any state → `TELEOP`. `TELEOP_OFF` → `IDLE` (never auto-resumes navigation). - -#### 3.2.2 Transition Table - -| State | GO_TO | ABORT | RETURN | `PLAN_FAILED` | TELEOP_ON | TELEOP_OFF | -|-------|-------|-------|--------|---------------|-----------|------------| -| `IDLE` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | -| `NAVIGATING` | → `NAVIGATING` (preempt) | → `ABORTING` | ignore | → `ABORTING` | → `TELEOP` | ignore | -| `ARRIVING` | → `NAVIGATING` (cancel) | → `ABORTING` | ignore | ignore | → `TELEOP` | ignore | -| `STOPPED_AT_TARGET` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | -| `ABORTING` | queued | ignore | queued | ignore | → `TELEOP` | ignore | -| `RETURNING` | → `NAVIGATING` (preempt) | → `ABORTING` | ignore | → `ABORTING` | → `TELEOP` | ignore | -| `STOPPED_AT_RETURN` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | -| `TELEOP` | ignore | ignore | ignore | ignore | ignore | → `IDLE` | - -\* `RETURN` requires the target to have been previously visited. Otherwise the action server returns failure immediately. - -#### 3.2.3 `PLAN_FAILED` Handling - -Subscribes to `/planner_event` (`PlannerEvent.msg`). On `event == PLAN_FAILED`: - -- In `NAVIGATING` or `RETURNING`: transition to `ABORTING`, publish `/nav_enabled = false`. -- In all other states: ignore (the executive is authoritative on state). - -#### 3.2.4 Stopped Detection - -Arrival confirmation uses **measured velocity from `/odom`** (published by the ZED SDK — not commanded `/cmd_vel`). The robot is considered stopped when `|twist.linear| < velocity_zero_threshold` is held for `arrival_hold_time` seconds. - -#### 3.2.5 Replanning - -The global planner does **not** self-trigger replans. `mission_executive` computes the robot's cross-track error (perpendicular distance from `/robot_pose` to the nearest segment of the current `/global_path`). When this exceeds `replan_distance_m`, the executive re-publishes `/goal_pose` to trigger a fresh plan. This keeps replanning fully observable via the `/goal_pose` topic stream. - -The executive caches the latest `/global_path` (subscribed with `transient_local` QoS) for this computation. - -#### 3.2.6 Executor Configuration - -`mission_executive` calls `gps_pose_publisher`'s `latlon_to_enu` service during `GO_TO` goal conversion. To avoid deadlock from blocking on `future.get()` inside a callback, **use a `MultiThreadedExecutor` with a reentrant callback group** for the service client. - -#### 3.2.7 Interfaces - -**Subscriptions:** - -| Topic | Type | Purpose | -|-------|------|---------| -| `/robot_pose` | `PoseStamped` | Current position | -| `/odom` | `nav_msgs/Odometry` | Measured velocity for stop detection (ZED SDK) | -| `/planner_event` | `PlannerEvent` | `PLAN_FAILED` triggers abort | -| `/global_path` | `nav_msgs/Path` | Cached for cross-track error (QoS: `transient_local`) | - -**Action servers:** - -| Name | Type | Handles | -|------|------|---------| -| `~/navigate_to_target` | `NavigateToTarget` | `GO_TO`, `RETURN` | - -**Services:** - -| Name | Type | Purpose | -|------|------|---------| -| `~/abort` | `std_srvs/Trigger` | Transition to `ABORTING` | -| `~/set_target` | `SetTarget` | Register/update a named target in the registry | - -**Publications:** - -| Topic | Type | Notes | -|-------|------|-------| -| `/goal_pose` | `PoseStamped` | Always ENU / `map` frame; consumed by global planner | -| `/nav_enabled` | `std_msgs/Bool` | `false` during teleop/stop; MPPI respects this | -| `/nav_mode` | `std_msgs/String` | `"autonomous"` / `"teleop"` / `"stopped"` | -| `/active_target` | `ActiveTarget` | Target metadata + status | -| `/nav_status` | `NavStatus` | 2 Hz + on every state change | - -#### 3.2.8 Target Types - -| Type | Tolerance | Notes | -|------|-----------|-------| -| `GNSS_ONLY` | 3.0 m | GPS-sourced goal | -| `ARUCO_POST` | 2.0 m | GNSS approach → ArUco refines (Phase 2c) | -| `OBJECT` | stop on detection | First two: GNSS < 3 m; third: < 10 m | -| `LOCAL` | user-defined | Meter-sourced goal; no GPS required | - ---- - -### 3.3 `global_planner` (Hybrid-A\* via `athena_smac_planner`) - -Wraps the vendored SMAC planner with `athena_map` costmap input. Plans **only** on receipt of a new `/goal_pose` — no self-triggered replanning. - -**Startup behavior:** On receiving a `/goal_pose` before a valid `/robot_pose` has been received, the planner logs a `WARN` and waits (does not publish `PLAN_FAILED`). Planning proceeds once both pose and map are available. This handles the GPS cold-start window gracefully. - -**Subscriptions:** - -| Topic | Type | Notes | -|-------|------|-------| -| `/goal_pose` | `PoseStamped` | Triggers planning | -| `/robot_pose` | `PoseStamped` | Planner start pose — must be received before planning | -| `/map` | `nav_msgs/OccupancyGrid` | Feeds `StaticLayer` (QoS: `transient_local`) | - -**Publications:** - -| Topic | Type | QoS | Notes | -|-------|------|-----|-------| -| `/global_path` | `nav_msgs/Path` | `transient_local` | Consumed by MPPI + mission_executive | -| `/planner_event` | `PlannerEvent` | | Enum-based event (see §6) | - ---- - -### 3.4 `mppi_runner` (existing `ackermann_mppi`) - -Minimal changes to the existing package. - -**Changes required:** - -1. Rename input topic `/plan` → `/global_path` -2. Subscribe to `/nav_enabled` — skip `evalControl()` when false -3. Add costmap debug publisher via `nav2_costmap_2d::CostmapPublisher` - -**Local costmap** is owned by `mppi_runner` as a child `Costmap2DROS` node, configured under the `local_costmap` namespace nested within `mppi_runner` parameters (see §7). - -**Subscriptions:** `/robot_pose`, `/odom`, `/global_path`, `/goal_pose`, `/nav_enabled` - -**Publications:** `/cmd_vel` (`TwistStamped`) - -**Control loop:** Timer at `controller_frequency` Hz (default 20). Calls `evalControl()` only when pose + path + goal are all available **and** `/nav_enabled` is true. - -**Initial critics:** -`ConstraintCritic`, `GoalCritic`, `GoalAngleCritic`, `PathFollowCritic`, `PathAlignCritic`, `PreferForwardCritic` - ---- - -## 4. Debugging & Observability - -### 4.1 `/nav_status` (published by `mission_executive`) - -Published at 2 Hz and on every state change. See §6 for full message definition. - -### 4.2 Logging Conventions - -| Event | Level | Format | -|-------|-------|--------| -| State transition | `INFO` | `[mission_executive] → : ` | -| Replan trigger | `INFO` | `[mission_executive] replan triggered: xtrack=X.Xm` | -| `evalControl()` skip | `DEBUG` | (fires at 20 Hz — never INFO) | -| GPS origin set | `INFO` | `[gps_pose_publisher] origin set: lat=X lon=Y alt=Z` | -| `PLAN_FAILED` received | `WARN` | `[mission_executive] PLAN_FAILED — transitioning to ABORTING` | -| Planner waiting for fix | `WARN` | `[global_planner] goal received but no robot_pose yet — waiting` | -| Service/action rejection | `WARN` | Reason logged; error returned to caller | - -### 4.3 Topic QoS - -| Topic | QoS | -|-------|-----| -| `/global_path` | `transient_local` (rviz2/Foxglove sees current path on connect) | -| `/map` | `transient_local` (from `athena_map`) | -| `/nav_status` | `keep_last(1)`, reliable | - ---- - -## 5. Extension Phases - -| Phase | Feature | Effort | -|-------|---------|--------| -| **2a** | Depth camera obstacle avoidance | Config only — add `ObstacleLayer` + `InflationLayer` to MPPI local costmap plugins, enable `CostCritic` + `ObstaclesCritic` | -| **2b** | GeoTIFF terrain obstacles | Config only — set `geotiff_path` on `global_planner` | -| **2c** | ArUco post refinement | Wire `correction_node` output to override `/goal_pose` when `ARUCO_POST` target is within ~10 m. Reuse `GetArucoPose` BT node. | -| **2d** | Object detection | `yolo_ros/` detects objects within tolerance → publish to C2 overlay → `mission_executive` transitions to `STOPPED_AT_TARGET` | -| **2e** | Spiral/lawnmower search | `SpiralCoverageAction` BT node (Archimedean spiral, $r = 15$ m, spacing $= 2$ m). Sub-mode when target reached via GNSS but not visually acquired. | - ---- - -## 6. Custom Message / Service / Action Definitions - -All defined in the `navigation_msgs` package. - -### Services - -``` -# LatLonToENU.srv -float64 lat -float64 lon ---- -float64 x -float64 y -float64 z -``` - -``` -# SetTarget.srv — pre-register a named target -string target_id -float64 lat # GPS goal (WGS84) — ignored if goal_type == METER -float64 lon -float64 x_m # Meter goal (ENU) — ignored if goal_type == GPS -float64 y_m -uint8 goal_type # GPS=0, METER=1 -uint8 GPS=0 -uint8 METER=1 -uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 -uint8 GNSS_ONLY=0 -uint8 ARUCO_POST=1 -uint8 OBJECT=2 -uint8 LOCAL=3 -float64 tolerance_m ---- -bool success -string message -``` - -### Actions - -``` -# NavigateToTarget.action -# --- Goal --- -# If target_id is non-empty, coordinates are looked up from the registry -# (previously loaded via SetTarget). If target_id is empty, inline -# coordinates are used as an ad-hoc goal. -string target_id # empty string = use inline coordinates -float64 lat # ad-hoc GPS goal (ignored if target_id set) -float64 lon -float64 x_m # ad-hoc meter goal (ignored if target_id set) -float64 y_m -uint8 goal_type # GPS=0, METER=1 -uint8 GPS=0 -uint8 METER=1 -uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 -uint8 GNSS_ONLY=0 -uint8 ARUCO_POST=1 -uint8 OBJECT=2 -uint8 LOCAL=3 -float64 tolerance_m # only used for ad-hoc goals; registry targets use stored tolerance -bool is_return ---- -# --- Result --- -bool success -string message ---- -# --- Feedback --- -float64 distance_to_goal_m -float64 cross_track_error_m -string state -``` - -### Messages - -``` -# PlannerEvent.msg -uint8 NEW_GOAL=0 -uint8 PLANNING=1 -uint8 PLAN_SUCCEEDED=2 -uint8 PLAN_FAILED=3 -uint8 event -``` - -``` -# ActiveTarget.msg -string target_id -uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 -float64 tolerance_m -geometry_msgs/PoseStamped goal_enu -uint8 goal_source # GPS=0, METER=1 -string status # NAVIGATING, SEARCHING, ARRIVED, ABORTED -``` - -``` -# NavStatus.msg -string state # IDLE, NAVIGATING, ARRIVING, STOPPED_AT_TARGET, - # ABORTING, RETURNING, STOPPED_AT_RETURN, TELEOP -string active_target_id -uint8 active_target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 -uint8 goal_source # GPS=0, METER=1 -float64 distance_to_goal_m # -1.0 if no active goal -float64 cross_track_error_m # -1.0 if no active path -float64 heading_error_rad -float64 robot_speed_mps # from /odom twist (ZED SDK) -bool is_return -uint8 last_planner_event # last PlannerEvent.event value -``` - ---- - -## 7. Parameter Reference (`nav_params.yaml`) - -```yaml -# ── Global ────────────────────────────────────────────── -use_sim_time: false # propagated to all nodes via launch - -# ── Robot geometry (referenced by costmaps + planner) ─── -robot_radius: &robot_radius 0.45 # meters — inscribed radius - -# ── gps_pose_publisher ───────────────────────────────── -gps_pose_publisher: - ros__parameters: - use_sim_time: false - heading_topic: "/gps/heading" - origin_lat: NaN # auto-set from first fix if NaN - origin_lon: NaN - origin_alt: 0.0 - use_start_gate_ref: true # if true, /start_gate_ref sets origin - -# ── mission_executive ────────────────────────────────── -mission_executive: - ros__parameters: - use_sim_time: false - default_targets: [] - velocity_zero_threshold: 0.05 # m/s — applied to /odom twist - arrival_hold_time: 1.0 # seconds of confirmed zero velocity - replan_distance_m: 3.0 # cross-track error threshold to trigger replan - latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" - -# ── global_planner (athena_smac_planner) ─────────────── -global_planner: - ros__parameters: - use_sim_time: false - geotiff_path: "" - grid_size_m: 500.0 - grid_resolution_m: 0.5 - obstacle_threshold: 50 - min_turning_r: 1.2 # must match mppi_runner - allow_reverse: false - -global_costmap: - ros__parameters: - use_sim_time: false - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 1.0 - publish_frequency: 0.5 - transform_tolerance: 1.0 # generous — GPS-only localization has jitter - rolling_window: false - plugins: ["static_layer", "inflation_layer"] - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_topic: /map - subscribe_to_updates: true - map_subscribe_transient_local: true - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 1.0 # >= robot_radius - -# ── mppi_runner (ackermann_mppi) ─────────────────────── -mppi_runner: - ros__parameters: - use_sim_time: false - controller_frequency: 20.0 - motion_model: "Ackermann" - min_turning_r: 1.2 - vx_max: 3.0 - vx_min: 0.0 - wz_max: 1.0 - time_steps: 56 - batch_size: 1000 - model_dt: 0.05 - critics: - - ConstraintCritic - - GoalCritic - - GoalAngleCritic - - PathFollowCritic - - PathAlignCritic - - PreferForwardCritic - # Local costmap — child Costmap2DROS node owned by mppi_runner - local_costmap: - ros__parameters: - use_sim_time: false - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 5.0 - publish_frequency: 2.0 - transform_tolerance: 1.0 - rolling_window: true - width: 20 - height: 20 - resolution: 0.1 - plugins: [] # Phase 2a: add ObstacleLayer + InflationLayer -``` - ---- - -## 8. Implementation Order - -1. **`gps_pose_publisher`** — full implementation; validates TF + ENU projection before anything else runs. -2. **`ackermann_mppi` modifications** — topic rename, `/nav_enabled` guard, costmap debug publisher. -3. **`global_planner` / `athena_smac_planner`** — package rename, `athena_map` integration, costmap wiring. -4. **`mission_executive`** — state machine, action/service servers, all operator commands. -5. **`nav.launch.py`** + `CMakeLists.txt` + `package.xml` — single-package build and launch. - -In the final launch file, you need to launch main/athena-code/src/subsystems/navigation/athena_gps/launch/gps_launch.py, and we will use the /odom/ground_truth topic for odom. you also need to map the output using a twist stamper like - - twist_stamper_node = Node( - package='twist_stamper', - executable='twist_stamper', - name='cmd_vel_stamper', - parameters=[{'use_sim_time': sim}], - remappings=[ - ('cmd_vel_in', '/cmd_vel_nav'), - ('cmd_vel_out', '/rear_ackermann_controller/reference'), - ], - ) - diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt b/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt deleted file mode 100644 index d0668b92..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt +++ /dev/null @@ -1,122 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ackermann_mppi) - -find_package(ament_cmake REQUIRED) -find_package(angles REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(builtin_interfaces REQUIRED) - -include_directories( - include - ${EIGEN3_INCLUDE_DIR} -) - -include(CheckCXXCompilerFlag) -check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA) -if(COMPILER_SUPPORTS_FMA) - add_compile_options(-mfma) -endif() - -# --- Core optimizer library (no ROS node, no pluginlib) --- -add_library(ackermann_mppi_core SHARED - src/noise_generator.cpp - src/optimizer.cpp - src/critics/constraint_critic.cpp - src/critics/goal_critic.cpp - src/critics/obstacles_critic.cpp - src/critics/path_align_critic.cpp - src/critics/path_follow_critic.cpp -) - -# Enable C++17 (required for inline constexpr, structured bindings, etc.) -target_compile_features(ackermann_mppi_core PUBLIC cxx_std_17) - -# Apply -O3 only for Release and RelWithDebInfo builds so Debug stays debuggable. -# Do not force -O3 unconditionally — it disrespects the developer's chosen build type. -target_compile_options(ackermann_mppi_core PUBLIC - $<$:-O3> - $<$:-O3> -) - -target_include_directories(ackermann_mppi_core - PUBLIC - "$" - "$") - -target_link_libraries(ackermann_mppi_core PUBLIC - angles::angles - ${geometry_msgs_TARGETS} - nav2_costmap_2d::layers - nav2_costmap_2d::nav2_costmap_2d_core - ${nav_msgs_TARGETS} - rclcpp::rclcpp - ${std_msgs_TARGETS} - tf2::tf2 - tf2_geometry_msgs::tf2_geometry_msgs - tf2_ros::tf2_ros - ${visualization_msgs_TARGETS} - ${builtin_interfaces_TARGETS} -) - -# --- ROS2 node executable --- -add_executable(ackermann_mppi_node - src/ackermann_mppi_node.cpp -) - -target_compile_options(ackermann_mppi_node PRIVATE - $<$:-O3> - $<$:-O3> -) - -target_include_directories(ackermann_mppi_node PRIVATE - "$") - -target_link_libraries(ackermann_mppi_node - ackermann_mppi_core - rclcpp::rclcpp - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - nav2_costmap_2d::nav2_costmap_2d_core - ${nav_msgs_TARGETS} - ${geometry_msgs_TARGETS} - ${visualization_msgs_TARGETS} -) - -# --- Install --- -install(TARGETS ackermann_mppi_core ackermann_mppi_node - EXPORT ackermann_mppi - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -ament_export_targets(ackermann_mppi HAS_LIBRARY_TARGET) -ament_export_dependencies( - angles - geometry_msgs - nav2_costmap_2d - nav_msgs - rclcpp - std_msgs - tf2 - tf2_geometry_msgs - tf2_ros - visualization_msgs - Eigen3 -) -ament_export_include_directories(include/${PROJECT_NAME}) - -ament_package() diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp deleted file mode 100644 index 14e89c51..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITIC_DATA_HPP_ -#define ACKERMANN_MPPI__CRITIC_DATA_HPP_ - -#include - -#include -#include -#include - -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/models/trajectories.hpp" -#include "ackermann_mppi/models/path.hpp" -#include "ackermann_mppi/motion_models.hpp" - -namespace mppi -{ - -/** - * @struct mppi::CriticData - * @brief Data passed to critics for scoring. GoalChecker replaced with a simple - * distance tolerance float — the optimizer sets this from its parameters. - * - * Has an explicit constructor to prevent silent wrong-field binding when using - * aggregate initialization in C++17 (designated initializers require C++20). - */ -struct CriticData -{ - const models::State & state; - const models::Trajectories & trajectories; - const models::Path & path; - const geometry_msgs::msg::Pose & goal; - - Eigen::ArrayXf & costs; - float & model_dt; - - bool fail_flag{false}; - - // Replaces nav2_core::GoalChecker — set by Optimizer::prepare() from settings - float goal_dist_tolerance{0.25f}; - - std::shared_ptr motion_model; - std::optional> path_pts_valid; - std::optional furthest_reached_path_point; - std::vector trajectories_in_collision; - - // Explicit constructor so call sites use named parameters, preventing silent - // mis-binding if fields are reordered. - CriticData( - const models::State & state_in, - const models::Trajectories & trajectories_in, - const models::Path & path_in, - const geometry_msgs::msg::Pose & goal_in, - Eigen::ArrayXf & costs_in, - float & model_dt_in, - float goal_dist_tolerance_in = 0.25f, - std::shared_ptr motion_model_in = nullptr) - : state(state_in), - trajectories(trajectories_in), - path(path_in), - goal(goal_in), - costs(costs_in), - model_dt(model_dt_in), - fail_flag(false), - goal_dist_tolerance(goal_dist_tolerance_in), - motion_model(std::move(motion_model_in)) {} -}; - -} // namespace mppi - -#endif // ACKERMANN_MPPI__CRITIC_DATA_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp deleted file mode 100644 index 90cff7f6..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ -#define ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" - -#include "ackermann_mppi/critic_data.hpp" - -namespace mppi::critics -{ - -struct CollisionCost -{ - float cost{0}; - bool using_footprint{false}; -}; - -/** - * @class mppi::critics::CriticFunction - * @brief Abstract base for MPPI trajectory scoring critics. - * - * Compared to the original nav2 version: - * - Uses rclcpp::Node::SharedPtr instead of LifecycleNode::WeakPtr - * - No ParametersHandler dependency — uses a simple getParam() helper inline - * - Instantiated directly by the Optimizer (no pluginlib) - */ -class CriticFunction -{ -public: - CriticFunction() = default; - virtual ~CriticFunction() = default; - - /** - * @brief Configure critic on startup. - * @param node ROS2 node (used for parameter access and logging) - * @param parent_name Name of the parent optimizer (for reading shared params) - * @param name Name of this critic (used as param namespace) - * @param costmap_ros Costmap for collision/cost queries - */ - void on_configure( - rclcpp::Node::SharedPtr node, - const std::string & parent_name, - const std::string & name, - std::shared_ptr costmap_ros) - { - node_ = node; - logger_ = node_->get_logger(); - name_ = name; - parent_name_ = parent_name; - costmap_ros_ = costmap_ros; - costmap_ = costmap_ros_->getCostmap(); - - // Declare and read the common 'enabled' parameter - declare_param(name_ + ".enabled", true); - node_->get_parameter(name_ + ".enabled", enabled_); - - initialize(); - } - - virtual void score(CriticData & data) = 0; - virtual void initialize() = 0; - - std::string getName() { return name_; } - -protected: - /** - * @brief Declare a parameter with a default value if not already declared. - */ - template - void declare_param(const std::string & full_name, T default_value) - { - if (!node_->has_parameter(full_name)) { - node_->declare_parameter(full_name, default_value); - } - } - - /** - * @brief Get a parameter accessor scoped to a given namespace. - * - * Usage (same pattern as original ParametersHandler::getParamGetter): - * auto getParam = getParamGetter(name_); - * getParam(weight_, "cost_weight", 5.0f); - */ - auto getParamGetter(const std::string & ns) - { - return [this, ns](auto & setting, const std::string & name, auto default_value) { - std::string full_name = ns.empty() ? name : ns + "." + name; - declare_param(full_name, default_value); - node_->get_parameter(full_name, setting); - }; - } - - bool enabled_{true}; - std::string name_, parent_name_; - rclcpp::Node::SharedPtr node_; - std::shared_ptr costmap_ros_; - nav2_costmap_2d::Costmap2D * costmap_{nullptr}; - rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp deleted file mode 100644 index ba4901a3..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ - -#include "ackermann_mppi/critic_function.hpp" - -namespace mppi::critics -{ - -/** - * @class ConstraintCritic (Ackermann-only) - * @brief Penalizes trajectories that violate velocity and turning radius constraints. - * - * For Ackermann: penalizes vx outside [vx_min, vx_max] and steering ratios - * (|vx|/|wz|) that would require a tighter turn than min_turning_r. - */ -class ConstraintCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - unsigned int power_{0}; - float weight_{0}; - // min_turning_r is vehicle geometry — fixed. Velocity limits are read from - // data.motion_model->getConstraints() in score() so speed-limit changes take effect. - float min_turning_r_{0.2f}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp deleted file mode 100644 index f8d69ed1..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ - -#include "ackermann_mppi/critic_function.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -namespace mppi::critics -{ - -class GoalCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - unsigned int power_{0}; - float weight_{0}; - float threshold_to_consider_{0}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp deleted file mode 100644 index 25a9ca14..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ - -#include -#include - -#include "nav2_costmap_2d/footprint_collision_checker.hpp" -#include "nav2_costmap_2d/inflation_layer.hpp" -#include "ackermann_mppi/critic_function.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -namespace mppi::critics -{ - -class ObstaclesCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - inline bool inCollision(float cost) const; - inline CollisionCost costAtPose(float x, float y, float theta); - inline float distanceToObstacle(const CollisionCost & cost); - float findCircumscribedCost(std::shared_ptr costmap); - -protected: - nav2_costmap_2d::FootprintCollisionChecker - collision_checker_{nullptr}; - - bool consider_footprint_{true}; - float collision_cost_{0}; - float inflation_scale_factor_{0}, inflation_radius_{0}; - float possible_collision_cost_; - float collision_margin_distance_; - float near_goal_distance_; - float circumscribed_cost_{0}, circumscribed_radius_{0}; - - unsigned int power_{0}; - float repulsion_weight_, critical_weight_{0}; - std::string inflation_layer_name_; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp deleted file mode 100644 index db17564e..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) 2023 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. - -#ifndef ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ - -#include "ackermann_mppi/critic_function.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -namespace mppi::critics -{ - -class PathAlignCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - size_t offset_from_furthest_{0}; - int trajectory_point_step_{0}; - float threshold_to_consider_{0}; - float max_path_occupancy_ratio_{0}; - bool use_path_orientations_{false}; - unsigned int power_{0}; - float weight_{0}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp deleted file mode 100644 index ef5485b7..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ - -#include "ackermann_mppi/critic_function.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -namespace mppi::critics -{ - -class PathFollowCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - float threshold_to_consider_{0}; - size_t offset_from_furthest_{0}; - unsigned int power_{0}; - float weight_{0}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp deleted file mode 100644 index 8f54c8d6..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ -#define ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ - -namespace mppi::models -{ - -/** - * @struct mppi::models::ControlConstraints - * @brief Constraints on control (Ackermann: no vy/ay terms) - */ -struct ControlConstraints -{ - float vx_max; - float vx_min; - float wz; - float ax_max; - float ax_min; - float az_max; -}; - -/** - * @struct mppi::models::SamplingStd - * @brief Noise standard deviations for MPPI sampling (Ackermann: vx and wz only) - */ -struct SamplingStd -{ - float vx; - float wz; -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp deleted file mode 100644 index b4781787..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ -#define ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ - -#include - -namespace mppi::models -{ - -/** - * @struct mppi::models::Control - * @brief A single control step (Ackermann: vx and wz only) - */ -struct Control -{ - float vx, wz; -}; - -/** - * @struct mppi::models::ControlSequence - * @brief A control sequence over time (Ackermann: vx and wz only) - */ -struct ControlSequence -{ - Eigen::ArrayXf vx; - Eigen::ArrayXf wz; - - void reset(unsigned int time_steps) - { - vx.setZero(time_steps); - wz.setZero(time_steps); - } -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp deleted file mode 100644 index af0ad621..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ -#define ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ - -#include -#include "ackermann_mppi/models/constraints.hpp" - -namespace mppi::models -{ - -/** - * @struct mppi::models::OptimizerSettings - * @brief Settings for the optimizer to use - */ -struct OptimizerSettings -{ - models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; - models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; - models::SamplingStd sampling_std{0.0f, 0.0f}; - float model_dt{0.0f}; - float temperature{0.0f}; - float gamma{0.0f}; - unsigned int batch_size{0u}; - unsigned int time_steps{0u}; - unsigned int iteration_count{0u}; - bool shift_control_sequence{false}; - size_t retry_attempt_limit{0}; - bool open_loop{false}; - bool visualize{false}; -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp deleted file mode 100644 index a9171d78..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__PATH_HPP_ -#define ACKERMANN_MPPI__MODELS__PATH_HPP_ - -#include - -namespace mppi::models -{ - -/** - * @struct mppi::models::Path - * @brief Path represented as a tensor - */ -struct Path -{ - Eigen::ArrayXf x; - Eigen::ArrayXf y; - Eigen::ArrayXf yaws; - - /** - * @brief Reset path data - */ - void reset(unsigned int size) - { - x.setZero(size); - y.setZero(size); - yaws.setZero(size); - } -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__PATH_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp deleted file mode 100644 index cb5018f4..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__STATE_HPP_ -#define ACKERMANN_MPPI__MODELS__STATE_HPP_ - -#include - -#include -#include - - -namespace mppi::models -{ - -/** - * @struct mppi::models::State - * @brief State information: velocities, controls, poses, speed - */ -struct State -{ - // Propagated velocities: [batch_size x time_steps] - Eigen::ArrayXXf vx; // longitudinal velocity - Eigen::ArrayXXf wz; // angular velocity (vy is always zero for Ackermann) - - // Noised controls: [batch_size x time_steps] - Eigen::ArrayXXf cvx; - Eigen::ArrayXXf cwz; - - geometry_msgs::msg::PoseStamped pose; - geometry_msgs::msg::Twist speed; - float local_path_length; - - void reset(unsigned int batch_size, unsigned int time_steps) - { - vx.setZero(batch_size, time_steps); - wz.setZero(batch_size, time_steps); - cvx.setZero(batch_size, time_steps); - cwz.setZero(batch_size, time_steps); - } -}; -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__STATE_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp deleted file mode 100644 index 8284872a..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ -#define ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ - -#include - -namespace mppi::models -{ - -/** - * @class mppi::models::Trajectories - * @brief Candidate Trajectories - */ -struct Trajectories -{ - Eigen::ArrayXXf x; - Eigen::ArrayXXf y; - Eigen::ArrayXXf yaws; - - /** - * @brief Reset state data - */ - void reset(unsigned int batch_size, unsigned int time_steps) - { - x.setZero(batch_size, time_steps); - y.setZero(batch_size, time_steps); - yaws.setZero(batch_size, time_steps); - } -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp deleted file mode 100644 index 87487587..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2025 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. - -#ifndef ACKERMANN_MPPI__MOTION_MODELS_HPP_ -#define ACKERMANN_MPPI__MOTION_MODELS_HPP_ - -#include -#include -#include - -#include "ackermann_mppi/models/control_sequence.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/models/constraints.hpp" - -namespace mppi -{ - -// Forward declaration used in utils.hpp to avoid circular include -namespace utils -{ -float clamp(const float lower_bound, const float upper_bound, const float input); -} - -/** - * @class mppi::MotionModel - * @brief Abstract motion model base class - */ -class MotionModel -{ -public: - MotionModel() = default; - virtual ~MotionModel() = default; - - void initialize(const models::ControlConstraints & control_constraints, float model_dt) - { - control_constraints_ = control_constraints; - model_dt_ = model_dt; - } - - /** - * @brief Propagate velocities forward one step using acceleration constraints. - * Batch operation: state matrices are [batch_size x time_steps]. - */ - virtual void predict(models::State & state) - { - float max_delta_vx = model_dt_ * control_constraints_.ax_max; - float min_delta_vx = model_dt_ * control_constraints_.ax_min; - float max_delta_wz = model_dt_ * control_constraints_.az_max; - - unsigned int n_cols = state.vx.cols(); - for (unsigned int i = 1; i < n_cols; i++) { - auto lower_bound_vx = (state.vx.col(i - 1) > 0).select( - state.vx.col(i - 1) + min_delta_vx, - state.vx.col(i - 1) - max_delta_vx); - auto upper_bound_vx = (state.vx.col(i - 1) > 0).select( - state.vx.col(i - 1) + max_delta_vx, - state.vx.col(i - 1) - min_delta_vx); - - state.cvx.col(i - 1) = state.cvx.col(i - 1) - .cwiseMax(lower_bound_vx) - .cwiseMin(upper_bound_vx); - state.vx.col(i) = state.cvx.col(i - 1); - - state.cwz.col(i - 1) = state.cwz.col(i - 1) - .cwiseMax(state.wz.col(i - 1) - max_delta_wz) - .cwiseMin(state.wz.col(i - 1) + max_delta_wz); - state.wz.col(i) = state.cwz.col(i - 1); - } - } - - virtual bool isHolonomic() = 0; - - /** - * @brief Apply model-specific hard constraints to the optimal control sequence. - * Called after softmax weighting, before returning the command. - */ - virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {} - - /** - * @brief Return the currently active constraints (may differ from base if speed limit is set). - * Critics should read from this instead of caching their own copies. - */ - const models::ControlConstraints & getConstraints() const { return control_constraints_; } - -protected: - float model_dt_{0.0}; - models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; -}; - -/** - * @class mppi::AckermannMotionModel - * @brief Ackermann steering motion model. - * - * Key constraint: angular velocity is bounded by |wz| <= |vx| / min_turning_r - * This prevents commanding steering angles that are physically impossible at a given speed. - */ -class AckermannMotionModel : public MotionModel -{ -public: - explicit AckermannMotionModel(float min_turning_r = 0.2f) - : min_turning_r_(min_turning_r) {} - - bool isHolonomic() override { return false; } - - void applyConstraints(models::ControlSequence & control_sequence) override - { - const auto wz_constrained = control_sequence.vx.abs() / min_turning_r_; - control_sequence.wz = control_sequence.wz - .max(-wz_constrained) - .min(wz_constrained); - } - - float getMinTurningRadius() const { return min_turning_r_; } - -private: - float min_turning_r_{0.2f}; -}; - -} // namespace mppi - -#endif // ACKERMANN_MPPI__MOTION_MODELS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp deleted file mode 100644 index bfcd2e41..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp +++ /dev/null @@ -1,238 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2025 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. - -#ifndef ACKERMANN_MPPI__OPTIMIZER_HPP_ -#define ACKERMANN_MPPI__OPTIMIZER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/buffer.hpp" - -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/path.hpp" - -#include "ackermann_mppi/models/optimizer_settings.hpp" -#include "ackermann_mppi/motion_models.hpp" -#include "ackermann_mppi/critic_data.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/models/trajectories.hpp" -#include "ackermann_mppi/models/path.hpp" -#include "ackermann_mppi/tools/noise_generator.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -// Forward declarations for critic types -namespace mppi::critics -{ -class CriticFunction; -class PathFollowCritic; -class PathAlignCritic; -class GoalCritic; -class ObstaclesCritic; -class ConstraintCritic; -} - -namespace mppi -{ - -/** - * @class mppi::Optimizer - * @brief Ackermann-specific MPPI optimizer. - * - * Differences from the nav2_mppi_controller Optimizer: - * - Initialized with rclcpp::Node::SharedPtr (not LifecycleNode) - * - Critics are created directly (no pluginlib) - * - No OptimalTrajectoryValidator — simple fallback on fail_flag - * - Motion model hardcoded to AckermannMotionModel - * - GoalChecker replaced with a distance threshold parameter - * - Parameters read directly via node->declare_parameter / node->get_parameter - */ -class Optimizer -{ -public: - // Both constructor and destructor defined in optimizer.cpp so critic forward declarations are sufficient here - Optimizer(); - ~Optimizer(); - - /** - * @brief Initialize the optimizer. - * @param node ROS2 node for parameters, logging, and clock - * @param name Parameter namespace prefix (e.g. "mppi") - * @param costmap_ros Costmap for obstacle avoidance critics - * @param tf_buffer TF buffer for robot pose lookups - */ - void initialize( - rclcpp::Node::SharedPtr node, - const std::string & name, - std::shared_ptr costmap_ros, - std::shared_ptr tf_buffer); - - void shutdown(); - - /** - * @brief Run MPPI to compute the next control command. - * @param robot_pose Current robot pose (in costmap frame) - * @param robot_speed Current robot velocity (from odometry) - * @param plan Global path to follow - * @param goal Goal pose (last pose of plan, or explicit goal) - * @return [TwistStamped command, optimal trajectory as [time_steps x 3] (x, y, yaw)] - * @throws std::runtime_error if all trajectories are in collision after retries - */ - std::tuple evalControl( - const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal); - - // --- Accessors for visualization / debugging --- - - models::Trajectories & getGeneratedTrajectories() { return generated_trajectories_; } - Eigen::ArrayXXf getOptimizedTrajectory(); - const models::ControlSequence & getOptimalControlSequence() { return control_sequence_; } - const Eigen::ArrayXf & getCosts() const { return costs_; } - - /** - * @brief Per-critic cost breakdown from last evalControl call. - * Each entry is (critic_name, cost_delta_array[batch_size]). - */ - const std::vector> & getCriticCosts() const - { - return critic_costs_; - } - - const std::vector & getCollisionFlags() const - { - return critics_data_.trajectories_in_collision; - } - - /** - * @brief Scale max speeds by a ratio (e.g. from a speed zone costmap filter). - * @param speed_limit Absolute speed OR percentage of max speed - * @param percentage True if speed_limit is a percentage [0–100] - */ - void setSpeedLimit(double speed_limit, bool percentage); - - void reset(bool reset_dynamic_speed_limits = true); - - bool isSpeedLimitActive() const; - - const models::OptimizerSettings & getSettings() const { return settings_; } - -protected: - void optimize(); - void evalTrajectoriesScores(); - - void prepare( - const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal); - - void getParams(); - void loadCritics(); - - void shiftControlSequence(); - void generateNoisedTrajectories(); - void applyControlSequenceConstraints(); - void updateStateVelocities(models::State & state) const; - void updateInitialStateVelocities(models::State & state) const; - void propagateStateVelocitiesFromInitials(models::State & state) const; - - void integrateStateVelocities( - models::Trajectories & trajectories, - const models::State & state) const; - - void integrateStateVelocities( - Eigen::Array & trajectory, - const Eigen::ArrayXXf & sequence) const; - - void updateControlSequence(); - - geometry_msgs::msg::TwistStamped - getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time & stamp); - - bool fallback(bool fail); - - size_t fallback_counter_{0}; // member — not static, so reset() clears it correctly - - template - void declareParam(const std::string & full_name, T default_value) - { - if (!node_->has_parameter(full_name)) { - node_->declare_parameter(full_name, default_value); - } - } - - auto getParamGetter(const std::string & ns) - { - return [this, ns](auto & setting, const std::string & name, auto default_value) { - std::string full_name = ns.empty() ? name : ns + "." + name; - declareParam(full_name, default_value); - node_->get_parameter(full_name, setting); - }; - } - -protected: - rclcpp::Node::SharedPtr node_; - std::shared_ptr costmap_ros_; - nav2_costmap_2d::Costmap2D * costmap_{nullptr}; - std::string name_; - std::string base_frame_; - std::shared_ptr tf_buffer_; - - std::shared_ptr motion_model_; - - // Critics owned directly (no pluginlib) - std::vector> critics_; - std::vector> critic_costs_; - - NoiseGenerator noise_generator_; - - models::OptimizerSettings settings_; - float goal_dist_tolerance_{0.25f}; - - models::State state_; - models::ControlSequence control_sequence_; - std::array control_history_; - models::Trajectories generated_trajectories_; - models::Path path_; - geometry_msgs::msg::Pose goal_; - Eigen::ArrayXf costs_; - - CriticData critics_data_{ - state_, - generated_trajectories_, - path_, - goal_, - costs_, - settings_.model_dt, - goal_dist_tolerance_}; - - rclcpp::Logger logger_{rclcpp::get_logger("AckermannMPPI")}; - geometry_msgs::msg::Twist last_command_vel_; -}; - -} // namespace mppi - -#endif // ACKERMANN_MPPI__OPTIMIZER_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp deleted file mode 100644 index c4b69720..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ -#define ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ - -#include - -#include -#include -#include -#include -#include - -#include "ackermann_mppi/models/optimizer_settings.hpp" -#include "ackermann_mppi/models/control_sequence.hpp" -#include "ackermann_mppi/models/state.hpp" - -namespace mppi -{ - -/** - * @class mppi::NoiseGenerator - * @brief Generates Gaussian-perturbed control sequences for MPPI sampling. - * - * When regenerate_noises=true (set via optimizer param), noise generation runs in a - * background thread that pre-generates noise for the *next* iteration while the - * current one scores trajectories. This hides latency on slower hardware. - * - * When regenerate_noises=false (default), noise is generated synchronously each call. - */ -class NoiseGenerator -{ -public: - NoiseGenerator() = default; - - /** - * @brief Initialize noise generator. - * @param settings Optimizer settings (batch_size, time_steps, sampling_std) - * @param regenerate_noises If true, run noise generation in background thread - */ - void initialize( - mppi::models::OptimizerSettings & settings, - bool regenerate_noises = false); - - void shutdown(); - - /** - * @brief Signal the background thread to generate next iteration's noises. - * No-op when regenerate_noises=false. - */ - void generateNextNoises(); - - /** - * @brief Apply noises to the current optimal control sequence to get noised controls. - * Stores in state.cvx / state.cwz for use by the motion model predict(). - */ - void setNoisedControls( - models::State & state, - const models::ControlSequence & control_sequence); - - void reset(mppi::models::OptimizerSettings & settings); - -protected: - void noiseThread(); - void generateNoisedControls(); - - Eigen::ArrayXXf noises_vx_; - Eigen::ArrayXXf noises_wz_; - - std::default_random_engine generator_; - std::normal_distribution ndistribution_vx_; - std::normal_distribution ndistribution_wz_; - - mppi::models::OptimizerSettings settings_; - bool regenerate_noises_{false}; - - std::thread noise_thread_; - std::condition_variable noise_cond_; - std::mutex noise_lock_; - bool active_{false}, ready_{false}; -}; - -} // namespace mppi - -#endif // ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp deleted file mode 100644 index f499ddea..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp +++ /dev/null @@ -1,387 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2023 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. - -#ifndef ACKERMANN_MPPI__TOOLS__UTILS_HPP_ -#define ACKERMANN_MPPI__TOOLS__UTILS_HPP_ - -#include - -#include -#include -#include -#include -#include - -#include "angles/angles.h" -#include "tf2/utils.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/path.hpp" -#include "visualization_msgs/msg/marker_array.hpp" -#include "std_msgs/msg/color_rgba.hpp" - -#include "rclcpp/rclcpp.hpp" - -#include "ackermann_mppi/models/optimizer_settings.hpp" -#include "ackermann_mppi/models/control_sequence.hpp" -#include "ackermann_mppi/models/path.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include "ackermann_mppi/critic_data.hpp" - -namespace mppi::utils -{ - -// Use constexpr instead of #define to avoid macro pollution and enable type checking. -inline constexpr float M_PIF = 3.141592653589793238462643383279502884e+00F; -inline constexpr float M_PIF_2 = 1.5707963267948966e+00F; - -inline geometry_msgs::msg::Pose createPose(double x, double y, double z) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = z; - pose.orientation.w = 1; - pose.orientation.x = 0; - pose.orientation.y = 0; - pose.orientation.z = 0; - return pose; -} - -inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) -{ - geometry_msgs::msg::Vector3 scale; - scale.x = x; - scale.y = y; - scale.z = z; - return scale; -} - -inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) -{ - std_msgs::msg::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - color.a = a; - return color; -} - -inline visualization_msgs::msg::Marker createMarker( - int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, - const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns) -{ - using visualization_msgs::msg::Marker; - Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = rclcpp::Time(0, 0); - marker.ns = ns; - marker.id = id; - marker.type = Marker::SPHERE; - marker.action = Marker::ADD; - marker.pose = pose; - marker.scale = scale; - marker.color = color; - return marker; -} - -inline geometry_msgs::msg::TwistStamped toTwistStamped( - float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) -{ - geometry_msgs::msg::TwistStamped twist; - twist.header.frame_id = frame; - twist.header.stamp = stamp; - twist.twist.linear.x = vx; - twist.twist.angular.z = wz; - return twist; -} - -/** - * @brief Convert path to a tensor of (x, y, yaw) arrays - */ -inline models::Path toTensor(const nav_msgs::msg::Path & path) -{ - auto result = models::Path{}; - result.reset(path.poses.size()); - for (size_t i = 0; i < path.poses.size(); ++i) { - result.x(i) = path.poses[i].pose.position.x; - result.y(i) = path.poses[i].pose.position.y; - result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation); - } - return result; -} - -/** - * @brief Get the last pose in the path tensor - */ -inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) -{ - const unsigned int path_last_idx = path.x.size() - 1; - tf2::Quaternion q; - q.setRPY(0.0, 0.0, path.yaws(path_last_idx)); - geometry_msgs::msg::Pose p; - p.position.x = path.x(path_last_idx); - p.position.y = path.y(path_last_idx); - p.orientation = tf2::toMsg(q); - return p; -} - -template -auto normalize_angles(const T & angles) -{ - return (angles + M_PIF).unaryExpr( - [&](const float x) { - float remainder = std::fmod(x, 2.0f * M_PIF); - return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; - }); -} - -template -auto shortest_angular_distance(const F & from, const T & to) -{ - return normalize_angles(to - from); -} - -/** - * @brief Find the path point index furthest along the path that any trajectory in the batch - * reaches (by closest-point mapping). Used by PathFollow and PathAlign critics. - * - * Vectorized: outer loop iterates over path points (n_path), inner computation is a - * batch-wide Eigen array op over all trajectories simultaneously. This is significantly - * faster than the naive scalar O(n_batch × n_path) nested loop because each iteration - * of the outer loop operates on a [batch_size] SIMD-vectorizable array. - */ -inline size_t findPathFurthestReachedPoint(const CriticData & data) -{ - const int last_col = data.trajectories.x.cols() - 1; - // End-positions of all trajectories: [batch_size] arrays. - const auto traj_x = data.trajectories.x.col(last_col); - const auto traj_y = data.trajectories.y.col(last_col); - - const Eigen::Index n_batch = traj_x.rows(); - const Eigen::Index n_path = static_cast(data.path.x.size()); - - // Per-trajectory: closest path index found so far and its squared distance. - Eigen::ArrayXf min_dist2 = Eigen::ArrayXf::Constant(n_batch, std::numeric_limits::max()); - Eigen::ArrayXi best_idx = Eigen::ArrayXi::Zero(n_batch); - - for (Eigen::Index j = 0; j != n_path; ++j) { - // Vectorized over all batch_size trajectories at once. - const Eigen::ArrayXf dx = traj_x - data.path.x(j); - const Eigen::ArrayXf dy = traj_y - data.path.y(j); - const Eigen::ArrayXf d2 = dx * dx + dy * dy; - - const auto better = (d2 < min_dist2).eval(); - min_dist2 = better.select(d2, min_dist2); - best_idx = better.select(Eigen::ArrayXi::Constant(n_batch, static_cast(j)), best_idx); - - // Early exit: already found a trajectory that reaches the last path point. - if (best_idx.maxCoeff() == static_cast(n_path) - 1) { - break; - } - } - - return static_cast(best_idx.maxCoeff()); -} - -inline void setPathFurthestPointIfNotSet(CriticData & data) -{ - if (!data.furthest_reached_path_point) { - data.furthest_reached_path_point = findPathFurthestReachedPoint(data); - } -} - -/** - * @brief Check which path points are collision-free according to the costmap. - * Stores results in data.path_pts_valid for reuse across critics. - */ -inline void findPathCosts( - CriticData & data, - std::shared_ptr costmap_ros) -{ - auto * costmap = costmap_ros->getCostmap(); - unsigned int map_x, map_y; - const size_t path_segments_count = data.path.x.size() - 1; - data.path_pts_valid = std::vector(path_segments_count, false); - const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); - - for (unsigned int idx = 0; idx < path_segments_count; idx++) { - if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) { - (*data.path_pts_valid)[idx] = false; - continue; - } - switch (costmap->getCost(map_x, map_y)) { - case (nav2_costmap_2d::LETHAL_OBSTACLE): - (*data.path_pts_valid)[idx] = false; - continue; - case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): - (*data.path_pts_valid)[idx] = false; - continue; - case (nav2_costmap_2d::NO_INFORMATION): - (*data.path_pts_valid)[idx] = tracking_unknown ? true : false; - continue; - } - (*data.path_pts_valid)[idx] = true; - } -} - -inline void setPathCostsIfNotSet( - CriticData & data, - std::shared_ptr costmap_ros) -{ - if (!data.path_pts_valid) { - findPathCosts(data, costmap_ros); - } -} - -inline float posePointAngle( - const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) -{ - float pose_x = pose.position.x; - float pose_y = pose.position.y; - float pose_yaw = tf2::getYaw(pose.orientation); - float yaw = atan2f(point_y - pose_y, point_x - pose_x); - - if (!forward_preference) { - return std::min( - fabs(angles::shortest_angular_distance(yaw, pose_yaw)), - fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); - } - return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); -} - -/** - * @brief Apply Savitzky-Golay smoothing filter to the optimal control sequence. - * Uses a 9-point quadratic filter to smooth sharp changes between control steps. - */ -inline void savitskyGolayFilter( - models::ControlSequence & control_sequence, - std::array & control_history, - const models::OptimizerSettings & settings) -{ - // Standard 9-point quadratic Savitzky-Golay smoothing coefficients (window = ±4). - // Denominator 231 is the normalization factor for this window/polynomial order. - // Reference: Savitzky & Golay, Analytical Chemistry, 1964, Table I (m=4, n=2). - Eigen::Array filter = { - -21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, -21.0f}; - filter /= 231.0f; - - const unsigned int num_sequences = control_sequence.vx.size() - 1; - // Skip smoothing for very short horizons: the 9-point window needs at least - // 5 look-ahead points (indices 0..4), so sequences shorter than 20 provide - // too little context for meaningful smoothing. - if (num_sequences < 20) {return;} - - auto applyFilter = [&](const Eigen::Array & data) -> float { - return (data * filter).eval().sum(); - }; - - auto applyFilterOverAxis = - [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence, - const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void - { - float pt_m4 = hist_0, pt_m3 = hist_1, pt_m2 = hist_2, pt_m1 = hist_3; - float pt = initial_sequence(0); - float pt_p1 = initial_sequence(1), pt_p2 = initial_sequence(2); - float pt_p3 = initial_sequence(3), pt_p4 = initial_sequence(4); - - for (unsigned int idx = 0; idx != num_sequences; idx++) { - sequence(idx) = applyFilter( - {pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); - pt_m4 = pt_m3; pt_m3 = pt_m2; pt_m2 = pt_m1; pt_m1 = pt; - pt = pt_p1; pt_p1 = pt_p2; pt_p2 = pt_p3; pt_p3 = pt_p4; - pt_p4 = (idx + 5 < num_sequences) ? - initial_sequence(idx + 5) : initial_sequence(num_sequences); - } - }; - - const models::ControlSequence initial_control_sequence = control_sequence; - applyFilterOverAxis( - control_sequence.vx, initial_control_sequence.vx, - control_history[0].vx, control_history[1].vx, - control_history[2].vx, control_history[3].vx); - applyFilterOverAxis( - control_sequence.wz, initial_control_sequence.wz, - control_history[0].wz, control_history[1].wz, - control_history[2].wz, control_history[3].wz); - - unsigned int offset = settings.shift_control_sequence ? 1 : 0; - control_history[0] = control_history[1]; - control_history[1] = control_history[2]; - control_history[2] = control_history[3]; - control_history[3] = {control_sequence.vx(offset), control_sequence.wz(offset)}; -} - -inline unsigned int findClosestPathPt( - const std::vector & vec, const float dist, const unsigned int init = 0u) -{ - float distim1 = init != 0u ? vec[init] : 0.0f; - float disti = 0.0f; - const unsigned int size = vec.size(); - for (unsigned int i = init + 1; i != size; i++) { - disti = vec[i]; - if (disti > dist) { - if (i > 0 && dist - distim1 < disti - dist) {return i - 1;} - return i; - } - distim1 = disti; - } - return size - 1; -} - -struct Pose2D { float x, y, theta; }; - -/** - * @brief Shift columns of an Eigen Array left (-1) or right (1) by one place. - * Used in trajectory integration and control sequence shifting. - */ -inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) -{ - int size = e.size(); - if (size == 1) {return;} - if (abs(direction) != 1) { - throw std::logic_error("Invalid direction, only 1 and -1 are valid values."); - } - if ((e.cols() == 1 || e.rows() == 1) && size > 1) { - auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; - auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; - while (start_ptr != end_ptr) { - *(start_ptr + direction) = *start_ptr; - start_ptr -= direction; - } - *(start_ptr + direction) = *start_ptr; - } else { - auto start_ptr = direction == 1 ? - e.data() + size - 2 * e.rows() : e.data() + e.rows(); - auto end_ptr = direction == 1 ? - e.data() : e.data() + size - e.rows(); - auto span = e.rows(); - while (start_ptr != end_ptr) { - std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); - start_ptr -= (direction * span); - } - std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); - } -} - -inline float clamp(const float lower_bound, const float upper_bound, const float input) -{ - return std::min(upper_bound, std::max(input, lower_bound)); -} - -} // namespace mppi::utils - -#endif // ACKERMANN_MPPI__TOOLS__UTILS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/package.xml b/src/subsystems/minimal_navigation/ackermann_mppi/package.xml deleted file mode 100644 index cf682381..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - ackermann_mppi - 0.1.0 - - Standalone MPPI controller for Ackermann steering robots. - A minimal extraction of nav2_mppi_controller with the nav2 plugin - scaffolding removed — no pluginlib, no LifecycleNode dependency, - no GoalChecker, no OptimalTrajectoryValidator. - Runs as a plain rclcpp::Node. - - UMD Loop - Apache-2.0 - - ament_cmake - - angles - builtin_interfaces - geometry_msgs - nav2_costmap_2d - nav_msgs - rclcpp - std_msgs - tf2 - tf2_geometry_msgs - tf2_ros - visualization_msgs - - eigen3_cmake_module - Eigen3 - - - ament_cmake - - diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp deleted file mode 100644 index fed24ea7..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp +++ /dev/null @@ -1,329 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 - -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" - -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "std_msgs/msg/bool.hpp" - -#include "ackermann_mppi/optimizer.hpp" - -/** - * @class AckermannMPPINode - * @brief Standalone ROS2 node that runs MPPI for an Ackermann steering vehicle. - * - * Inputs: - * /global_path (nav_msgs/msg/Path) — global path to follow - * /odom (nav_msgs/msg/Odometry) — robot velocity feedback - * - * Outputs: - * /cmd_vel (geometry_msgs/msg/Twist) — velocity command - * - * The node also owns a Costmap2DROS instance (subscribes to sensor topics - * independently). Configure it via the standard nav2 costmap parameters. - * - * Key parameters (under the node's namespace): - * controller_frequency (double, default 10.0) — control loop rate in Hz - * plan_timeout (double, default 2.0) — seconds before stale plan triggers stop - * mppi.model_dt (float, default 0.05) — must equal 1/controller_frequency - * mppi.batch_size (int, default 1000) - * mppi.time_steps (int, default 56) - * mppi.vx_max (float, default 0.5) - * mppi.vx_min (float, default -0.35) - * mppi.wz_max (float, default 1.9) - * mppi.AckermannConstraints.min_turning_r (float, default 0.2) - * ... (see optimizer.cpp getParams() for full list) - */ -class AckermannMPPINode : public rclcpp::Node -{ -public: - explicit AckermannMPPINode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("ackermann_mppi", options) - { - // Declare parameters in constructor so they're available before configure() - declare_parameter("controller_frequency", 10.0); - declare_parameter("plan_timeout", 2.0); - declare_parameter("odom_topic", std::string("/odom")); - declare_parameter("plan_topic", std::string("/global_path")); - declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); - declare_parameter("cmd_vel_topic", std::string("/rear_ackermann_controller/reference")); - declare_parameter("robot_base_frame", std::string("base_link")); - declare_parameter("global_frame", std::string("map")); - } - - /** - * @brief Finish setup after construction (requires shared_from_this()). - * Call this immediately after make_shared(). - */ - void configure() - { - RCLCPP_INFO(get_logger(), "AckermannMPPINode configuring..."); - - // Cache frame names — these never change at runtime. - get_parameter("global_frame", global_frame_); - get_parameter("robot_base_frame", base_frame_); - get_parameter("plan_timeout", plan_timeout_s_); - - // TF buffer and listener - tf_ = std::make_shared(get_clock()); - tf_listener_ = std::make_shared(*tf_); - - // Costmap2DROS manages its own ROS2 lifecycle (sensors, inflation layers, etc). - // It needs to be spun in the same executor as this node. - // Parameters are read from the "local_costmap" namespace (standard nav2 costmap params). - costmap_ros_ = std::make_shared( - "local_costmap", - get_namespace(), // parent namespace - "local_costmap" // local namespace - ); - // Transfer the TF buffer so the costmap shares our TF instance - bool use_sim_time = false; - get_parameter_or("use_sim_time", use_sim_time, false); - costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", use_sim_time)); - // Empty plugin list: rcl_yaml_param_parser cannot represent an empty YAML - // sequence (plugins: []) — it produces a null rcl_variant_s that crashes - // NodeParameters. Set it programmatically instead. - costmap_ros_->set_parameter(rclcpp::Parameter("plugins", std::vector{})); - costmap_ros_->configure(); - costmap_ros_->activate(); - - optimizer_.initialize(shared_from_this(), "mppi", costmap_ros_, tf_); - - double controller_frequency; - get_parameter("controller_frequency", controller_frequency); - - // Subscriptions - std::string odom_topic, plan_topic, nav_enabled_topic; - get_parameter("odom_topic", odom_topic); - get_parameter("plan_topic", plan_topic); - get_parameter("nav_enabled_topic", nav_enabled_topic); - - // NOTE: both callbacks write shared state that is read by the control timer. - // The mutexes (plan_mutex_, odom_mutex_) below protect against the data race - // in the MultiThreadedExecutor where these run in parallel with controlLoop(). - odom_sub_ = create_subscription( - odom_topic, rclcpp::SensorDataQoS(), - [this](const nav_msgs::msg::Odometry::SharedPtr msg) { - std::lock_guard lock(odom_mutex_); - last_odom_ = *msg; - }); - - plan_sub_ = create_subscription( - plan_topic, rclcpp::QoS(1).transient_local(), - [this](const nav_msgs::msg::Path::SharedPtr msg) { - if (!msg->poses.empty()) { - std::lock_guard lock(plan_mutex_); - current_plan_ = *msg; - last_plan_time_ = now(); - RCLCPP_INFO(get_logger(), "[mppi] plan received: %zu poses", msg->poses.size()); - } else { - RCLCPP_WARN(get_logger(), "[mppi] received empty plan, ignoring"); - } - }); - - nav_enabled_sub_ = create_subscription( - nav_enabled_topic, 1, - [this](const std_msgs::msg::Bool::SharedPtr msg) { - nav_enabled_.store(msg->data); - RCLCPP_INFO(get_logger(), "[mppi] nav_enabled changed → %s", msg->data ? "TRUE" : "FALSE"); - }); - - // Publisher - std::string cmd_vel_topic; - get_parameter("cmd_vel_topic", cmd_vel_topic); - cmd_vel_pub_ = create_publisher(cmd_vel_topic, 1); - - // Control loop timer - const auto period = std::chrono::duration(1.0 / controller_frequency); - control_timer_ = create_wall_timer(period, [this]() { controlLoop(); }); - - RCLCPP_INFO( - get_logger(), - "AckermannMPPINode configured. Control loop: %.1f Hz, plan timeout: %.1f s", - controller_frequency, plan_timeout_s_); - } - - std::shared_ptr getCostmapROS() - { - return costmap_ros_; - } - - void deactivate() - { - control_timer_->cancel(); - optimizer_.shutdown(); - costmap_ros_->deactivate(); - costmap_ros_->cleanup(); - } - -private: - void controlLoop() - { - if (!nav_enabled_.load()) { - RCLCPP_DEBUG(get_logger(), "Navigation disabled — sending zero velocity."); - geometry_msgs::msg::TwistStamped zero; - zero.header.stamp = now(); - zero.header.frame_id = base_frame_; - cmd_vel_pub_->publish(zero); - return; - } - - // --- Snapshot shared state under locks --- - nav_msgs::msg::Path plan; - nav_msgs::msg::Odometry odom; - rclcpp::Time plan_time; - { - std::lock_guard lock(plan_mutex_); - if (current_plan_.poses.empty()) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "[mppi] nav enabled but no plan yet"); - return; - } - plan = current_plan_; - plan_time = last_plan_time_; - } - { - std::lock_guard lock(odom_mutex_); - odom = last_odom_; - } - - // --- Staleness check --- - const double plan_age = (now() - plan_time).seconds(); - if (plan_age > plan_timeout_s_) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 2000, - "Plan is stale (%.1f s old, timeout %.1f s) — sending zero velocity.", - plan_age, plan_timeout_s_); - geometry_msgs::msg::TwistStamped zero; - zero.header.stamp = now(); - zero.header.frame_id = base_frame_; - cmd_vel_pub_->publish(zero); - optimizer_.reset(); - return; - } - - // --- Look up robot pose in the global frame --- - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = global_frame_; - robot_pose.header.stamp = now(); - - try { - auto transform = tf_->lookupTransform( - global_frame_, base_frame_, - tf2::TimePointZero); - robot_pose.pose.position.x = transform.transform.translation.x; - robot_pose.pose.position.y = transform.transform.translation.y; - robot_pose.pose.position.z = transform.transform.translation.z; - robot_pose.pose.orientation = transform.transform.rotation; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 1000, - "Could not get robot pose: %s", ex.what()); - return; - } - - // Robot speed from odometry - geometry_msgs::msg::Twist robot_speed; - robot_speed.linear.x = odom.twist.twist.linear.x; - robot_speed.linear.y = odom.twist.twist.linear.y; - robot_speed.angular.z = odom.twist.twist.angular.z; - - // Use the last pose of the plan as the goal - const auto & goal = plan.poses.back().pose; - - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, - "[mppi] running evalControl — pose=(%.2f, %.2f) plan_poses=%zu", - robot_pose.pose.position.x, robot_pose.pose.position.y, plan.poses.size()); - - try { - auto [cmd, optimal_traj] = optimizer_.evalControl( - robot_pose, robot_speed, plan, goal); - geometry_msgs::msg::TwistStamped stamped; - stamped.header.stamp = now(); - stamped.header.frame_id = base_frame_; - stamped.twist = cmd.twist; - cmd_vel_pub_->publish(stamped); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR(get_logger(), "MPPI failed: %s — sending zero velocity.", ex.what()); - geometry_msgs::msg::TwistStamped zero; - zero.header.stamp = now(); - zero.header.frame_id = base_frame_; - cmd_vel_pub_->publish(zero); - optimizer_.reset(); - } - } - - // TF - std::shared_ptr tf_; - std::shared_ptr tf_listener_; - - // Costmap (sensor integration + inflation) - std::shared_ptr costmap_ros_; - - // MPPI optimizer - mppi::Optimizer optimizer_; - - // ROS interfaces - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr plan_sub_; - rclcpp::Subscription::SharedPtr nav_enabled_sub_; - rclcpp::Publisher::SharedPtr cmd_vel_pub_; - rclcpp::TimerBase::SharedPtr control_timer_; - - std::atomic nav_enabled_{true}; - - // Shared state — protected by corresponding mutexes. - // Written by subscription callbacks, read by control timer; all run in a - // MultiThreadedExecutor so concurrent access is real. - std::mutex plan_mutex_; - std::mutex odom_mutex_; - nav_msgs::msg::Path current_plan_; - nav_msgs::msg::Odometry last_odom_; - rclcpp::Time last_plan_time_{0, 0, RCL_ROS_TIME}; - - // Cached parameters (never change after configure()) - std::string global_frame_{"map"}; - std::string base_frame_{"base_link"}; - double plan_timeout_s_{2.0}; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - node->configure(); - - // Use a multi-threaded executor so the Costmap2DROS lifecycle node - // (which has its own callbacks for sensor data) runs alongside the control loop. - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.add_node(node->getCostmapROS()->get_node_base_interface()); - executor.spin(); - - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp deleted file mode 100644 index 3b985feb..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 "ackermann_mppi/critics/constraint_critic.hpp" - -namespace mppi::critics -{ - -void ConstraintCritic::initialize() -{ - auto getParam = getParamGetter(name_); - auto getParentParam = getParamGetter(parent_name_); - - getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 4.0f); - - // min_turning_r is vehicle geometry — fixed at init. - // vx_min/vx_max are read live from data.motion_model in score() so speed limits take effect. - getParentParam(min_turning_r_, "AckermannConstraints.min_turning_r", 0.2f); - - RCLCPP_INFO( - logger_, "ConstraintCritic (Ackermann) instantiated with %d power and %f weight.", - power_, weight_); -} - -void ConstraintCritic::score(CriticData & data) -{ - if (!enabled_) {return;} - - // Read velocity limits live so that setSpeedLimit() changes take effect immediately. - const auto & c = data.motion_model->getConstraints(); - const float max_vel = c.vx_max; - // Preserve original sign convention: vx_min is typically negative for reverse. - const float min_vel = (c.vx_min > 0.0f ? 1.0f : -1.0f) * std::abs(c.vx_min); - - auto & vx = data.state.vx; - auto & wz = data.state.wz; - - // Penalize steering ratio violations: |wz| <= |vx| / min_turning_r - constexpr float kEpsilon = 1e-6f; - auto wz_safe = wz.abs().max(kEpsilon); - auto out_of_turning_rad_motion = (min_turning_r_ - (vx.abs() / wz_safe)).max(0.0f); - - if (power_ > 1u) { - data.costs += ((((vx - max_vel).max(0.0f) + (min_vel - vx).max(0.0f) + - out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * - weight_).pow(power_).eval(); - } else { - data.costs += ((((vx - max_vel).max(0.0f) + (min_vel - vx).max(0.0f) + - out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_).eval(); - } -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp deleted file mode 100644 index 27c9baa7..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 "ackermann_mppi/critics/goal_critic.hpp" - -namespace mppi::critics -{ - -void GoalCritic::initialize() -{ - auto getParam = getParamGetter(name_); - getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 5.0f); - getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); - - RCLCPP_INFO( - logger_, "GoalCritic instantiated with %d power and %f weight.", - power_, weight_); -} - -void GoalCritic::score(CriticData & data) -{ - // Only activate when near the goal to provide terminal cost - if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { - return; - } - - geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); - const auto goal_x = goal.position.x; - const auto goal_y = goal.position.y; - - const auto delta_x = data.trajectories.x - goal_x; - const auto delta_y = data.trajectories.y - goal_y; - - if (power_ > 1u) { - data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * - weight_).pow(power_); - } else { - data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * - weight_).eval(); - } -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp deleted file mode 100644 index 128204e2..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2023 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. - -#include -#include "ackermann_mppi/critics/obstacles_critic.hpp" - -namespace mppi::critics -{ - -void ObstaclesCritic::initialize() -{ - auto getParam = getParamGetter(name_); - getParam(consider_footprint_, "consider_footprint", false); - getParam(power_, "cost_power", 1); - getParam(repulsion_weight_, "repulsion_weight", 1.5f); - getParam(critical_weight_, "critical_weight", 20.0f); - getParam(collision_cost_, "collision_cost", 100000.0f); - getParam(collision_margin_distance_, "collision_margin_distance", 0.10f); - getParam(near_goal_distance_, "near_goal_distance", 0.5f); - getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); - - collision_checker_.setCostmap(costmap_); - possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - - if (possible_collision_cost_ < 1.0f) { - RCLCPP_ERROR( - logger_, - "Inflation layer either not found or inflation radius is not set sufficiently for " - "non-circular collision checking. Set inflation_radius >= half of the robot's largest " - "cross-section. This will substantially impact run-time performance."); - } - - RCLCPP_INFO( - logger_, - "ObstaclesCritic instantiated with %d power and %f / %f weights. " - "Collision check based on %s cost.", - power_, critical_weight_, repulsion_weight_, - consider_footprint_ ? "footprint" : "circular"); -} - -float ObstaclesCritic::findCircumscribedCost( - std::shared_ptr costmap) -{ - double result = -1.0; - const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); - if (static_cast(circum_radius) == circumscribed_radius_) { - return circumscribed_cost_; - } - - std::shared_ptr inflation_layer; - for (auto & layer : *costmap->getLayeredCostmap()->getPlugins()) { - auto candidate = std::dynamic_pointer_cast(layer); - if (candidate && - (inflation_layer_name_.empty() || candidate->getName() == inflation_layer_name_)) - { - inflation_layer = candidate; - break; - } - } - if (inflation_layer != nullptr) { - const double resolution = costmap->getCostmap()->getResolution(); - result = inflation_layer->computeCost(circum_radius / resolution); - const std::string & ln = inflation_layer->getName(); - rclcpp::Parameter p; - if (costmap->get_parameter(ln + ".cost_scaling_factor", p)) { - inflation_scale_factor_ = static_cast(p.as_double()); - } - if (costmap->get_parameter(ln + ".inflation_radius", p)) { - inflation_radius_ = static_cast(p.as_double()); - } - } else { - RCLCPP_WARN( - logger_, - "No inflation layer found. Cannot use costmap potential field for fast collision " - "pre-screening. Only absolute collisions will be detected."); - } - - circumscribed_radius_ = static_cast(circum_radius); - circumscribed_cost_ = static_cast(result); - return circumscribed_cost_; -} - -float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) -{ - const float scale_factor = inflation_scale_factor_; - const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); - // nav2 inflation layer maps INSCRIBED_INFLATED_OBSTACLE (254) - 1 = 253 to the inscribed - // radius. We invert the exponential cost formula to recover metric distance. - constexpr float kNavInscribedCostThreshold = 253.0f; - float dist_to_obj = - (scale_factor * min_radius - logf(cost.cost) + logf(kNavInscribedCostThreshold)) / - scale_factor; - if (!cost.using_footprint) { - dist_to_obj -= min_radius; - } - return dist_to_obj; -} - -void ObstaclesCritic::score(CriticData & data) -{ - if (!enabled_) {return;} - - if (consider_footprint_) { - possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - } - - bool near_goal = data.state.local_path_length < near_goal_distance_; - - Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size()); - Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size()); - - const unsigned int traj_len = data.trajectories.x.cols(); - const unsigned int batch_size = data.trajectories.x.rows(); - bool all_trajectories_collide = true; - - auto & collisions = data.trajectories_in_collision; - const bool track_collisions = !collisions.empty(); - - for (unsigned int i = 0; i != batch_size; i++) { - bool trajectory_collide = false; - float traj_cost = 0.0f; - const auto & traj = data.trajectories; - CollisionCost pose_cost; - - for (unsigned int j = 0; j != traj_len; j++) { - pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); - if (pose_cost.cost < 1.0f) {continue;} - - if (inCollision(pose_cost.cost)) { - trajectory_collide = true; - break; - } - - if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {continue;} - - const float dist_to_obj = distanceToObstacle(pose_cost); - if (dist_to_obj < collision_margin_distance_) { - traj_cost += (collision_margin_distance_ - dist_to_obj); - } - if (!near_goal) { - repulsive_cost[i] += inflation_radius_ - dist_to_obj; - } - } - - if (!trajectory_collide) {all_trajectories_collide = false;} - raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost; - if (trajectory_collide && track_collisions) {collisions[i] = true;} - } - - auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len; - - if (power_ > 1u) { - data.costs += - ((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized)) - .pow(power_); - } else { - data.costs += - (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized); - } - - data.fail_flag = all_trajectories_collide; -} - -bool ObstaclesCritic::inCollision(float cost) const -{ - bool is_tracking_unknown = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); - using namespace nav2_costmap_2d; // NOLINT - switch (static_cast(cost)) { - case (LETHAL_OBSTACLE): - return true; - case (INSCRIBED_INFLATED_OBSTACLE): - return consider_footprint_ ? false : true; - case (NO_INFORMATION): - return is_tracking_unknown ? false : true; - } - return false; -} - -CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) -{ - CollisionCost collision_cost; - float & cost = collision_cost.cost; - collision_cost.using_footprint = false; - - unsigned int x_i, y_i; - if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { - cost = nav2_costmap_2d::NO_INFORMATION; - return collision_cost; - } - cost = collision_checker_.pointCost(x_i, y_i); - - if (consider_footprint_ && - (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) - { - cost = static_cast(collision_checker_.footprintCostAtPose( - x, y, theta, costmap_ros_->getRobotFootprint())); - collision_cost.using_footprint = true; - } - return collision_cost; -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp deleted file mode 100644 index be4813a5..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp +++ /dev/null @@ -1,141 +0,0 @@ -// Copyright (c) 2023 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. - -#include "ackermann_mppi/critics/path_align_critic.hpp" - -namespace mppi::critics -{ - -void PathAlignCritic::initialize() -{ - auto getParam = getParamGetter(name_); - getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 10.0f); - getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); - getParam(offset_from_furthest_, "offset_from_furthest", 20); - getParam(trajectory_point_step_, "trajectory_point_step", 4); - getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); - getParam(use_path_orientations_, "use_path_orientations", false); - - RCLCPP_INFO( - logger_, "PathAlignCritic instantiated with %d power and %f weight", - power_, weight_); -} - -void PathAlignCritic::score(CriticData & data) -{ - if (!enabled_ || data.state.local_path_length < threshold_to_consider_) {return;} - - utils::setPathFurthestPointIfNotSet(data); - const size_t path_segments_count = *data.furthest_reached_path_point; - float path_segments_flt = static_cast(path_segments_count); - if (path_segments_count < offset_from_furthest_) {return;} - - utils::setPathCostsIfNotSet(data, costmap_ros_); - std::vector & path_pts_valid = *data.path_pts_valid; - float invalid_ctr = 0.0f; - for (size_t i = 0; i < path_segments_count; i++) { - if (!path_pts_valid[i]) {invalid_ctr += 1.0f;} - if (invalid_ctr / path_segments_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) { - return; - } - } - - const size_t batch_size = data.trajectories.x.rows(); - Eigen::ArrayXf cost(data.costs.rows()); - cost.setZero(); - - std::vector path_integrated_distances(path_segments_count, 0.0f); - std::vector path(path_segments_count); - float dx = 0.0f, dy = 0.0f; - for (unsigned int i = 1; i != path_segments_count; i++) { - auto & pose = path[i - 1]; - pose.x = data.path.x(i - 1); - pose.y = data.path.y(i - 1); - pose.theta = data.path.yaws(i - 1); - dx = data.path.x(i) - pose.x; - dy = data.path.y(i) - pose.y; - path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy); - } - - auto & final_pose = path[path_segments_count - 1]; - final_pose.x = data.path.x(path_segments_count - 1); - final_pose.y = data.path.y(path_segments_count - 1); - final_pose.theta = data.path.yaws(path_segments_count - 1); - - int strided_traj_rows = data.trajectories.x.rows(); - int strided_traj_cols = - static_cast(floor((data.trajectories.x.cols() - 1) / trajectory_point_step_)) + 1; - int outer_stride = strided_traj_rows * trajectory_point_step_; - - const auto T_x = Eigen::Map>( - data.trajectories.x.data(), - strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto T_y = Eigen::Map>( - data.trajectories.y.data(), - strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto T_yaw = Eigen::Map>( - data.trajectories.yaws.data(), - strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto traj_sampled_size = T_x.cols(); - - float summed_path_dist = 0.0f, dyaw = 0.0f; - unsigned int num_samples = 0u, path_pt = 0u; - float traj_integrated_distance = 0.0f; - float Tx_m1, Ty_m1; - - for (size_t t = 0; t < batch_size; ++t) { - summed_path_dist = 0.0f; - num_samples = 0u; - traj_integrated_distance = 0.0f; - path_pt = 0u; - Tx_m1 = T_x(t, 0); - Ty_m1 = T_y(t, 0); - - for (int p = 1; p < traj_sampled_size; p++) { - const float Tx = T_x(t, p); - const float Ty = T_y(t, p); - dx = Tx - Tx_m1; - dy = Ty - Ty_m1; - Tx_m1 = Tx; - Ty_m1 = Ty; - traj_integrated_distance += sqrtf(dx * dx + dy * dy); - path_pt = utils::findClosestPathPt(path_integrated_distances, traj_integrated_distance, - path_pt); - - if (path_pts_valid[path_pt]) { - const auto & pose = path[path_pt]; - dx = pose.x - Tx; - dy = pose.y - Ty; - num_samples++; - if (use_path_orientations_) { - dyaw = angles::shortest_angular_distance(pose.theta, T_yaw(t, p)); - summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); - } else { - summed_path_dist += sqrtf(dx * dx + dy * dy); - } - } - } - cost(t) = num_samples > 0u ? - summed_path_dist / static_cast(num_samples) : 0.0f; - } - - if (power_ > 1u) { - data.costs += (cost * weight_).pow(power_).eval(); - } else { - data.costs += (cost * weight_).eval(); - } -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp deleted file mode 100644 index d6e1207e..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 "ackermann_mppi/critics/path_follow_critic.hpp" -#include - -namespace mppi::critics -{ - -void PathFollowCritic::initialize() -{ - auto getParam = getParamGetter(name_); - getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); - getParam(offset_from_furthest_, "offset_from_furthest", 6); - getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 5.0f); -} - -void PathFollowCritic::score(CriticData & data) -{ - if (!enabled_) {return;} - - if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) { - return; - } - - utils::setPathFurthestPointIfNotSet(data); - utils::setPathCostsIfNotSet(data, costmap_ros_); - const size_t path_size = data.path.x.size() - 1; - - auto offsetted_idx = std::min( - *data.furthest_reached_path_point + offset_from_furthest_, path_size); - - // Drive to first valid (non-obstructed) path point at or after the lookahead - bool valid = false; - while (!valid && offsetted_idx < path_size - 1) { - valid = (*data.path_pts_valid)[offsetted_idx]; - if (!valid) {offsetted_idx++;} - } - - const auto path_x = data.path.x(offsetted_idx); - const auto path_y = data.path.y(offsetted_idx); - - const int rightmost_idx = data.trajectories.x.cols() - 1; - const auto last_x = data.trajectories.x.col(rightmost_idx); - const auto last_y = data.trajectories.y.col(rightmost_idx); - - const auto delta_x = last_x - path_x; - const auto delta_y = last_y - path_y; - - if (power_ > 1u) { - data.costs += (((delta_x.square() + delta_y.square()).sqrt()) * weight_).pow(power_); - } else { - data.costs += ((delta_x.square() + delta_y.square()).sqrt()) * weight_; - } -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp deleted file mode 100644 index 2160d7f2..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp +++ /dev/null @@ -1,103 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 "ackermann_mppi/tools/noise_generator.hpp" - -namespace mppi -{ - -void NoiseGenerator::initialize( - mppi::models::OptimizerSettings & settings, - bool regenerate_noises) -{ - settings_ = settings; - regenerate_noises_ = regenerate_noises; - active_ = true; - - ndistribution_vx_ = std::normal_distribution(0.0f, settings_.sampling_std.vx); - ndistribution_wz_ = std::normal_distribution(0.0f, settings_.sampling_std.wz); - - if (regenerate_noises_) { - noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); - } else { - generateNoisedControls(); - } -} - -void NoiseGenerator::shutdown() -{ - active_ = false; - ready_ = true; - noise_cond_.notify_all(); - if (noise_thread_.joinable()) { - noise_thread_.join(); - } -} - -void NoiseGenerator::generateNextNoises() -{ - { - std::unique_lock guard(noise_lock_); - ready_ = true; - } - noise_cond_.notify_all(); -} - -void NoiseGenerator::setNoisedControls( - models::State & state, - const models::ControlSequence & control_sequence) -{ - std::unique_lock guard(noise_lock_); - state.cvx = noises_vx_.rowwise() + control_sequence.vx.transpose(); - state.cwz = noises_wz_.rowwise() + control_sequence.wz.transpose(); -} - -void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings) -{ - settings_ = settings; - - { - std::unique_lock guard(noise_lock_); - noises_vx_.setZero(settings_.batch_size, settings_.time_steps); - noises_wz_.setZero(settings_.batch_size, settings_.time_steps); - ready_ = true; - } - - if (regenerate_noises_) { - noise_cond_.notify_all(); - } else { - generateNoisedControls(); - } -} - -void NoiseGenerator::noiseThread() -{ - do { - std::unique_lock guard(noise_lock_); - noise_cond_.wait(guard, [this]() {return ready_;}); - ready_ = false; - generateNoisedControls(); - } while (active_); -} - -void NoiseGenerator::generateNoisedControls() -{ - auto & s = settings_; - noises_vx_ = Eigen::ArrayXXf::NullaryExpr( - s.batch_size, s.time_steps, [&]() {return ndistribution_vx_(generator_);}); - noises_wz_ = Eigen::ArrayXXf::NullaryExpr( - s.batch_size, s.time_steps, [&]() {return ndistribution_wz_(generator_);}); -} - -} // namespace mppi diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp deleted file mode 100644 index 17d75f66..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp +++ /dev/null @@ -1,486 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2025 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. - -#include "ackermann_mppi/optimizer.hpp" - -#include -#include -#include -#include -#include -#include - -#include "ackermann_mppi/critics/path_follow_critic.hpp" -#include "ackermann_mppi/critics/path_align_critic.hpp" -#include "ackermann_mppi/critics/goal_critic.hpp" -#include "ackermann_mppi/critics/obstacles_critic.hpp" -#include "ackermann_mppi/critics/constraint_critic.hpp" - -namespace mppi -{ - -Optimizer::Optimizer() = default; - -Optimizer::~Optimizer() -{ - shutdown(); -} - -void Optimizer::initialize( - rclcpp::Node::SharedPtr node, - const std::string & name, - std::shared_ptr costmap_ros, - std::shared_ptr tf_buffer) -{ - node_ = node; - name_ = name; - costmap_ros_ = costmap_ros; - costmap_ = costmap_ros_->getCostmap(); - tf_buffer_ = tf_buffer; - logger_ = node_->get_logger(); - base_frame_ = costmap_ros_->getBaseFrameID(); - - getParams(); - loadCritics(); - - bool regenerate_noises = false; - declareParam(name_ + ".regenerate_noises", false); - node_->get_parameter(name_ + ".regenerate_noises", regenerate_noises); - noise_generator_.initialize(settings_, regenerate_noises); - - reset(); -} - -void Optimizer::loadCritics() -{ - // Direct instantiation — add or remove critics here to change scoring behavior. - // Order matters: critics run in order and can set fail_flag to short-circuit the rest. - auto add = [&](auto * raw, const std::string & short_name) { - critics_.emplace_back(raw); - const std::string full_name = name_ + "." + short_name; - critics_.back()->on_configure(node_, name_, full_name, costmap_ros_); - RCLCPP_INFO(logger_, "Loaded critic: %s", short_name.c_str()); - }; - - add(new critics::ConstraintCritic(), "ConstraintCritic"); - add(new critics::ObstaclesCritic(), "ObstaclesCritic"); - add(new critics::PathFollowCritic(), "PathFollowCritic"); - add(new critics::PathAlignCritic(), "PathAlignCritic"); - add(new critics::GoalCritic(), "GoalCritic"); -} - -void Optimizer::shutdown() -{ - noise_generator_.shutdown(); -} - -void Optimizer::getParams() -{ - auto & s = settings_; - auto getParam = getParamGetter(name_); - - getParam(s.model_dt, "model_dt", 0.05f); - getParam(s.time_steps, "time_steps", 56); - getParam(s.batch_size, "batch_size", 1000); - getParam(s.iteration_count, "iteration_count", 1); - getParam(s.temperature, "temperature", 0.3f); - getParam(s.gamma, "gamma", 0.015f); - getParam(s.base_constraints.vx_max, "vx_max", 0.5f); - getParam(s.base_constraints.vx_min, "vx_min", -0.35f); - getParam(s.base_constraints.wz, "wz_max", 1.9f); - getParam(s.base_constraints.ax_max, "ax_max", 3.0f); - getParam(s.base_constraints.ax_min, "ax_min", -3.0f); - getParam(s.base_constraints.az_max, "az_max", 3.5f); - getParam(s.sampling_std.vx, "vx_std", 0.2f); - getParam(s.sampling_std.wz, "wz_std", 0.4f); - getParam(s.retry_attempt_limit, "retry_attempt_limit", 1); - getParam(s.open_loop, "open_loop", false); - getParam(s.visualize, "visualize", false); - getParam(goal_dist_tolerance_, "goal_dist_tolerance", 0.25f); - - s.base_constraints.ax_max = fabs(s.base_constraints.ax_max); - if (s.base_constraints.ax_min > 0.0f) { - s.base_constraints.ax_min = -s.base_constraints.ax_min; - RCLCPP_WARN(logger_, "ax_min sign incorrect, setting negative."); - } - - float min_turning_r = 0.2f; - getParam(min_turning_r, "AckermannConstraints.min_turning_r", 0.2f); - motion_model_ = std::make_shared(min_turning_r); - - s.constraints = s.base_constraints; - - // Determine if control period matches model_dt to enable sequence shifting - double controller_frequency = 10.0; - declareParam("controller_frequency", 10.0); - node_->get_parameter("controller_frequency", controller_frequency); - const double controller_period = 1.0 / controller_frequency; - constexpr double eps = 1e-6; - if (std::abs(controller_period - s.model_dt) < eps) { - s.shift_control_sequence = true; - RCLCPP_INFO(logger_, "Control sequence shifting enabled (controller_period == model_dt)."); - } else if (controller_period > s.model_dt + eps) { - RCLCPP_WARN( - logger_, - "controller_frequency (%.2f Hz, period=%.4f s) > model_dt (%.4f s). " - "Set controller_frequency = 1/model_dt for best performance.", - controller_frequency, controller_period, s.model_dt); - } -} - -void Optimizer::reset(bool reset_dynamic_speed_limits) -{ - state_.reset(settings_.batch_size, settings_.time_steps); - control_sequence_.reset(settings_.time_steps); - control_history_.fill({}); - - if (settings_.open_loop) { - last_command_vel_ = geometry_msgs::msg::Twist(); - } - - if (reset_dynamic_speed_limits) { - settings_.constraints = settings_.base_constraints; - } - - costs_.setZero(settings_.batch_size); - generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); - noise_generator_.reset(settings_); - motion_model_->initialize(settings_.constraints, settings_.model_dt); - - // Update critic_data_ references (they hold refs to member variables) - critics_data_.goal_dist_tolerance = goal_dist_tolerance_; - - RCLCPP_INFO(logger_, "Optimizer reset"); -} - -bool Optimizer::isSpeedLimitActive() const -{ - const auto & base = settings_.base_constraints; - const auto & curr = settings_.constraints; - return base.vx_max != curr.vx_max || - base.vx_min != curr.vx_min || - base.wz != curr.wz; -} - -std::tuple Optimizer::evalControl( - const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal) -{ - prepare(robot_pose, robot_speed, plan, goal); - Eigen::ArrayXXf optimal_trajectory; - - do { - optimize(); - optimal_trajectory = getOptimizedTrajectory(); - } while (fallback(critics_data_.fail_flag)); - - auto control = getControlFromSequenceAsTwist(plan.header.stamp); - last_command_vel_ = control.twist; - - if (settings_.shift_control_sequence) { - shiftControlSequence(); - } - - return std::make_tuple(control, optimal_trajectory); -} - -void Optimizer::optimize() -{ - for (size_t i = 0; i < settings_.iteration_count; ++i) { - generateNoisedTrajectories(); - evalTrajectoriesScores(); - updateControlSequence(); - } -} - -void Optimizer::evalTrajectoriesScores() -{ - critic_costs_.clear(); - - for (auto & critic : critics_) { - if (critics_data_.fail_flag) {break;} - - if (settings_.visualize) { - Eigen::ArrayXf costs_before = critics_data_.costs; - critic->score(critics_data_); - critic_costs_.emplace_back(critic->getName(), critics_data_.costs - costs_before); - } else { - critic->score(critics_data_); - } - } -} - -bool Optimizer::fallback(bool fail) -{ - if (!fail) { - fallback_counter_ = 0; - return false; - } - - reset(false); - - if (++fallback_counter_ > settings_.retry_attempt_limit) { - fallback_counter_ = 0; - throw std::runtime_error("AckermannMPPI: all trajectories in collision, no valid control."); - } - return true; -} - -void Optimizer::prepare( - const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal) -{ - state_.pose = robot_pose; - state_.speed = settings_.open_loop ? last_command_vel_ : robot_speed; - - // Compute approximate path length (sum of Euclidean segment distances) - float path_length = 0.0f; - for (size_t i = 1; i < plan.poses.size(); ++i) { - float dx = plan.poses[i].pose.position.x - plan.poses[i - 1].pose.position.x; - float dy = plan.poses[i].pose.position.y - plan.poses[i - 1].pose.position.y; - path_length += sqrtf(dx * dx + dy * dy); - } - state_.local_path_length = path_length; - - path_ = utils::toTensor(plan); - costs_.setZero(settings_.batch_size); - goal_ = goal; - - critics_data_.fail_flag = false; - critics_data_.motion_model = motion_model_; - critics_data_.furthest_reached_path_point.reset(); - critics_data_.path_pts_valid.reset(); - critics_data_.trajectories_in_collision.clear(); -} - -void Optimizer::shiftControlSequence() -{ - auto size = control_sequence_.vx.size(); - utils::shiftColumnsByOnePlace(control_sequence_.vx, -1); - utils::shiftColumnsByOnePlace(control_sequence_.wz, -1); - control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2); - control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2); -} - -void Optimizer::generateNoisedTrajectories() -{ - noise_generator_.setNoisedControls(state_, control_sequence_); - noise_generator_.generateNextNoises(); - updateStateVelocities(state_); - integrateStateVelocities(generated_trajectories_, state_); -} - -void Optimizer::applyControlSequenceConstraints() -{ - auto & s = settings_; - - float max_delta_vx = s.model_dt * s.constraints.ax_max; - float min_delta_vx = s.model_dt * s.constraints.ax_min; - float max_delta_wz = s.model_dt * s.constraints.az_max; - - float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, - control_sequence_.vx(0)); - float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0)); - control_sequence_.vx(0) = vx_last; - control_sequence_.wz(0) = wz_last; - - for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { - float & vx_curr = control_sequence_.vx(i); - vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr); - if (vx_last > 0) { - vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr); - } else { - vx_curr = utils::clamp(vx_last - max_delta_vx, vx_last - min_delta_vx, vx_curr); - } - vx_last = vx_curr; - - float & wz_curr = control_sequence_.wz(i); - wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr); - wz_curr = utils::clamp(wz_last - max_delta_wz, wz_last + max_delta_wz, wz_curr); - wz_last = wz_curr; - } - - motion_model_->applyConstraints(control_sequence_); -} - -void Optimizer::updateStateVelocities(models::State & state) const -{ - updateInitialStateVelocities(state); - propagateStateVelocitiesFromInitials(state); -} - -void Optimizer::updateInitialStateVelocities(models::State & state) const -{ - state.vx.col(0) = static_cast(state.speed.linear.x); - state.wz.col(0) = static_cast(state.speed.angular.z); - // vy is always zero for Ackermann (non-holonomic) -} - -void Optimizer::propagateStateVelocitiesFromInitials(models::State & state) const -{ - motion_model_->predict(state); -} - -void Optimizer::integrateStateVelocities( - Eigen::Array & trajectory, - const Eigen::ArrayXXf & sequence) const -{ - float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); - - const auto vx = sequence.col(0); - const auto wz = sequence.col(1); - auto traj_x = trajectory.col(0); - auto traj_y = trajectory.col(1); - auto traj_yaws = trajectory.col(2); - - const size_t n_size = traj_yaws.size(); - if (n_size == 0) {return;} - - float last_yaw = initial_yaw; - for (size_t i = 0; i != n_size; i++) { - last_yaw += wz(i) * settings_.model_dt; - traj_yaws(i) = last_yaw; - } - - Eigen::ArrayXf yaw_cos = traj_yaws.cos(); - Eigen::ArrayXf yaw_sin = traj_yaws.sin(); - utils::shiftColumnsByOnePlace(yaw_cos, 1); - utils::shiftColumnsByOnePlace(yaw_sin, 1); - yaw_cos(0) = cosf(initial_yaw); - yaw_sin(0) = sinf(initial_yaw); - - float last_x = state_.pose.pose.position.x; - float last_y = state_.pose.pose.position.y; - for (size_t i = 0; i != n_size; i++) { - last_x += vx(i) * yaw_cos(i) * settings_.model_dt; - last_y += vx(i) * yaw_sin(i) * settings_.model_dt; - traj_x(i) = last_x; - traj_y(i) = last_y; - } -} - -void Optimizer::integrateStateVelocities( - models::Trajectories & trajectories, - const models::State & state) const -{ - auto initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); - const size_t n_cols = trajectories.yaws.cols(); - - Eigen::ArrayXf last_yaws = Eigen::ArrayXf::Constant(trajectories.yaws.rows(), initial_yaw); - for (size_t i = 0; i != n_cols; i++) { - last_yaws += state.wz.col(i) * settings_.model_dt; - trajectories.yaws.col(i) = last_yaws; - } - - Eigen::ArrayXXf yaw_cos = trajectories.yaws.cos(); - Eigen::ArrayXXf yaw_sin = trajectories.yaws.sin(); - utils::shiftColumnsByOnePlace(yaw_cos, 1); - utils::shiftColumnsByOnePlace(yaw_sin, 1); - yaw_cos.col(0) = cosf(initial_yaw); - yaw_sin.col(0) = sinf(initial_yaw); - - // Ackermann: no vy term - auto dx = (state.vx * yaw_cos).eval(); - auto dy = (state.vx * yaw_sin).eval(); - - Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant( - trajectories.x.rows(), state.pose.pose.position.x); - Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant( - trajectories.y.rows(), state.pose.pose.position.y); - - for (size_t i = 0; i != n_cols; i++) { - last_x += dx.col(i) * settings_.model_dt; - last_y += dy.col(i) * settings_.model_dt; - trajectories.x.col(i) = last_x; - trajectories.y.col(i) = last_y; - } -} - -Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() -{ - // Build [time_steps x 2] sequence (vx, wz) and integrate to poses - Eigen::ArrayXXf sequence(settings_.time_steps, 2); - Eigen::Array trajectory(settings_.time_steps, 3); - - sequence.col(0) = control_sequence_.vx; - sequence.col(1) = control_sequence_.wz; - - integrateStateVelocities(trajectory, sequence); - return trajectory; -} - -void Optimizer::updateControlSequence() -{ - auto & s = settings_; - - auto vx_T = control_sequence_.vx.transpose(); - auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; - const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx); - costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); - - if (s.sampling_std.wz > 0.0f) { - auto wz_T = control_sequence_.wz.transpose(); - auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; - const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz); - costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); - } - - auto costs_normalized = costs_ - costs_.minCoeff(); - const float inv_temp = 1.0f / s.temperature; - auto softmaxes = (-inv_temp * costs_normalized).exp().eval(); - softmaxes /= softmaxes.sum(); - - auto softmax_mat = softmaxes.matrix(); - control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat; - control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat; - - utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); - applyControlSequenceConstraints(); -} - -geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( - const builtin_interfaces::msg::Time & stamp) -{ - unsigned int offset = settings_.shift_control_sequence ? 1 : 0; - return utils::toTwistStamped( - control_sequence_.vx(offset), - control_sequence_.wz(offset), - stamp, base_frame_); -} - -void Optimizer::setSpeedLimit(double speed_limit, bool percentage) -{ - auto & s = settings_; - // nav2 costmap speed filter sentinel: 255.0 means "no limit active". - // Matches nav2_costmap_2d::NO_SPEED_LIMIT from filter_values.hpp. - constexpr double NO_SPEED_LIMIT = 255.0; - if (speed_limit == NO_SPEED_LIMIT) { - s.constraints.vx_max = s.base_constraints.vx_max; - s.constraints.vx_min = s.base_constraints.vx_min; - s.constraints.wz = s.base_constraints.wz; - } else { - double ratio = percentage ? speed_limit / 100.0 : speed_limit / s.base_constraints.vx_max; - s.constraints.vx_max = s.base_constraints.vx_max * ratio; - s.constraints.vx_min = s.base_constraints.vx_min * ratio; - s.constraints.wz = s.base_constraints.wz * ratio; - } - motion_model_->initialize(settings_.constraints, settings_.model_dt); -} - -} // namespace mppi diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt b/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt deleted file mode 100644 index 80e5cd00..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt +++ /dev/null @@ -1,114 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(athena_smac_planner) - -find_package(ament_cmake REQUIRED) -find_package(angles REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav2_common REQUIRED) -find_package(nav2_core REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(nav2_smoother REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(nlohmann_json REQUIRED) -find_package(ompl REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(msgs REQUIRED) - -nav2_package() - -if(MSVC) - add_compile_definitions(_USE_MATH_DEFINES) -else() - add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) -endif() - -set(library_name athena_smac_planner) - -add_library(${library_name} SHARED - src/a_star.cpp - src/analytic_expansion.cpp - src/collision_checker.cpp - src/costmap_downsampler.cpp - src/node_basic.cpp - src/node_hybrid.cpp - src/obstacle_heuristic.cpp - src/distance_heuristic.cpp - src/smoother.cpp - src/smac_planner_hybrid.cpp -) -target_include_directories(${library_name} - PUBLIC - "$" - "$" - ${OMPL_INCLUDE_DIRS} - ${nav2_core_INCLUDE_DIRS} - ${nav2_smoother_INCLUDE_DIRS} -) -target_link_libraries(${library_name} PUBLIC - ${geometry_msgs_TARGETS} - nav2_costmap_2d::layers - nav2_costmap_2d::nav2_costmap_2d_core - ${nav_msgs_TARGETS} - nlohmann_json::nlohmann_json - ${OMPL_LIBRARIES} - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle - tf2::tf2 - tf2_ros::tf2_ros - ${visualization_msgs_TARGETS} -) -target_link_libraries(${library_name} PRIVATE - angles::angles -) - -install(TARGETS ${library_name} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -add_executable(global_planner_node src/global_planner_node.cpp) -target_include_directories(global_planner_node PRIVATE - "$" - ${OMPL_INCLUDE_DIRS} - ${nav2_core_INCLUDE_DIRS} -) -target_link_libraries(global_planner_node - ${library_name} - rclcpp::rclcpp - ${geometry_msgs_TARGETS} - ${nav_msgs_TARGETS} - nav2_costmap_2d::nav2_costmap_2d_core - ${msgs_TARGETS} -) - -install(TARGETS global_planner_node - DESTINATION lib/${PROJECT_NAME} -) - -ament_export_include_directories(include/${PROJECT_NAME}) -ament_export_libraries(${library_name}) -ament_export_dependencies( - geometry_msgs - nav2_core - nav2_costmap_2d - nav_msgs - nlohmann_json - ompl - rclcpp - rclcpp_lifecycle - tf2 - tf2_ros - visualization_msgs -) -ament_export_targets(${PROJECT_NAME}) -ament_package() diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp deleted file mode 100644 index fc587fb8..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp +++ /dev/null @@ -1,323 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__A_STAR_HPP_ -#define ATHENA_SMAC_PLANNER__A_STAR_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_core/exceptions.hpp" - -#include "athena_smac_planner/thirdparty/robin_hood.h" -#include "athena_smac_planner/analytic_expansion.hpp" -#include "athena_smac_planner/node_hybrid.hpp" -#include "athena_smac_planner/node_basic.hpp" -#include "athena_smac_planner/goal_manager.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/constants.hpp" - -namespace athena_smac_planner -{ - -/** - * @class athena_smac_planner::AStarAlgorithm - * @brief An A* implementation for planning in a costmap. Templated based on the Node type. - */ -template -class AStarAlgorithm -{ -public: - typedef NodeT * NodePtr; - typedef robin_hood::unordered_node_map Graph; - typedef std::vector NodeVector; - typedef std::pair> NodeElement; - typedef typename NodeT::Coordinates Coordinates; - typedef typename NodeT::CoordinateVector CoordinateVector; - typedef typename NodeVector::iterator NeighborIterator; - typedef std::function NodeGetter; - typedef GoalManager GoalManagerT; - using NodeContext = typename NodeT::NodeContext; - - - /** - * @struct athena_smac_planner::NodeComparator - * @brief Node comparison for priority queue sorting - */ - struct NodeComparator - { - bool operator()(const NodeElement & a, const NodeElement & b) const - { - return a.first > b.first; - } - }; - - typedef std::priority_queue, NodeComparator> NodeQueue; - - /** - * @brief A constructor for athena_smac_planner::AStarAlgorithm - */ - explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); - - /** - * @brief A destructor for athena_smac_planner::AStarAlgorithm - */ - ~AStarAlgorithm(); - - /** - * @brief Initialization of the planner with defaults - * @param allow_unknown Allow search in unknown space, good for navigation while mapping - * @param max_iterations Maximum number of iterations to use while expanding search - * @param max_on_approach_iterations Maximum number of iterations before returning a valid - * path once within thresholds to refine path - * comes at more compute time but smoother paths. - * @param terminal_checking_interval Number of iterations to check if the task has been canceled or - * or planning time exceeded - * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns - * false after this timeout - * @param lookup_table_size Size of the lookup table to store heuristic values - * @param dim_3_size Number of quantization bins - */ - void initialize( - const bool & allow_unknown, - int & max_iterations, - const int & max_on_approach_iterations, - const int & terminal_checking_interval, - const double & max_planning_time, - const float & lookup_table_size, - const unsigned int & dim_3_size); - - /** - * @brief Creating path from given costmap, start, and goal - * @param path Reference to a vector of indices of generated path - * @param num_iterations Reference to number of iterations to create plan - * @param tolerance Reference to tolerance in costmap nodes - * @param cancel_checker Function to check if the task has been canceled - * @param expansions_log Optional expansions logged for debug - * @return if plan was successful - */ - bool createPath( - CoordinateVector & path, int & num_iterations, const float & tolerance, - std::function cancel_checker, - std::vector> * expansions_log = nullptr); - - /** - * @brief Update the search info used by the algorithm - * @param search_info Search info to update - */ - void setSearchInfo(const SearchInfo & search_info) {_search_info = search_info;} - - /** - * @brief Sets the collision checker to use - * @param collision_checker Collision checker to use for checking state validity - */ - void setCollisionChecker(GridCollisionChecker * collision_checker); - - /** - * @brief Set the goal for planning, as a node index - * @param mx The node X index of the goal - * @param my The node Y index of the goal - * @param dim_3 The node dim_3 index of the goal - * @param goal_heading_mode The goal heading mode to use - * @param coarse_search_resolution The resolution to search for goal heading - */ - void setGoal( - const float & mx, - const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT, - const int & coarse_search_resolution = 1); - - /** - * @brief Set the starting pose for planning, as a node index - * @param mx The node X index of the goal - * @param my The node Y index of the goal - * @param dim_3 The node dim_3 index of the goal - */ - void setStart( - const float & mx, - const float & my, - const unsigned int & dim_3); - - /** - * @brief Get maximum number of iterations to plan - * @return Reference to Maximum iterations parameter - */ - int & getMaxIterations(); - - /** - * @brief Get pointer reference to starting node - * @return Node pointer reference to starting node - */ - NodePtr & getStart(); - - /** - * @brief Get maximum number of on-approach iterations after within threshold - * @return Reference to Maximum on-approach iterations parameter - */ - int & getOnApproachMaxIterations(); - - /** - * @brief Get tolerance, in node nodes - * @return Reference to tolerance parameter - */ - float & getToleranceHeuristic(); - - /** - * @brief Get size of graph in X - * @return Size in X - */ - unsigned int & getSizeX(); - - /** - * @brief Get size of graph in Y - * @return Size in Y - */ - unsigned int & getSizeY(); - - /** - * @brief Get number of angle quantization bins (SE2) or Z coordinate (XYZ) - * @return Number of angle bins / Z dimension - */ - unsigned int & getSizeDim3(); - - /** - * @brief Get the resolution of the coarse search - * @return Size of the goals to expand - */ - unsigned int getCoarseSearchResolution(); - - /** - * @brief Get the goals manager class - * @return Goal manager class - */ - GoalManagerT getGoalManager(); - - /** - * @brief Get pointer to shared node context - * @return Node context pointer - */ - NodeContext * getContext(); - -protected: - /** - * @brief Get pointer to next goal in open set - * @return Node pointer reference to next heuristically scored node - */ - inline NodePtr getNextNode(); - - /** - * @brief Add a node to the open set - * @param cost The cost to sort into the open set of the node - * @param node Node pointer reference to add to open set - */ - inline void addNode(const float & cost, NodePtr & node); - - /** - * @brief Adds node to graph - * @param index Node index to add - */ - inline NodePtr addToGraph(const uint64_t & index); - - /** - * @brief Get cost of heuristic of node - * @param node Node pointer to get heuristic for - * @return Heuristic cost for node - */ - inline float getHeuristicCost(const NodePtr & node); - - /** - * @brief Check if inputs to planner are valid - * @return Are valid - */ - inline bool areInputsValid(); - - /** - * @brief Get the closest path within tolerance if available - * @param path Vector of coordinates to fill with path - * @return if a valid path was found within tolerance - */ - inline bool getClosestPathWithinTolerance(CoordinateVector & path); - - /** - * @brief Clear heuristic queue of nodes to search - */ - inline void clearQueue(); - - /** - * @brief Clear graph of nodes searched - */ - inline void clearGraph(); - - /** - * @brief Get index at coordinates - * @param x X coordinate of point - * @param y Y coordinate of point - * @param dim3 Z coordinate / theta bin of point - * @return Index - */ - inline uint64_t getIndex( - const unsigned int & x, const unsigned int & y, const unsigned int & dim3); - - /** - * @brief Check if node has been visited - * @param current_node Node to check if visited - * @return if node has been visited - */ - inline bool onVisitationCheckNode(const NodePtr & node); - - /** - * @brief Populate a debug log of expansions for Hybrid-A* for visualization - * @param node Node expanded - * @param expansions_log Log to add not expanded to - */ - inline void populateExpansionsLog( - const NodePtr & node, std::vector> * expansions_log); - - bool _traverse_unknown; - bool _is_initialized; - int _max_iterations; - int _max_on_approach_iterations; - int _terminal_checking_interval; - double _max_planning_time; - float _tolerance; - unsigned int _x_size; - unsigned int _y_size; - unsigned int _dim3_size; - unsigned int _coarse_search_resolution; - SearchInfo _search_info; - - NodePtr _start; - GoalManagerT _goal_manager; - Graph _graph; - NodeQueue _queue; - - MotionModel _motion_model; - NodeHeuristicPair _best_heuristic_node; - - GridCollisionChecker * _collision_checker; - nav2_costmap_2d::Costmap2D * _costmap; - std::unique_ptr> _expander; - std::shared_ptr _shared_ctx; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__A_STAR_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp deleted file mode 100644 index 1c3263cc..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp +++ /dev/null @@ -1,200 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ -#define ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "athena_smac_planner/node_hybrid.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/constants.hpp" - -namespace athena_smac_planner -{ - -template -class AnalyticExpansion -{ -public: - typedef NodeT * NodePtr; - typedef std::vector NodeVector; - typedef typename NodeT::Coordinates Coordinates; - typedef std::function NodeGetter; - typedef typename NodeT::CoordinateVector CoordinateVector; - using NodeContext = typename NodeT::NodeContext; - - /** - * @struct athena_smac_planner::AnalyticExpansion::AnalyticExpansionNodes - * @brief Analytic expansion nodes and associated metadata - */ - struct AnalyticExpansionNode - { - AnalyticExpansionNode( - NodePtr & node_in, - Coordinates & initial_coords_in, - Coordinates & proposed_coords_in) - : node(node_in), - initial_coords(initial_coords_in), - proposed_coords(proposed_coords_in) - { - } - - NodePtr node; - Coordinates initial_coords; - Coordinates proposed_coords; - }; - - /** - * @struct AnalyticExpansionNodes - * @brief Analytic expansion nodes and associated metadata - * - * This structure holds a collection of analytic expansion nodes and the number of direction - * changes encountered during the expansion. - */ - struct AnalyticExpansionNodes - { - AnalyticExpansionNodes() = default; - - void add( - NodePtr & node, - Coordinates & initial_coords, - Coordinates & proposed_coords) - { - nodes.emplace_back(node, initial_coords, proposed_coords); - } - - void setDirectionChanges(int changes) - { - direction_changes = changes; - } - - std::vector nodes; - int direction_changes{0}; - }; - - /** - * @brief Constructor for analytic expansion object - */ - AnalyticExpansion( - const MotionModel & motion_model, - const SearchInfo & search_info, - const bool & traverse_unknown, - const unsigned int & dim_3_size); - - /** - * @brief Sets the collision checker and costmap to use in expansion validation - * @param collision_checker Collision checker to use - */ - void setCollisionChecker(GridCollisionChecker * collision_checker); - - /** - * @brief Sets the shared context to use - * @param ctx Shared context pointer - */ - void setContext(NodeContext * ctx); - - /** - * @brief Attempt an analytic path completion - * @param node The node to start the analytic path from - * @param coarse_check_goals Coarse list of goals nodes to plan to - * @param fine_check_goals Fine list of goals nodes to plan to - * @param goals_coords vector of goal coordinates to plan to - * @param getter Gets a node at a set of coordinates - * @param iterations Iterations to run over - * @param closest_distance Closest distance to goal - * @return Node pointer reference to goal node with the best score out of the goals node if - * successful, else return nullptr - */ - NodePtr tryAnalyticExpansion( - const NodePtr & current_node, - const NodeVector & coarse_check_goals, - const NodeVector & fine_check_goals, - const CoordinateVector & goals_coords, - const NodeGetter & getter, int & iterations, - int & closest_distance); - - /** - * @brief Perform an analytic path expansion to the goal - * @param node The node to start the analytic path from - * @param goal The goal node to plan to - * @param getter The function object that gets valid nodes from the graph - * @param state_space State space to use for computing analytic expansions - * @return A set of analytically expanded nodes to the goal from current node, if possible - */ - AnalyticExpansionNodes getAnalyticPath( - const NodePtr & node, const NodePtr & goal, - const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space); - - /** - * @brief Refined analytic path from the current node to the goal - * @param node The node to start the analytic path from. Node head may - * change as a result of refinement - * @param goal_node The goal node to plan to - * @param getter The function object that gets valid nodes from the graph - * @param analytic_nodes The set of analytic nodes to refine - * @return The score of the refined path - */ - float refineAnalyticPath( - NodePtr & node, - const NodePtr & goal_node, - const NodeGetter & getter, - AnalyticExpansionNodes & analytic_nodes); - - /** - * @brief Takes final analytic expansion and appends to current expanded node - * @param node The node to start the analytic path from - * @param goal The goal node to plan to - * @param expanded_nodes Expanded nodes to append to end of current search path - * @return Node pointer to goal node if successful, else return nullptr - */ - NodePtr setAnalyticPath( - const NodePtr & node, const NodePtr & goal, - const AnalyticExpansionNodes & expanded_nodes); - - /** - * @brief Counts the number of direction changes in a Reeds-Shepp path - * @param path The Reeds-Shepp path to count direction changes in - * @return The number of direction changes in the path - */ - int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path); - - /** - * @brief Takes an expanded nodes to clean up, if necessary, of any state - * information that may be polluting it from a prior search iteration - * @param expanded_nodes Expanded node to clean up from search - */ - void cleanNode(const NodePtr & nodes); - -protected: - MotionModel _motion_model; - SearchInfo _search_info; - bool _traverse_unknown; - unsigned int _dim_3_size; - GridCollisionChecker * _collision_checker; - std::list> _detached_nodes; - NodeContext * _ctx = nullptr; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp deleted file mode 100644 index ebd5023f..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#include -#include - -#include "nav2_costmap_2d/footprint_collision_checker.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "athena_smac_planner/constants.hpp" -#include "rclcpp/rclcpp.hpp" - -#ifndef ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ -#define ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ - -namespace athena_smac_planner -{ - -/** - * @class athena_smac_planner::GridCollisionChecker - * @brief A costmap grid collision checker - */ -class GridCollisionChecker - : public nav2_costmap_2d::FootprintCollisionChecker -{ -public: - /** - * @brief A constructor for athena_smac_planner::GridCollisionChecker - * for use when regular bin intervals are appropriate - * @param costmap The costmap to collision check against - * @param num_quantizations The number of quantizations to precompute footprint - * @param node Node to extract clock and logger from - * orientations for to speed up collision checking - */ - GridCollisionChecker( - std::shared_ptr costmap, - unsigned int num_quantizations, - rclcpp::Node::SharedPtr node); - - /** - * @brief A constructor for athena_smac_planner::GridCollisionChecker - * for use when irregular bin intervals are appropriate - * @param costmap The costmap to collision check against - * @param angles The vector of possible angle bins to precompute for - * orientations for to speed up collision checking, in radians - */ - // GridCollisionChecker( - // nav2_costmap_2d::Costmap2D * costmap, - // std::vector & angles); - - /** - * @brief Set the footprint to use with collision checker - * @param footprint The footprint to collision check against - * @param radius Whether or not the footprint is a circle and use radius collision checking - */ - void setFootprint( - const nav2_costmap_2d::Footprint & footprint, - const bool & radius, - const double & possible_collision_cost); - - /** - * @brief Check if in collision with costmap and footprint at pose - * @param x X coordinate of pose to check against - * @param y Y coordinate of pose to check against - * @param theta Angle bin number of pose to check against (NOT radians) - * @param traverse_unknown Whether or not to traverse in unknown space - * @return boolean if in collision or not. - */ - bool inCollision( - const float & x, - const float & y, - const float & theta, - const bool & traverse_unknown); - - /** - * @brief Check if in collision with costmap and footprint at pose - * @param i Index to search collision status of - * @param traverse_unknown Whether or not to traverse in unknown space - * @return boolean if in collision or not. - */ - bool inCollision( - const unsigned int & i, - const bool & traverse_unknown); - - /** - * @brief Get cost at footprint pose in costmap - * @return the cost at the pose in costmap - */ - float getCost(); - - /** - * @brief Get the angles of the precomputed footprint orientations - * @return the ordered vector of angles corresponding to footprints - */ - std::vector & getPrecomputedAngles() - { - return angles_; - } - - /** - * @brief Get costmap ros object for inflation layer params - * @return Costmap ros - */ - std::shared_ptr getCostmapROS() {return costmap_ros_;} - - /** - * @brief Check if value outside the range - * @param min Minimum value of the range - * @param max Maximum value of the range - * @param value the value to check if it is within the range - * @return boolean if in range or not - */ - bool outsideRange(const unsigned int & max, const float & value); - -protected: - std::shared_ptr costmap_ros_; - std::vector oriented_footprints_; - nav2_costmap_2d::Footprint unoriented_footprint_; - float center_cost_; - bool footprint_is_radius_{false}; - std::vector angles_; - float possible_collision_cost_{-1}; - rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; - rclcpp::Clock::SharedPtr clock_; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp deleted file mode 100644 index 78e145ef..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ -#define ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ - -#include - -namespace athena_smac_planner -{ -enum class MotionModel -{ - UNKNOWN = 0, - TWOD = 1, - DUBIN = 2, - REEDS_SHEPP = 3, - STATE_LATTICE = 4, -}; - -enum class GoalHeadingMode -{ - UNKNOWN = 0, - DEFAULT = 1, - BIDIRECTIONAL = 2, - ALL_DIRECTION = 3, -}; - -inline std::string toString(const MotionModel & n) -{ - switch (n) { - case MotionModel::TWOD: - return "2D"; - case MotionModel::DUBIN: - return "Dubin"; - case MotionModel::REEDS_SHEPP: - return "Reeds-Shepp"; - case MotionModel::STATE_LATTICE: - return "State Lattice"; - default: - return "Unknown"; - } -} - -inline MotionModel fromString(const std::string & n) -{ - if (n == "2D") { - return MotionModel::TWOD; - } else if (n == "DUBIN") { - return MotionModel::DUBIN; - } else if (n == "REEDS_SHEPP") { - return MotionModel::REEDS_SHEPP; - } else if (n == "STATE_LATTICE") { - return MotionModel::STATE_LATTICE; - } else { - return MotionModel::UNKNOWN; - } -} - -inline std::string toString(const GoalHeadingMode & n) -{ - switch (n) { - case GoalHeadingMode::DEFAULT: - return "DEFAULT"; - case GoalHeadingMode::BIDIRECTIONAL: - return "BIDIRECTIONAL"; - case GoalHeadingMode::ALL_DIRECTION: - return "ALL_DIRECTION"; - default: - return "Unknown"; - } -} - -inline GoalHeadingMode fromStringToGH(const std::string & n) -{ - if (n == "DEFAULT") { - return GoalHeadingMode::DEFAULT; - } else if (n == "BIDIRECTIONAL") { - return GoalHeadingMode::BIDIRECTIONAL; - } else if (n == "ALL_DIRECTION") { - return GoalHeadingMode::ALL_DIRECTION; - } else { - return GoalHeadingMode::UNKNOWN; - } -} - -const float UNKNOWN_COST = 255.0; -const float OCCUPIED_COST = 254.0; -const float INSCRIBED_COST = 253.0; -const float MAX_NON_OBSTACLE_COST = 252.0; -const float FREE_COST = 0; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp deleted file mode 100644 index 00d49f3f..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright (c) 2020, Carlos Luis -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ -#define ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ - -#include - -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "athena_smac_planner/constants.hpp" - -namespace athena_smac_planner -{ - -/** - * @class athena_smac_planner::CostmapDownsampler - * @brief A costmap downsampler for more efficient path planning - */ -class CostmapDownsampler -{ -public: - /** - * @brief A constructor for CostmapDownsampler - */ - CostmapDownsampler(); - - /** - * @brief A destructor for CostmapDownsampler - */ - ~CostmapDownsampler(); - - /** - * @brief Configure the downsampled costmap object - * @param costmap The costmap we want to downsample - * @param downsampling_factor Multiplier for the costmap resolution - * @param use_min_cost_neighbor If true, min function is used instead of max for downsampling - */ - void on_configure( - nav2_costmap_2d::Costmap2D * const costmap, - const unsigned int & downsampling_factor, - const bool & use_min_cost_neighbor = false); - - /** - * @brief Cleanup the downsampled costmap - */ - void on_cleanup(); - - /** - * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap - * @param downsampling_factor Multiplier for the costmap resolution - * @return A ptr to the downsampled costmap - */ - nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor); - - /** - * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version - */ - void resizeCostmap(); - -protected: - /** - * @brief Update the sizes X-Y of the costmap and its downsampled version - */ - void updateCostmapSize(); - - /** - * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell - * @param new_mx The X-coordinate of the cell in the new costmap - * @param new_my The Y-coordinate of the cell in the new costmap - */ - void setCostOfCell( - const unsigned int & new_mx, - const unsigned int & new_my); - - unsigned int _size_x; - unsigned int _size_y; - unsigned int _downsampled_size_x; - unsigned int _downsampled_size_y; - unsigned int _downsampling_factor; - bool _use_min_cost_neighbor; - float _downsampled_resolution; - nav2_costmap_2d::Costmap2D * _costmap; - std::unique_ptr _downsampled_costmap; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp deleted file mode 100644 index 3a74bf2e..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright (c) 2026, 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ -#define ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ - -#include "athena_smac_planner/constants.hpp" -#include "athena_smac_planner/types.hpp" - -namespace athena_smac_planner -{ -struct HybridMotionTable; -class NodeHybrid; - -/** - * @class athena_smac_planner::DistanceHeuristic - * @brief Distance Heuristic implementation for graph, Hybrid-A* - */ -template -class DistanceHeuristic -{ -public: - /** - * @brief A constructor for athena_smac_planner::DistanceHeuristic - */ - DistanceHeuristic() {} - - /** - * @brief Compute the SE2 distance heuristic - * @param lookup_table_dim Size, in costmap pixels, of the - * each lookup table dimension to populate - * @param motion_model Motion model to use for state space - * @param dim_3_size Number of quantization bins for caching - * @param search_info Info containing minimum radius to use - */ - template - void precomputeDistanceHeuristic( - const float & lookup_table_dim, - const MotionModel & motion_model, - const unsigned int & dim_3_size, - const SearchInfo & search_info, - MotionTableT & motion_table); - - /** - * @brief Compute the Distance heuristic - * @param node_coords Coordinates to get heuristic at - * @param goal_coords Coordinates to compute heuristic to - * @param obstacle_heuristic Value of the obstacle heuristic to compute - * additional motion heuristics if required - * @return heuristic Heuristic value - */ - template - float getDistanceHeuristic( - const Coordinates & node_coords, - const Coordinates & goal_coords, - const float & obstacle_heuristic, - MotionTableT & motion_table); - -protected: - // Dubin / Reeds-Shepp lookup and size for dereferencing - LookupTable dist_heuristic_lookup_table_; - float size_lookup_; -}; - -} // namespace athena_smac_planner -#endif // ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp deleted file mode 100644 index 4c2c1d87..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp +++ /dev/null @@ -1,289 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// Copyright (c) 2024, Stevedan Ogochukwu Omodolor Omodia -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ -#define ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ - -#include -#include -#include -#include - -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/node_hybrid.hpp" -#include "athena_smac_planner/node_basic.hpp" -#include "athena_smac_planner/collision_checker.hpp" - - -namespace athena_smac_planner -{ - -/** -* @class athena_smac_planner::GoalManager -* @brief Responsible for managing multiple variables storing information on the goal -*/ -template -class GoalManager -{ -public: - typedef NodeT * NodePtr; - typedef std::vector NodeVector; - typedef std::unordered_set NodeSet; - typedef std::vector> GoalStateVector; - typedef typename NodeT::Coordinates Coordinates; - typedef typename NodeT::CoordinateVector CoordinateVector; - using NodeContext = typename NodeT::NodeContext; - - /** - * @brief Constructor: Initializes empty goal state. sets and coordinate lists. - */ - GoalManager() - : _goals_set(NodeSet()), - _goals_state(GoalStateVector()), - _goals_coordinate(CoordinateVector()), - _ref_goal_coord(Coordinates()) - { - } - - /** - * @brief Destructor for the GoalManager - */ - ~GoalManager() = default; - - /** - * @brief Sets the node context for goal nodes - * @param ctx Pointer to the NodeContext - */ - void setContext(NodeContext * ctx) - { - _ctx = ctx; - } - - /** - * @brief Checks if the goals set is empty - * @return true if the goals set is empty - */ - bool goalsIsEmpty() - { - return _goals_state.empty(); - } - - /** - * @brief Adds goal to the goal vector - *@param goal Reference to the NodePtr - */ - void addGoal(NodePtr & goal) - { - _goals_state.push_back({goal, true}); - } - - /** - * @brief Clears all internal goal data, including goals, states, and coordinates. - */ - void clear() - { - _goals_set.clear(); - _goals_state.clear(); - _goals_coordinate.clear(); - } - - /** - * @brief Populates coarse and fine goal lists for analytic expansion. - * @param coarse_check_goals Output list of goals for coarse search expansion. - * @param fine_check_goals Output list of goals for fine search refinement. - * @param coarse_search_resolution Number of fine goals per coarse goal. - */ - void prepareGoalsForAnalyticExpansion( - NodeVector & coarse_check_goals, NodeVector & fine_check_goals, - int coarse_search_resolution) - { - for (unsigned int i = 0; i < _goals_state.size(); i++) { - if (_goals_state[i].is_valid) { - if (i % coarse_search_resolution == 0) { - coarse_check_goals.push_back(_goals_state[i].goal); - } else { - fine_check_goals.push_back(_goals_state[i].goal); - } - } - } - } - - /** - * @brief Checks if zone within the radius of a node is feasible. Returns true if - * there's at least one non-lethal cell within the node radius. - * @param node Input node. - * @param radius Search radius. - * @param collision_checker Collision checker to validate nearby nodes. - * @param traverse_unknown Flag whether traversal through unknown space is allowed. - * @return true - * @return false - */ - bool isZoneValid( - const NodePtr node, const float & radius, GridCollisionChecker * collision_checker, - const bool & traverse_unknown) const - { - if (radius < 1) { - return false; - } - - const auto size_x = collision_checker->getCostmap()->getSizeInCellsX(); - const auto size_y = collision_checker->getCostmap()->getSizeInCellsY(); - - auto getIndexFromPoint = [this](const Coordinates & point) { - const auto mx = static_cast(point.x); - const auto my = static_cast(point.y); - const auto angle = static_cast(point.theta); - return NodeT::getIndex(mx, my, angle, _ctx->motion_table.size_x, - _ctx->motion_table.num_angle_quantization); - }; - - const Coordinates & center_point = node->pose; - const float min_x = std::max(0.0f, std::floor(center_point.x - radius)); - const float min_y = std::max(0.0f, std::floor(center_point.y - radius)); - const float max_x = - std::min(static_cast(size_x - 1), std::ceil(center_point.x + radius)); - const float max_y = - std::min(static_cast(size_y - 1), std::ceil(center_point.y + radius)); - const float radius_sq = radius * radius; - - Coordinates m; - for (m.x = min_x; m.x <= max_x; m.x += 1.0f) { - for (m.y = min_y; m.y <= max_y; m.y += 1.0f) { - const float dx = m.x - center_point.x; - const float dy = m.y - center_point.y; - - if (dx * dx + dy * dy > radius_sq) { - continue; - } - - NodeT current_node(getIndexFromPoint(m), _ctx); - current_node.setPose(m); - - if (current_node.isNodeValid(traverse_unknown, collision_checker)) { - return true; - } - } - } - - return false; - } - - /** - * @brief Filters and marks invalid goals based on collision checking and tolerance thresholds. - * - * Stores only valid (or tolerably infeasible) goals into internal goal sets and coordinates. - * - * @param tolerance Heuristic tolerance allowed for infeasible goals. - * @param collision_checker Collision checker to validate goal positions. - * @param traverse_unknown Flag whether traversal through unknown space is allowed. - */ - void removeInvalidGoals( - const float & tolerance, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown) - { - // Make sure that there was a goal clear before this was run - if (!_goals_set.empty() || !_goals_coordinate.empty()) { - throw std::runtime_error( - "Goal set should be cleared before calling " - "removeinvalidgoals"); - } - for (unsigned int i = 0; i < _goals_state.size(); i++) { - if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) || - isZoneValid(_goals_state[i].goal, tolerance, collision_checker, traverse_unknown)) - { - _goals_state[i].is_valid = true; - _goals_set.insert(_goals_state[i].goal); - _goals_coordinate.push_back(_goals_state[i].goal->pose); - } else { - _goals_state[i].is_valid = false; - } - } - } - - /** - * @brief Check if a given node is part of the goal set. - * @param node Node pointer to check. - * @return if node matches any goal in the goal set. - */ - inline bool isGoal(const NodePtr & node) - { - return _goals_set.find(node) != _goals_set.end(); - } - - /** - * @brief Get pointer reference to goals set vector - * @return unordered_set of node pointers reference to the goals nodes - */ - inline NodeSet & getGoalsSet() - { - return _goals_set; - } - - /** - * @brief Get pointer reference to goals state - * @return vector of node pointers reference to the goals state - */ - inline GoalStateVector & getGoalsState() - { - return _goals_state; - } - - /** - * @brief Get pointer reference to goals coordinates - * @return vector of goals coordinates reference to the goals coordinates - */ - inline CoordinateVector & getGoalsCoordinates() - { - return _goals_coordinate; - } - - /** - * @brief Set the Reference goal coordinate - * @param coord Coordinates to set as Reference goal - */ - inline void setRefGoalCoordinates(const Coordinates & coord) - { - _ref_goal_coord = coord; - } - - /** - * @brief Checks whether the Reference goal coordinate has changed. - * @param coord Coordinates to compare with the current Reference goal coordinate. - * @return true if the Reference goal coordinate has changed, false otherwise. - */ - inline bool hasGoalChanged(const Coordinates & coord) - { - /** - * Note: This function checks if the goal has changed. This has to be done with - * the coordinates not the Node pointer because the Node pointer - * can be reused for different goals, but the coordinates will always - * be unique for each goal. - */ - return _ref_goal_coord != coord; - } - -protected: - NodeSet _goals_set; - GoalStateVector _goals_state; - CoordinateVector _goals_coordinate; - Coordinates _ref_goal_coord; - NodeContext * _ctx = nullptr; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp deleted file mode 100644 index 70f464c9..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ -#define ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ - -#include "athena_smac_planner/constants.hpp" -#include "athena_smac_planner/node_hybrid.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/collision_checker.hpp" - -namespace athena_smac_planner -{ - -/** - * @class athena_smac_planner::NodeBasic - * @brief NodeBasic implementation for priority queue insertion - */ -template -class NodeBasic -{ -public: - /** - * @brief A constructor for athena_smac_planner::NodeBasic - * @param index The index of this node for self-reference - */ - explicit NodeBasic(const uint64_t new_index) - : graph_node_ptr(nullptr), - index(new_index) - { - } - - /** - * @brief Take a NodeBasic and populate it with any necessary state - * cached in the queue for NodeT. - * @param node NodeT ptr to populate metadata into NodeBasic - */ - void populateSearchNode(NodeT * & node); - - /** - * @brief Take a NodeBasic and populate it with any necessary state - * cached in the queue for NodeTs. - * @param node Search node (basic) object to initialize internal node - * with state - */ - void processSearchNode(); - - typename NodeT::Coordinates pose; // Used by NodeHybrid - NodeT * graph_node_ptr; - uint64_t index; - unsigned int motion_index; - bool backward; - TurnDirection turn_dir; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp deleted file mode 100644 index b9ed6092..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp +++ /dev/null @@ -1,386 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ -#define ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ - -#include -#include -#include -#include - -#include "ompl/base/StateSpace.h" - -#include "athena_smac_planner/constants.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/collision_checker.hpp" -#include "athena_smac_planner/costmap_downsampler.hpp" -#include "athena_smac_planner/obstacle_heuristic.hpp" -#include "athena_smac_planner/distance_heuristic.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/inflation_layer.hpp" - -namespace athena_smac_planner -{ - -// Must forward declare -class NodeHybrid; - -/** - * @struct athena_smac_planner::HybridMotionTable - * @brief A table of motion primitives and related functions - */ -struct HybridMotionTable -{ - /** - * @brief A constructor for athena_smac_planner::HybridMotionTable - */ - HybridMotionTable() {} - - /** - * @brief Initializing using Dubin model - * @param size_x_in Size of costmap in X - * @param size_y_in Size of costmap in Y - * @param angle_quantization_in Size of costmap in bin sizes - * @param search_info Parameters for searching - */ - void initDubin( - unsigned int & size_x_in, - unsigned int & size_y_in, - unsigned int & angle_quantization_in, - SearchInfo & search_info); - - /** - * @brief Initializing using Reeds-Shepp model - * @param size_x_in Size of costmap in X - * @param size_y_in Size of costmap in Y - * @param angle_quantization_in Size of costmap in bin sizes - * @param search_info Parameters for searching - */ - void initReedsShepp( - unsigned int & size_x_in, - unsigned int & size_y_in, - unsigned int & angle_quantization_in, - SearchInfo & search_info); - - /** - * @brief Get projections of motion models - * @param node Ptr to NodeHybrid - * @return A set of motion poses - */ - MotionPoses getProjections(const NodeHybrid * node); - - /** - * @brief Get the angular bin to use from a raw orientation - * @param theta Angle in radians - * @return bin index of closest angle to request - */ - unsigned int getClosestAngularBin(const double & theta); - - /** - * @brief Get the raw orientation from an angular bin - * @param bin_idx Index of the bin - * @return Raw orientation in radians - */ - float getAngleFromBin(const unsigned int & bin_idx); - - /** - * @brief Get the angle scaled across bins from a raw orientation - * @param theta Angle in radians - * @return angle scaled across bins - */ - double getAngle(const double & theta); - - MotionModel motion_model = MotionModel::UNKNOWN; - MotionPoses projections; - unsigned int size_x; - unsigned int num_angle_quantization; - float num_angle_quantization_float; - float min_turning_radius; - float bin_size; - float change_penalty; - float non_straight_penalty; - float cost_penalty; - float reverse_penalty; - float travel_distance_reward; - bool downsample_obstacle_heuristic; - bool use_quadratic_cost_penalty; - bool allow_primitive_interpolation; - ompl::base::StateSpacePtr state_space; - std::vector> delta_xs; - std::vector> delta_ys; - std::vector trig_values; - std::vector travel_costs; -}; - -/** - * @class athena_smac_planner::NodeHybrid - * @brief NodeHybrid implementation for graph, Hybrid-A* - */ -class NodeHybrid -{ -public: - typedef NodeHybrid * NodePtr; - typedef std::unique_ptr> Graph; - typedef std::vector NodeVector; - using Coordinates = athena_smac_planner::Coordinates; - typedef std::vector CoordinateVector; - - struct NodeContext - { - /** - * @brief A constructor for athena_smac_planner::NodeContext - */ - NodeContext() - { - obstacle_heuristic = std::make_unique(); - distance_heuristic = std::make_unique>(); - } - HybridMotionTable motion_table; - std::unique_ptr obstacle_heuristic; - std::unique_ptr> distance_heuristic; - }; - /** - * @brief A constructor for athena_smac_planner::NodeHybrid - * @param index The index of this node for self-reference - */ - explicit NodeHybrid(const uint64_t index, NodeContext * ctx); - - /** - * @brief A destructor for athena_smac_planner::NodeHybrid - */ - ~NodeHybrid(); - - /** - * @brief operator== for comparisons - * @param NodeHybrid right hand side node reference - * @return If cell indices are equal - */ - bool operator==(const NodeHybrid & rhs) - { - return this->_index == rhs._index; - } - - /** - * @brief setting continuous coordinate search poses (in partial-cells) - * @param Pose pose - */ - inline void setPose(const Coordinates & pose_in) - { - pose = pose_in; - } - - /** - * @brief Reset method for new search - */ - void reset(); - - /** - * @brief Gets the accumulated cost at this node - * @return accumulated cost - */ - inline float getAccumulatedCost() - { - return _accumulated_cost; - } - - /** - * @brief Sets the accumulated cost at this node - * @param reference to accumulated cost - */ - inline void setAccumulatedCost(const float & cost_in) - { - _accumulated_cost = cost_in; - } - - /** - * @brief Sets the motion primitive index used to achieve node in search - * @param reference to motion primitive idx - */ - inline void setMotionPrimitiveIndex(const unsigned int & idx, const TurnDirection & turn_dir) - { - _motion_primitive_index = idx; - _turn_dir = turn_dir; - } - - /** - * @brief Gets the motion primitive index used to achieve node in search - * @return reference to motion primitive idx - */ - inline unsigned int & getMotionPrimitiveIndex() - { - return _motion_primitive_index; - } - - /** - * @brief Gets the motion primitive turning direction used to achieve node in search - * @return reference to motion primitive turning direction - */ - inline TurnDirection & getTurnDirection() - { - return _turn_dir; - } - - /** - * @brief Gets the costmap cost at this node - * @return costmap cost - */ - inline float getCost() - { - return _cell_cost; - } - - /** - * @brief Gets if cell has been visited in search - * @param If cell was visited - */ - inline bool wasVisited() - { - return _was_visited; - } - - /** - * @brief Sets if cell has been visited in search - */ - inline void visited() - { - _was_visited = true; - } - - /** - * @brief Gets cell index - * @return Reference to cell index - */ - inline uint64_t getIndex() - { - return _index; - } - - /** - * @brief Check if this node is valid - * @param traverse_unknown If we can explore unknown nodes on the graph - * @param collision_checker: Collision checker object - * @return whether this node is valid and collision free - */ - bool isNodeValid( - const bool & traverse_unknown, - GridCollisionChecker * collision_checker); - - /** - * @brief Get traversal cost of parent node to child node - * @param child Node pointer to child - * @return traversal cost - */ - float getTraversalCost(const NodePtr & child); - - /** - * @brief Get index at coordinates - * @param x X coordinate of point - * @param y Y coordinate of point - * @param angle Theta coordinate of point - * @param width Width of costmap - * @param angle_quantization Number of theta bins - * @return Index - */ - static inline uint64_t getIndex( - const unsigned int & x, const unsigned int & y, const unsigned int & angle, - const unsigned int & width, const unsigned int & angle_quantization) - { - return static_cast(angle) + static_cast(x) * - static_cast(angle_quantization) + - static_cast(y) * static_cast(width) * - static_cast(angle_quantization); - } - - /** - * @brief Get coordinates at index - * @param index Index of point - * @param width Width of costmap - * @param angle_quantization Theta size of costmap - * @return Coordinates - */ - static inline Coordinates getCoords( - const uint64_t & index, - const unsigned int & width, const unsigned int & angle_quantization) - { - return Coordinates( - (index / angle_quantization) % width, // x - index / (angle_quantization * width), // y - index % angle_quantization); // theta - } - - /** - * @brief Get cost of heuristic of node - * @param node Node index current - * @param node Node index of new - * @return Heuristic cost between the nodes - */ - float getHeuristicCost( - const Coordinates & node_coords, - const CoordinateVector & goals_coords); - - /** - * @brief Initialize motion models - * @param motion_model Motion model enum to use - * @param size_x Size of X of graph - * @param size_y Size of y of graph - * @param angle_quantization Size of theta bins of graph - * @param search_info Search info to use - */ - static void initMotionModel( - NodeContext * ctx, - const MotionModel & motion_model, - unsigned int & size_x, - unsigned int & size_y, - unsigned int & angle_quantization, - SearchInfo & search_info); - - /** - * @brief Retrieve all valid neighbors of a node. - * @param validity_checker Functor for state validity checking - * @param collision_checker Collision checker to use - * @param traverse_unknown If unknown costs are valid to traverse - * @param neighbors Vector of neighbors to be filled - */ - void getNeighbors( - std::function & validity_checker, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown, - NodeVector & neighbors); - - /** - * @brief Set the starting pose for planning, as a node index - * @param path Reference to a vector of indices of generated path - * @return whether the path was able to be backtraced - */ - bool backtracePath(CoordinateVector & path); - - NodeHybrid * parent; - Coordinates pose; - -private: - float _cell_cost; - float _accumulated_cost; - uint64_t _index; - bool _was_visited; - unsigned int _motion_primitive_index; - TurnDirection _turn_dir; - bool _is_node_valid{false}; - NodeContext * _ctx = nullptr; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp deleted file mode 100644 index 2a439d02..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright (c) 2026, 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ -#define ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ - -#include -#include -#include -#include "athena_smac_planner/constants.hpp" -#include "athena_smac_planner/types.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" - -namespace athena_smac_planner -{ - -typedef std::pair ObstacleHeuristicElement; -struct ObstacleHeuristicComparator -{ - bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const - { - return a.first > b.first; - } -}; - -typedef std::vector ObstacleHeuristicQueue; - -/** - * @class athena_smac_planner::ObstacleHeuristic - * @brief Obstacle Heuristic implementation for graph, Hybrid-A* - */ -class ObstacleHeuristic -{ -public: - /** - * @brief A constructor for athena_smac_planner::ObstacleHeuristic - */ - ObstacleHeuristic() {} - - /** - * @brief A destructor for athena_smac_planner::ObstacleHeuristic - */ - ~ObstacleHeuristic() {} - - /** - * @brief Compute the wavefront heuristic - * @param costmap Costmap to use - * @param goal_coords Coordinates to start heuristic expansion at - */ - void resetObstacleHeuristic( - std::shared_ptr costmap_ros_i, - const unsigned int & start_x, const unsigned int & start_y, - const unsigned int & goal_x, const unsigned int & goal_y, - const bool downsample_obstacle_heuristic); - - /** - * @brief Compute the Obstacle heuristic - * @param node_coords Coordinates to get heuristic at - * @param goal_coords Coordinates to compute heuristic to - * @return heuristic Heuristic value - */ - float getObstacleHeuristic( - const Coordinates & node_coords, - const float & cost_penalty, - const bool use_quadratic_cost_penalty, - const bool downsample_obstacle_heuristic); - - inline float distanceHeuristic2D( - const uint64_t idx, const unsigned int size_x, - const unsigned int target_x, const unsigned int target_y) - { - int dx = static_cast(idx % size_x) - static_cast(target_x); - int dy = static_cast(idx / size_x) - static_cast(target_y); - return std::sqrt(dx * dx + dy * dy); - } - -protected: - LookupTable obstacle_heuristic_lookup_table_; - ObstacleHeuristicQueue obstacle_heuristic_queue_; - std::shared_ptr costmap_ros; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp deleted file mode 100644 index c5a2cd84..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ -#define ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ - -#include -#include -#include -#include - -#include "athena_smac_planner/a_star.hpp" -#include "athena_smac_planner/smoother.hpp" -#include "athena_smac_planner/utils.hpp" -#include "athena_smac_planner/costmap_downsampler.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_array.hpp" -#include "visualization_msgs/msg/marker_array.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2/utils.hpp" - -namespace athena_smac_planner -{ - -class SmacPlannerHybrid -{ -public: - /** - * @brief Constructor — initializes the planner from ROS parameters. - * @param node Shared ptr to an rclcpp::Node for parameter reading and publisher creation. - * @param costmap_ros Costmap2DROS providing the collision costmap. - * @param name Parameter namespace prefix (default "SmacPlannerHybrid"). - */ - SmacPlannerHybrid( - rclcpp::Node::SharedPtr node, - std::shared_ptr costmap_ros, - const std::string & name = "SmacPlannerHybrid"); - - /** - * @brief Destructor - */ - ~SmacPlannerHybrid(); - - /** - * @brief Create a path from start to goal. - * @param start Start pose in map frame. - * @param goal Goal pose in map frame. - * @param cancel_checker Callable that returns true when planning should abort. - * @return nav_msgs::msg::Path of the generated path. - */ - nav_msgs::msg::Path createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal, - std::function cancel_checker); - -protected: - std::unique_ptr> _a_star; - GridCollisionChecker _collision_checker; - std::unique_ptr _smoother; - rclcpp::Clock::SharedPtr _clock; - rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")}; - nav2_costmap_2d::Costmap2D * _costmap; - std::shared_ptr _costmap_ros; - std::unique_ptr _costmap_downsampler; - std::string _global_frame, _name; - float _lookup_table_dim; - float _tolerance; - bool _downsample_costmap; - int _downsampling_factor; - double _angle_bin_size; - unsigned int _angle_quantizations; - bool _allow_unknown; - int _max_iterations; - int _max_on_approach_iterations; - int _terminal_checking_interval; - SearchInfo _search_info; - double _max_planning_time; - double _lookup_table_size; - double _minimum_turning_radius_global_coords; - bool _debug_visualizations; - std::string _motion_model_for_search; - MotionModel _motion_model; - GoalHeadingMode _goal_heading_mode; - int _coarse_search_resolution; - rclcpp::Publisher::SharedPtr _raw_plan_publisher; - rclcpp::Publisher::SharedPtr - _planned_footprints_publisher; - rclcpp::Publisher::SharedPtr - _smoothed_footprints_publisher; - rclcpp::Publisher::SharedPtr - _expansions_publisher; - std::mutex _mutex; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp deleted file mode 100644 index 06997543..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp +++ /dev/null @@ -1,208 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ -#define ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ - -#include - -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/constants.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav_msgs/msg/path.hpp" -#include "ompl/base/StateSpace.h" - -namespace athena_smac_planner -{ - -/** - * @struct athena_smac_planner::BoundaryPoints - * @brief Set of boundary condition points from expansion - */ -struct BoundaryPoints -{ - /** - * @brief A constructor for BoundaryPoints - */ - BoundaryPoints(double & x_in, double & y_in, double & theta_in) - : x(x_in), y(y_in), theta(theta_in) - {} - - double x; - double y; - double theta; -}; - -/** - * @struct athena_smac_planner::BoundaryExpansion - * @brief Boundary expansion state - */ -struct BoundaryExpansion -{ - double path_end_idx{0.0}; - double expansion_path_length{0.0}; - double original_path_length{0.0}; - std::vector pts; - bool in_collision{false}; -}; - -typedef std::vector BoundaryExpansions; -typedef std::vector::iterator PathIterator; -typedef std::vector::reverse_iterator ReversePathIterator; - -/** - * @class athena_smac_planner::Smoother - * @brief A path smoother implementation - */ -class Smoother -{ -public: - /** - * @brief A constructor for athena_smac_planner::Smoother - */ - explicit Smoother(const SmootherParams & params); - - /** - * @brief A destructor for athena_smac_planner::Smoother - */ - ~Smoother() {} - - /** - * @brief Initialization of the smoother - * @param min_turning_radius Minimum turning radius (m) - * @param motion_model Motion model type - */ - void initialize( - const double & min_turning_radius); - - /** - * @brief Smoother API method - * @param path Reference to path - * @param costmap Pointer to minimal costmap - * @param max_time Maximum time to compute, stop early if over limit - * @return If smoothing was successful - */ - bool smooth( - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time); - -protected: - /** - * @brief Smoother method - does the smoothing on a segment - * @param path Reference to path - * @param reversing_segment Return if this is a reversing segment - * @param costmap Pointer to minimal costmap - * @param max_time Maximum time to compute, stop early if over limit - * @return If smoothing was successful - */ - bool smoothImpl( - nav_msgs::msg::Path & path, - bool & reversing_segment, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time); - - /** - * @brief Get the field value for a given dimension - * @param msg Current pose to sample - * @param dim Dimension ID of interest - * @return dim value - */ - inline double getFieldByDim( - const geometry_msgs::msg::PoseStamped & msg, - const unsigned int & dim); - - /** - * @brief Set the field value for a given dimension - * @param msg Current pose to sample - * @param dim Dimension ID of interest - * @param value to set the dimension to for the pose - */ - inline void setFieldByDim( - geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, - const double & value); - - /** - * @brief Enforced minimum curvature boundary conditions on plan output - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param start_pose Start pose of the feasible path to maintain - * @param path Path to modify for curvature constraints on start / end of path - * @param costmap Costmap to check for collisions - * @param reversing_segment Whether this path segment is in reverse - */ - void enforceStartBoundaryConditions( - const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment); - - /** - * @brief Enforced minimum curvature boundary conditions on plan output - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param end_pose End pose of the feasible path to maintain - * @param path Path to modify for curvature constraints on start / end of path - * @param costmap Costmap to check for collisions - * @param reversing_segment Whether this path segment is in reverse - */ - void enforceEndBoundaryConditions( - const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment); - - /** - * @brief Given a set of boundary expansion, find the one which is shortest - * such that it is least likely to contain a loop-de-loop when working with - * close-by primitive markers. Instead, select a further away marker which - * generates a shorter ` - * @param boundary_expansions Set of boundary expansions - * @return Idx of the shorest boundary expansion option - */ - unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions & boundary_expansions); - - /** - * @brief Populate a motion model expansion from start->end into expansion - * @param start Start pose of the feasible path to maintain - * @param end End pose of the feasible path to maintain - * @param expansion Expansion object to populate - * @param costmap Costmap to check for collisions - * @param reversing_segment Whether this path segment is in reverse - */ - void findBoundaryExpansion( - const geometry_msgs::msg::Pose & start, - const geometry_msgs::msg::Pose & end, - BoundaryExpansion & expansion, - const nav2_costmap_2d::Costmap2D * costmap); - - /** - * @brief Generates boundary expansions with end idx at least strategic - * distances away, using either Reverse or (Forward) Path Iterators. - * @param start iterator to start search in path for - * @param end iterator to end search for - * @return Boundary expansions with end idxs populated - */ - template - BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end); - - double min_turning_rad_, tolerance_, data_w_, smooth_w_; - int max_its_, refinement_ctr_, refinement_num_; - bool is_holonomic_, do_refinement_; - MotionModel motion_model_; - ompl::base::StateSpacePtr state_space_; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h deleted file mode 100644 index c6b0fd60..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h +++ /dev/null @@ -1,2539 +0,0 @@ -// Copyright (c) 2018-2021 Martin Ankerl -// ______ _____ ______ _________ -// ______________ ___ /_ ___(_)_______ ___ /_ ______ ______ ______ / -// __ ___/_ __ \__ __ \__ / __ __ \ __ __ \_ __ \_ __ \_ __ / -// _ / / /_/ /_ /_/ /_ / _ / / / _ / / // /_/ // /_/ // /_/ / -// /_/ \____/ /_.___/ /_/ /_/ /_/ ________/_/ /_/ \____/ \____/ \__,_/ -// _/_____/ -// -// Fast & memory efficient hashtable based on robin hood hashing for C++11/14/17/20 -// https://github.com/martinus/robin-hood-hashing -// -// Licensed under the MIT License . -// SPDX-License-Identifier: MIT -// Copyright (c) 2018-2021 Martin Ankerl -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ -#define NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ - -// see https://semver.org/ -#define ROBIN_HOOD_VERSION_MAJOR 3 // for incompatible API changes -#define ROBIN_HOOD_VERSION_MINOR 11 // for adding functionality in a backwards-compatible manner -#define ROBIN_HOOD_VERSION_PATCH 5 // for backwards-compatible bug fixes - -#include -#include -#include -#include -#include -#include // only to support hash of smart pointers -#include -#include -#include -#include -#include -#if __cplusplus >= 201703L -# include -#endif -/* *INDENT-OFF* */ - -// #define ROBIN_HOOD_LOG_ENABLED -#ifdef ROBIN_HOOD_LOG_ENABLED -# include -# define ROBIN_HOOD_LOG(...) \ - std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; -#else -# define ROBIN_HOOD_LOG(x) -#endif - -// #define ROBIN_HOOD_TRACE_ENABLED -#ifdef ROBIN_HOOD_TRACE_ENABLED -# include -# define ROBIN_HOOD_TRACE(...) \ - std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; -#else -# define ROBIN_HOOD_TRACE(x) -#endif - -// #define ROBIN_HOOD_COUNT_ENABLED -#ifdef ROBIN_HOOD_COUNT_ENABLED -# include -# define ROBIN_HOOD_COUNT(x) ++counts().x; -namespace robin_hood { -struct Counts { - uint64_t shiftUp{}; - uint64_t shiftDown{}; -}; -inline std::ostream& operator<<(std::ostream& os, Counts const& c) { - return os << c.shiftUp << " shiftUp" << std::endl << c.shiftDown << " shiftDown" << std::endl; -} - -static Counts& counts() { - static Counts counts{}; - return counts; -} -} // namespace robin_hood -#else -# define ROBIN_HOOD_COUNT(x) -#endif - -// all non-argument macros should use this facility. See -// https://www.fluentcpp.com/2019/05/28/better-macros-better-flags/ -#define ROBIN_HOOD(x) ROBIN_HOOD_PRIVATE_DEFINITION_##x() - -// mark unused members with this macro -#define ROBIN_HOOD_UNUSED(identifier) - -// bitness -#if SIZE_MAX == UINT32_MAX -# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 32 -#elif SIZE_MAX == UINT64_MAX -# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 -#else -# error Unsupported bitness -#endif - -// endianness -#ifdef _MSC_VER -# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() 1 -# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() 0 -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() \ - (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) -# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) -#endif - -// inline -#ifdef _MSC_VER -# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __declspec(noinline) -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __attribute__((noinline)) -#endif - -// exceptions -#if !defined(__cpp_exceptions) && !defined(__EXCEPTIONS) && !defined(_CPPUNWIND) -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 0 -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 1 -#endif - -// count leading/trailing bits -#if !defined(ROBIN_HOOD_DISABLE_INTRINSICS) -# ifdef _MSC_VER -# if ROBIN_HOOD(BITNESS) == 32 -# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward -# else -# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward64 -# endif -# include -# pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD)) -# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ - [](size_t mask) noexcept -> int { \ - unsigned long index; \ // NOLINT - return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast(index) \ - : ROBIN_HOOD(BITNESS); \ - }(x) -# else -# if ROBIN_HOOD(BITNESS) == 32 -# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzl -# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzl -# else -# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzll -# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzll -# endif -# define ROBIN_HOOD_COUNT_LEADING_ZEROES(x) ((x) ? ROBIN_HOOD(CLZ)(x) : ROBIN_HOOD(BITNESS)) -# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) ((x) ? ROBIN_HOOD(CTZ)(x) : ROBIN_HOOD(BITNESS)) -# endif -#endif - -// fallthrough -#ifndef __has_cpp_attribute // For backwards compatibility -# define __has_cpp_attribute(x) 0 -#endif -#if __has_cpp_attribute(clang::fallthrough) -# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[clang::fallthrough]] -#elif __has_cpp_attribute(gnu::fallthrough) -# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[gnu::fallthrough]] -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() -#endif - -// likely/unlikely -#ifdef _MSC_VER -# define ROBIN_HOOD_LIKELY(condition) condition -# define ROBIN_HOOD_UNLIKELY(condition) condition -#else -# define ROBIN_HOOD_LIKELY(condition) __builtin_expect(condition, 1) -# define ROBIN_HOOD_UNLIKELY(condition) __builtin_expect(condition, 0) -#endif - -// detect if native wchar_t type is available in MSVC -#ifdef _MSC_VER -# ifdef _NATIVE_WCHAR_T_DEFINED -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 -# else -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 0 -# endif -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 -#endif - -// detect if MSVC supports the pair(std::piecewise_construct_t,...) constructor being constexpr -#ifdef _MSC_VER -# if _MSC_VER <= 1900 -# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 1 -# else -# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 -# endif -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 -#endif - -// workaround missing "is_trivially_copyable" in g++ < 5.0 -// See https://stackoverflow.com/a/31798726/48181 -#if defined(__GNUC__) && __GNUC__ < 5 -# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) __has_trivial_copy(__VA_ARGS__) -#else -# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) std::is_trivially_copyable<__VA_ARGS__>::value -#endif - -// helpers for C++ versions, see https://gcc.gnu.org/onlinedocs/cpp/Standard-Predefined-Macros.html -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX() __cplusplus -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX98() 199711L -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX11() 201103L -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX14() 201402L -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX17() 201703L - -#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) -# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() [[nodiscard]] -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() -#endif - -namespace robin_hood { - -#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) -# define ROBIN_HOOD_STD std -#else - -// c++11 compatibility layer -namespace ROBIN_HOOD_STD { -template -struct alignment_of - : std::integral_constant::type)> {}; - -template -class integer_sequence { -public: - using value_type = T; - static_assert(std::is_integral::value, "not integral type"); - static constexpr std::size_t size() noexcept { - return sizeof...(Ints); - } -}; -template -using index_sequence = integer_sequence; - -namespace detail_ { -template -struct IntSeqImpl { - using TValue = T; - static_assert(std::is_integral::value, "not integral type"); - static_assert(Begin >= 0 && Begin < End, "unexpected argument (Begin<0 || Begin<=End)"); - - template - struct IntSeqCombiner; - - template - struct IntSeqCombiner, integer_sequence> { - using TResult = integer_sequence; - }; - - using TResult = - typename IntSeqCombiner::TResult, - typename IntSeqImpl::TResult>::TResult; -}; - -template -struct IntSeqImpl { - using TValue = T; - static_assert(std::is_integral::value, "not integral type"); - static_assert(Begin >= 0, "unexpected argument (Begin<0)"); - using TResult = integer_sequence; -}; - -template -struct IntSeqImpl { - using TValue = T; - static_assert(std::is_integral::value, "not integral type"); - static_assert(Begin >= 0, "unexpected argument (Begin<0)"); - using TResult = integer_sequence; -}; -} // namespace detail_ - -template -using make_integer_sequence = typename detail_::IntSeqImpl::TResult; - -template -using make_index_sequence = make_integer_sequence; - -template -using index_sequence_for = make_index_sequence; - -} // namespace ROBIN_HOOD_STD - -#endif - -namespace detail { - -// make sure we static_cast to the correct type for hash_int -#if ROBIN_HOOD(BITNESS) == 64 -using SizeT = uint64_t; -#else -using SizeT = uint32_t; -#endif - -template -T rotr(T x, unsigned k) { - return (x >> k) | (x << (8U * sizeof(T) - k)); -} - -// This cast gets rid of warnings like "cast from 'uint8_t*' {aka 'unsigned char*'} to -// 'uint64_t*' {aka 'long unsigned int*'} increases required alignment of target type". Use with -// care! -template -inline T reinterpret_cast_no_cast_align_warning(void* ptr) noexcept { - return reinterpret_cast(ptr); -} - -template -inline T reinterpret_cast_no_cast_align_warning(void const* ptr) noexcept { - return reinterpret_cast(ptr); -} - -// make sure this is not inlined as it is slow and dramatically enlarges code, thus making other -// inlinings more difficult. Throws are also generally the slow path. -template -[[noreturn]] ROBIN_HOOD(NOINLINE) -#if ROBIN_HOOD(HAS_EXCEPTIONS) - void doThrow(Args&&... args) { - throw E(std::forward(args)...); -} -#else - void doThrow(Args&&... ROBIN_HOOD_UNUSED(args) /*unused*/) { - abort(); -} -#endif - -template -T* assertNotNull(T* t, Args&&... args) { - if (ROBIN_HOOD_UNLIKELY(nullptr == t)) { - doThrow(std::forward(args)...); - } - return t; -} - -template -inline T unaligned_load(void const* ptr) noexcept { - // using memcpy so we don't get into unaligned load problems. - // compiler should optimize this very well anyways. - T t; - std::memcpy(&t, ptr, sizeof(T)); - return t; -} - -// Allocates bulks of memory for objects of type T. This deallocates the memory in the destructor, -// and keeps a linked list of the allocated memory around. Overhead per allocation is the size of a -// pointer. -template -class BulkPoolAllocator { -public: - BulkPoolAllocator() noexcept = default; - - // does not copy anything, just creates a new allocator. - BulkPoolAllocator(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept - : mHead(nullptr) - , mListForFree(nullptr) {} - - BulkPoolAllocator(BulkPoolAllocator&& o) noexcept - : mHead(o.mHead) - , mListForFree(o.mListForFree) { - o.mListForFree = nullptr; - o.mHead = nullptr; - } - - BulkPoolAllocator& operator=(BulkPoolAllocator&& o) noexcept { - reset(); - mHead = o.mHead; - mListForFree = o.mListForFree; - o.mListForFree = nullptr; - o.mHead = nullptr; - return *this; - } - - BulkPoolAllocator& - operator=(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept { - // does not do anything - return *this; - } - - ~BulkPoolAllocator() noexcept { - reset(); - } - - // Deallocates all allocated memory. - void reset() noexcept { - while (mListForFree) { - T* tmp = *mListForFree; - ROBIN_HOOD_LOG("std::free") - std::free(mListForFree); - mListForFree = reinterpret_cast_no_cast_align_warning(tmp); - } - mHead = nullptr; - } - - // allocates, but does NOT initialize. Use in-place new constructor, e.g. - // T* obj = pool.allocate(); - // ::new (static_cast(obj)) T(); - T* allocate() { - T* tmp = mHead; - if (!tmp) { - tmp = performAllocation(); - } - - mHead = *reinterpret_cast_no_cast_align_warning(tmp); - return tmp; - } - - // does not actually deallocate but puts it in store. - // make sure you have already called the destructor! e.g. with - // obj->~T(); - // pool.deallocate(obj); - void deallocate(T* obj) noexcept { - *reinterpret_cast_no_cast_align_warning(obj) = mHead; - mHead = obj; - } - - // Adds an already allocated block of memory to the allocator. This allocator is from now on - // responsible for freeing the data (with free()). If the provided data is not large enough to - // make use of, it is immediately freed. Otherwise it is reused and freed in the destructor. - void addOrFree(void* ptr, const size_t numBytes) noexcept { - // calculate number of available elements in ptr - if (numBytes < ALIGNMENT + ALIGNED_SIZE) { - // not enough data for at least one element. Free and return. - ROBIN_HOOD_LOG("std::free") - std::free(ptr); - } else { - ROBIN_HOOD_LOG("add to buffer") - add(ptr, numBytes); - } - } - - void swap(BulkPoolAllocator& other) noexcept { - using std::swap; - swap(mHead, other.mHead); - swap(mListForFree, other.mListForFree); - } - -private: - // iterates the list of allocated memory to calculate how many to alloc next. - // Recalculating this each time saves us a size_t member. - // This ignores the fact that memory blocks might have been added manually with addOrFree. In - // practice, this should not matter much. - ROBIN_HOOD(NODISCARD) size_t calcNumElementsToAlloc() const noexcept { - auto tmp = mListForFree; - size_t numAllocs = MinNumAllocs; - - while (numAllocs * 2 <= MaxNumAllocs && tmp) { - auto x = reinterpret_cast(tmp); // NOLINT - tmp = *x; - numAllocs *= 2; - } - - return numAllocs; - } - - // WARNING: Underflow if numBytes < ALIGNMENT! This is guarded in addOrFree(). - void add(void* ptr, const size_t numBytes) noexcept { - const size_t numElements = (numBytes - ALIGNMENT) / ALIGNED_SIZE; - auto data = reinterpret_cast(ptr); - - // link free list - auto x = reinterpret_cast(data); - *x = mListForFree; - mListForFree = data; - - // create linked list for newly allocated data - auto* const headT = - reinterpret_cast_no_cast_align_warning(reinterpret_cast(ptr) + ALIGNMENT); - - auto* const head = reinterpret_cast(headT); - - // Visual Studio compiler automatically unrolls this loop, which is pretty cool - for (size_t i = 0; i < numElements; ++i) { - *reinterpret_cast_no_cast_align_warning(head + i * ALIGNED_SIZE) = - head + (i + 1) * ALIGNED_SIZE; - } - - // last one points to 0 - *reinterpret_cast_no_cast_align_warning(head + (numElements - 1) * ALIGNED_SIZE) = - mHead; - mHead = headT; - } - - // Called when no memory is available (mHead == 0). - // Don't inline this slow path. - ROBIN_HOOD(NOINLINE) T* performAllocation() { - size_t const numElementsToAlloc = calcNumElementsToAlloc(); - - // alloc new memory: [prev |T, T, ... T] - size_t const bytes = ALIGNMENT + ALIGNED_SIZE * numElementsToAlloc; - ROBIN_HOOD_LOG("std::malloc " << bytes << " = " << ALIGNMENT << " + " << ALIGNED_SIZE - << " * " << numElementsToAlloc) - add(assertNotNull(std::malloc(bytes)), bytes); - return mHead; - } - - // enforce byte alignment of the T's -#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) - static constexpr size_t ALIGNMENT = - (std::max)(std::alignment_of::value, std::alignment_of::value); -#else - static const size_t ALIGNMENT = - (ROBIN_HOOD_STD::alignment_of::value > ROBIN_HOOD_STD::alignment_of::value) - ? ROBIN_HOOD_STD::alignment_of::value - : +ROBIN_HOOD_STD::alignment_of::value; // the + is for walkarround -#endif - - static constexpr size_t ALIGNED_SIZE = ((sizeof(T) - 1) / ALIGNMENT + 1) * ALIGNMENT; - - static_assert(MinNumAllocs >= 1, "MinNumAllocs"); - static_assert(MaxNumAllocs >= MinNumAllocs, "MaxNumAllocs"); - static_assert(ALIGNED_SIZE >= sizeof(T*), "ALIGNED_SIZE"); - static_assert(0 == (ALIGNED_SIZE % sizeof(T*)), "ALIGNED_SIZE mod"); - static_assert(ALIGNMENT >= sizeof(T*), "ALIGNMENT"); - - T* mHead{nullptr}; - T** mListForFree{nullptr}; -}; - -template -struct NodeAllocator; - -// dummy allocator that does nothing -template -struct NodeAllocator { - // we are not using the data, so just free it. - void addOrFree(void* ptr, size_t ROBIN_HOOD_UNUSED(numBytes)/*unused*/) noexcept { - ROBIN_HOOD_LOG("std::free") - std::free(ptr); - } -}; - -template -struct NodeAllocator : public BulkPoolAllocator {}; - -// c++14 doesn't have is_nothrow_swappable, and clang++ 6.0.1 doesn't like it either, so I'm making -// my own here. -namespace swappable { -#if ROBIN_HOOD(CXX) < ROBIN_HOOD(CXX17) -using std::swap; -template -struct nothrow { - static const bool value = noexcept(swap(std::declval(), std::declval())); -}; -#else -template -struct nothrow { - static const bool value = std::is_nothrow_swappable::value; -}; -#endif -} // namespace swappable - -} // namespace detail - -struct is_transparent_tag {}; - -// A custom pair implementation is used in the map because std::pair is not is_trivially_copyable, -// which means it would not be allowed to be used in std::memcpy. This struct is copiable, which is -// also tested. -template -struct pair { - using first_type = T1; - using second_type = T2; - - template ::value && - std::is_default_constructible::value>::type> - constexpr pair() noexcept(noexcept(U1()) && noexcept(U2())) - : first() - , second() {} - - // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. - explicit constexpr pair(std::pair const& o) noexcept( - noexcept(T1(std::declval())) && noexcept(T2(std::declval()))) - : first(o.first) - , second(o.second) {} - - // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. - explicit constexpr pair(std::pair&& o) noexcept(noexcept( - T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) - : first(std::move(o.first)) - , second(std::move(o.second)) {} - - constexpr pair(T1&& a, T2&& b) noexcept(noexcept( - T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) - : first(std::move(a)) - , second(std::move(b)) {} - - template - constexpr pair(U1&& a, U2&& b) noexcept(noexcept(T1(std::forward( - std::declval()))) && noexcept(T2(std::forward(std::declval())))) - : first(std::forward(a)) - , second(std::forward(b)) {} - - template - // MSVC 2015 produces error "C2476: ‘constexpr’ constructor does not initialize all members" - // if this constructor is constexpr -#if !ROBIN_HOOD(BROKEN_CONSTEXPR) - constexpr -#endif - pair(std::piecewise_construct_t /*unused*/, std::tuple a, - std::tuple - b) noexcept(noexcept(pair(std::declval&>(), - std::declval&>(), - ROBIN_HOOD_STD::index_sequence_for(), - ROBIN_HOOD_STD::index_sequence_for()))) - : pair(a, b, ROBIN_HOOD_STD::index_sequence_for(), - ROBIN_HOOD_STD::index_sequence_for()) { - } - - // constructor called from the std::piecewise_construct_t ctor - template - pair( - std::tuple& a, std::tuple& b, - ROBIN_HOOD_STD::index_sequence /*unused*/, - ROBIN_HOOD_STD::index_sequence /*unused*/) noexcept( - noexcept(T1(std::forward(std::get( - std::declval&>()))...)) && noexcept(T2(std:: - forward(std::get( - std::declval&>()))...))) - : first(std::forward(std::get(a))...) - , second(std::forward(std::get(b))...) { - // make visual studio compiler happy about warning about unused a & b. - // Visual studio's pair implementation disables warning 4100. - (void)a; - (void)b; - } - - void swap(pair& o) noexcept((detail::swappable::nothrow::value) && - (detail::swappable::nothrow::value)) { - using std::swap; - swap(first, o.first); - swap(second, o.second); - } - - T1 first; // NOLINT - T2 second; // NOLINT -}; - -template -inline void swap(pair& a, pair& b) noexcept( - noexcept(std::declval&>().swap(std::declval&>()))) { - a.swap(b); -} - -template -inline constexpr bool operator==(pair const& x, pair const& y) { - return (x.first == y.first) && (x.second == y.second); -} -template -inline constexpr bool operator!=(pair const& x, pair const& y) { - return !(x == y); -} -template -inline constexpr bool operator<(pair const& x, pair const& y) noexcept(noexcept( - std::declval() < std::declval()) && noexcept(std::declval() < - std::declval())) { - return x.first < y.first || (!(y.first < x.first) && x.second < y.second); -} -template -inline constexpr bool operator>(pair const& x, pair const& y) { - return y < x; -} -template -inline constexpr bool operator<=(pair const& x, pair const& y) { - return !(x > y); -} -template -inline constexpr bool operator>=(pair const& x, pair const& y) { - return !(x < y); -} - -inline size_t hash_bytes(void const* ptr, size_t len) noexcept { - static constexpr uint64_t m = UINT64_C(0xc6a4a7935bd1e995); - static constexpr uint64_t seed = UINT64_C(0xe17a1465); - static constexpr unsigned int r = 47; - - auto const* const data64 = static_cast(ptr); - uint64_t h = seed ^ (len * m); - - size_t const n_blocks = len / 8; - for (size_t i = 0; i < n_blocks; ++i) { - auto k = detail::unaligned_load(data64 + i); - - k *= m; - k ^= k >> r; - k *= m; - - h ^= k; - h *= m; - } - - auto const* const data8 = reinterpret_cast(data64 + n_blocks); - switch (len & 7U) { - case 7: - h ^= static_cast(data8[6]) << 48U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 6: - h ^= static_cast(data8[5]) << 40U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 5: - h ^= static_cast(data8[4]) << 32U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 4: - h ^= static_cast(data8[3]) << 24U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 3: - h ^= static_cast(data8[2]) << 16U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 2: - h ^= static_cast(data8[1]) << 8U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 1: - h ^= static_cast(data8[0]); - h *= m; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - default: - break; - } - - h ^= h >> r; - - // not doing the final step here, because this will be done by keyToIdx anyways - // h *= m; - // h ^= h >> r; - return static_cast(h); -} - -inline size_t hash_int(uint64_t x) noexcept { - // tried lots of different hashes, let's stick with murmurhash3. It's simple, fast, well tested, - // and doesn't need any special 128bit operations. - x ^= x >> 33U; - x *= UINT64_C(0xff51afd7ed558ccd); - x ^= x >> 33U; - - // not doing the final step here, because this will be done by keyToIdx anyways - // x *= UINT64_C(0xc4ceb9fe1a85ec53); - // x ^= x >> 33U; - return static_cast(x); -} - -// A thin wrapper around std::hash, performing an additional simple mixing step of the result. -template -struct hash : public std::hash { - size_t operator()(T const& obj) const - noexcept(noexcept(std::declval>().operator()(std::declval()))) { - // call base hash - auto result = std::hash::operator()(obj); - // return mixed of that, to be save against identity has - return hash_int(static_cast(result)); - } -}; - -template -struct hash> { - size_t operator()(std::basic_string const& str) const noexcept { - return hash_bytes(str.data(), sizeof(CharT) * str.size()); - } -}; - -#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) -template -struct hash> { - size_t operator()(std::basic_string_view const& sv) const noexcept { - return hash_bytes(sv.data(), sizeof(CharT) * sv.size()); - } -}; -#endif - -template -struct hash { - size_t operator()(T* ptr) const noexcept { - return hash_int(reinterpret_cast(ptr)); - } -}; - -template -struct hash> { - size_t operator()(std::unique_ptr const& ptr) const noexcept { - return hash_int(reinterpret_cast(ptr.get())); - } -}; - -template -struct hash> { - size_t operator()(std::shared_ptr const& ptr) const noexcept { - return hash_int(reinterpret_cast(ptr.get())); - } -}; - -template -struct hash::value>::type> { - size_t operator()(Enum e) const noexcept { - using Underlying = typename std::underlying_type::type; - return hash{}(static_cast(e)); - } -}; - -#define ROBIN_HOOD_HASH_INT(T) \ - template <> \ - struct hash { \ - size_t operator()(T const& obj) const noexcept { \ - return hash_int(static_cast(obj)); \ - } \ - } - -#if defined(__GNUC__) && !defined(__clang__) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wuseless-cast" -#endif -// see https://en.cppreference.com/w/cpp/utility/hash -ROBIN_HOOD_HASH_INT(bool); -ROBIN_HOOD_HASH_INT(char); -ROBIN_HOOD_HASH_INT(signed char); -ROBIN_HOOD_HASH_INT(unsigned char); -ROBIN_HOOD_HASH_INT(char16_t); -ROBIN_HOOD_HASH_INT(char32_t); -#if ROBIN_HOOD(HAS_NATIVE_WCHART) -ROBIN_HOOD_HASH_INT(wchar_t); -#endif -ROBIN_HOOD_HASH_INT(short); // NOLINT -ROBIN_HOOD_HASH_INT(unsigned short); // NOLINT -ROBIN_HOOD_HASH_INT(int); -ROBIN_HOOD_HASH_INT(unsigned int); -ROBIN_HOOD_HASH_INT(long); // NOLINT -ROBIN_HOOD_HASH_INT(long long); // NOLINT -ROBIN_HOOD_HASH_INT(unsigned long); // NOLINT -ROBIN_HOOD_HASH_INT(unsigned long long); // NOLINT -#if defined(__GNUC__) && !defined(__clang__) -# pragma GCC diagnostic pop -#endif -namespace detail { - -template -struct void_type { - using type = void; -}; - -template -struct has_is_transparent : public std::false_type {}; - -template -struct has_is_transparent::type> - : public std::true_type {}; - -// using wrapper classes for hash and key_equal prevents the diamond problem when the same type -// is used. see https://stackoverflow.com/a/28771920/48181 -template -struct WrapHash : public T { - WrapHash() = default; - explicit WrapHash(T const& o) noexcept(noexcept(T(std::declval()))) - : T(o) {} -}; - -template -struct WrapKeyEqual : public T { - WrapKeyEqual() = default; - explicit WrapKeyEqual(T const& o) noexcept(noexcept(T(std::declval()))) - : T(o) {} -}; - -// A highly optimized hashmap implementation, using the Robin Hood algorithm. -// -// In most cases, this map should be usable as a drop-in replacement for std::unordered_map, but -// be about 2x faster in most cases and require much less allocations. -// -// This implementation uses the following memory layout: -// -// [Node, Node, ... Node | info, info, ... infoSentinel ] -// -// * Node: either a DataNode that directly has the std::pair as member, -// or a DataNode with a pointer to std::pair. Which DataNode representation to use -// depends on how fast the swap() operation is. Heuristically, this is automatically chosen -// based on sizeof(). there are always 2^n Nodes. -// -// * info: Each Node in the map has a corresponding info byte, so there are 2^n info bytes. -// Each byte is initialized to 0, meaning the corresponding Node is empty. Set to 1 means the -// corresponding node contains data. Set to 2 means the corresponding Node is filled, but it -// actually belongs to the previous position and was pushed out because that place is already -// taken. -// -// * infoSentinel: Sentinel byte set to 1, so that iterator's ++ can stop at end() without the -// need for a idx variable. -// -// According to STL, order of templates has effect on throughput. That's why I've moved the -// boolean to the front. -// https://www.reddit.com/r/cpp/comments/ahp6iu/compile_time_binary_size_reductions_and_cs_future/eeguck4/ -template -class Table - : public WrapHash, - public WrapKeyEqual, - detail::NodeAllocator< - typename std::conditional< - std::is_void::value, Key, - robin_hood::pair::type, T>>::type, - 4, 16384, IsFlat> { -public: - static constexpr bool is_flat = IsFlat; - static constexpr bool is_map = !std::is_void::value; - static constexpr bool is_set = !is_map; - static constexpr bool is_transparent = - has_is_transparent::value && has_is_transparent::value; - - using key_type = Key; - using mapped_type = T; - using value_type = typename std::conditional< - is_set, Key, - robin_hood::pair::type, T>>::type; - using size_type = size_t; - using hasher = Hash; - using key_equal = KeyEqual; - using Self = Table; - -private: - static_assert(MaxLoadFactor100 > 10 && MaxLoadFactor100 < 100, - "MaxLoadFactor100 needs to be >10 && < 100"); - - using WHash = WrapHash; - using WKeyEqual = WrapKeyEqual; - - // configuration defaults - - // make sure we have 8 elements, needed to quickly rehash mInfo - static constexpr size_t InitialNumElements = sizeof(uint64_t); - static constexpr uint32_t InitialInfoNumBits = 5; - static constexpr uint8_t InitialInfoInc = 1U << InitialInfoNumBits; - static constexpr size_t InfoMask = InitialInfoInc - 1U; - static constexpr uint8_t InitialInfoHashShift = 0; - using DataPool = detail::NodeAllocator; - - // type needs to be wider than uint8_t. - using InfoType = uint32_t; - - // DataNode //////////////////////////////////////////////////////// - - // Primary template for the data node. We have special implementations for small and big - // objects. For large objects it is assumed that swap() is fairly slow, so we allocate these - // on the heap so swap merely swaps a pointer. - template - class DataNode {}; - - // Small: just allocate on the stack. - template - class DataNode final { - public: - template - explicit DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, Args&&... args) noexcept( - noexcept(value_type(std::forward(args)...))) - : mData(std::forward(args)...) {} - - DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept( - std::is_nothrow_move_constructible::value) - : mData(std::move(n.mData)) {} - - // doesn't do anything - void destroy(M& ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {} - void destroyDoNotDeallocate() noexcept {} - - value_type const* operator->() const noexcept { - return &mData; - } - value_type* operator->() noexcept { - return &mData; - } - - const value_type& operator*() const noexcept { - return mData; - } - - value_type& operator*() noexcept { - return mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() noexcept { - return mData.first; - } - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() noexcept { - return mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type - getFirst() const noexcept { - return mData.first; - } - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() const noexcept { - return mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getSecond() noexcept { - return mData.second; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getSecond() const noexcept { - return mData.second; - } - - void swap(DataNode& o) noexcept( - noexcept(std::declval().swap(std::declval()))) { - mData.swap(o.mData); - } - - private: - value_type mData; - }; - - // big object: allocate on heap. - template - class DataNode { - public: - template - explicit DataNode(M& map, Args&&... args) - : mData(map.allocate()) { - ::new (static_cast(mData)) value_type(std::forward(args)...); - } - - DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept - : mData(std::move(n.mData)) {} - - void destroy(M& map) noexcept { - // don't deallocate, just put it into list of datapool. - mData->~value_type(); - map.deallocate(mData); - } - - void destroyDoNotDeallocate() noexcept { - mData->~value_type(); - } - - value_type const* operator->() const noexcept { - return mData; - } - - value_type* operator->() noexcept { - return mData; - } - - const value_type& operator*() const { - return *mData; - } - - value_type& operator*() { - return *mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() noexcept { - return mData->first; - } - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() noexcept { - return *mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type - getFirst() const noexcept { - return mData->first; - } - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() const noexcept { - return *mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getSecond() noexcept { - return mData->second; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getSecond() const noexcept { - return mData->second; - } - - void swap(DataNode& o) noexcept { - using std::swap; - swap(mData, o.mData); - } - - private: - value_type* mData; - }; - - using Node = DataNode; - - // helpers for insertKeyPrepareEmptySpot: extract first entry (only const required) - ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(Node const& n) const noexcept { - return n.getFirst(); - } - - // in case we have void mapped_type, we are not using a pair, thus we just route k through. - // No need to disable this because it's just not used if not applicable. - ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(key_type const& k) const noexcept { - return k; - } - - // in case we have non-void mapped_type, we have a standard robin_hood::pair - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::value, key_type const&>::type - getFirstConst(value_type const& vt) const noexcept { - return vt.first; - } - - // Cloner ////////////////////////////////////////////////////////// - - template - struct Cloner; - - // fast path: Just copy data, without allocating anything. - template - struct Cloner { - void operator()(M const& source, M& target) const { - auto const* const src = reinterpret_cast(source.mKeyVals); - auto* tgt = reinterpret_cast(target.mKeyVals); - auto const numElementsWithBuffer = target.calcNumElementsWithBuffer(target.mMask + 1); - std::copy(src, src + target.calcNumBytesTotal(numElementsWithBuffer), tgt); - } - }; - - template - struct Cloner { - void operator()(M const& s, M& t) const { - auto const numElementsWithBuffer = t.calcNumElementsWithBuffer(t.mMask + 1); - std::copy(s.mInfo, s.mInfo + t.calcNumBytesInfo(numElementsWithBuffer), t.mInfo); - - for (size_t i = 0; i < numElementsWithBuffer; ++i) { - if (t.mInfo[i]) { - ::new (static_cast(t.mKeyVals + i)) Node(t, *s.mKeyVals[i]); - } - } - } - }; - - // Destroyer /////////////////////////////////////////////////////// - - template - struct Destroyer {}; - - template - struct Destroyer { - void nodes(M& m) const noexcept { - m.mNumElements = 0; - } - - void nodesDoNotDeallocate(M& m) const noexcept { - m.mNumElements = 0; - } - }; - - template - struct Destroyer { - void nodes(M& m) const noexcept { - m.mNumElements = 0; - // clear also resets mInfo to 0, that's sometimes not necessary. - auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); - - for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { - if (0 != m.mInfo[idx]) { - Node& n = m.mKeyVals[idx]; - n.destroy(m); - n.~Node(); - } - } - } - - void nodesDoNotDeallocate(M& m) const noexcept { - m.mNumElements = 0; - // clear also resets mInfo to 0, that's sometimes not necessary. - auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); - for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { - if (0 != m.mInfo[idx]) { - Node& n = m.mKeyVals[idx]; - n.destroyDoNotDeallocate(); - n.~Node(); - } - } - } - }; - - // Iter //////////////////////////////////////////////////////////// - - struct fast_forward_tag {}; - - // generic iterator for both const_iterator and iterator. - template - class Iter { - private: - using NodePtr = typename std::conditional::type; - - public: - using difference_type = std::ptrdiff_t; - using value_type = typename Self::value_type; - using reference = typename std::conditional::type; - using pointer = typename std::conditional::type; - using iterator_category = std::forward_iterator_tag; - - // default constructed iterator can be compared to itself, but WON'T return true when - // compared to end(). - Iter() = default; - - // Rule of zero: nothing specified. The conversion constructor is only enabled for - // iterator to const_iterator, so it doesn't accidentally work as a copy ctor. - - // Conversion constructor from iterator to const_iterator. - template ::type> - Iter(Iter const& other) noexcept - : mKeyVals(other.mKeyVals) - , mInfo(other.mInfo) {} - - Iter(NodePtr valPtr, uint8_t const* infoPtr) noexcept - : mKeyVals(valPtr) - , mInfo(infoPtr) {} - - Iter(NodePtr valPtr, uint8_t const* infoPtr, - fast_forward_tag ROBIN_HOOD_UNUSED(tag) /*unused*/) noexcept - : mKeyVals(valPtr) - , mInfo(infoPtr) { - fastForward(); - } - - template ::type> - Iter& operator=(Iter const& other) noexcept { - mKeyVals = other.mKeyVals; - mInfo = other.mInfo; - return *this; - } - - // prefix increment. Undefined behavior if we are at end()! - Iter& operator++() noexcept { - mInfo++; - mKeyVals++; - fastForward(); - return *this; - } - - Iter operator++(int) noexcept { - Iter tmp = *this; - ++(*this); - return tmp; - } - - reference operator*() const { - return **mKeyVals; - } - - pointer operator->() const { - return &**mKeyVals; - } - - template - bool operator==(Iter const& o) const noexcept { - return mKeyVals == o.mKeyVals; - } - - template - bool operator!=(Iter const& o) const noexcept { - return mKeyVals != o.mKeyVals; - } - - private: - // fast forward to the next non-free info byte - // I've tried a few variants that don't depend on intrinsics, but unfortunately they are - // quite a bit slower than this one. So I've reverted that change again. See map_benchmark. - void fastForward() noexcept { - size_t n = 0; - while (0U == (n = detail::unaligned_load(mInfo))) { - mInfo += sizeof(size_t); - mKeyVals += sizeof(size_t); - } -#if defined(ROBIN_HOOD_DISABLE_INTRINSICS) - // we know for certain that within the next 8 bytes we'll find a non-zero one. - if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { - mInfo += 4; - mKeyVals += 4; - } - if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { - mInfo += 2; - mKeyVals += 2; - } - if (ROBIN_HOOD_UNLIKELY(0U == *mInfo)) { - mInfo += 1; - mKeyVals += 1; - } -#else -# if ROBIN_HOOD(LITTLE_ENDIAN) - auto inc = ROBIN_HOOD_COUNT_TRAILING_ZEROES(n) / 8; -# else - auto inc = ROBIN_HOOD_COUNT_LEADING_ZEROES(n) / 8; -# endif - mInfo += inc; - mKeyVals += inc; -#endif - } - - friend class Table; - NodePtr mKeyVals{nullptr}; - uint8_t const* mInfo{nullptr}; - }; - - //////////////////////////////////////////////////////////////////// - - // highly performance relevant code. - // Lower bits are used for indexing into the array (2^n size) - // The upper 1-5 bits need to be a reasonable good hash, to save comparisons. - template - void keyToIdx(HashKey&& key, size_t* idx, InfoType* info) const { - // In addition to whatever hash is used, add another mul & shift so we get better hashing. - // This serves as a bad hash prevention, if the given data is - // badly mixed. - auto h = static_cast(WHash::operator()(key)); - - h *= mHashMultiplier; - h ^= h >> 33U; - - // the lower InitialInfoNumBits are reserved for info. - *info = mInfoInc + static_cast((h & InfoMask) >> mInfoHashShift); - *idx = (static_cast(h) >> InitialInfoNumBits) & mMask; - } - - // forwards the index by one, wrapping around at the end - void next(InfoType* info, size_t* idx) const noexcept { - *idx = *idx + 1; - *info += mInfoInc; - } - - void nextWhileLess(InfoType* info, size_t* idx) const noexcept { - // unrolling this by hand did not bring any speedups. - while (*info < mInfo[*idx]) { - next(info, idx); - } - } - - // Shift everything up by one element. Tries to move stuff around. - void - shiftUp(size_t startIdx, - size_t const insertion_idx) noexcept(std::is_nothrow_move_assignable::value) { - auto idx = startIdx; - ::new (static_cast(mKeyVals + idx)) Node(std::move(mKeyVals[idx - 1])); - while (--idx != insertion_idx) { - mKeyVals[idx] = std::move(mKeyVals[idx - 1]); - } - - idx = startIdx; - while (idx != insertion_idx) { - ROBIN_HOOD_COUNT(shiftUp) - mInfo[idx] = static_cast(mInfo[idx - 1] + mInfoInc); - if (ROBIN_HOOD_UNLIKELY(mInfo[idx] + mInfoInc > 0xFF)) { - mMaxNumElementsAllowed = 0; - } - --idx; - } - } - - void shiftDown(size_t idx) noexcept(std::is_nothrow_move_assignable::value) { - // until we find one that is either empty or has zero offset. - // TODO(martinus) we don't need to move everything, just the last one for the same - // bucket. - mKeyVals[idx].destroy(*this); - - // until we find one that is either empty or has zero offset. - while (mInfo[idx + 1] >= 2 * mInfoInc) { - ROBIN_HOOD_COUNT(shiftDown) - mInfo[idx] = static_cast(mInfo[idx + 1] - mInfoInc); - mKeyVals[idx] = std::move(mKeyVals[idx + 1]); - ++idx; - } - - mInfo[idx] = 0; - // don't destroy, we've moved it - // mKeyVals[idx].destroy(*this); - mKeyVals[idx].~Node(); - } - - // copy of find(), except that it returns iterator instead of const_iterator. - template - ROBIN_HOOD(NODISCARD) - size_t findIdx(Other const& key) const { - size_t idx{}; - InfoType info{}; - keyToIdx(key, &idx, &info); - - do { - // unrolling this twice gives a bit of a speedup. More unrolling did not help. - if (info == mInfo[idx] && - ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { - return idx; - } - next(&info, &idx); - if (info == mInfo[idx] && - ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { - return idx; - } - next(&info, &idx); - } while (info <= mInfo[idx]); - - // nothing found! - return mMask == 0 ? 0 - : static_cast(std::distance( - mKeyVals, reinterpret_cast_no_cast_align_warning(mInfo))); - } - - void cloneData(const Table& o) { - Cloner()(o, *this); - } - - // inserts a keyval that is guaranteed to be new, e.g. when the hashmap is resized. - // @return True on success, false if something went wrong - void insert_move(Node&& keyval) { - // we don't retry, fail if overflowing - // don't need to check max num elements - if (0 == mMaxNumElementsAllowed && !try_increase_info()) { - throwOverflowError(); - } - - size_t idx{}; - InfoType info{}; - keyToIdx(keyval.getFirst(), &idx, &info); - - // skip forward. Use <= because we are certain that the element is not there. - while (info <= mInfo[idx]) { - idx = idx + 1; - info += mInfoInc; - } - - // key not found, so we are now exactly where we want to insert it. - auto const insertion_idx = idx; - auto const insertion_info = static_cast(info); - if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { - mMaxNumElementsAllowed = 0; - } - - // find an empty spot - while (0 != mInfo[idx]) { - next(&info, &idx); - } - - auto& l = mKeyVals[insertion_idx]; - if (idx == insertion_idx) { - ::new (static_cast(&l)) Node(std::move(keyval)); - } else { - shiftUp(idx, insertion_idx); - l = std::move(keyval); - } - - // put at empty spot - mInfo[insertion_idx] = insertion_info; - - ++mNumElements; - } - -public: - using iterator = Iter; - using const_iterator = Iter; - - Table() noexcept(noexcept(Hash()) && noexcept(KeyEqual())) - : WHash() - , WKeyEqual() { - ROBIN_HOOD_TRACE(this) - } - - // Creates an empty hash map. Nothing is allocated yet, this happens at the first insert. - // This tremendously speeds up ctor & dtor of a map that never receives an element. The - // penalty is paid at the first insert, and not before. Lookup of this empty map works - // because everybody points to DummyInfoByte::b. parameter bucket_count is dictated by the - // standard, but we can ignore it. - explicit Table( - size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/, const Hash& h = Hash{}, - const KeyEqual& equal = KeyEqual{}) noexcept(noexcept(Hash(h)) && noexcept(KeyEqual(equal))) - : WHash(h) - , WKeyEqual(equal) { - ROBIN_HOOD_TRACE(this) - } - - template - Table(Iter first, Iter last, size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, - const Hash& h = Hash{}, const KeyEqual& equal = KeyEqual{}) - : WHash(h) - , WKeyEqual(equal) { - ROBIN_HOOD_TRACE(this) - insert(first, last); - } - - Table(std::initializer_list initlist, - size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, const Hash& h = Hash{}, - const KeyEqual& equal = KeyEqual{}) - : WHash(h) - , WKeyEqual(equal) { - ROBIN_HOOD_TRACE(this) - insert(initlist.begin(), initlist.end()); - } - - Table(Table&& o) noexcept - : WHash(std::move(static_cast(o))) - , WKeyEqual(std::move(static_cast(o))) - , DataPool(std::move(static_cast(o))) { - ROBIN_HOOD_TRACE(this) - if (o.mMask) { - mHashMultiplier = std::move(o.mHashMultiplier); - mKeyVals = std::move(o.mKeyVals); - mInfo = std::move(o.mInfo); - mNumElements = std::move(o.mNumElements); - mMask = std::move(o.mMask); - mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); - mInfoInc = std::move(o.mInfoInc); - mInfoHashShift = std::move(o.mInfoHashShift); - // set other's mask to 0 so its destructor won't do anything - o.init(); - } - } - - Table& operator=(Table&& o) noexcept { - ROBIN_HOOD_TRACE(this) - if (&o != this) { - if (o.mMask) { - // only move stuff if the other map actually has some data - destroy(); - mHashMultiplier = std::move(o.mHashMultiplier); - mKeyVals = std::move(o.mKeyVals); - mInfo = std::move(o.mInfo); - mNumElements = std::move(o.mNumElements); - mMask = std::move(o.mMask); - mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); - mInfoInc = std::move(o.mInfoInc); - mInfoHashShift = std::move(o.mInfoHashShift); - WHash::operator=(std::move(static_cast(o))); - WKeyEqual::operator=(std::move(static_cast(o))); - DataPool::operator=(std::move(static_cast(o))); - - o.init(); - - } else { - // nothing in the other map => just clear us. - clear(); - } - } - return *this; - } - - Table(const Table& o) - : WHash(static_cast(o)) - , WKeyEqual(static_cast(o)) - , DataPool(static_cast(o)) { - ROBIN_HOOD_TRACE(this) - if (!o.empty()) { - // not empty: create an exact copy. it is also possible to just iterate through all - // elements and insert them, but copying is probably faster. - - auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); - auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); - - ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" - << numElementsWithBuffer << ")") - mHashMultiplier = o.mHashMultiplier; - mKeyVals = static_cast( - detail::assertNotNull(std::malloc(numBytesTotal))); - // no need for calloc because clonData does memcpy - mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); - mNumElements = o.mNumElements; - mMask = o.mMask; - mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; - mInfoInc = o.mInfoInc; - mInfoHashShift = o.mInfoHashShift; - cloneData(o); - } - } - - // Creates a copy of the given map. Copy constructor of each entry is used. - // Not sure why clang-tidy thinks this doesn't handle self assignment, it does - Table& operator=(Table const& o) { - ROBIN_HOOD_TRACE(this) - if (&o == this) { - // prevent assigning of itself - return *this; - } - - // we keep using the old allocator and not assign the new one, because we want to keep - // the memory available. when it is the same size. - if (o.empty()) { - if (0 == mMask) { - // nothing to do, we are empty too - return *this; - } - - // not empty: destroy what we have there - // clear also resets mInfo to 0, that's sometimes not necessary. - destroy(); - init(); - WHash::operator=(static_cast(o)); - WKeyEqual::operator=(static_cast(o)); - DataPool::operator=(static_cast(o)); - - return *this; - } - - // clean up old stuff - Destroyer::value>{}.nodes(*this); - - if (mMask != o.mMask) { - // no luck: we don't have the same array size allocated, so we need to realloc. - if (0 != mMask) { - // only deallocate if we actually have data! - ROBIN_HOOD_LOG("std::free") - std::free(mKeyVals); - } - - auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); - auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); - ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" - << numElementsWithBuffer << ")") - mKeyVals = static_cast( - detail::assertNotNull(std::malloc(numBytesTotal))); - - // no need for calloc here because cloneData performs a memcpy. - mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); - // sentinel is set in cloneData - } - WHash::operator=(static_cast(o)); - WKeyEqual::operator=(static_cast(o)); - DataPool::operator=(static_cast(o)); - mHashMultiplier = o.mHashMultiplier; - mNumElements = o.mNumElements; - mMask = o.mMask; - mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; - mInfoInc = o.mInfoInc; - mInfoHashShift = o.mInfoHashShift; - cloneData(o); - - return *this; - } - - // Swaps everything between the two maps. - void swap(Table& o) { - ROBIN_HOOD_TRACE(this) - using std::swap; - swap(o, *this); - } - - // Clears all data, without resizing. - void clear() { - ROBIN_HOOD_TRACE(this) - if (empty()) { - // don't do anything! also important because we don't want to write to - // DummyInfoByte::b, even though we would just write 0 to it. - return; - } - - Destroyer::value>{}.nodes(*this); - - auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); - // clear everything, then set the sentinel again - uint8_t const z = 0; - std::fill(mInfo, mInfo + calcNumBytesInfo(numElementsWithBuffer), z); - mInfo[numElementsWithBuffer] = 1; - - mInfoInc = InitialInfoInc; - mInfoHashShift = InitialInfoHashShift; - } - - // Destroys the map and all it's contents. - ~Table() { - ROBIN_HOOD_TRACE(this) - destroy(); - } - - // Checks if both tables contain the same entries. Order is irrelevant. - bool operator==(const Table& other) const { - ROBIN_HOOD_TRACE(this) - if (other.size() != size()) { - return false; - } - for (auto const& otherEntry : other) { - if (!has(otherEntry)) { - return false; - } - } - - return true; - } - - bool operator!=(const Table& other) const { - ROBIN_HOOD_TRACE(this) - return !operator==(other); - } - - template - typename std::enable_if::value, Q&>::type operator[](const key_type& key) { - ROBIN_HOOD_TRACE(this) - auto idxAndState = insertKeyPrepareEmptySpot(key); - switch (idxAndState.second) { - case InsertionState::key_found: - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) - Node(*this, std::piecewise_construct, std::forward_as_tuple(key), - std::forward_as_tuple()); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, - std::forward_as_tuple(key), std::forward_as_tuple()); - break; - - case InsertionState::overflow_error: - throwOverflowError(); - } - - return mKeyVals[idxAndState.first].getSecond(); - } - - template - typename std::enable_if::value, Q&>::type operator[](key_type&& key) { - ROBIN_HOOD_TRACE(this) - auto idxAndState = insertKeyPrepareEmptySpot(key); - switch (idxAndState.second) { - case InsertionState::key_found: - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) - Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), - std::forward_as_tuple()); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = - Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), - std::forward_as_tuple()); - break; - - case InsertionState::overflow_error: - throwOverflowError(); - } - - return mKeyVals[idxAndState.first].getSecond(); - } - - template - void insert(Iter first, Iter last) { - for (; first != last; ++first) { - // value_type ctor needed because this might be called with std::pair's - insert(value_type(*first)); - } - } - - void insert(std::initializer_list ilist) { - for (auto&& vt : ilist) { - insert(std::move(vt)); - } - } - - template - std::pair emplace(Args&&... args) { - ROBIN_HOOD_TRACE(this) - Node n{*this, std::forward(args)...}; - auto idxAndState = insertKeyPrepareEmptySpot(getFirstConst(n)); - switch (idxAndState.second) { - case InsertionState::key_found: - n.destroy(*this); - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) Node(*this, std::move(n)); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = std::move(n); - break; - - case InsertionState::overflow_error: - n.destroy(*this); - throwOverflowError(); - break; - } - - return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), - InsertionState::key_found != idxAndState.second); - } - - template - iterator emplace_hint(const_iterator position, Args&&... args) { - (void)position; - return emplace(std::forward(args)...).first; - } - - template - std::pair try_emplace(const key_type& key, Args&&... args) { - return try_emplace_impl(key, std::forward(args)...); - } - - template - std::pair try_emplace(key_type&& key, Args&&... args) { - return try_emplace_impl(std::move(key), std::forward(args)...); - } - - template - iterator try_emplace(const_iterator hint, const key_type& key, Args&&... args) { - (void)hint; - return try_emplace_impl(key, std::forward(args)...).first; - } - - template - iterator try_emplace(const_iterator hint, key_type&& key, Args&&... args) { - (void)hint; - return try_emplace_impl(std::move(key), std::forward(args)...).first; - } - - template - std::pair insert_or_assign(const key_type& key, Mapped&& obj) { - return insertOrAssignImpl(key, std::forward(obj)); - } - - template - std::pair insert_or_assign(key_type&& key, Mapped&& obj) { - return insertOrAssignImpl(std::move(key), std::forward(obj)); - } - - template - iterator insert_or_assign(const_iterator hint, const key_type& key, Mapped&& obj) { - (void)hint; - return insertOrAssignImpl(key, std::forward(obj)).first; - } - - template - iterator insert_or_assign(const_iterator hint, key_type&& key, Mapped&& obj) { - (void)hint; - return insertOrAssignImpl(std::move(key), std::forward(obj)).first; - } - - std::pair insert(const value_type& keyval) { - ROBIN_HOOD_TRACE(this) - return emplace(keyval); - } - - iterator insert(const_iterator hint, const value_type& keyval) { - (void)hint; - return emplace(keyval).first; - } - - std::pair insert(value_type&& keyval) { - return emplace(std::move(keyval)); - } - - iterator insert(const_iterator hint, value_type&& keyval) { - (void)hint; - return emplace(std::move(keyval)).first; - } - - // Returns 1 if key is found, 0 otherwise. - size_t count(const key_type& key) const { // NOLINT - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { - return 1; - } - return 0; - } - - template - typename std::enable_if::type count(const OtherKey& key) const { - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { - return 1; - } - return 0; - } - - bool contains(const key_type& key) const { // NOLINT - return 1U == count(key); - } - - template - typename std::enable_if::type contains(const OtherKey& key) const { - return 1U == count(key); - } - - // Returns a reference to the value found for key. - // Throws std::out_of_range if element cannot be found - template - typename std::enable_if::value, Q&>::type at(key_type const& key) { - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { - doThrow("key not found"); - } - return kv->getSecond(); - } - - // Returns a reference to the value found for key. - // Throws std::out_of_range if element cannot be found - template - typename std::enable_if::value, Q const&>::type at(key_type const& key) const { - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { - doThrow("key not found"); - } - return kv->getSecond(); - } - - const_iterator find(const key_type& key) const { // NOLINT - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return const_iterator{mKeyVals + idx, mInfo + idx}; - } - - template - const_iterator find(const OtherKey& key, is_transparent_tag /*unused*/) const { - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return const_iterator{mKeyVals + idx, mInfo + idx}; - } - - template - typename std::enable_if::type // NOLINT - find(const OtherKey& key) const { // NOLINT - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return const_iterator{mKeyVals + idx, mInfo + idx}; - } - - iterator find(const key_type& key) { - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return iterator{mKeyVals + idx, mInfo + idx}; - } - - template - iterator find(const OtherKey& key, is_transparent_tag/*unused*/) { - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return iterator{mKeyVals + idx, mInfo + idx}; - } - - template - typename std::enable_if::type find(const OtherKey& key) { - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return iterator{mKeyVals + idx, mInfo + idx}; - } - - iterator begin() { - ROBIN_HOOD_TRACE(this) - if (empty()) { - return end(); - } - return iterator(mKeyVals, mInfo, fast_forward_tag{}); - } - const_iterator begin() const { // NOLINT - ROBIN_HOOD_TRACE(this) - return cbegin(); - } - const_iterator cbegin() const { // NOLINT - ROBIN_HOOD_TRACE(this) - if (empty()) { - return cend(); - } - return const_iterator(mKeyVals, mInfo, fast_forward_tag{}); - } - - iterator end() { - ROBIN_HOOD_TRACE(this) - // no need to supply valid info pointer: end() must not be dereferenced, and only node - // pointer is compared. - return iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; - } - const_iterator end() const { // NOLINT - ROBIN_HOOD_TRACE(this) - return cend(); - } - const_iterator cend() const { // NOLINT - ROBIN_HOOD_TRACE(this) - return const_iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; - } - - iterator erase(const_iterator pos) { - ROBIN_HOOD_TRACE(this) - // its safe to perform const cast here - return erase(iterator{const_cast(pos.mKeyVals), const_cast(pos.mInfo)}); - } - - // Erases element at pos, returns iterator to the next element. - iterator erase(iterator pos) { - ROBIN_HOOD_TRACE(this) - // we assume that pos always points to a valid entry, and not end(). - auto const idx = static_cast(pos.mKeyVals - mKeyVals); - - shiftDown(idx); - --mNumElements; - - if (*pos.mInfo) { - // we've backward shifted, return this again - return pos; - } - - // no backward shift, return next element - return ++pos; - } - - size_t erase(const key_type& key) { - ROBIN_HOOD_TRACE(this) - size_t idx{}; - InfoType info{}; - keyToIdx(key, &idx, &info); - - // check while info matches with the source idx - do { - if (info == mInfo[idx] && WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { - shiftDown(idx); - --mNumElements; - return 1; - } - next(&info, &idx); - } while (info <= mInfo[idx]); - - // nothing found to delete - return 0; - } - - // reserves space for the specified number of elements. Makes sure the old data fits. - // exactly the same as reserve(c). - void rehash(size_t c) { - // forces a reserve - reserve(c, true); - } - - // reserves space for the specified number of elements. Makes sure the old data fits. - // Exactly the same as rehash(c). Use rehash(0) to shrink to fit. - void reserve(size_t c) { - // reserve, but don't force rehash - reserve(c, false); - } - - // If possible reallocates the map to a smaller one. This frees the underlying table. - // Does not do anything if load_factor is too large for decreasing the table's size. - void compact() { - ROBIN_HOOD_TRACE(this) - auto newSize = InitialNumElements; - while (calcMaxNumElementsAllowed(newSize) < mNumElements && newSize != 0) { - newSize *= 2; - } - if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { - throwOverflowError(); - } - - ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") - - // only actually do anything when the new size is bigger than the old one. This prevents to - // continuously allocate for each reserve() call. - if (newSize < mMask + 1) { - rehashPowerOfTwo(newSize, true); - } - } - - size_type size() const noexcept { // NOLINT - ROBIN_HOOD_TRACE(this) - return mNumElements; - } - - size_type max_size() const noexcept { // NOLINT - ROBIN_HOOD_TRACE(this) - return static_cast(-1); - } - - ROBIN_HOOD(NODISCARD) bool empty() const noexcept { - ROBIN_HOOD_TRACE(this) - return 0 == mNumElements; - } - - float max_load_factor() const noexcept { // NOLINT - ROBIN_HOOD_TRACE(this) - return MaxLoadFactor100 / 100.0F; - } - - // Average number of elements per bucket. Since we allow only 1 per bucket - float load_factor() const noexcept { // NOLINT - ROBIN_HOOD_TRACE(this) - return static_cast(size()) / static_cast(mMask + 1); - } - - ROBIN_HOOD(NODISCARD) size_t mask() const noexcept { - ROBIN_HOOD_TRACE(this) - return mMask; - } - - ROBIN_HOOD(NODISCARD) size_t calcMaxNumElementsAllowed(size_t maxElements) const noexcept { - if (ROBIN_HOOD_LIKELY(maxElements <= (std::numeric_limits::max)() / 100)) { - return maxElements * MaxLoadFactor100 / 100; - } - - // we might be a bit imprecise, but since maxElements is quite large that doesn't matter - return (maxElements / 100) * MaxLoadFactor100; - } - - ROBIN_HOOD(NODISCARD) size_t calcNumBytesInfo(size_t numElements) const noexcept { - // we add a uint64_t, which houses the sentinel (first byte) and padding so we can load - // 64bit types. - return numElements + sizeof(uint64_t); - } - - ROBIN_HOOD(NODISCARD) - size_t calcNumElementsWithBuffer(size_t numElements) const noexcept { - auto maxNumElementsAllowed = calcMaxNumElementsAllowed(numElements); - return numElements + (std::min)(maxNumElementsAllowed, (static_cast(0xFF))); - } - - // calculation only allowed for 2^n values - ROBIN_HOOD(NODISCARD) size_t calcNumBytesTotal(size_t numElements) const { -#if ROBIN_HOOD(BITNESS) == 64 - return numElements * sizeof(Node) + calcNumBytesInfo(numElements); -#else - // make sure we're doing 64bit operations, so we are at least safe against 32bit overflows. - auto const ne = static_cast(numElements); - auto const s = static_cast(sizeof(Node)); - auto const infos = static_cast(calcNumBytesInfo(numElements)); - - auto const total64 = ne * s + infos; - auto const total = static_cast(total64); - - if (ROBIN_HOOD_UNLIKELY(static_cast(total) != total64)) { - throwOverflowError(); - } - return total; -#endif - } - -private: - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::value, bool>::type has(const value_type& e) const { - ROBIN_HOOD_TRACE(this) - auto it = find(e.first); - return it != end() && it->second == e.second; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::value, bool>::type has(const value_type& e) const { - ROBIN_HOOD_TRACE(this) - return find(e) != end(); - } - - void reserve(size_t c, bool forceRehash) { - ROBIN_HOOD_TRACE(this) - auto const minElementsAllowed = (std::max)(c, mNumElements); - auto newSize = InitialNumElements; - while (calcMaxNumElementsAllowed(newSize) < minElementsAllowed && newSize != 0) { - newSize *= 2; - } - if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { - throwOverflowError(); - } - - ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") - - // only actually do anything when the new size is bigger than the old one. This prevents to - // continuously allocate for each reserve() call. - if (forceRehash || newSize > mMask + 1) { - rehashPowerOfTwo(newSize, false); - } - } - - // reserves space for at least the specified number of elements. - // only works if numBuckets if power of two - // True on success, false otherwise - void rehashPowerOfTwo(size_t numBuckets, bool forceFree) { - ROBIN_HOOD_TRACE(this) - - Node* const oldKeyVals = mKeyVals; - uint8_t const* const oldInfo = mInfo; - - const size_t oldMaxElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); - - // resize operation: move stuff - initData(numBuckets); - if (oldMaxElementsWithBuffer > 1) { - for (size_t i = 0; i < oldMaxElementsWithBuffer; ++i) { - if (oldInfo[i] != 0) { - // might throw an exception, which is really bad since we are in the middle of - // moving stuff. - insert_move(std::move(oldKeyVals[i])); - // destroy the node but DON'T destroy the data. - oldKeyVals[i].~Node(); - } - } - - // this check is not necessary as it's guarded by the previous if, but it helps - // silence g++'s overeager "attempt to free a non-heap object 'map' - // [-Werror=free-nonheap-object]" warning. - if (oldKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { - // don't destroy old data: put it into the pool instead - if (forceFree) { - std::free(oldKeyVals); - } else { - DataPool::addOrFree(oldKeyVals, calcNumBytesTotal(oldMaxElementsWithBuffer)); - } - } - } - } - - ROBIN_HOOD(NOINLINE) void throwOverflowError() const { -#if ROBIN_HOOD(HAS_EXCEPTIONS) - throw std::overflow_error("robin_hood::map overflow"); -#else - abort(); -#endif - } - - template - std::pair try_emplace_impl(OtherKey&& key, Args&&... args) { - ROBIN_HOOD_TRACE(this) - auto idxAndState = insertKeyPrepareEmptySpot(key); - switch (idxAndState.second) { - case InsertionState::key_found: - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) Node( - *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), - std::forward_as_tuple(std::forward(args)...)); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, - std::forward_as_tuple(std::forward(key)), - std::forward_as_tuple(std::forward(args)...)); - break; - - case InsertionState::overflow_error: - throwOverflowError(); - break; - } - - return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), - InsertionState::key_found != idxAndState.second); - } - - template - std::pair insertOrAssignImpl(OtherKey&& key, Mapped&& obj) { - ROBIN_HOOD_TRACE(this) - auto idxAndState = insertKeyPrepareEmptySpot(key); - switch (idxAndState.second) { - case InsertionState::key_found: - mKeyVals[idxAndState.first].getSecond() = std::forward(obj); - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) Node( - *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), - std::forward_as_tuple(std::forward(obj))); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, - std::forward_as_tuple(std::forward(key)), - std::forward_as_tuple(std::forward(obj))); - break; - - case InsertionState::overflow_error: - throwOverflowError(); - break; - } - - return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), - InsertionState::key_found != idxAndState.second); - } - - void initData(size_t max_elements) { - mNumElements = 0; - mMask = max_elements - 1; - mMaxNumElementsAllowed = calcMaxNumElementsAllowed(max_elements); - - auto const numElementsWithBuffer = calcNumElementsWithBuffer(max_elements); - - // malloc & zero mInfo. Faster than calloc everything. - auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); - ROBIN_HOOD_LOG("std::calloc " << numBytesTotal << " = calcNumBytesTotal(" - << numElementsWithBuffer << ")") - mKeyVals = reinterpret_cast( - detail::assertNotNull(std::malloc(numBytesTotal))); - mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); - std::memset(mInfo, 0, numBytesTotal - numElementsWithBuffer * sizeof(Node)); - - // set sentinel - mInfo[numElementsWithBuffer] = 1; - - mInfoInc = InitialInfoInc; - mInfoHashShift = InitialInfoHashShift; - } - - enum class InsertionState { overflow_error, key_found, new_node, overwrite_node }; - - // Finds key, and if not already present prepares a spot where to pot the key & value. - // This potentially shifts nodes out of the way, updates mInfo and number of inserted - // elements, so the only operation left to do is create/assign a new node at that spot. - template - std::pair insertKeyPrepareEmptySpot(OtherKey&& key) { - for (int i = 0; i < 256; ++i) { - size_t idx{}; - InfoType info{}; - keyToIdx(key, &idx, &info); - nextWhileLess(&info, &idx); - - // while we potentially have a match - while (info == mInfo[idx]) { - if (WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { - // key already exists, do NOT insert. - // see http://en.cppreference.com/w/cpp/container/unordered_map/insert - return std::make_pair(idx, InsertionState::key_found); - } - next(&info, &idx); - } - - // unlikely that this evaluates to true - if (ROBIN_HOOD_UNLIKELY(mNumElements >= mMaxNumElementsAllowed)) { - if (!increase_size()) { - return std::make_pair(size_t(0), InsertionState::overflow_error); - } - continue; - } - - // key not found, so we are now exactly where we want to insert it. - auto const insertion_idx = idx; - auto const insertion_info = info; - if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { - mMaxNumElementsAllowed = 0; - } - - // find an empty spot - while (0 != mInfo[idx]) { - next(&info, &idx); - } - - if (idx != insertion_idx) { - shiftUp(idx, insertion_idx); - } - // put at empty spot - mInfo[insertion_idx] = static_cast(insertion_info); - ++mNumElements; - return std::make_pair(insertion_idx, idx == insertion_idx - ? InsertionState::new_node - : InsertionState::overwrite_node); - } - - // enough attempts failed, so finally give up. - return std::make_pair(size_t(0), InsertionState::overflow_error); - } - - bool try_increase_info() { - ROBIN_HOOD_LOG("mInfoInc=" << mInfoInc << ", numElements=" << mNumElements - << ", maxNumElementsAllowed=" - << calcMaxNumElementsAllowed(mMask + 1)) - if (mInfoInc <= 2) { - // need to be > 2 so that shift works (otherwise undefined behavior!) - return false; - } - // we got space left, try to make info smaller - mInfoInc = static_cast(mInfoInc >> 1U); - - // remove one bit of the hash, leaving more space for the distance info. - // This is extremely fast because we can operate on 8 bytes at once. - ++mInfoHashShift; - auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); - - for (size_t i = 0; i < numElementsWithBuffer; i += 8) { - auto val = unaligned_load(mInfo + i); - val = (val >> 1U) & UINT64_C(0x7f7f7f7f7f7f7f7f); - std::memcpy(mInfo + i, &val, sizeof(val)); - } - // update sentinel, which might have been cleared out! - mInfo[numElementsWithBuffer] = 1; - - mMaxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); - return true; - } - - // True if resize was possible, false otherwise - bool increase_size() { - // nothing allocated yet? just allocate InitialNumElements - if (0 == mMask) { - initData(InitialNumElements); - return true; - } - - auto const maxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); - if (mNumElements < maxNumElementsAllowed && try_increase_info()) { - return true; - } - - ROBIN_HOOD_LOG("mNumElements=" << mNumElements << ", maxNumElementsAllowed=" - << maxNumElementsAllowed << ", load=" - << (static_cast(mNumElements) * 100.0 / - (static_cast(mMask) + 1))) - - if (mNumElements * 2 < calcMaxNumElementsAllowed(mMask + 1)) { - // we have to resize, even though there would still be plenty of space left! - // Try to rehash instead. Delete freed memory so we don't steadyily increase mem in case - // we have to rehash a few times - nextHashMultiplier(); - rehashPowerOfTwo(mMask + 1, true); - } else { - // we've reached the capacity of the map, so the hash seems to work nice. Keep using it. - rehashPowerOfTwo((mMask + 1) * 2, false); - } - return true; - } - - void nextHashMultiplier() { - // adding an *even* number, so that the multiplier will always stay odd. This is necessary - // so that the hash stays a mixing function (and thus doesn't have any information loss). - mHashMultiplier += UINT64_C(0xc4ceb9fe1a85ec54); - } - - void destroy() { - if (0 == mMask) { - // don't deallocate! - return; - } - - Destroyer::value>{} - .nodesDoNotDeallocate(*this); - - // This protection against not deleting mMask shouldn't be needed as it's sufficiently - // protected with the 0==mMask check, but I have this anyways because g++ 7 otherwise - // reports a compile error: attempt to free a non-heap object 'fm' - // [-Werror=free-nonheap-object] - if (mKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { - ROBIN_HOOD_LOG("std::free") - std::free(mKeyVals); - } - } - - void init() noexcept { - mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); - mInfo = reinterpret_cast(&mMask); - mNumElements = 0; - mMask = 0; - mMaxNumElementsAllowed = 0; - mInfoInc = InitialInfoInc; - mInfoHashShift = InitialInfoHashShift; - } - - // members are sorted so no padding occurs - uint64_t mHashMultiplier = UINT64_C(0xc4ceb9fe1a85ec53); // 8 byte 8 - Node* mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); // 8 byte 16 - uint8_t* mInfo = reinterpret_cast(&mMask); // 8 byte 24 - size_t mNumElements = 0; // 8 byte 32 - size_t mMask = 0; // 8 byte 40 - size_t mMaxNumElementsAllowed = 0; // 8 byte 48 - InfoType mInfoInc = InitialInfoInc; // 4 byte 52 - InfoType mInfoHashShift = InitialInfoHashShift; // 4 byte 56 - // 16 byte 56 if NodeAllocator -}; - -} // namespace detail - -// map - -template , - typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> -using unordered_flat_map = detail::Table; - -template , - typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> -using unordered_node_map = detail::Table; - -template , - typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> -using unordered_map = - detail::Table) <= sizeof(size_t) * 6 && - std::is_nothrow_move_constructible>::value && - std::is_nothrow_move_assignable>::value, - MaxLoadFactor100, Key, T, Hash, KeyEqual>; - -// set - -template , typename KeyEqual = std::equal_to, - size_t MaxLoadFactor100 = 80> -using unordered_flat_set = detail::Table; - -template , typename KeyEqual = std::equal_to, - size_t MaxLoadFactor100 = 80> -using unordered_node_set = detail::Table; - -template , typename KeyEqual = std::equal_to, - size_t MaxLoadFactor100 = 80> -using unordered_set = detail::Table < sizeof(Key) <= sizeof(size_t) * 6 && - std::is_nothrow_move_constructible::value && - std::is_nothrow_move_assignable::value, - MaxLoadFactor100, Key, void, Hash, KeyEqual>; - -} // namespace robin_hood -/* *INDENT-ON* */ - -#endif // NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp deleted file mode 100644 index f8aa72ff..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__TYPES_HPP_ -#define ATHENA_SMAC_PLANNER__TYPES_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -namespace athena_smac_planner -{ - -typedef std::pair NodeHeuristicPair; -typedef std::vector LookupTable; -typedef std::pair TrigValues; - -/** - * @struct athena_smac_planner::SearchInfo - * @brief Search properties and penalties - */ -struct SearchInfo -{ - float minimum_turning_radius{8.0}; - float non_straight_penalty{1.05}; - float change_penalty{0.0}; - float reverse_penalty{2.0}; - float cost_penalty{2.0}; - float retrospective_penalty{0.015}; - float rotation_penalty{5.0}; - float analytic_expansion_ratio{3.5}; - float analytic_expansion_max_length{60.0}; - float analytic_expansion_max_cost{200.0}; - bool analytic_expansion_max_cost_override{false}; - std::string lattice_filepath; - bool cache_obstacle_heuristic{false}; - bool allow_reverse_expansion{false}; - bool allow_primitive_interpolation{false}; - bool downsample_obstacle_heuristic{true}; - bool use_quadratic_cost_penalty{false}; -}; - -/** - * @struct athena_smac_planner::SmootherParams - * @brief Parameters for the smoother - */ -struct SmootherParams -{ - /** - * @brief A constructor for athena_smac_planner::SmootherParams - */ - SmootherParams() - : holonomic_(false) - { - } - - /** - * @brief Get params from ROS parameter - * @param node Ptr to node - * @param name Name of plugin - */ - void get(rclcpp::Node * node, const std::string & name) - { - std::string prefix = name + ".smoother."; - auto dp = [node, &prefix](const std::string & p, auto def) { - if (!node->has_parameter(prefix + p)) { - node->declare_parameter(prefix + p, def); - } - return node->get_parameter(prefix + p).get_value(); - }; - tolerance_ = dp("tolerance", 1e-10); - max_its_ = dp("max_iterations", 1000); - w_data_ = dp("w_data", 0.2); - w_smooth_ = dp("w_smooth", 0.3); - do_refinement_ = dp("do_refinement", true); - refinement_num_ = dp("refinement_num", 2); - } - - double tolerance_; - int max_its_; - double w_data_; - double w_smooth_; - bool holonomic_; - bool do_refinement_; - int refinement_num_; -}; - -/** - * @struct athena_smac_planner::TurnDirection - * @brief A struct with the motion primitive's direction embedded - */ -enum class TurnDirection -{ - UNKNOWN = 0, - FORWARD = 1, - LEFT = 2, - RIGHT = 3, - REVERSE = 4, - REV_LEFT = 5, - REV_RIGHT = 6 -}; - -/** - * @struct athena_smac_planner::MotionPose - * @brief A struct for poses in motion primitives - */ -struct MotionPose -{ - /** - * @brief A constructor for athena_smac_planner::MotionPose - */ - MotionPose() {} - - /** - * @brief A constructor for athena_smac_planner::MotionPose - * @param x X pose - * @param y Y pose - * @param theta Angle of pose - * @param TurnDirection Direction of the primitive's turn - */ - MotionPose(const float & x, const float & y, const float & theta, const TurnDirection & turn_dir) - : _x(x), _y(y), _theta(theta), _turn_dir(turn_dir) - {} - - MotionPose operator-(const MotionPose & p2) - { - return MotionPose( - this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta, TurnDirection::UNKNOWN); - } - - float _x; - float _y; - float _theta; - TurnDirection _turn_dir; -}; - -typedef std::vector MotionPoses; - -/** - * @struct athena_smac_planner::LatticeMetadata - * @brief A struct of all lattice metadata - */ -struct LatticeMetadata -{ - float min_turning_radius; - float grid_resolution; - unsigned int number_of_headings; - std::vector heading_angles; - unsigned int number_of_trajectories; - std::string motion_model; -}; - -/** - * @struct athena_smac_planner::MotionPrimitive - * @brief A struct of all motion primitive data - */ -struct MotionPrimitive -{ - unsigned int trajectory_id; - float start_angle; - float end_angle; - float turning_radius; - float trajectory_length; - float arc_length; - float straight_length; - bool left_turn; - MotionPoses poses; -}; - -/** - * @struct athena_smac_planner::GoalState - * @brief A struct to store the goal state - */ -template -struct GoalState -{ - NodeT * goal = nullptr; - bool is_valid = true; -}; - -typedef std::vector MotionPrimitives; -typedef std::vector MotionPrimitivePtrs; - -/** - * @class athena_smac_planner::Coordinates - * @brief Implementation of coordinate2d structure - */ -struct Coordinates2D -{ - Coordinates2D() {} - Coordinates2D(const float & x_in, const float & y_in) - : x(x_in), y(y_in) - {} - - inline bool operator==(const Coordinates2D & rhs) const - { - return this->x == rhs.x && this->y == rhs.y; - } - - inline bool operator!=(const Coordinates2D & rhs) const - { - return !(*this == rhs); - } - - float x, y; -}; - -/** - * @class athena_smac_planner::Coordinates - * @brief Implementation of coordinate structure - */ -struct Coordinates -{ - /** - * @brief A constructor for athena_smac_planner::NodeHybrid::Coordinates - */ - Coordinates() {} - - /** - * @brief A constructor for athena_smac_planner::NodeHybrid::Coordinates - * @param x_in X coordinate - * @param y_in Y coordinate - * @param theta_in Theta coordinate - */ - Coordinates(const float & x_in, const float & y_in, const float & theta_in) - : x(x_in), y(y_in), theta(theta_in) - {} - - inline bool operator==(const Coordinates & rhs) const - { - return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; - } - - inline bool operator!=(const Coordinates & rhs) const - { - return !(*this == rhs); - } - - float x, y, theta; -}; -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__TYPES_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp deleted file mode 100644 index 494f68d4..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp +++ /dev/null @@ -1,231 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// Copyright (c) 2023, 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__UTILS_HPP_ -#define ATHENA_SMAC_PLANNER__UTILS_HPP_ - -#include -#include -#include - -#include "nlohmann/json.hpp" -#include "geometry_msgs/msg/quaternion.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "tf2/utils.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/inflation_layer.hpp" -#include "visualization_msgs/msg/marker_array.hpp" -#include "athena_smac_planner/types.hpp" -#include - -namespace athena_smac_planner -{ - -/** -* @brief Create an Eigen Vector2D of world poses from continuous map coords -* @param mx float of map X coordinate -* @param my float of map Y coordinate -* @param costmap Costmap pointer -* @return Eigen::Vector2d eigen vector of the generated path -*/ -inline geometry_msgs::msg::Pose getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) -{ - geometry_msgs::msg::Pose msg; - msg.position.x = - static_cast(costmap->getOriginX()) + mx * costmap->getResolution(); - msg.position.y = - static_cast(costmap->getOriginY()) + my * costmap->getResolution(); - return msg; -} - -/** -* @brief Create quaternion from radians -* @param theta continuous bin coordinates angle -* @return quaternion orientation in map frame -*/ -inline geometry_msgs::msg::Quaternion getWorldOrientation( - const float & theta) -{ - // theta is in radians already - tf2::Quaternion q; - q.setEuler(0.0, 0.0, theta); - return tf2::toMsg(q); -} - -/** -* @brief Find the min cost of the inflation decay function for which the robot MAY be -* in collision in any orientation -* @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) -* @return double circumscribed cost, any higher than this and need to do full footprint collision checking -* since some element of the robot could be in collision -*/ -inline double findCircumscribedCost(std::shared_ptr costmap) -{ - double result = -1.0; - std::vector>::iterator layer; - - // check if the costmap has an inflation layer - std::shared_ptr inflation_layer = nullptr; - for (auto & layer : *costmap->getLayeredCostmap()->getPlugins()) { - auto candidate = std::dynamic_pointer_cast(layer); - if (candidate) { - inflation_layer = candidate; - break; - } - } - if (inflation_layer != nullptr) { - double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); - double resolution = costmap->getCostmap()->getResolution(); - result = static_cast(inflation_layer->computeCost(circum_radius / resolution)); - } else { - RCLCPP_WARN( - rclcpp::get_logger("computeCircumscribedCost"), - "No inflation layer found in costmap configuration. " - "If this is an SE2-collision checking plugin, it cannot use costmap potential " - "field to speed up collision checking by only checking the full footprint " - "when robot is within possibly-inscribed radius of an obstacle. This may " - "significantly slow down planning times!"); - } - - return result; -} - -/** - * @brief convert json to lattice metadata - * @param[in] json json object - * @param[out] lattice meta data - */ -inline void fromJsonToMetaData(const nlohmann::json & json, LatticeMetadata & lattice_metadata) -{ - json.at("turning_radius").get_to(lattice_metadata.min_turning_radius); - json.at("grid_resolution").get_to(lattice_metadata.grid_resolution); - json.at("num_of_headings").get_to(lattice_metadata.number_of_headings); - json.at("heading_angles").get_to(lattice_metadata.heading_angles); - json.at("number_of_trajectories").get_to(lattice_metadata.number_of_trajectories); - json.at("motion_model").get_to(lattice_metadata.motion_model); -} - -/** - * @brief convert json to pose - * @param[in] json json object - * @param[out] pose - */ -inline void fromJsonToPose(const nlohmann::json & json, MotionPose & pose) -{ - pose._x = json[0]; - pose._y = json[1]; - pose._theta = json[2]; -} - -/** - * @brief convert json to motion primitive - * @param[in] json json object - * @param[out] motion primitive - */ -inline void fromJsonToMotionPrimitive( - const nlohmann::json & json, MotionPrimitive & motion_primitive) -{ - json.at("trajectory_id").get_to(motion_primitive.trajectory_id); - json.at("start_angle_index").get_to(motion_primitive.start_angle); - json.at("end_angle_index").get_to(motion_primitive.end_angle); - json.at("trajectory_radius").get_to(motion_primitive.turning_radius); - json.at("trajectory_length").get_to(motion_primitive.trajectory_length); - json.at("arc_length").get_to(motion_primitive.arc_length); - json.at("straight_length").get_to(motion_primitive.straight_length); - json.at("left_turn").get_to(motion_primitive.left_turn); - - for (unsigned int i = 0; i < json["poses"].size(); i++) { - MotionPose pose; - fromJsonToPose(json["poses"][i], pose); - motion_primitive.poses.push_back(pose); - } -} - -/** - * @brief transform footprint into edges - * @param[in] robot position , orientation and footprint - * @param[out] robot footprint edges - */ -inline std::vector transformFootprintToEdges( - const geometry_msgs::msg::Pose & pose, - const std::vector & footprint) -{ - const double & x = pose.position.x; - const double & y = pose.position.y; - const double & yaw = tf2::getYaw(pose.orientation); - const double sin_yaw = sin(yaw); - const double cos_yaw = cos(yaw); - - std::vector out_footprint; - out_footprint.resize(2 * footprint.size()); - for (unsigned int i = 0; i < footprint.size(); i++) { - out_footprint[2 * i].x = x + cos_yaw * footprint[i].x - sin_yaw * footprint[i].y; - out_footprint[2 * i].y = y + sin_yaw * footprint[i].x + cos_yaw * footprint[i].y; - if (i == 0) { - out_footprint.back().x = out_footprint[i].x; - out_footprint.back().y = out_footprint[i].y; - } else { - out_footprint[2 * i - 1].x = out_footprint[2 * i].x; - out_footprint[2 * i - 1].y = out_footprint[2 * i].y; - } - } - return out_footprint; -} - -/** - * @brief initializes marker to visualize shape of linestring - * @param edge edge to mark of footprint - * @param i marker ID - * @param frame_id frame of the marker - * @param timestamp timestamp of the marker - * @return marker populated - */ -inline visualization_msgs::msg::Marker createMarker( - const std::vector edge, - unsigned int i, const std::string & frame_id, const rclcpp::Time & timestamp) -{ - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = timestamp; - marker.frame_locked = false; - marker.ns = "planned_footprint"; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.type = visualization_msgs::msg::Marker::LINE_LIST; - marker.lifetime = rclcpp::Duration(0, 0); - - marker.id = i; - for (auto & point : edge) { - marker.points.push_back(point); - } - - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.05; - marker.scale.y = 0.05; - marker.scale.z = 0.05; - marker.color.r = 0.0f; - marker.color.g = 0.0f; - marker.color.b = 1.0f; - marker.color.a = 1.3f; - return marker; -} - - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__UTILS_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/package.xml b/src/subsystems/minimal_navigation/athena_smac_planner/package.xml deleted file mode 100644 index fbae2a06..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - athena_smac_planner - 1.4.0 - Vendored Hybrid-A* planner (SMAC) for the Athena navigation stack - UMD Loop - Apache-2.0 - - ament_cmake - nav2_common - - angles - eigen - geometry_msgs - msgs - nav2_core - nav2_costmap_2d - nav_msgs - nlohmann-json-dev - ompl - rclcpp - rclcpp_lifecycle - tf2_ros - tf2 - visualization_msgs - - - ament_cmake - - diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp deleted file mode 100644 index 9f2ce25f..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp +++ /dev/null @@ -1,526 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// -// 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. Reserved. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "athena_smac_planner/a_star.hpp" -using namespace std::chrono; // NOLINT - -namespace athena_smac_planner -{ - -template -AStarAlgorithm::AStarAlgorithm( - const MotionModel & motion_model, - const SearchInfo & search_info) -: _traverse_unknown(true), - _is_initialized(false), - _max_iterations(0), - _terminal_checking_interval(5000), - _max_planning_time(0), - _x_size(0), - _y_size(0), - _search_info(search_info), - _start(nullptr), - _goal_manager(GoalManagerT()), - _motion_model(motion_model) -{ - _graph.reserve(100000); -} - -template -AStarAlgorithm::~AStarAlgorithm() -{ -} - -template -void AStarAlgorithm::initialize( - const bool & allow_unknown, - int & max_iterations, - const int & max_on_approach_iterations, - const int & terminal_checking_interval, - const double & max_planning_time, - const float & lookup_table_size, - const unsigned int & dim_3_size) -{ - _traverse_unknown = allow_unknown; - _max_iterations = max_iterations; - _max_on_approach_iterations = max_on_approach_iterations; - _terminal_checking_interval = terminal_checking_interval; - _max_planning_time = max_planning_time; - if (!_is_initialized) { - _shared_ctx = std::make_shared(); - _shared_ctx->distance_heuristic->precomputeDistanceHeuristic(lookup_table_size, _motion_model, - dim_3_size, - _search_info, _shared_ctx->motion_table); - } - _is_initialized = true; - _dim3_size = dim_3_size; - _expander = std::make_unique>( - _motion_model, _search_info, _traverse_unknown, _dim3_size); -} - -template -void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision_checker) -{ - _collision_checker = collision_checker; - _costmap = collision_checker->getCostmap(); - unsigned int x_size = _costmap->getSizeInCellsX(); - unsigned int y_size = _costmap->getSizeInCellsY(); - - clearGraph(); - - if (getSizeX() != x_size || getSizeY() != y_size) { - _x_size = x_size; - _y_size = y_size; - } - - // Always refresh the motion model so dynamic penalty parameters take effect immediately - NodeT::initMotionModel(_shared_ctx.get(), _motion_model, _x_size, _y_size, _dim3_size, - _search_info); - - // Always set context pointers to ensure newly allocated objects get their contexts restored - _goal_manager.setContext(_shared_ctx.get()); - _expander->setContext(_shared_ctx.get()); - _expander->setCollisionChecker(_collision_checker); -} - -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( - const uint64_t & index) -{ - auto iter = _graph.find(index); - if (iter != _graph.end()) { - return &(iter->second); - } - - return &(_graph.emplace(index, NodeT(index, _shared_ctx.get())).first->second); -} - -template -void AStarAlgorithm::setStart( - const float & mx, - const float & my, - const unsigned int & dim_3) -{ - _start = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - dim_3)); - _start->setPose(Coordinates(mx, my, dim_3)); -} - -template -void AStarAlgorithm::populateExpansionsLog( - const NodePtr & node, - std::vector> * expansions_log) -{ - typename NodeT::Coordinates coords = node->pose; - expansions_log->emplace_back( - _costmap->getOriginX() + ((coords.x + 0.5) * _costmap->getResolution()), - _costmap->getOriginY() + ((coords.y + 0.5) * _costmap->getResolution()), - _shared_ctx->motion_table.getAngleFromBin(coords.theta)); -} - -template -void AStarAlgorithm::setGoal( - const float & mx, - const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & goal_heading_mode, - const int & coarse_search_resolution) -{ - // Default to minimal resolution unless overridden for ALL_DIRECTION - _coarse_search_resolution = 1; - - _goal_manager.clear(); - Coordinates ref_goal_coord(mx, my, static_cast(dim_3)); - - if (!_search_info.cache_obstacle_heuristic || - _goal_manager.hasGoalChanged(ref_goal_coord)) - { - if (!_start) { - throw std::runtime_error("Start must be set before goal."); - } - - _shared_ctx->obstacle_heuristic->resetObstacleHeuristic( - _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my, - _shared_ctx->motion_table.downsample_obstacle_heuristic); - } - - _goal_manager.setRefGoalCoordinates(ref_goal_coord); - - unsigned int num_bins = _shared_ctx->motion_table.num_angle_quantization; - // set goal based on heading mode - switch (goal_heading_mode) { - case GoalHeadingMode::DEFAULT: { - // add a single goal node with single heading - auto goal = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - dim_3)); - goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); - _goal_manager.addGoal(goal); - break; - } - - case GoalHeadingMode::BIDIRECTIONAL: { - // Add two goals, one for each direction - // add goal in original direction - auto goal = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - dim_3)); - goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); - _goal_manager.addGoal(goal); - - // Add goal node in opposite (180°) direction - unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; - auto opposite_goal = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - opposite_heading)); - opposite_goal->setPose( - typename NodeT::Coordinates(mx, my, static_cast(opposite_heading))); - _goal_manager.addGoal(opposite_goal); - break; - } - - case GoalHeadingMode::ALL_DIRECTION: { - // Set the coarse search resolution only for all direction - _coarse_search_resolution = coarse_search_resolution; - - // Add goal nodes for all headings - for (unsigned int i = 0; i < num_bins; ++i) { - auto goal = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - i)); - goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(i))); - _goal_manager.addGoal(goal); - } - break; - } - case GoalHeadingMode::UNKNOWN: - throw std::runtime_error("Goal heading is UNKNOWN."); - } -} - -template -bool AStarAlgorithm::areInputsValid() -{ - // Check if graph was filled in - if (_graph.empty()) { - throw std::runtime_error("Failed to compute path, no costmap given."); - } - - // Check if points were filled in - if (!_start || _goal_manager.goalsIsEmpty()) { - throw std::runtime_error("Failed to compute path, no valid start or goal given."); - } - - // remove invalid goals - _goal_manager.removeInvalidGoals(getToleranceHeuristic(), _collision_checker, _traverse_unknown); - - // Check if ending point is valid - if (_goal_manager.getGoalsSet().empty()) { - throw nav2_core::PlannerException("Goal was in lethal cost"); - } - - // Note: We do not check the if the start is valid because it is cleared - return true; -} - -template -bool AStarAlgorithm::getClosestPathWithinTolerance(CoordinateVector & path) -{ - if (_best_heuristic_node.first < getToleranceHeuristic()) { - _graph.at(_best_heuristic_node.second).backtracePath(path); - return true; - } - - return false; -} - -template -bool AStarAlgorithm::createPath( - CoordinateVector & path, int & iterations, - const float & tolerance, - std::function cancel_checker, - std::vector> * expansions_log) -{ - steady_clock::time_point start_time = steady_clock::now(); - _tolerance = tolerance; - _best_heuristic_node = {std::numeric_limits::max(), 0}; - clearQueue(); - - if (!areInputsValid()) { - return false; - } - - NodeVector coarse_check_goals, fine_check_goals; - _goal_manager.prepareGoalsForAnalyticExpansion( - coarse_check_goals, fine_check_goals, - _coarse_search_resolution); - - // 0) Add starting point to the open set - addNode(0.0, getStart()); - getStart()->setAccumulatedCost(0.0); - - // Optimization: preallocate all variables - NodePtr current_node = nullptr; - NodePtr neighbor = nullptr; - NodePtr expansion_result = nullptr; - float g_cost = 0.0; - NodeVector neighbors; - int approach_iterations = 0; - NeighborIterator neighbor_iterator; - int analytic_iterations = 0; - int closest_distance = std::numeric_limits::max(); - - // Given an index, return a node ptr reference if its collision-free and valid - const uint64_t max_index = static_cast(getSizeX()) * - static_cast(getSizeY()) * - static_cast(getSizeDim3()); - NodeGetter neighborGetter = - [&, this](const uint64_t & index, NodePtr & neighbor_rtn) -> bool - { - if (index >= max_index) { - return false; - } - - neighbor_rtn = addToGraph(index); - return true; - }; - - while (iterations < getMaxIterations() && !_queue.empty()) { - // Check for planning timeout and cancel only on every Nth iteration - if (iterations % _terminal_checking_interval == 0) { - if (cancel_checker()) { - throw nav2_core::PlannerException("Planner was cancelled"); - } - std::chrono::duration planning_duration = - std::chrono::duration_cast>(steady_clock::now() - start_time); - if (static_cast(planning_duration.count()) >= _max_planning_time) { - // In case of timeout, return the path that is closest, if within tolerance. - return getClosestPathWithinTolerance(path); - } - } - - // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue - current_node = getNextNode(); - - // Save current node coordinates for debug - if (expansions_log) { - populateExpansionsLog(current_node, expansions_log); - } - - // We allow for nodes to be queued multiple times in case - // shorter paths result in it, but we can visit only once - // Also a chance to perform last-checks necessary. - if (onVisitationCheckNode(current_node)) { - continue; - } - - iterations++; - - // 2) Mark Nbest as visited - current_node->visited(); - - // 2.1) Use an analytic expansion (if available) to generate a path - expansion_result = nullptr; - expansion_result = _expander->tryAnalyticExpansion( - current_node, coarse_check_goals, fine_check_goals, - _goal_manager.getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); - if (expansion_result != nullptr) { - current_node = expansion_result; - } - - // 3) Check if we're at the goal, backtrace if required - if (_goal_manager.isGoal(current_node)) { - return current_node->backtracePath(path); - } else if (_best_heuristic_node.first < getToleranceHeuristic()) { - // Optimization: Let us find when in tolerance and refine within reason - approach_iterations++; - if (approach_iterations >= getOnApproachMaxIterations()) { - return _graph.at(_best_heuristic_node.second).backtracePath(path); - } - } - - // 4) Expand neighbors of Nbest not visited - neighbors.clear(); - current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors); - - for (neighbor_iterator = neighbors.begin(); - neighbor_iterator != neighbors.end(); ++neighbor_iterator) - { - neighbor = *neighbor_iterator; - - // 4.1) Compute the cost to go to this node - g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor); - - // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach - if (g_cost < neighbor->getAccumulatedCost()) { - neighbor->setAccumulatedCost(g_cost); - neighbor->parent = current_node; - - // 4.3) Add to queue with heuristic cost - addNode(g_cost + getHeuristicCost(neighbor), neighbor); - } - } - } - - // If we run out of search options, return the path that is closest, if within tolerance. - return getClosestPathWithinTolerance(path); -} - -template -typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() -{ - return _start; -} - -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() -{ - NodeBasic node = _queue.top().second; - _queue.pop(); - node.processSearchNode(); - return node.graph_node_ptr; -} - -template -void AStarAlgorithm::addNode(const float & cost, NodePtr & node) -{ - NodeBasic queued_node(node->getIndex()); - queued_node.populateSearchNode(node); - _queue.emplace(cost, queued_node); -} - -template -float AStarAlgorithm::getHeuristicCost(const NodePtr & node) -{ - const Coordinates node_coords = - NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = node->getHeuristicCost(node_coords, _goal_manager.getGoalsCoordinates()); - if (heuristic < _best_heuristic_node.first) { - _best_heuristic_node = {heuristic, node->getIndex()}; - } - - return heuristic; -} - -template -bool AStarAlgorithm::onVisitationCheckNode(const NodePtr & current_node) -{ - return current_node->wasVisited(); -} - -template -void AStarAlgorithm::clearQueue() -{ - NodeQueue q; - std::swap(_queue, q); -} - -template -void AStarAlgorithm::clearGraph() -{ - Graph g; - std::swap(_graph, g); - _graph.reserve(100000); -} - -template -uint64_t AStarAlgorithm::getIndex( - const unsigned int & x, const unsigned int & y, - const unsigned int & dim_3) -{ - return NodeT::getIndex(x, y, dim_3, _shared_ctx->motion_table.size_x, - _shared_ctx->motion_table.num_angle_quantization); -} - -template -int & AStarAlgorithm::getMaxIterations() -{ - return _max_iterations; -} - -template -int & AStarAlgorithm::getOnApproachMaxIterations() -{ - return _max_on_approach_iterations; -} - -template -float & AStarAlgorithm::getToleranceHeuristic() -{ - return _tolerance; -} - -template -unsigned int & AStarAlgorithm::getSizeX() -{ - return _x_size; -} - -template -unsigned int & AStarAlgorithm::getSizeY() -{ - return _y_size; -} - -template -unsigned int & AStarAlgorithm::getSizeDim3() -{ - return _dim3_size; -} - -template -unsigned int AStarAlgorithm::getCoarseSearchResolution() -{ - return _coarse_search_resolution; -} - -template -typename AStarAlgorithm::GoalManagerT AStarAlgorithm::getGoalManager() -{ - return _goal_manager; -} - -template -typename AStarAlgorithm::NodeContext * AStarAlgorithm::getContext() -{ - return _shared_ctx.get(); -} - -// Instantiate algorithm for the supported template types -template class AStarAlgorithm; - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp deleted file mode 100644 index c8c057a1..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp +++ /dev/null @@ -1,449 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#include -#include -#include - -#include "athena_smac_planner/analytic_expansion.hpp" - -namespace athena_smac_planner -{ - -template -AnalyticExpansion::AnalyticExpansion( - const MotionModel & motion_model, - const SearchInfo & search_info, - const bool & traverse_unknown, - const unsigned int & dim_3_size) -: _motion_model(motion_model), - _search_info(search_info), - _traverse_unknown(traverse_unknown), - _dim_3_size(dim_3_size), - _collision_checker(nullptr) -{ -} - -template -void AnalyticExpansion::setCollisionChecker( - GridCollisionChecker * collision_checker) -{ - _collision_checker = collision_checker; -} - -template -void AnalyticExpansion::setContext(NodeContext * ctx) -{ - _ctx = ctx; -} - -template -typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, - const NodeVector & coarse_check_goals, - const NodeVector & fine_check_goals, - const CoordinateVector & goals_coords, - const NodeGetter & getter, int & analytic_iterations, - int & closest_distance) -{ - // This must be a valid motion model for analytic expansion to be attempted - if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP || - _motion_model == MotionModel::STATE_LATTICE) - { - // See if we are closer and should be expanding more often - const Coordinates node_coords = - NodeT::getCoords( - current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); - - AnalyticExpansionNodes current_best_analytic_nodes; - NodePtr current_best_goal = nullptr; - NodePtr current_best_node = nullptr; - float current_best_score = std::numeric_limits::max(); - - closest_distance = std::min( - closest_distance, - static_cast(current_node->getHeuristicCost(node_coords, goals_coords))); - // We want to expand at a rate of d/expansion_ratio, - // but check to see if we are so close that we would be expanding every iteration - // If so, limit it to the expansion ratio (rounded up) - int desired_iterations = std::max( - static_cast(closest_distance / _search_info.analytic_expansion_ratio), - static_cast(std::ceil(_search_info.analytic_expansion_ratio))); - - // If we are closer now, we should update the target number of iterations to go - analytic_iterations = - std::min(analytic_iterations, desired_iterations); - - // Always run the expansion on the first run in case there is a - // trivial path to be found - if (analytic_iterations <= 0) { - // Reset the counter and try the analytic path expansion - analytic_iterations = desired_iterations; - bool found_valid_expansion = false; - - // First check the coarse search resolution goals - for (auto & current_goal_node : coarse_check_goals) { - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath( - current_node, current_goal_node, getter, - _ctx->motion_table.state_space); - if (!analytic_nodes.nodes.empty()) { - found_valid_expansion = true; - NodePtr node = current_node; - float score = refineAnalyticPath( - node, current_goal_node, getter, analytic_nodes); - // Update the best score if we found a better path - if (score < current_best_score) { - current_best_analytic_nodes = analytic_nodes; - current_best_goal = current_goal_node; - current_best_score = score; - current_best_node = node; - } - } - } - - // perform a final search if we found a goal - if (found_valid_expansion) { - for (auto & current_goal_node : fine_check_goals) { - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath( - current_node, current_goal_node, getter, - _ctx->motion_table.state_space); - if (!analytic_nodes.nodes.empty()) { - NodePtr node = current_node; - float score = refineAnalyticPath( - node, current_goal_node, getter, analytic_nodes); - // Update the best score if we found a better path - if (score < current_best_score) { - current_best_analytic_nodes = analytic_nodes; - current_best_goal = current_goal_node; - current_best_score = score; - current_best_node = node; - } - } - } - } - } - - if (!current_best_analytic_nodes.nodes.empty()) { - return setAnalyticPath( - current_best_node, current_best_goal, - current_best_analytic_nodes); - } - analytic_iterations--; - } - - // No valid motion model - return nullptr - return NodePtr(nullptr); -} - -template -int AnalyticExpansion::countDirectionChanges( - const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path) -{ - const double * lengths = path.length_; - int changes = 0; - int last_dir = 0; - for (int i = 0; i < 5; ++i) { - if (lengths[i] == 0.0) { - continue; - } - - int currentDirection = (lengths[i] > 0.0) ? 1 : -1; - if (last_dir != 0 && currentDirection != last_dir) { - ++changes; - } - last_dir = currentDirection; - } - - return changes; -} - -template -typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( - const NodePtr & node, - const NodePtr & goal, - const NodeGetter & node_getter, - const ompl::base::StateSpacePtr & state_space) -{ - ompl::base::ScopedState<> from(state_space), to(state_space), s(state_space); - from[0] = node->pose.x; - from[1] = node->pose.y; - from[2] = _ctx->motion_table.getAngleFromBin(node->pose.theta); - to[0] = goal->pose.x; - to[1] = goal->pose.y; - to[2] = _ctx->motion_table.getAngleFromBin(goal->pose.theta); - - float d = state_space->distance(from(), to()); - - auto rs_state_space = dynamic_cast(state_space.get()); - int direction_changes = 0; - if (rs_state_space) { - direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get())); - } - - // A move of sqrt(2) is guaranteed to be in a new cell - static const float sqrt_2 = sqrtf(2.0f); - - // If the length is too far, exit. This prevents unsafe shortcutting of paths - // into higher cost areas far out from the goal itself, let search to the work of getting - // close before the analytic expansion brings it home. This should never be smaller than - // 4-5x the minimum turning radius being used, or planning times will begin to spike. - if (d > _search_info.analytic_expansion_max_length || d < sqrt_2) { - return AnalyticExpansionNodes(); - } - - unsigned int num_intervals = static_cast(std::floor(d / sqrt_2)); - - AnalyticExpansionNodes possible_nodes; - // When "from" and "to" are zero or one cell away, - // num_intervals == 0 - possible_nodes.nodes.reserve(num_intervals); // We won't store this node or the goal - std::vector reals; - double theta; - - // Pre-allocate - NodePtr prev(node); - uint64_t index = 0; - NodePtr next(nullptr); - float angle = 0.0; - Coordinates proposed_coordinates; - bool failure = false; - std::vector node_costs; - node_costs.reserve(num_intervals); - - // Check intermediary poses (non-goal, non-start) - for (float i = 1; i <= num_intervals; i++) { - state_space->interpolate(from(), to(), i / num_intervals, s()); - reals = s.reals(); - // Make sure in range [0, 2PI) - theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; - theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; - angle = _ctx->motion_table.getAngle(theta); - - // Turn the pose into a node, and check if it is valid - index = NodeT::getIndex( - static_cast(reals[0]), - static_cast(reals[1]), - static_cast(angle), - _ctx->motion_table.size_x, - _ctx->motion_table.num_angle_quantization); - // Get the node from the graph - if (node_getter(index, next)) { - Coordinates initial_node_coords = next->pose; - proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; - next->setPose(proposed_coordinates); - if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { - // Save the node, and its previous coordinates in case we need to abort - possible_nodes.add(next, initial_node_coords, proposed_coordinates); - node_costs.emplace_back(next->getCost()); - prev = next; - } else { - // Abort - next->setPose(initial_node_coords); - failure = true; - break; - } - } else { - // Abort - failure = true; - break; - } - } - - if (!failure) { - // We found 'a' valid expansion. Now to tell if its a quality option... - const float max_cost = _search_info.analytic_expansion_max_cost; - auto max_cost_it = std::max_element(node_costs.begin(), node_costs.end()); - if (max_cost_it != node_costs.end() && *max_cost_it > max_cost) { - // If any element is above the comfortable cost limit, check edge cases: - // (1) Check if goal is in greater than max_cost space requiring - // entering it, but only entering it on final approach, not in-and-out - // (2) Checks if goal is in normal space, but enters costed space unnecessarily - // mid-way through, skirting obstacle or in non-globally confined space - bool cost_exit_high_cost_region = false; - for (auto iter = node_costs.rbegin(); iter != node_costs.rend(); ++iter) { - const float & curr_cost = *iter; - if (curr_cost <= max_cost) { - cost_exit_high_cost_region = true; - } else if (curr_cost > max_cost && cost_exit_high_cost_region) { - failure = true; - break; - } - } - - // (3) Handle exception: there may be no other option close to goal - // if max cost is set too low (optional) - if (failure) { - if (d < 2.0f * M_PI * _ctx->motion_table.min_turning_radius && - _search_info.analytic_expansion_max_cost_override) - { - failure = false; - } - } - } - } - - // Reset to initial poses to not impact future searches - for (const auto & node_pose : possible_nodes.nodes) { - const auto & n = node_pose.node; - n->setPose(node_pose.initial_coords); - } - - if (failure) { - return AnalyticExpansionNodes(); - } - - possible_nodes.setDirectionChanges(direction_changes); - return possible_nodes; -} - -template -float AnalyticExpansion::refineAnalyticPath( - NodePtr & node, - const NodePtr & goal_node, - const NodeGetter & getter, - AnalyticExpansionNodes & analytic_nodes) -{ - NodePtr test_node = node; - AnalyticExpansionNodes refined_analytic_nodes; - for (int i = 0; i < 8; i++) { - // Attempt to create better paths in 5 node increments, need to make sure - // they exist for each in order to do so (maximum of 40 points back). - if (test_node->parent && test_node->parent->parent && - test_node->parent->parent->parent && - test_node->parent->parent->parent->parent && - test_node->parent->parent->parent->parent->parent) - { - test_node = test_node->parent->parent->parent->parent->parent; - // print the goals pose - refined_analytic_nodes = - getAnalyticPath( - test_node, goal_node, getter, - _ctx->motion_table.state_space); - if (refined_analytic_nodes.nodes.empty()) { - break; - } - if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) { - // If the direction changes are worse, we don't want to use this path - continue; - } - analytic_nodes = refined_analytic_nodes; - node = test_node; - } else { - break; - } - } - - // The analytic expansion can short-cut near obstacles when closer to a goal - // So, we can attempt to refine it more by increasing the possible radius - // higher than the minimum turning radius and use the best solution based on - // a scoring function similar to that used in traversal cost estimation. - auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.nodes.size() < 2) { - return std::numeric_limits::max(); - } - - float score = 0.0; - float normalized_cost = 0.0; - // Analytic expansions are consistently spaced - const float distance = hypotf( - expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x, - expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y); - const float & weight = _ctx->motion_table.cost_penalty; - for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) { - normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function - score += distance * (1.0 + weight * normalized_cost); - } - return score; - }; - - float original_score = scoringFn(analytic_nodes); - float best_score = original_score; - float score = std::numeric_limits::max(); - float min_turn_rad = _ctx->motion_table.min_turning_radius; - const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius - while (min_turn_rad < max_min_turn_rad) { - min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps - ompl::base::StateSpacePtr state_space; - if (_ctx->motion_table.motion_model == MotionModel::DUBIN) { - state_space = std::make_shared(min_turn_rad); - } else { - state_space = std::make_shared(min_turn_rad); - } - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); - score = scoringFn(refined_analytic_nodes); - - // Normal scoring: prioritize lower cost as long as not more directional changes - if (score <= best_score && - refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes) - { - analytic_nodes = refined_analytic_nodes; - best_score = score; - continue; - } - - // Special case: If we have a better score than original (only) and less directional changes - // the path quality is still better than the original and is less operationally complex - if (score <= original_score && - refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes) - { - analytic_nodes = refined_analytic_nodes; - best_score = score; - } - } - - return best_score; -} - -template -typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( - const NodePtr & node, - const NodePtr & goal_node, - const AnalyticExpansionNodes & expanded_nodes) -{ - _detached_nodes.clear(); - // Legitimate final path - set the parent relationships, states, and poses - NodePtr prev = node; - for (const auto & node_pose : expanded_nodes.nodes) { - auto n = node_pose.node; - cleanNode(n); - if (n->getIndex() != goal_node->getIndex()) { - if (n->wasVisited()) { - _detached_nodes.push_back(std::make_unique(-1, _ctx)); - n = _detached_nodes.back().get(); - } - n->parent = prev; - n->pose = node_pose.proposed_coords; - n->visited(); - prev = n; - } - } - if (goal_node != prev) { - goal_node->parent = prev; - cleanNode(goal_node); - goal_node->visited(); - } - return goal_node; -} - -template -void AnalyticExpansion::cleanNode(const NodePtr & /*expanded_nodes*/) -{ -} - -template class AnalyticExpansion; - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp deleted file mode 100644 index 24c8baf7..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp +++ /dev/null @@ -1,196 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#include "athena_smac_planner/collision_checker.hpp" - -namespace athena_smac_planner -{ - -GridCollisionChecker::GridCollisionChecker( - std::shared_ptr costmap_ros, - unsigned int num_quantizations, - rclcpp::Node::SharedPtr node) -: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr) -{ - if (node) { - clock_ = node->get_clock(); - logger_ = node->get_logger(); - } - - if (costmap_ros) { - costmap_ros_ = costmap_ros; - } - - // Convert number of regular bins into angles - float bin_size = 2 * M_PI / static_cast(num_quantizations); - angles_.reserve(num_quantizations); - for (unsigned int i = 0; i != num_quantizations; i++) { - angles_.push_back(bin_size * i); - } -} - -// GridCollisionChecker::GridCollisionChecker( -// nav2_costmap_2d::Costmap2D * costmap, -// std::vector & angles) -// : FootprintCollisionChecker(costmap), -// angles_(angles) -// { -// } - -void GridCollisionChecker::setFootprint( - const nav2_costmap_2d::Footprint & footprint, - const bool & radius, - const double & possible_collision_cost) -{ - possible_collision_cost_ = static_cast(possible_collision_cost); - if (possible_collision_cost_ <= 0.0f) { - RCLCPP_ERROR_THROTTLE( - logger_, *clock_, 1000, - "Inflation layer either not found or inflation is not set sufficiently for " - "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" - " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " - "github.com/ros-planning/navigation2/tree/main/athena_smac_planner#potential-fields" - " for full instructions. This will substantially impact run-time performance."); - } - - footprint_is_radius_ = radius; - - // Use radius, no caching required - if (radius) { - return; - } - - // No change, no updates required - if (footprint == unoriented_footprint_) { - return; - } - - oriented_footprints_.clear(); - oriented_footprints_.reserve(angles_.size()); - double sin_th, cos_th; - geometry_msgs::msg::Point new_pt; - const unsigned int footprint_size = footprint.size(); - - // Precompute the orientation bins for checking to use - for (unsigned int i = 0; i != angles_.size(); i++) { - sin_th = sin(angles_[i]); - cos_th = cos(angles_[i]); - nav2_costmap_2d::Footprint oriented_footprint; - oriented_footprint.reserve(footprint_size); - - for (unsigned int j = 0; j < footprint_size; j++) { - new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; - new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; - oriented_footprint.push_back(new_pt); - } - - oriented_footprints_.push_back(oriented_footprint); - } - - unoriented_footprint_ = footprint; -} - -bool GridCollisionChecker::inCollision( - const float & x, - const float & y, - const float & angle_bin, - const bool & traverse_unknown) -{ - // Check to make sure cell is inside the map - if (outsideRange(costmap_->getSizeInCellsX(), x) || - outsideRange(costmap_->getSizeInCellsY(), y)) - { - return true; - } - - // Assumes setFootprint already set - center_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5f), static_cast(y + 0.5f))); - - if (!footprint_is_radius_) { - // if footprint, then we check for the footprint's points, but first see - // if the robot is even potentially in an inscribed collision - if (center_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { - return false; - } - - // If its inscribed, in collision, or unknown in the middle, - // no need to even check the footprint, its invalid - if (center_cost_ == UNKNOWN_COST && !traverse_unknown) { - return true; - } - - if (center_cost_ == INSCRIBED_COST || center_cost_ == OCCUPIED_COST) { - return true; - } - - // if possible inscribed, need to check actual footprint pose. - // Use precomputed oriented footprints are done on initialization, - // offset by translation value to collision check - double wx, wy; - costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); - geometry_msgs::msg::Point new_pt; - const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; - nav2_costmap_2d::Footprint current_footprint; - current_footprint.reserve(oriented_footprint.size()); - for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { - new_pt.x = wx + oriented_footprint[i].x; - new_pt.y = wy + oriented_footprint[i].y; - current_footprint.push_back(new_pt); - } - - float footprint_cost = static_cast(footprintCost(current_footprint)); - - if (footprint_cost == UNKNOWN_COST && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost >= OCCUPIED_COST; - } else { - // if radius, then we can check the center of the cost assuming inflation is used - if (center_cost_ == UNKNOWN_COST && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return center_cost_ >= INSCRIBED_COST; - } -} - -bool GridCollisionChecker::inCollision( - const unsigned int & i, - const bool & traverse_unknown) -{ - center_cost_ = costmap_->getCost(i); - if (center_cost_ == UNKNOWN_COST && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return center_cost_ >= INSCRIBED_COST; -} - -float GridCollisionChecker::getCost() -{ - // Assumes inCollision called prior - return static_cast(center_cost_); -} - -bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value) -{ - return value < 0.0f || value > max; -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp deleted file mode 100644 index 4cfac234..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright (c) 2020, Carlos Luis -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#include "athena_smac_planner/costmap_downsampler.hpp" - -#include -#include -#include - -namespace athena_smac_planner -{ - -CostmapDownsampler::CostmapDownsampler() -: _costmap(nullptr), - _downsampled_costmap(nullptr) -{ -} - -CostmapDownsampler::~CostmapDownsampler() -{ -} - -void CostmapDownsampler::on_configure( - nav2_costmap_2d::Costmap2D * const costmap, - const unsigned int & downsampling_factor, - const bool & use_min_cost_neighbor) -{ - _costmap = costmap; - _downsampling_factor = downsampling_factor; - _use_min_cost_neighbor = use_min_cost_neighbor; - updateCostmapSize(); - - _downsampled_costmap = std::make_unique( - _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, - _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN_COST); -} - -void CostmapDownsampler::on_cleanup() -{ - _costmap = nullptr; - _downsampled_costmap.reset(); -} - -nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( - const unsigned int & downsampling_factor) -{ - _downsampling_factor = downsampling_factor; - updateCostmapSize(); - - // Adjust costmap size if needed - if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || - _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || - _downsampled_costmap->getResolution() != _downsampled_resolution) - { - resizeCostmap(); - } - - // Assign costs - for (unsigned int i = 0; i < _downsampled_size_x; ++i) { - for (unsigned int j = 0; j < _downsampled_size_y; ++j) { - setCostOfCell(i, j); - } - } - - return _downsampled_costmap.get(); -} - -void CostmapDownsampler::updateCostmapSize() -{ - _size_x = _costmap->getSizeInCellsX(); - _size_y = _costmap->getSizeInCellsY(); - _downsampled_size_x = ceil(static_cast(_size_x) / _downsampling_factor); - _downsampled_size_y = ceil(static_cast(_size_y) / _downsampling_factor); - _downsampled_resolution = _downsampling_factor * _costmap->getResolution(); -} - -void CostmapDownsampler::resizeCostmap() -{ - _downsampled_costmap->resizeMap( - _downsampled_size_x, - _downsampled_size_y, - _downsampled_resolution, - _costmap->getOriginX(), - _costmap->getOriginY()); -} - -void CostmapDownsampler::setCostOfCell( - const unsigned int & new_mx, - const unsigned int & new_my) -{ - unsigned int mx, my; - unsigned char cost = _use_min_cost_neighbor ? 255 : 0; - unsigned int x_offset = new_mx * _downsampling_factor; - unsigned int y_offset = new_my * _downsampling_factor; - - for (unsigned int i = 0; i < _downsampling_factor; ++i) { - mx = x_offset + i; - if (mx >= _size_x) { - continue; - } - for (unsigned int j = 0; j < _downsampling_factor; ++j) { - my = y_offset + j; - if (my >= _size_y) { - continue; - } - cost = _use_min_cost_neighbor ? - std::min(cost, _costmap->getCost(mx, my)) : - std::max(cost, _costmap->getCost(mx, my)); - } - } - - _downsampled_costmap->setCost(new_mx, new_my, cost); -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp deleted file mode 100644 index 1b16307e..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright (c) 2026, 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. Reserved. - -#include "ompl/base/ScopedState.h" -#include "ompl/base/spaces/DubinsStateSpace.h" -#include "ompl/base/spaces/ReedsSheppStateSpace.h" -#include "athena_smac_planner/distance_heuristic.hpp" -#include "athena_smac_planner/node_hybrid.hpp" - -namespace athena_smac_planner -{ - -template<> -template -void DistanceHeuristic::precomputeDistanceHeuristic( - const float & lookup_table_dim, - const MotionModel & motion_model, - const unsigned int & dim_3_size, - const SearchInfo & search_info, - MotionTableT & motion_table) -{ - // Dubin or Reeds-Shepp shortest distances - if (motion_model == MotionModel::DUBIN) { - motion_table.state_space = std::make_shared( - search_info.minimum_turning_radius); - } else if (motion_model == MotionModel::REEDS_SHEPP) { - motion_table.state_space = std::make_shared( - search_info.minimum_turning_radius); - } else { - throw std::runtime_error( - "Node attempted to precompute distance heuristics " - "with invalid motion model!"); - } - - ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); - to[0] = 0.0; - to[1] = 0.0; - to[2] = 0.0; - size_lookup_ = lookup_table_dim; - float motion_heuristic = 0.0; - unsigned int index = 0; - int dim_3_size_int = static_cast(dim_3_size); - float angular_bin_size = 2 * M_PI / static_cast(dim_3_size); - - // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal - // to help drive the search towards admissible approaches. Deu to symmetries in the - // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror - // around the X axis any relative node lookup. This reduces memory overhead and increases - // the size of a window a platform can store in memory. - dist_heuristic_lookup_table_.resize(size_lookup_ * ceil(size_lookup_ / 2.0) * dim_3_size_int); - for (float x = ceil(-size_lookup_ / 2.0); x <= floor(size_lookup_ / 2.0); x += 1.0) { - for (float y = 0.0; y <= floor(size_lookup_ / 2.0); y += 1.0) { - for (int heading = 0; heading != dim_3_size_int; heading++) { - from[0] = x; - from[1] = y; - from[2] = heading * angular_bin_size; - motion_heuristic = motion_table.state_space->distance(from(), to()); - dist_heuristic_lookup_table_[index] = motion_heuristic; - index++; - } - } - } -} - -template -template -float DistanceHeuristic::getDistanceHeuristic( - const Coordinates & node_coords, - const Coordinates & goal_coords, - const float & obstacle_heuristic, - MotionTableT & motion_table) -{ - // rotate and translate node_coords such that goal_coords relative is (0,0,0) - // Due to the rounding involved in exact cell increments for caching, - // this is not an exact replica of a live heuristic, but has bounded error. - // (Usually less than 1 cell length) - - // This angle is negative since we are de-rotating the current node - // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th) - const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta]; - const float cos_th = trig_vals.first; - const float sin_th = -trig_vals.second; - const float dx = node_coords.x - goal_coords.x; - const float dy = node_coords.y - goal_coords.y; - - double dtheta_bin = node_coords.theta - goal_coords.theta; - if (dtheta_bin < 0) { - dtheta_bin += motion_table.num_angle_quantization; - } - if (dtheta_bin > motion_table.num_angle_quantization) { - dtheta_bin -= motion_table.num_angle_quantization; - } - - Coordinates node_coords_relative( - round(dx * cos_th - dy * sin_th), - round(dx * sin_th + dy * cos_th), - round(dtheta_bin)); - - // Check if the relative node coordinate is within the localized window around the goal - // to apply the distance heuristic. Since the lookup table is contains only the positive - // X axis, we mirror the Y and theta values across the X axis to find the heuristic values. - float motion_heuristic = 0.0; - const int floored_size = floor(size_lookup_ / 2.0); - const int ceiling_size = ceil(size_lookup_ / 2.0); - const float mirrored_relative_y = abs(node_coords_relative.y); - if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) { - // Need to mirror angle if Y coordinate was mirrored - int theta_pos; - if (node_coords_relative.y < 0.0) { - theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta; - } else { - theta_pos = node_coords_relative.theta; - } - const int x_pos = node_coords_relative.x + floored_size; - const int y_pos = static_cast(mirrored_relative_y); - const int index = - x_pos * ceiling_size * motion_table.num_angle_quantization + - y_pos * motion_table.num_angle_quantization + - theta_pos; - motion_heuristic = dist_heuristic_lookup_table_[index]; - } else if (obstacle_heuristic <= 0.0) { - ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); - to[0] = goal_coords.x; - to[1] = goal_coords.y; - from[0] = node_coords.x; - from[1] = node_coords.y; - if constexpr (std::is_same_v) { - to[2] = goal_coords.theta * motion_table.num_angle_quantization; - from[2] = node_coords.theta * motion_table.num_angle_quantization; - } else { - to[2] = motion_table.getAngleFromBin(goal_coords.theta); - from[2] = motion_table.getAngleFromBin(node_coords.theta); - } - motion_heuristic = motion_table.state_space->distance(from(), to()); - } - - return motion_heuristic; -} - -// Instantiate algorithm for the supported template types -template void DistanceHeuristic::precomputeDistanceHeuristic( - const float &, const MotionModel &, const unsigned int &, const SearchInfo &, - HybridMotionTable &); -template float DistanceHeuristic::getDistanceHeuristic( - const Coordinates &, const Coordinates &, const float &, HybridMotionTable &); -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp deleted file mode 100644 index 259ec178..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp +++ /dev/null @@ -1,162 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 - -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "athena_smac_planner/smac_planner_hybrid.hpp" -#include "msgs/msg/planner_event.hpp" - -class GlobalPlannerNode : public rclcpp::Node -{ -public: - explicit GlobalPlannerNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("global_planner", options) - { - } - - void configure() - { - costmap_ros_ = std::make_shared( - "global_costmap", - get_namespace(), - "global_costmap"); - costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", get_parameter("use_sim_time").as_bool())); - // Empty plugin list: rcl_yaml_param_parser cannot represent an empty YAML - // sequence (plugins: []) — it produces a null rcl_variant_s that crashes - // NodeParameters. Set it programmatically instead. - costmap_ros_->set_parameter(rclcpp::Parameter("plugins", std::vector{})); - costmap_ros_->configure(); - costmap_ros_->activate(); - - planner_ = std::make_shared( - shared_from_this(), costmap_ros_); - - path_pub_ = create_publisher( - "/global_path", rclcpp::QoS(1).transient_local()); - event_pub_ = create_publisher("/planner_event", 10); - - robot_pose_sub_ = create_subscription( - "/robot_pose", 10, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - std::optional pending; - { - std::lock_guard lk(mutex_); - robot_pose_ = *msg; - // If a goal arrived before robot_pose was ready, plan now - if (pending_goal_.has_value()) { - pending = *pending_goal_; - pending_goal_.reset(); - } - } - if (pending.has_value()) { - RCLCPP_INFO(get_logger(), "[global_planner] robot_pose received — planning pending goal"); - plan(*pending); - } - }); - - goal_pose_sub_ = create_subscription( - "/goal_pose", 10, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { onGoal(msg); }); - - RCLCPP_INFO(get_logger(), "GlobalPlannerNode configured."); - } - - std::shared_ptr getCostmapROS() - { - return costmap_ros_; - } - -private: - void publishEvent(uint8_t event_type) - { - msgs::msg::PlannerEvent ev; - ev.event = event_type; - event_pub_->publish(ev); - } - - void onGoal(const geometry_msgs::msg::PoseStamped::SharedPtr goal) - { - publishEvent(msgs::msg::PlannerEvent::NEW_GOAL); - - { - std::lock_guard lk(mutex_); - if (!robot_pose_.has_value()) { - RCLCPP_WARN(get_logger(), - "[global_planner] goal received but no robot_pose yet — waiting"); - pending_goal_ = *goal; // cache; plan() called once robot_pose arrives - return; - } - // Overwrite any stale pending goal — the new goal supersedes it - pending_goal_.reset(); - } - - plan(*goal); - } - - void plan(const geometry_msgs::msg::PoseStamped & goal) - { - geometry_msgs::msg::PoseStamped start; - { - std::lock_guard lk(mutex_); - start = robot_pose_.value(); - } - - publishEvent(msgs::msg::PlannerEvent::PLANNING); - - try { - auto path = planner_->createPlan(start, goal, [] { return false; }); - path_pub_->publish(path); - publishEvent(msgs::msg::PlannerEvent::PLAN_SUCCEEDED); - } catch (const std::exception & ex) { - RCLCPP_ERROR(get_logger(), "Planning failed: %s", ex.what()); - publishEvent(msgs::msg::PlannerEvent::PLAN_FAILED); - } - } - - std::shared_ptr costmap_ros_; - std::shared_ptr planner_; - - rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Publisher::SharedPtr event_pub_; - rclcpp::Subscription::SharedPtr robot_pose_sub_; - rclcpp::Subscription::SharedPtr goal_pose_sub_; - - std::mutex mutex_; - std::optional robot_pose_; - std::optional pending_goal_; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - node->configure(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.add_node(node->getCostmapROS()->get_node_base_interface()); - executor.spin(); - - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp deleted file mode 100644 index ee1dc183..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#include "athena_smac_planner/node_basic.hpp" - -namespace athena_smac_planner -{ - -template -void NodeBasic::processSearchNode() -{ -} - -template<> -void NodeBasic::processSearchNode() -{ - // We only want to override the node's pose if it has not yet been visited - // to prevent the case that a node has been queued multiple times and - // a new branch is overriding one of lower cost already visited. - if (!this->graph_node_ptr->wasVisited()) { - this->graph_node_ptr->pose = this->pose; - this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index, this->turn_dir); - } -} - -template -void NodeBasic::populateSearchNode(NodeT * & node) -{ - this->graph_node_ptr = node; -} - -template<> -void NodeBasic::populateSearchNode(NodeHybrid * & node) -{ - this->pose = node->pose; - this->graph_node_ptr = node; - this->motion_index = node->getMotionPrimitiveIndex(); - this->turn_dir = node->getTurnDirection(); -} - -template class NodeBasic; - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp deleted file mode 100644 index 0c71a622..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp +++ /dev/null @@ -1,540 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// Copyright (c) 2023, 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. Reserved. - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ompl/base/ScopedState.h" -#include "ompl/base/spaces/DubinsStateSpace.h" -#include "ompl/base/spaces/ReedsSheppStateSpace.h" - -#include "athena_smac_planner/node_hybrid.hpp" - -using namespace std::chrono; // NOLINT - -namespace athena_smac_planner -{ - -// Each of these tables are the projected motion models through -// time and space applied to the search on the current node in -// continuous map-coordinates (e.g. not meters but partial map cells) -// Currently, these are set to project *at minimum* into a neighboring -// cell. Though this could be later modified to project a certain -// amount of time or particular distance forward. - -// http://planning.cs.uiuc.edu/planning/node821.html -// Model for ackermann style vehicle with minimum radius restriction -void HybridMotionTable::initDubin( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, - SearchInfo & search_info) -{ - size_x = size_x_in; - change_penalty = search_info.change_penalty; - non_straight_penalty = search_info.non_straight_penalty; - cost_penalty = search_info.cost_penalty; - reverse_penalty = search_info.reverse_penalty; - travel_distance_reward = 1.0f - search_info.retrospective_penalty; - downsample_obstacle_heuristic = search_info.downsample_obstacle_heuristic; - use_quadratic_cost_penalty = search_info.use_quadratic_cost_penalty; - - // if nothing changed, no need to re-compute primitives - if (num_angle_quantization_in == num_angle_quantization && - min_turning_radius == search_info.minimum_turning_radius && - allow_primitive_interpolation == search_info.allow_primitive_interpolation && - motion_model == MotionModel::DUBIN) - { - return; - } - - num_angle_quantization = num_angle_quantization_in; - num_angle_quantization_float = static_cast(num_angle_quantization); - min_turning_radius = search_info.minimum_turning_radius; - allow_primitive_interpolation = search_info.allow_primitive_interpolation; - motion_model = MotionModel::DUBIN; - - // angle must meet 3 requirements: - // 1) be increment of quantized bin size - // 2) chord length must be greater than sqrt(2) to leave current cell - // 3) maximum curvature must be respected, represented by minimum turning angle - // Thusly: - // On circle of radius minimum turning angle, we need select motion primitives - // with chord length > sqrt(2) and be an increment of our bin size - // - // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size - // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) - float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); - // Now make sure angle is an increment of the quantized bin size - // And since its based on the minimum chord, we need to make sure its always larger - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); - float increments; - if (angle < bin_size) { - increments = 1.0f; - } else { - // Search dimensions are clean multiples of quantization - this prevents - // paths with loops in them - increments = ceil(angle / bin_size); - } - angle = increments * bin_size; - - // find deflections - // If we make a right triangle out of the chord in circle of radius - // min turning angle, we can see that delta X = R * sin (angle) - const float delta_x = min_turning_radius * sin(angle); - // Using that same right triangle, we can see that the complement - // to delta Y is R * cos (angle). If we subtract R, we get the actual value - const float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); - const float delta_dist = hypotf(delta_x, delta_y); - - projections.clear(); - projections.reserve(3); - projections.emplace_back(delta_dist, 0.0, 0.0, TurnDirection::FORWARD); // Forward - projections.emplace_back(delta_x, delta_y, increments, TurnDirection::LEFT); // Left - projections.emplace_back(delta_x, -delta_y, -increments, TurnDirection::RIGHT); // Right - - if (search_info.allow_primitive_interpolation && increments > 1.0f) { - // Create primitives that are +/- N to fill in search space to use all set angular quantizations - // Allows us to create N many primitives so that each search iteration can expand into any angle - // bin possible with the minimum turning radius constraint, not just the most extreme turns. - projections.reserve(3 + (2 * (increments - 1))); - for (unsigned int i = 1; i < static_cast(increments); i++) { - const float angle_n = static_cast(i) * bin_size; - const float turning_rad_n = delta_dist / (2.0f * sin(angle_n / 2.0f)); - const float delta_x_n = turning_rad_n * sin(angle_n); - const float delta_y_n = turning_rad_n - (turning_rad_n * cos(angle_n)); - projections.emplace_back( - delta_x_n, delta_y_n, static_cast(i), TurnDirection::LEFT); // Left - projections.emplace_back( - delta_x_n, -delta_y_n, -static_cast(i), TurnDirection::RIGHT); // Right - } - } - - // Create the correct OMPL state space - state_space = std::make_shared(min_turning_radius); - - // Precompute projection deltas - delta_xs.resize(projections.size()); - delta_ys.resize(projections.size()); - trig_values.resize(num_angle_quantization); - - for (unsigned int i = 0; i != projections.size(); i++) { - delta_xs[i].resize(num_angle_quantization); - delta_ys[i].resize(num_angle_quantization); - - for (unsigned int j = 0; j != num_angle_quantization; j++) { - double cos_theta = cos(bin_size * j); - double sin_theta = sin(bin_size * j); - if (i == 0) { - // if first iteration, cache the trig values for later - trig_values[j] = {cos_theta, sin_theta}; - } - delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; - delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; - } - } - - // Precompute travel costs for each motion primitive - travel_costs.resize(projections.size()); - for (unsigned int i = 0; i != projections.size(); i++) { - const TurnDirection turn_dir = projections[i]._turn_dir; - if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { - // Turning, so length is the arc length - const float arc_angle = projections[i]._theta * bin_size; - const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); - travel_costs[i] = turning_rad * arc_angle; - } else { - travel_costs[i] = delta_dist; - } - } -} - -// http://planning.cs.uiuc.edu/planning/node822.html -// Same as Dubin model but now reverse is valid -// See notes in Dubin for explanation -void HybridMotionTable::initReedsShepp( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, - SearchInfo & search_info) -{ - size_x = size_x_in; - change_penalty = search_info.change_penalty; - non_straight_penalty = search_info.non_straight_penalty; - cost_penalty = search_info.cost_penalty; - reverse_penalty = search_info.reverse_penalty; - travel_distance_reward = 1.0f - search_info.retrospective_penalty; - downsample_obstacle_heuristic = search_info.downsample_obstacle_heuristic; - use_quadratic_cost_penalty = search_info.use_quadratic_cost_penalty; - - // if nothing changed, no need to re-compute primitives - if (num_angle_quantization_in == num_angle_quantization && - min_turning_radius == search_info.minimum_turning_radius && - allow_primitive_interpolation == search_info.allow_primitive_interpolation && - motion_model == MotionModel::REEDS_SHEPP) - { - return; - } - - num_angle_quantization = num_angle_quantization_in; - num_angle_quantization_float = static_cast(num_angle_quantization); - min_turning_radius = search_info.minimum_turning_radius; - allow_primitive_interpolation = search_info.allow_primitive_interpolation; - motion_model = MotionModel::REEDS_SHEPP; - - float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); - float increments; - if (angle < bin_size) { - increments = 1.0f; - } else { - increments = ceil(angle / bin_size); - } - angle = increments * bin_size; - - const float delta_x = min_turning_radius * sin(angle); - const float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); - const float delta_dist = hypotf(delta_x, delta_y); - - projections.clear(); - projections.reserve(6); - projections.emplace_back(delta_dist, 0.0, 0.0, TurnDirection::FORWARD); // Forward - projections.emplace_back( - delta_x, delta_y, increments, TurnDirection::LEFT); // Forward + Left - projections.emplace_back( - delta_x, -delta_y, -increments, TurnDirection::RIGHT); // Forward + Right - projections.emplace_back(-delta_dist, 0.0, 0.0, TurnDirection::REVERSE); // Backward - projections.emplace_back( - -delta_x, delta_y, -increments, TurnDirection::REV_LEFT); // Backward + Left - projections.emplace_back( - -delta_x, -delta_y, increments, TurnDirection::REV_RIGHT); // Backward + Right - - if (search_info.allow_primitive_interpolation && increments > 1.0f) { - // Create primitives that are +/- N to fill in search space to use all set angular quantizations - // Allows us to create N many primitives so that each search iteration can expand into any angle - // bin possible with the minimum turning radius constraint, not just the most extreme turns. - projections.reserve(6 + (4 * (increments - 1))); - for (unsigned int i = 1; i < static_cast(increments); i++) { - const float angle_n = static_cast(i) * bin_size; - const float turning_rad_n = delta_dist / (2.0f * sin(angle_n / 2.0f)); - const float delta_x_n = turning_rad_n * sin(angle_n); - const float delta_y_n = turning_rad_n - (turning_rad_n * cos(angle_n)); - projections.emplace_back( - delta_x_n, delta_y_n, static_cast(i), TurnDirection::LEFT); // Forward + Left - projections.emplace_back( - delta_x_n, -delta_y_n, -static_cast(i), TurnDirection::RIGHT); // Forward + Right - projections.emplace_back( - -delta_x_n, delta_y_n, -static_cast(i), - TurnDirection::REV_LEFT); // Backward + Left - projections.emplace_back( - -delta_x_n, -delta_y_n, static_cast(i), - TurnDirection::REV_RIGHT); // Backward + Right - } - } - - // Create the correct OMPL state space - state_space = std::make_shared(min_turning_radius); - - // Precompute projection deltas - delta_xs.resize(projections.size()); - delta_ys.resize(projections.size()); - trig_values.resize(num_angle_quantization); - - for (unsigned int i = 0; i != projections.size(); i++) { - delta_xs[i].resize(num_angle_quantization); - delta_ys[i].resize(num_angle_quantization); - - for (unsigned int j = 0; j != num_angle_quantization; j++) { - double cos_theta = cos(bin_size * j); - double sin_theta = sin(bin_size * j); - if (i == 0) { - // if first iteration, cache the trig values for later - trig_values[j] = {cos_theta, sin_theta}; - } - delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; - delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; - } - } - - // Precompute travel costs for each motion primitive - travel_costs.resize(projections.size()); - for (unsigned int i = 0; i != projections.size(); i++) { - const TurnDirection turn_dir = projections[i]._turn_dir; - if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { - // Turning, so length is the arc length - const float arc_angle = projections[i]._theta * bin_size; - const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); - travel_costs[i] = turning_rad * arc_angle; - } else { - travel_costs[i] = delta_dist; - } - } -} - -MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) -{ - MotionPoses projection_list; - projection_list.reserve(projections.size()); - - for (unsigned int i = 0; i != projections.size(); i++) { - const MotionPose & proj_motion_model = projections[i]; - - // normalize theta, I know its overkill, but I've been burned before... - const float & node_heading = node->pose.theta; - float new_heading = node_heading + proj_motion_model._theta; - - if (new_heading < 0.0) { - new_heading += num_angle_quantization_float; - } - - if (new_heading >= num_angle_quantization_float) { - new_heading -= num_angle_quantization_float; - } - - projection_list.emplace_back( - delta_xs[i][node_heading] + node->pose.x, - delta_ys[i][node_heading] + node->pose.y, - new_heading, proj_motion_model._turn_dir); - } - - return projection_list; -} - -unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) -{ - auto bin = static_cast(round(static_cast(theta) / bin_size)); - return bin < num_angle_quantization ? bin : 0u; -} - -float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) -{ - return bin_idx * bin_size; -} - -double HybridMotionTable::getAngle(const double & theta) -{ - return theta / bin_size; -} - -NodeHybrid::NodeHybrid(const uint64_t index, NodeContext * ctx) -: parent(nullptr), - pose(0.0f, 0.0f, 0.0f), - _cell_cost(std::numeric_limits::quiet_NaN()), - _accumulated_cost(std::numeric_limits::max()), - _index(index), - _was_visited(false), - _motion_primitive_index(std::numeric_limits::max()), - _is_node_valid(false), - _ctx(ctx) -{ -} - -NodeHybrid::~NodeHybrid() -{ - parent = nullptr; -} - -void NodeHybrid::reset() -{ - parent = nullptr; - _cell_cost = std::numeric_limits::quiet_NaN(); - _accumulated_cost = std::numeric_limits::max(); - _was_visited = false; - _motion_primitive_index = std::numeric_limits::max(); - pose.x = 0.0f; - pose.y = 0.0f; - pose.theta = 0.0f; - _is_node_valid = false; -} - -bool NodeHybrid::isNodeValid( - const bool & traverse_unknown, - GridCollisionChecker * collision_checker) -{ - // Already found, we can return the result - if (!std::isnan(_cell_cost)) { - return _is_node_valid; - } - - _is_node_valid = !collision_checker->inCollision( - this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown); - _cell_cost = collision_checker->getCost(); - return _is_node_valid; -} - -float NodeHybrid::getTraversalCost(const NodePtr & child) -{ - const float normalized_cost = child->getCost() / 252.0f; - if (std::isnan(normalized_cost)) { - throw std::runtime_error( - "Node attempted to get traversal " - "cost without a known SE2 collision cost!"); - } - - const TurnDirection & child_turn_dir = child->getTurnDirection(); - float travel_cost_raw = _ctx->motion_table.travel_costs[child->getMotionPrimitiveIndex()]; - float travel_cost = 0.0; - - if (_ctx->motion_table.use_quadratic_cost_penalty) { - travel_cost_raw *= - (_ctx->motion_table.travel_distance_reward + - (_ctx->motion_table.cost_penalty * normalized_cost * normalized_cost)); - } else { - travel_cost_raw *= - (_ctx->motion_table.travel_distance_reward + _ctx->motion_table.cost_penalty * - normalized_cost); - } - - if (child_turn_dir == TurnDirection::FORWARD || child_turn_dir == TurnDirection::REVERSE || - getMotionPrimitiveIndex() == std::numeric_limits::max()) - { - // New motion is a straight motion, no additional costs to be applied - travel_cost = travel_cost_raw; - } else { - if (getTurnDirection() == child_turn_dir) { - // Turning motion but keeps in same direction: encourages to commit to turning if starting it - travel_cost = travel_cost_raw * _ctx->motion_table.non_straight_penalty; - } else { - // Turning motion and changing direction: penalizes wiggling - travel_cost = travel_cost_raw * - (_ctx->motion_table.non_straight_penalty + _ctx->motion_table.change_penalty); - } - } - - if (child_turn_dir == TurnDirection::REV_RIGHT || - child_turn_dir == TurnDirection::REV_LEFT || - child_turn_dir == TurnDirection::REVERSE) - { - // reverse direction - travel_cost *= _ctx->motion_table.reverse_penalty; - } - - return travel_cost; -} - -float NodeHybrid::getHeuristicCost( - const Coordinates & node_coords, - const CoordinateVector & goals_coords) -{ - // obstacle heuristic does not depend on goal heading - const float obstacle_heuristic = - _ctx->obstacle_heuristic->getObstacleHeuristic(node_coords, _ctx->motion_table.cost_penalty, - _ctx->motion_table.use_quadratic_cost_penalty, - _ctx->motion_table.downsample_obstacle_heuristic); - float distance_heuristic = std::numeric_limits::max(); - for (unsigned int i = 0; i < goals_coords.size(); i++) { - distance_heuristic = std::min( - distance_heuristic, - _ctx->distance_heuristic->getDistanceHeuristic(node_coords, goals_coords[i], - obstacle_heuristic, _ctx->motion_table)); - } - return std::max(obstacle_heuristic, distance_heuristic); -} - -void NodeHybrid::initMotionModel( - NodeContext * ctx, - const MotionModel & motion_model, - unsigned int & size_x, - unsigned int & size_y, - unsigned int & num_angle_quantization, - SearchInfo & search_info) -{ - // find the motion model selected - switch (motion_model) { - case MotionModel::DUBIN: - ctx->motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); - break; - case MotionModel::REEDS_SHEPP: - ctx->motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); - break; - default: - throw std::runtime_error( - "Invalid motion model for Hybrid A*. Please select between" - " Dubin (Ackermann forward only)," - " Reeds-Shepp (Ackermann forward and back)."); - } -} - -void NodeHybrid::getNeighbors( - std::function & NeighborGetter, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown, - NodeVector & neighbors) -{ - uint64_t index = 0; - NodePtr neighbor = nullptr; - Coordinates initial_node_coords; - const MotionPoses motion_projections = _ctx->motion_table.getProjections(this); - - for (unsigned int i = 0; i != motion_projections.size(); i++) { - index = NodeHybrid::getIndex( - static_cast(motion_projections[i]._x), - static_cast(motion_projections[i]._y), - static_cast(motion_projections[i]._theta), - _ctx->motion_table.size_x, _ctx->motion_table.num_angle_quantization); - - if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { - // Cache the initial pose in case it was visited but valid - // don't want to disrupt continuous coordinate expansion - initial_node_coords = neighbor->pose; - neighbor->setPose( - Coordinates( - motion_projections[i]._x, - motion_projections[i]._y, - motion_projections[i]._theta)); - if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { - neighbor->setMotionPrimitiveIndex(i, motion_projections[i]._turn_dir); - neighbors.push_back(neighbor); - } else { - neighbor->setPose(initial_node_coords); - } - } - } -} - -bool NodeHybrid::backtracePath(CoordinateVector & path) -{ - if (!this->parent) { - return false; - } - - NodePtr current_node = this; - - while (current_node->parent) { - path.push_back(current_node->pose); - // Convert angle to radians - path.back().theta = _ctx->motion_table.getAngleFromBin(path.back().theta); - current_node = current_node->parent; - } - - // add the start pose - path.push_back(current_node->pose); - // Convert angle to radians - path.back().theta = _ctx->motion_table.getAngleFromBin(path.back().theta); - - return true; -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp deleted file mode 100644 index 74715329..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp +++ /dev/null @@ -1,230 +0,0 @@ -// Copyright (c) 2026, 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. Reserved. - -#include "athena_smac_planner/obstacle_heuristic.hpp" - -namespace athena_smac_planner -{ - -void ObstacleHeuristic::resetObstacleHeuristic( - std::shared_ptr costmap_ros_i, - const unsigned int & start_x, const unsigned int & start_y, - const unsigned int & goal_x, const unsigned int & goal_y, - const bool downsample_obstacle_heuristic) -{ - // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up - // the planner considerably to search through 75% less cells with no detectable - // erosion of path quality after even modest smoothing. The error would be no more - // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality - costmap_ros = costmap_ros_i; - auto costmap = costmap_ros->getCostmap(); - - // Clear lookup table - unsigned int size = 0u; - unsigned int size_x = 0u; - if (downsample_obstacle_heuristic) { - size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); - size = size_x * - ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); - } else { - size_x = costmap->getSizeInCellsX(); - size = size_x * costmap->getSizeInCellsY(); - } - - if (obstacle_heuristic_lookup_table_.size() == size) { - // must reset all values - std::fill( - obstacle_heuristic_lookup_table_.begin(), - obstacle_heuristic_lookup_table_.end(), 0.0f); - } else { - unsigned int obstacle_size = obstacle_heuristic_lookup_table_.size(); - obstacle_heuristic_lookup_table_.resize(size, 0.0f); - // must reset values for non-constructed indices - std::fill_n( - obstacle_heuristic_lookup_table_.begin(), obstacle_size, 0.0f); - } - - obstacle_heuristic_queue_.clear(); - obstacle_heuristic_queue_.reserve(size); - - // Set initial goal point to queue from. Divided by 2 due to downsampled costmap. - unsigned int goal_index; - if (downsample_obstacle_heuristic) { - goal_index = floor(goal_y / 2.0f) * size_x + floor(goal_x / 2.0f); - } else { - goal_index = floor(goal_y) * size_x + floor(goal_x); - } - - obstacle_heuristic_queue_.emplace_back( - distanceHeuristic2D(goal_index, size_x, start_x, start_y), goal_index); - - // initialize goal cell with a very small value to differentiate it from 0.0 (~uninitialized) - // the negative value means the cell is in the open set - obstacle_heuristic_lookup_table_[goal_index] = -0.00001f; -} - -float ObstacleHeuristic::getObstacleHeuristic( - const Coordinates & node_coords, - const float & cost_penalty, - const bool use_quadratic_cost_penalty, - const bool downsample_obstacle_heuristic) -{ - // If already expanded, return the cost - auto costmap = costmap_ros->getCostmap(); - unsigned int size_x = 0u; - unsigned int size_y = 0u; - if (downsample_obstacle_heuristic) { - size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); - size_y = ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); - } else { - size_x = costmap->getSizeInCellsX(); - size_y = costmap->getSizeInCellsY(); - } - - // Divided by 2 due to downsampled costmap. - unsigned int start_y, start_x; - if (downsample_obstacle_heuristic) { - start_y = floor(node_coords.y / 2.0f); - start_x = floor(node_coords.x / 2.0f); - } else { - start_y = floor(node_coords.y); - start_x = floor(node_coords.x); - } - - const unsigned int start_index = start_y * size_x + start_x; - const float & requested_node_cost = obstacle_heuristic_lookup_table_[start_index]; - if (requested_node_cost > 0.0f) { - // costs are doubled due to downsampling - return downsample_obstacle_heuristic ? 2.0f * requested_node_cost : requested_node_cost; - } - - // If not, expand until it is included. This dynamic programming ensures that - // we only expand the MINIMUM spanning set of the costmap per planning request. - // Rather than naively expanding the entire (potentially massive) map for a limited - // path, we only expand to the extent required for the furthest expansion in the - // search-planning request that dynamically updates during search as needed. - - // start_x and start_y have changed since last call - // we need to recompute 2D distance heuristic and reprioritize queue - for (auto & n : obstacle_heuristic_queue_) { - n.first = -obstacle_heuristic_lookup_table_[n.second] + - distanceHeuristic2D(n.second, size_x, start_x, start_y); - } - std::make_heap( - obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), - ObstacleHeuristicComparator{}); - - const int size_x_int = static_cast(size_x); - const float sqrt2 = sqrtf(2.0f); - float c_cost, cost, travel_cost, new_cost, existing_cost; - unsigned int mx, my; - unsigned int idx, new_idx = 0; - - const std::vector neighborhood = {1, -1, // left right - size_x_int, -size_x_int, // up down - size_x_int + 1, size_x_int - 1, // upper diagonals - -size_x_int + 1, -size_x_int - 1}; // lower diagonals - - while (!obstacle_heuristic_queue_.empty()) { - idx = obstacle_heuristic_queue_.front().second; - std::pop_heap( - obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), - ObstacleHeuristicComparator{}); - obstacle_heuristic_queue_.pop_back(); - c_cost = obstacle_heuristic_lookup_table_[idx]; - if (c_cost > 0.0f) { - // cell has been processed and closed, no further cost improvements - // are mathematically possible thanks to euclidean distance heuristic consistency - continue; - } - c_cost = -c_cost; - obstacle_heuristic_lookup_table_[idx] = c_cost; // set a positive value to close the cell - - // find neighbors - for (unsigned int i = 0; i != neighborhood.size(); i++) { - new_idx = static_cast(static_cast(idx) + neighborhood[i]); - - // if neighbor path is better and non-lethal, set new cost and add to queue - if (new_idx < size_x * size_y) { - if (downsample_obstacle_heuristic) { - // Get costmap values as if downsampled - unsigned int y_offset = (new_idx / size_x) * 2; - unsigned int x_offset = (new_idx - ((new_idx / size_x) * size_x)) * 2; - cost = costmap->getCost(x_offset, y_offset); - for (unsigned int k = 0; k < 2u; ++k) { - unsigned int mxd = x_offset + k; - if (mxd >= costmap->getSizeInCellsX()) { - continue; - } - for (unsigned int j = 0; j < 2u; ++j) { - unsigned int myd = y_offset + j; - if (myd >= costmap->getSizeInCellsY()) { - continue; - } - if (k == 0 && j == 0) { - continue; - } - cost = std::min(cost, static_cast(costmap->getCost(mxd, myd))); - } - } - } else { - cost = static_cast(costmap->getCost(new_idx)); - } - - if (cost >= INSCRIBED_COST) { - continue; - } - - my = new_idx / size_x; - mx = new_idx - (my * size_x); - - if (mx >= size_x - 3 || mx <= 3) { - continue; - } - if (my >= size_y - 3 || my <= 3) { - continue; - } - - existing_cost = obstacle_heuristic_lookup_table_[new_idx]; - if (existing_cost <= 0.0f) { - if (use_quadratic_cost_penalty) { - travel_cost = - (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 63504.0f)); // 252^2 - } else { - travel_cost = - ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 252.0f)); - } - - new_cost = c_cost + travel_cost; - if (existing_cost == 0.0f || -existing_cost > new_cost) { - // the negative value means the cell is in the open set - obstacle_heuristic_lookup_table_[new_idx] = -new_cost; - obstacle_heuristic_queue_.emplace_back( - new_cost + distanceHeuristic2D(new_idx, size_x, start_x, start_y), new_idx); - std::push_heap( - obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), - ObstacleHeuristicComparator{}); - } - } - } - } - - if (idx == start_index) { - break; - } - } - return downsample_obstacle_heuristic ? 2.0f * requested_node_cost : requested_node_cost; -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp deleted file mode 100644 index bfe1879d..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp +++ /dev/null @@ -1,478 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2023, 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. Reserved. - -#include -#include -#include -#include -#include -#include - -#include "athena_smac_planner/smac_planner_hybrid.hpp" - -// #define BENCHMARK_TESTING - -namespace athena_smac_planner -{ - -using namespace std::chrono; // NOLINT - -SmacPlannerHybrid::SmacPlannerHybrid( - rclcpp::Node::SharedPtr node, - std::shared_ptr costmap_ros, - const std::string & name) -: _a_star(nullptr), - _collision_checker(nullptr, 1, nullptr), - _smoother(nullptr), - _costmap(nullptr), - _costmap_downsampler(nullptr) -{ - _name = name; - _logger = node->get_logger(); - _clock = node->get_clock(); - _costmap = costmap_ros->getCostmap(); - _costmap_ros = costmap_ros; - _global_frame = costmap_ros->getGlobalFrameID(); - - RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerHybrid", _name.c_str()); - - // Helper: declare param if absent then get its value - auto p = [&node, &name](const std::string & param, auto def) { - if (!node->has_parameter(name + "." + param)) { - node->declare_parameter(name + "." + param, def); - } - return node->get_parameter(name + "." + param).get_value(); - }; - - int angle_quantizations; - double analytic_expansion_max_length_m; - bool smooth_path; - - // General planner params - _downsample_costmap = p("downsample_costmap", false); - _downsampling_factor = p("downsampling_factor", 1); - - angle_quantizations = p("angle_quantization_bins", 72); - _angle_bin_size = 2.0 * M_PI / angle_quantizations; - _angle_quantizations = static_cast(angle_quantizations); - - _tolerance = static_cast(p("tolerance", 0.25)); - _allow_unknown = p("allow_unknown", true); - _max_iterations = p("max_iterations", 1000000); - _max_on_approach_iterations = p("max_on_approach_iterations", 1000); - _terminal_checking_interval = p("terminal_checking_interval", 5000); - smooth_path = p("smooth_path", true); - - _minimum_turning_radius_global_coords = p("minimum_turning_radius", 0.4); - _search_info.allow_primitive_interpolation = p("allow_primitive_interpolation", false); - _search_info.cache_obstacle_heuristic = p("cache_obstacle_heuristic", false); - _search_info.reverse_penalty = p("reverse_penalty", 2.0); - _search_info.change_penalty = p("change_penalty", 0.0); - _search_info.non_straight_penalty = p("non_straight_penalty", 1.2); - _search_info.cost_penalty = p("cost_penalty", 2.0); - _search_info.retrospective_penalty = p("retrospective_penalty", 0.015); - _search_info.analytic_expansion_ratio = p("analytic_expansion_ratio", 3.5); - _search_info.analytic_expansion_max_cost = p("analytic_expansion_max_cost", 200.0); - _search_info.analytic_expansion_max_cost_override = - p("analytic_expansion_max_cost_override", false); - _search_info.use_quadratic_cost_penalty = p("use_quadratic_cost_penalty", false); - _search_info.downsample_obstacle_heuristic = p("downsample_obstacle_heuristic", true); - - analytic_expansion_max_length_m = p("analytic_expansion_max_length", 3.0); - _search_info.analytic_expansion_max_length = - analytic_expansion_max_length_m / _costmap->getResolution(); - - _max_planning_time = p("max_planning_time", 5.0); - _lookup_table_size = p("lookup_table_size", 20.0); - _debug_visualizations = p("debug_visualizations", false); - _motion_model_for_search = p("motion_model_for_search", std::string("DUBIN")); - - std::string goal_heading_type = p("goal_heading_mode", std::string("DEFAULT")); - _goal_heading_mode = fromStringToGH(goal_heading_type); - - _coarse_search_resolution = p("coarse_search_resolution", 1); - - if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { - throw std::runtime_error( - "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION."); - } - - _motion_model = fromString(_motion_model_for_search); - - if (_motion_model == MotionModel::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get MotionModel search type. Given '%s', " - "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.", - _motion_model_for_search.c_str()); - } - - if (_max_on_approach_iterations <= 0) { - RCLCPP_WARN( - _logger, "On approach iteration selected as <= 0, " - "disabling tolerance and on approach iterations."); - _max_on_approach_iterations = std::numeric_limits::max(); - } - - if (_max_iterations <= 0) { - RCLCPP_WARN( - _logger, "maximum iteration selected as <= 0, " - "disabling maximum iterations."); - _max_iterations = std::numeric_limits::max(); - } - - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0, " - "disabling coarse iteration resolution search for goal heading"); - _coarse_search_resolution = 1; - } - - if (_angle_quantizations % _coarse_search_resolution != 0) { - throw std::runtime_error( - "coarse iteration should be an increment of the number of angular bins configured"); - } - - if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) { - RCLCPP_WARN( - _logger, "Min turning radius cannot be less than the search grid cell resolution!"); - _minimum_turning_radius_global_coords = _costmap->getResolution() * _downsampling_factor; - } - - // Convert to grid coordinates - if (!_downsample_costmap) { - _downsampling_factor = 1; - } - _search_info.minimum_turning_radius = - _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); - _lookup_table_dim = - static_cast(_lookup_table_size) / - static_cast(_costmap->getResolution() * _downsampling_factor); - - // Make sure its a whole number - _lookup_table_dim = static_cast(static_cast(_lookup_table_dim)); - - // Make sure its an odd number - if (static_cast(_lookup_table_dim) % 2 == 0) { - RCLCPP_INFO( - _logger, - "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", - _lookup_table_dim); - _lookup_table_dim += 1.0; - } - - // Initialize collision checker - _collision_checker = GridCollisionChecker( - _costmap_ros, _angle_quantizations, node); - _collision_checker.setFootprint( - costmap_ros->getRobotFootprint(), - costmap_ros->getUseRadius(), - findCircumscribedCost(_costmap_ros)); - - // Initialize A* template - _a_star = std::make_unique>(_motion_model, _search_info); - _a_star->initialize( - _allow_unknown, - _max_iterations, - _max_on_approach_iterations, - _terminal_checking_interval, - _max_planning_time, - _lookup_table_dim, - _angle_quantizations); - - // Initialize path smoother - if (smooth_path) { - SmootherParams smoother_params; - smoother_params.get(node.get(), _name); - _smoother = std::make_unique(smoother_params); - _smoother->initialize(_minimum_turning_radius_global_coords); - } - - // Initialize costmap downsampler - if (_downsample_costmap && _downsampling_factor > 1) { - _costmap_downsampler = std::make_unique(); - _costmap_downsampler->on_configure(_costmap, _downsampling_factor); - } - - // Create publishers - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); - - if (_debug_visualizations) { - _expansions_publisher = - node->create_publisher("expansions", 1); - _planned_footprints_publisher = - node->create_publisher("planned_footprints", 1); - _smoothed_footprints_publisher = - node->create_publisher("smoothed_footprints", 1); - } - - RCLCPP_INFO( - _logger, "Configured %s of type SmacPlannerHybrid with " - "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f." - "Using motion model: %s.", - _name.c_str(), _max_iterations, _max_on_approach_iterations, - _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - _tolerance, toString(_motion_model).c_str()); -} - -SmacPlannerHybrid::~SmacPlannerHybrid() -{ - RCLCPP_INFO(_logger, "Destroying %s of type SmacPlannerHybrid", _name.c_str()); - _a_star.reset(); - _smoother.reset(); - if (_costmap_downsampler) { - _costmap_downsampler->on_cleanup(); - _costmap_downsampler.reset(); - } -} - -nav_msgs::msg::Path SmacPlannerHybrid::createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal, - std::function cancel_checker) -{ - std::lock_guard lock_reinit(_mutex); - steady_clock::time_point a = steady_clock::now(); - - std::unique_lock lock(*(_costmap->getMutex())); - - // Downsample costmap, if required - nav2_costmap_2d::Costmap2D * costmap = _costmap; - if (_downsample_costmap && _downsampling_factor > 1) { - costmap = _costmap_downsampler->downsample(_downsampling_factor); - _collision_checker.setCostmap(costmap); - } - - // Set collision checker and costmap information - _collision_checker.setFootprint( - _costmap_ros->getRobotFootprint(), - _costmap_ros->getUseRadius(), - findCircumscribedCost(_costmap_ros)); - _a_star->setCollisionChecker(&_collision_checker); - - // Set starting point, in A* bin search coordinates - float mx_start, my_start, mx_goal, my_goal; - { - unsigned int umx, umy; - if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, umx, umy)) { - throw std::runtime_error( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was outside bounds"); - } - mx_start = static_cast(umx); - my_start = static_cast(umy); - } - - double start_orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); - while (start_orientation_bin < 0.0) { - start_orientation_bin += static_cast(_angle_quantizations); - } - // This is needed to handle precision issues - if (start_orientation_bin >= static_cast(_angle_quantizations)) { - start_orientation_bin -= static_cast(_angle_quantizations); - } - unsigned int start_orientation_bin_int = - static_cast(start_orientation_bin); - _a_star->setStart(mx_start, my_start, start_orientation_bin_int); - - // Set goal point, in A* bin search coordinates - { - unsigned int umx, umy; - if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, umx, umy)) { - throw std::runtime_error( - "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + - std::to_string(goal.pose.position.y) + ") was outside bounds"); - } - mx_goal = static_cast(umx); - my_goal = static_cast(umy); - } - double goal_orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); - while (goal_orientation_bin < 0.0) { - goal_orientation_bin += static_cast(_angle_quantizations); - } - // This is needed to handle precision issues - if (goal_orientation_bin >= static_cast(_angle_quantizations)) { - goal_orientation_bin -= static_cast(_angle_quantizations); - } - unsigned int goal_orientation_bin_int = - static_cast(goal_orientation_bin); - _a_star->setGoal( - mx_goal, my_goal, static_cast(goal_orientation_bin_int), - _goal_heading_mode, _coarse_search_resolution); - - // Setup message - nav_msgs::msg::Path plan; - plan.header.stamp = _clock->now(); - plan.header.frame_id = _global_frame; - geometry_msgs::msg::PoseStamped pose; - pose.header = plan.header; - pose.pose.position.z = 0.0; - pose.pose.orientation.x = 0.0; - pose.pose.orientation.y = 0.0; - pose.pose.orientation.z = 0.0; - pose.pose.orientation.w = 1.0; - - // Corner case of start and goal being on the same cell - if (std::floor(mx_start) == std::floor(mx_goal) && - std::floor(my_start) == std::floor(my_goal) && - start_orientation_bin_int == goal_orientation_bin_int) - { - pose.pose = start.pose; - pose.pose.orientation = goal.pose.orientation; - plan.poses.push_back(pose); - - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - auto msg = std::make_unique(plan); - _raw_plan_publisher->publish(std::move(msg)); - } - - return plan; - } - - // Compute plan - NodeHybrid::CoordinateVector path; - int num_iterations = 0; - std::string error; - std::unique_ptr>> expansions = nullptr; - if (_debug_visualizations) { - expansions = std::make_unique>>(); - } - - if (!_a_star->createPath( - path, num_iterations, - _tolerance / static_cast(costmap->getResolution()), cancel_checker, expansions.get())) - { - if (_debug_visualizations) { - auto msg = std::make_unique(); - geometry_msgs::msg::Pose msg_pose; - msg->header.stamp = _clock->now(); - msg->header.frame_id = _global_frame; - for (auto & e : *expansions) { - msg_pose.position.x = std::get<0>(e); - msg_pose.position.y = std::get<1>(e); - msg_pose.orientation = getWorldOrientation(std::get<2>(e)); - msg->poses.push_back(msg_pose); - } - _expansions_publisher->publish(std::move(msg)); - } - - if (num_iterations == 1) { - throw std::runtime_error("Start occupied"); - } - - if (num_iterations < _a_star->getMaxIterations()) { - throw std::runtime_error("No valid path could be found"); - } else { - throw std::runtime_error("Exceeded maximum iterations"); - } - } - - // Convert to world coordinates - plan.poses.reserve(path.size()); - for (int i = path.size() - 1; i >= 0; --i) { - pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); - pose.pose.orientation = getWorldOrientation(path[i].theta); - plan.poses.push_back(pose); - } - - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - auto msg = std::make_unique(plan); - _raw_plan_publisher->publish(std::move(msg)); - } - - if (_debug_visualizations) { - // Publish expansions for debug - auto now = _clock->now(); - auto msg = std::make_unique(); - geometry_msgs::msg::Pose msg_pose; - msg->header.stamp = now; - msg->header.frame_id = _global_frame; - for (auto & e : *expansions) { - msg_pose.position.x = std::get<0>(e); - msg_pose.position.y = std::get<1>(e); - msg_pose.orientation = getWorldOrientation(std::get<2>(e)); - msg->poses.push_back(msg_pose); - } - _expansions_publisher->publish(std::move(msg)); - - if (_planned_footprints_publisher->get_subscription_count() > 0) { - // Clear all markers first - auto marker_array = std::make_unique(); - visualization_msgs::msg::Marker clear_all_marker; - clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array->markers.push_back(clear_all_marker); - _planned_footprints_publisher->publish(std::move(marker_array)); - - // Publish planned footprints for debug - marker_array = std::make_unique(); - for (size_t i = 0; i < plan.poses.size(); i++) { - const std::vector edge = - transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); - marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); - } - _planned_footprints_publisher->publish(std::move(marker_array)); - } - } - - // Find how much time we have left to do smoothing - steady_clock::time_point b = steady_clock::now(); - duration time_span = duration_cast>(b - a); - double time_remaining = _max_planning_time - static_cast(time_span.count()); - -#ifdef BENCHMARK_TESTING - std::cout << "It took " << time_span.count() * 1000 << - " milliseconds with " << num_iterations << " iterations." << std::endl; -#endif - - // Smooth plan - if (_smoother && num_iterations > 1) { - _smoother->smooth(plan, costmap, time_remaining); - } - -#ifdef BENCHMARK_TESTING - steady_clock::time_point c = steady_clock::now(); - duration time_span2 = duration_cast>(c - b); - std::cout << "It took " << time_span2.count() * 1000 << - " milliseconds to smooth path." << std::endl; -#endif - - if (_debug_visualizations) { - if (_smoothed_footprints_publisher->get_subscription_count() > 0) { - // Clear all markers first - auto marker_array = std::make_unique(); - visualization_msgs::msg::Marker clear_all_marker; - clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array->markers.push_back(clear_all_marker); - _smoothed_footprints_publisher->publish(std::move(marker_array)); - - // Publish smoothed footprints for debug - marker_array = std::make_unique(); - auto now = _clock->now(); - for (size_t i = 0; i < plan.poses.size(); i++) { - const std::vector edge = - transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); - marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); - } - _smoothed_footprints_publisher->publish(std::move(marker_array)); - } - } - - return plan; -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp deleted file mode 100644 index 7438c3d8..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp +++ /dev/null @@ -1,436 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#include -#include - -#include -#include -#include - -#include "angles/angles.h" - -#include "tf2/utils.hpp" - -#include "athena_smac_planner/smoother.hpp" -#include "nav2_smoother/smoother_utils.hpp" - -namespace athena_smac_planner -{ -using namespace nav2_util::geometry_utils; // NOLINT -using namespace std::chrono; // NOLINT -using smoother_utils::PathSegment; - -Smoother::Smoother(const SmootherParams & params) -{ - tolerance_ = params.tolerance_; - max_its_ = params.max_its_; - data_w_ = params.w_data_; - smooth_w_ = params.w_smooth_; - is_holonomic_ = params.holonomic_; - do_refinement_ = params.do_refinement_; - refinement_num_ = params.refinement_num_; -} - -void Smoother::initialize(const double & min_turning_radius) -{ - min_turning_rad_ = min_turning_radius; - state_space_ = std::make_unique(min_turning_rad_); -} - -bool Smoother::smooth( - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time) -{ - // by-pass path orientations approximation when skipping smac smoother - if (max_its_ == 0) { - return false; - } - - steady_clock::time_point start = steady_clock::now(); - double time_remaining = max_time; - bool success = true, reversing_segment; - nav_msgs::msg::Path curr_path_segment; - curr_path_segment.header = path.header; - std::vector path_segments = smoother_utils::findDirectionalPathSegments(path); - - for (unsigned int i = 0; i != path_segments.size(); i++) { - if (path_segments[i].end - path_segments[i].start > 10) { - // Populate path segment - curr_path_segment.poses.clear(); - std::copy( - path.poses.begin() + path_segments[i].start, - path.poses.begin() + path_segments[i].end + 1, - std::back_inserter(curr_path_segment.poses)); - - // Make sure we're still able to smooth with time remaining - steady_clock::time_point now = steady_clock::now(); - time_remaining = max_time - duration_cast>(now - start).count(); - refinement_ctr_ = 0; - - // Smooth path segment naively - const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose; - const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back().pose; - bool local_success = - smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining); - success = success && local_success; - - // Enforce boundary conditions - if (!is_holonomic_ && local_success) { - enforceStartBoundaryConditions(start_pose, curr_path_segment, costmap, reversing_segment); - enforceEndBoundaryConditions(goal_pose, curr_path_segment, costmap, reversing_segment); - } - - // Assemble the path changes to the main path - std::copy( - curr_path_segment.poses.begin(), - curr_path_segment.poses.end(), - path.poses.begin() + path_segments[i].start); - } - } - - return success; -} - -bool Smoother::smoothImpl( - nav_msgs::msg::Path & path, - bool & reversing_segment, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time) -{ - steady_clock::time_point a = steady_clock::now(); - rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time); - - int its = 0; - double change = tolerance_; - const unsigned int & path_size = path.poses.size(); - double x_i, y_i, y_m1, y_ip1, y_i_org; - unsigned int mx, my; - - nav_msgs::msg::Path new_path = path; - nav_msgs::msg::Path last_path = path; - - while (change >= tolerance_) { - its += 1; - change = 0.0; - - // Make sure the smoothing function will converge - if (its >= max_its_) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Number of iterations has exceeded limit of %i.", max_its_); - path = last_path; - smoother_utils::updateApproximatePathOrientations(path, reversing_segment); - return false; - } - - // Make sure still have time left to process - steady_clock::time_point b = steady_clock::now(); - rclcpp::Duration timespan(duration_cast>(b - a)); - if (timespan > max_dur) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Smoothing time exceeded allowed duration of %0.2f.", max_time); - path = last_path; - smoother_utils::updateApproximatePathOrientations(path, reversing_segment); - return false; - } - - for (unsigned int i = 1; i != path_size - 1; i++) { - for (unsigned int j = 0; j != 2; j++) { - x_i = getFieldByDim(path.poses[i], j); - y_i = getFieldByDim(new_path.poses[i], j); - y_m1 = getFieldByDim(new_path.poses[i - 1], j); - y_ip1 = getFieldByDim(new_path.poses[i + 1], j); - y_i_org = y_i; - - // Smooth based on local 3 point neighborhood and original data locations - y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i)); - setFieldByDim(new_path.poses[i], j, y_i); - change += abs(y_i - y_i_org); - } - - // validate update is admissible, only checks cost if a valid costmap pointer is provided - float cost = 0.0; - if (costmap) { - costmap->worldToMap( - getFieldByDim(new_path.poses[i], 0), - getFieldByDim(new_path.poses[i], 1), - mx, my); - cost = static_cast(costmap->getCost(mx, my)); - } - - if (cost > MAX_NON_OBSTACLE_COST && cost != UNKNOWN_COST) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Smoothing process resulted in an infeasible collision. " - "Returning the last path before the infeasibility was introduced."); - path = last_path; - smoother_utils::updateApproximatePathOrientations(path, reversing_segment); - return false; - } - } - - last_path = new_path; - } - - // Let's do additional refinement, it shouldn't take more than a couple milliseconds - // but really puts the path quality over the top. - if (do_refinement_ && refinement_ctr_ < refinement_num_) { - refinement_ctr_++; - smoothImpl(new_path, reversing_segment, costmap, max_time); - } - - smoother_utils::updateApproximatePathOrientations(new_path, reversing_segment); - path = new_path; - return true; -} - -double Smoother::getFieldByDim( - const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim) -{ - if (dim == 0) { - return msg.pose.position.x; - } else if (dim == 1) { - return msg.pose.position.y; - } else { - return msg.pose.position.z; - } -} - -void Smoother::setFieldByDim( - geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, - const double & value) -{ - if (dim == 0) { - msg.pose.position.x = value; - } else if (dim == 1) { - msg.pose.position.y = value; - } else { - msg.pose.position.z = value; - } -} - -unsigned int Smoother::findShortestBoundaryExpansionIdx( - const BoundaryExpansions & boundary_expansions) -{ - // Check which is valid with the minimum integrated length such that - // shorter end-points away that are infeasible to achieve without - // a loop-de-loop are punished - double min_length = 1e9; - int shortest_boundary_expansion_idx = 1e9; - for (unsigned int idx = 0; idx != boundary_expansions.size(); idx++) { - if (boundary_expansions[idx].expansion_path_length0.0 && - boundary_expansions[idx].expansion_path_length > 0.0) - { - min_length = boundary_expansions[idx].expansion_path_length; - shortest_boundary_expansion_idx = idx; - } - } - - return shortest_boundary_expansion_idx; -} - -void Smoother::findBoundaryExpansion( - const geometry_msgs::msg::Pose & start, - const geometry_msgs::msg::Pose & end, - BoundaryExpansion & expansion, - const nav2_costmap_2d::Costmap2D * costmap) -{ - ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_); - - from[0] = start.position.x; - from[1] = start.position.y; - from[2] = tf2::getYaw(start.orientation); - to[0] = end.position.x; - to[1] = end.position.y; - to[2] = tf2::getYaw(end.orientation); - - double d = state_space_->distance(from(), to()); - // If this path is too long compared to the original, then this is probably - // a loop-de-loop, treat as invalid as to not deviate too far from the original path. - // 2.0 selected from prinicipled choice of boundary test points - // r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be - // approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI. - // For all but the last backup test point, a loop would be approximately - // 2x greater than any of the selections. - if (d > 2.0 * expansion.original_path_length) { - return; - } - - std::vector reals; - double theta(0.0), x(0.0), y(0.0); - double x_m = start.position.x; - double y_m = start.position.y; - - // Get intermediary poses - for (double i = 0; i <= expansion.path_end_idx; i++) { - state_space_->interpolate(from(), to(), i / expansion.path_end_idx, s()); - reals = s.reals(); - // Make sure in range [0, 2PI) - theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; - theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; - x = reals[0]; - y = reals[1]; - - // Check for collision - unsigned int mx, my; - costmap->worldToMap(x, y, mx, my); - if (static_cast(costmap->getCost(mx, my)) >= INSCRIBED_COST) { - expansion.in_collision = true; - } - - // Integrate path length - expansion.expansion_path_length += hypot(x - x_m, y - y_m); - x_m = x; - y_m = y; - - // Store point - expansion.pts.emplace_back(x, y, theta); - } -} - -template -BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, IteratorT end) -{ - std::vector distances = { - min_turning_rad_, // Radius - 2.0 * min_turning_rad_, // Diameter - M_PI * min_turning_rad_, // 50% Circumference - 2.0 * M_PI * min_turning_rad_ // Circumference - }; - - BoundaryExpansions boundary_expansions; - boundary_expansions.resize(distances.size()); - double curr_dist = 0.0; - double x_last = start->pose.position.x; - double y_last = start->pose.position.y; - geometry_msgs::msg::Point pt; - unsigned int curr_dist_idx = 0; - - for (IteratorT iter = start; iter != end; iter++) { - pt = iter->pose.position; - curr_dist += hypot(pt.x - x_last, pt.y - y_last); - x_last = pt.x; - y_last = pt.y; - - if (curr_dist >= distances[curr_dist_idx]) { - boundary_expansions[curr_dist_idx].path_end_idx = iter - start; - boundary_expansions[curr_dist_idx].original_path_length = curr_dist; - curr_dist_idx++; - } - - if (curr_dist_idx == boundary_expansions.size()) { - break; - } - } - - return boundary_expansions; -} - -void Smoother::enforceStartBoundaryConditions( - const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment) -{ - // Find range of points for testing - BoundaryExpansions boundary_expansions = - generateBoundaryExpansionPoints(path.poses.begin(), path.poses.end()); - - // Generate the motion model and metadata from start -> test points - for (unsigned int i = 0; i != boundary_expansions.size(); i++) { - BoundaryExpansion & expansion = boundary_expansions[i]; - if (expansion.path_end_idx == 0.0) { - continue; - } - - if (!reversing_segment) { - findBoundaryExpansion( - start_pose, path.poses[expansion.path_end_idx].pose, expansion, - costmap); - } else { - findBoundaryExpansion( - path.poses[expansion.path_end_idx].pose, start_pose, expansion, - costmap); - } - } - - // Find the shortest kinematically feasible boundary expansion - unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); - if (best_expansion_idx > boundary_expansions.size()) { - return; - } - - // Override values to match curve - BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; - if (reversing_segment) { - std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); - } - for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { - path.poses[i].pose.position.x = best_expansion.pts[i].x; - path.poses[i].pose.position.y = best_expansion.pts[i].y; - path.poses[i].pose.orientation = orientationAroundZAxis(best_expansion.pts[i].theta); - } -} - -void Smoother::enforceEndBoundaryConditions( - const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment) -{ - // Find range of points for testing - BoundaryExpansions boundary_expansions = - generateBoundaryExpansionPoints(path.poses.rbegin(), path.poses.rend()); - - // Generate the motion model and metadata from start -> test points - unsigned int expansion_starting_idx; - for (unsigned int i = 0; i != boundary_expansions.size(); i++) { - BoundaryExpansion & expansion = boundary_expansions[i]; - if (expansion.path_end_idx == 0.0) { - continue; - } - expansion_starting_idx = path.poses.size() - expansion.path_end_idx - 1; - if (!reversing_segment) { - findBoundaryExpansion(path.poses[expansion_starting_idx].pose, end_pose, expansion, costmap); - } else { - findBoundaryExpansion(end_pose, path.poses[expansion_starting_idx].pose, expansion, costmap); - } - } - - // Find the shortest kinematically feasible boundary expansion - unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); - if (best_expansion_idx > boundary_expansions.size()) { - return; - } - - // Override values to match curve - BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; - if (reversing_segment) { - std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); - } - expansion_starting_idx = path.poses.size() - best_expansion.path_end_idx - 1; - for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { - path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x; - path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y; - path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis( - best_expansion.pts[i].theta); - } -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml deleted file mode 100644 index f58c3009..00000000 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml +++ /dev/null @@ -1,85 +0,0 @@ -# nav_params.yaml — single parameter file passed to all navigation nodes. -# See PLAN.md §7 for full documentation. - -# ── gps_pose_publisher ─────────────────────────────────────────────────────── -gps_pose_publisher: - ros__parameters: - use_sim_time: true - heading_topic: "/gps/heading" - origin_alt: 0.0 - use_start_gate_ref: false # wait for /start_gate_ref NavSatFix to set origin - -# ── mission_executive ──────────────────────────────────────────────────────── -mission_executive: - ros__parameters: - use_sim_time: true - velocity_zero_threshold: 0.05 # m/s — applied to /odom twist (ZED SDK) - arrival_hold_time: 1.0 # seconds of continuous zero velocity - replan_distance_m: 3.0 # cross-track error threshold to trigger replan - latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" - -# ── global_planner (athena_smac_planner) ───────────────────────────────────── -global_planner: - ros__parameters: - use_sim_time: true - # SmacPlannerHybrid params (read under "SmacPlannerHybrid.*" prefix by the planner) - SmacPlannerHybrid: - minimum_turning_radius: 1.2 # must match mppi_runner - angle_quantization_bins: 36 - allow_unknown: true - max_iterations: 10000000 - max_on_approach_iterations: 10000 - smooth_path: false # skip smoother for now - -# ── global_costmap (owned by global_planner) ───────────────────────────────── -# Costmap2DROS creates the node at /global_costmap/global_costmap (nav2 double-naming) -global_costmap: - global_costmap: - ros__parameters: - use_sim_time: true - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 1.0 - publish_frequency: 0.5 - transform_tolerance: 1.0 # generous — GPS-only localization has jitter - rolling_window: false - width: 1000 - height: 1000 - resolution: 1.0 - origin_x: -500.0 - origin_y: -500.0 - # plugins set programmatically in global_planner_node.cpp (empty [] crashes rcl_yaml_param_parser) - -# ── local_costmap (owned by mppi_runner) ───────────────────────────────────── -# Costmap2DROS creates the node at /local_costmap/local_costmap (nav2 double-naming) -local_costmap: - local_costmap: - ros__parameters: - use_sim_time: true - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 5.0 - publish_frequency: 2.0 - transform_tolerance: 1.0 - rolling_window: true - width: 20 - height: 20 - resolution: 0.1 - # plugins omitted — set to [] programmatically in AckermannMPPINode before configure() - -# ── mppi_runner (ackermann_mppi) ───────────────────────────────────────────── -mppi_runner: - ros__parameters: - use_sim_time: true - controller_frequency: 20.0 - mppi: - model_dt: 0.05 - time_steps: 56 - batch_size: 1000 - vx_max: 3.0 - vx_min: 0.0 - wz_max: 1.0 - AckermannConstraints: - min_turning_r: 1.2 diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_view.rviz b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_view.rviz deleted file mode 100644 index 01d04502..00000000 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_view.rviz +++ /dev/null @@ -1,270 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 87 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - - /Global Plan1 - - /Local Plan1 - - /Sampled Paths1 - - /Local Costmap1 - Splitter Ratio: 0.5 - Tree Height: 576 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Update Interval: 0 - Value: false - - Alpha: 0.7 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Global Map - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map - Update Interval: 0 - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Path - Color: 0; 255; 0 - Enabled: true - Line Style: Lines - Line Width: 0.05 - Name: Global Plan - Offset: - X: 0 - Y: 0 - Z: 0.1 - Pose Color: 255; 255; 255 - Pose Style: None - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_path - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Path - Color: 255; 255; 0 - Enabled: true - Line Style: Lines - Line Width: 0.1 - Name: Local Plan (MPPI) - Offset: - X: 0 - Y: 0 - Z: 0.2 - Pose Color: 255; 255; 255 - Pose Style: None - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /mppi_runner/optimal_path - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Sampled Paths - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /mppi_runner/sampled_paths - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Path - Color: 255; 0; 0 - Enabled: false - Line Style: Lines - Line Width: 0.03 - Name: Unsmoothed Plan - Offset: - X: 0 - Y: 0 - Z: 0.05 - Pose Color: 255; 255; 255 - Pose Style: None - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_planner/unsmoothed_plan - Value: false - - Class: rviz_default_plugins/PoseArray - Color: 255; 255; 255 - Enabled: false - Head Length: 0.1 - Head Radius: 0.03 - Name: Search Expansions - Shaft Length: 0.2 - Shaft Radius: 0.01 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_planner/expansions - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Planned Footprints - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_planner/planned_footprints - Value: false - - Alpha: 0.7 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Local Costmap - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/local_costmap/costmap - Update Interval: 0 - Value: true - - Alpha: 0.7 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: false - Name: Global Costmap - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/global_costmap/costmap - Update Interval: 0 - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Topic: - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 10 - Focal Point: - X: 0 - Y: 0 - Z: 0 - Name: Current View - Pitch: 1.57 - Target Frame: base_link - Value: Orbit (rviz) - Yaw: 0 - Saved: ~ diff --git a/src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/nav_visualizer_node.py b/src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/nav_visualizer_node.py deleted file mode 100644 index d6dc0d1e..00000000 --- a/src/subsystems/minimal_navigation/nav_visualizer/nav_visualizer/nav_visualizer_node.py +++ /dev/null @@ -1,154 +0,0 @@ -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseStamped -from nav_msgs.msg import Path -from msgs.msg import NavStatus -import matplotlib.pyplot as plt -from matplotlib.animation import FuncAnimation -import numpy as np -import threading -import math - -def quat_to_yaw(q): - # q is geometry_msgs/Quaternion - siny_cosp = 2 * (q.w * q.z + q.x * q.y) - cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z) - return math.atan2(siny_cosp, cosy_cosp) - -class NavVisualizer(Node): - def __init__(self): - super().__init__('nav_visualizer') - - # Data storage - self.robot_pose = None - self.global_path = None - self.mppi_path = None - self.goal_pose = None - self.nav_status = None - - # Subscriptions - self.create_subscription(PoseStamped, '/robot_pose', self.pose_cb, 10) - self.create_subscription(Path, '/global_path', self.global_path_cb, 10) - self.create_subscription(Path, '/optimal_path', self.mppi_path_cb, 10) - self.create_subscription(PoseStamped, '/goal_pose', self.goal_cb, 10) - self.create_subscription(NavStatus, '/nav_status', self.status_cb, 10) - - self.get_logger().info("Nav Visualizer Node Started") - - # Lock for thread safety between ROS and Matplotlib - self.lock = threading.Lock() - - def pose_cb(self, msg): - with self.lock: - self.robot_pose = msg - - def global_path_cb(self, msg): - with self.lock: - self.global_path = msg - - def mppi_path_cb(self, msg): - with self.lock: - self.mppi_path = msg - - def goal_cb(self, msg): - with self.lock: - self.goal_pose = msg - - def status_cb(self, msg): - with self.lock: - self.nav_status = msg - -class VisualizerPlot: - def __init__(self, node): - self.node = node - self.fig, self.ax = plt.subplots(figsize=(10, 8)) - - # Plot elements - self.robot_marker, = self.ax.plot([], [], 'ro', label='Robot', markersize=10) - self.heading_line, = self.ax.plot([], [], 'r-', linewidth=2) - self.global_path_line, = self.ax.plot([], [], 'b--', label='Global Path') - self.mppi_path_line, = self.ax.plot([], [], 'g-', label='MPPI Path', linewidth=2) - self.goal_marker, = self.ax.plot([], [], 'kx', label='Goal', markersize=12, markeredgewidth=2) - - self.text_status = self.ax.text(0.02, 0.95, '', transform=self.ax.transAxes, - bbox=dict(facecolor='white', alpha=0.7)) - - self.ax.set_aspect('equal') - self.ax.grid(True) - self.ax.legend(loc='upper right') - self.ax.set_xlabel('East (m)') - self.ax.set_ylabel('North (m)') - self.ax.set_title('Athena 2D Navigation Visualizer') - - def init_plot(self): - return self.robot_marker, self.heading_line, self.global_path_line, self.mppi_path_line, self.goal_marker, self.text_status - - def update(self, frame): - with self.node.lock: - # Update Robot - if self.node.robot_pose: - x = self.node.robot_pose.pose.position.x - y = self.node.robot_pose.pose.position.y - yaw = quat_to_yaw(self.node.robot_pose.pose.orientation) - self.robot_marker.set_data([x], [y]) - - # Heading arrow (0.5m long) - arrow_len = 1.0 - self.heading_line.set_data([x, x + arrow_len * math.cos(yaw)], - [y, y + arrow_len * math.sin(yaw)]) - - # Update Goal - if self.node.goal_pose: - self.goal_marker.set_data([self.node.goal_pose.pose.position.x], - [self.node.goal_pose.pose.position.y]) - - # Update Global Path - if self.node.global_path: - px = [p.pose.position.x for p in self.node.global_path.poses] - py = [p.pose.position.y for p in self.node.global_path.poses] - self.global_path_line.set_data(px, py) - - # Update MPPI Path - if self.node.mppi_path: - mx = [p.pose.position.x for p in self.node.mppi_path.poses] - my = [p.pose.position.y for p in self.node.mppi_path.poses] - self.mppi_path_line.set_data(mx, my) - - # Update Status Text - if self.node.nav_status: - status_text = (f"State: {self.node.nav_status.state}\n" - f"Target: {self.node.nav_status.active_target_id}\n" - f"Dist to Goal: {self.node.nav_status.distance_to_goal_m:.2f}m\n" - f"X-Track Error: {self.node.nav_status.cross_track_error_m:.2f}m\n" - f"Speed: {self.node.nav_status.robot_speed_mps:.2f} m/s") - self.text_status.set_text(status_text) - - # Auto-center view on robot - if self.node.robot_pose: - cx = self.node.robot_pose.pose.position.x - cy = self.node.robot_pose.pose.position.y - self.ax.set_xlim(cx - 15, cx + 15) - self.ax.set_ylim(cy - 15, cy + 15) - - return self.robot_marker, self.heading_line, self.global_path_line, self.mppi_path_line, self.goal_marker, self.text_status - -def main(args=None): - rclpy.init(args=args) - node = NavVisualizer() - - # Run ROS spin in a separate thread - thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) - thread.start() - - # Setup Matplotlib - viz = VisualizerPlot(node) - ani = FuncAnimation(viz.fig, viz.update, frames=None, init_func=viz.init_plot, - blit=True, interval=100, cache_frame_data=False) - - plt.show() - - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/src/subsystems/minimal_navigation/nav_visualizer/setup.cfg b/src/subsystems/minimal_navigation/nav_visualizer/setup.cfg deleted file mode 100644 index bd1b4600..00000000 --- a/src/subsystems/minimal_navigation/nav_visualizer/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/nav_visualizer -[install] -install_scripts=$base/lib/nav_visualizer diff --git a/src/subsystems/minimal_navigation/nav_visualizer/test/test_copyright.py b/src/subsystems/minimal_navigation/nav_visualizer/test/test_copyright.py deleted file mode 100644 index 97a39196..00000000 --- a/src/subsystems/minimal_navigation/nav_visualizer/test/test_copyright.py +++ /dev/null @@ -1,25 +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 - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/subsystems/minimal_navigation/nav_visualizer/test/test_flake8.py b/src/subsystems/minimal_navigation/nav_visualizer/test/test_flake8.py deleted file mode 100644 index 27ee1078..00000000 --- a/src/subsystems/minimal_navigation/nav_visualizer/test/test_flake8.py +++ /dev/null @@ -1,25 +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(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/subsystems/minimal_navigation/nav_visualizer/test/test_pep257.py b/src/subsystems/minimal_navigation/nav_visualizer/test/test_pep257.py deleted file mode 100644 index b234a384..00000000 --- a/src/subsystems/minimal_navigation/nav_visualizer/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(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/src/subsystems/minimal_navigation/todo.md b/src/subsystems/minimal_navigation/todo.md deleted file mode 100644 index 284b0975..00000000 --- a/src/subsystems/minimal_navigation/todo.md +++ /dev/null @@ -1,69 +0,0 @@ -# Navigation Stack — Implementation Progress - -## Legend -- [x] Done -- [ ] Not started - ---- - -## Plan 1 — `gps_pose_publisher` ✅ -- [x] New package: `gps_pose_publisher/` -- [x] WGS84 → ENU via `GeographicLib::LocalCartesian` -- [x] Publishes `/robot_pose` (PoseStamped, frame: map) -- [x] Broadcasts `map → base_link` TF -- [x] Publishes `/gps_status` (String) -- [x] Service `~/latlon_to_enu` (msgs/srv/LatLonToENU) -- [x] `use_start_gate_ref` param — waits for `/start_gate_ref` NavSatFix if true -- [x] Heading conversion: compass deg → ENU rad (`(90 - deg) * π/180`) -- [x] Only publishes once both GPS fix AND heading are available - -## Plan 2 — `ackermann_mppi` modifications ✅ -- [x] Input topic renamed: `/plan` → `/global_path` (transient_local QoS) -- [x] `nav_enabled_topic` param (default `/nav_enabled`) -- [x] Subscribe `std_msgs/Bool` on nav_enabled topic -- [x] `controlLoop()` short-circuits (zero cmd_vel, DEBUG log) when nav disabled -- [x] `std::atomic nav_enabled_` — thread-safe with MultiThreadedExecutor - -## Plan 3 — `athena_smac_planner` + `global_planner` ✅ -- [x] Package rename: `nav2_smac_planner` → `athena_smac_planner` (package.xml + CMakeLists.txt) -- [x] Library rename: `nav2_smac_planner` → `athena_smac_planner` -- [x] Removed nonexistent plugin XML exports from package.xml -- [x] `global_planner_node.cpp` — subscribes `/goal_pose`, `/robot_pose` -- [x] Owns `Costmap2DROS` under `"global_costmap"` namespace (StaticLayer from `/map`) -- [x] Uses `SmacPlannerHybrid::createPlan()` directly (no lifecycle) -- [x] Publishes `/global_path` (transient_local) and `/planner_event` -- [x] Emits `PLAN_FAILED` event on exception; `PLAN_SUCCEEDED` on success -- [x] WARN if `/goal_pose` arrives before `/robot_pose` -- [x] MultiThreadedExecutor with node + costmap node - ---- - -## Plan 4 — `mission_executive` ✅ -- [x] Package: `mission_executive/` (package.xml, CMakeLists.txt) -- [x] Action server `~/navigate_to_target` (NavigateToTarget) -- [x] Services: `~/abort` (Trigger), `~/set_target` (SetTarget), `~/teleop` (SetBool) -- [x] Publishes: `/goal_pose`, `/nav_enabled`, `/nav_mode`, `/active_target`, `/nav_status` -- [x] Subscribes: `/robot_pose`, `/odom`, `/planner_event`, `/global_path` -- [x] States: IDLE, NAVIGATING, ARRIVING, STOPPED_AT_TARGET, ABORTING, RETURNING, STOPPED_AT_RETURN, TELEOP -- [x] Full transition table from PLAN.md §3.2.2 -- [x] Calls `gps_pose_publisher/latlon_to_enu` for coordinate conversion -- [x] MultiThreadedExecutor + reentrant callback group (deadlock-safe) -- [x] Cross-track error replanning: replan when xtrack > `replan_distance_m` -- [x] Stop detection: `|twist.linear| < velocity_zero_threshold` held for `arrival_hold_time` seconds -- [x] Target registry via `~/set_target`; RETURN requires visited target -- [x] Preemption: new GO_TO aborts current active goal - -## Plan 5 — `nav.launch.py` + unified build ✅ -- [x] Package: `nav_bringup/` (package.xml, CMakeLists.txt) -- [x] `config/nav_params.yaml` — single param file for all nodes (from PLAN.md §7) -- [x] `launch/nav.launch.py` — includes `athena_gps/launch/gps_launch.py` -- [x] `/odom` → `/odom/ground_truth` remapped for `mission_executive` + `ackermann_mppi` -- [x] twist_stamper node: `cmd_vel_nav` → `/rear_ackermann_controller/reference` -- [x] Launches: `gps_pose_publisher`, `map_node`, `global_planner`, `ackermann_mppi`, `mission_executive` -- [x] Single `nav_params.yaml` passed to all nodes - ---- - -## Notes -- `msgs` package (at `src/msgs/`) already contains all required message/service/action definitions -- Phase 2 extensions (obstacle layers, ArUco, object detection) are deferred per PLAN.md §5 From 3b54366f4c4ce5fc91d65896a02621bfaf2a0e47 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Mon, 30 Mar 2026 18:47:01 -0400 Subject: [PATCH 04/21] obstacle avoidance --- .../emmn_visualizer/emmn_visualizer_node.py | 74 +++- .../minimal_nav_bringup/CMakeLists.txt | 4 + .../config/nav_params.yaml | 31 +- .../minimal_nav_bringup/launch/nav.launch.py | 46 ++- .../minimal_nav_bringup/package.xml | 1 + .../scripts/reframe_pointcloud.py | 75 ++++ .../vector_field_planner/CMakeLists.txt | 2 + .../vector_field_planner/package.xml | 1 + .../src/vector_field_planner_node.cpp | 321 +++++++++++++++--- 9 files changed, 496 insertions(+), 59 deletions(-) create mode 100755 src/subsystems/even_more_minimal_nav/minimal_nav_bringup/scripts/reframe_pointcloud.py diff --git a/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py b/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py index 3e24dd18..64eb9c4b 100644 --- a/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py +++ b/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py @@ -11,10 +11,10 @@ import numpy as np import rclpy from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, QoSPresetProfiles from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Path, OccupancyGrid -from sensor_msgs.msg import NavSatFix +from sensor_msgs.msg import LaserScan, NavSatFix from std_msgs.msg import Bool, String from msgs.msg import NavStatus, Heading, PlannerEvent @@ -110,6 +110,7 @@ def __init__(self): self.nav_mode = 'stopped' self.heading_msg = None self.costmap = None + self.latest_scan = None # sensor_msgs/LaserScan self.converter = CoordinateConverter() self.event_log = deque(maxlen=10) # (timestamp_str, event_name, is_failure) self.new_path_received = False @@ -136,6 +137,8 @@ def __init__(self): self.create_subscription(PlannerEvent, '/planner_event', self._event_cb, 10) self.create_subscription(NavSatFix, '/gps/fix', self._gps_cb, 10) self.create_subscription(Heading, '/gps/heading', self._heading_cb, 10) + self.create_subscription(LaserScan, '/scan', self._scan_cb, + QoSPresetProfiles.SENSOR_DATA.value) # ── Callbacks ────────────────────────────────────────────────────────────── @@ -186,6 +189,10 @@ def _heading_cb(self, msg): with self.lock: self.heading_msg = msg + def _scan_cb(self, msg): + with self.lock: + self.latest_scan = msg + # ─── Plotting ───────────────────────────────────────────────────────────────── @@ -289,6 +296,10 @@ def __init__(self, node: EMMNVisualizerNode): markersize=12, markeredgewidth=2.5, zorder=9, label='Goal') + # Laser scan hits — scatter of obstacle points in map frame + self.scan_scatter = self.ax_map.scatter( + [], [], s=4, c='#ff6633', alpha=0.65, zorder=8, label='Scan hits') + self.ax_map.legend(loc='lower left', framealpha=0.6, facecolor='#1a1a2e', labelcolor='white', fontsize=9) @@ -311,9 +322,9 @@ def __init__(self, node: EMMNVisualizerNode): ) # ── Checkboxes (bottom strip) ─────────────────────────────────────────── - cb_labels = ['Auto-Fit', 'Show Costmap', 'Show Lookahead', 'Show XTE'] - cb_states = [True, True, True, True] - rax = self.fig.add_axes([0.04, 0.01, 0.30, 0.07]) + cb_labels = ['Auto-Fit', 'Show Costmap', 'Show Lookahead', 'Show XTE', 'Show Scan'] + cb_states = [True, True, True, True, True] + rax = self.fig.add_axes([0.04, 0.01, 0.38, 0.07]) rax.set_facecolor('#1a1a2e') self.check = CheckButtons(rax, cb_labels, cb_states) for txt in self.check.labels: @@ -323,6 +334,7 @@ def __init__(self, node: EMMNVisualizerNode): self.show_costmap = True self.show_lookahead = True self.show_xte = True + self.show_scan = True self.check.on_clicked(self._on_toggle) # ── Events ───────────────────────────────────────────────────────────── @@ -367,6 +379,9 @@ def _on_toggle(self, label): self.show_lookahead = not self.show_lookahead elif label == 'Show XTE': self.show_xte = not self.show_xte + elif label == 'Show Scan': + self.show_scan = not self.show_scan + self.scan_scatter.set_visible(self.show_scan) # ── Hover ────────────────────────────────────────────────────────────────── @@ -395,6 +410,7 @@ def update(self, _frame): nav_mode = self.node.nav_mode heading_msg = self.node.heading_msg costmap = self.node.costmap + latest_scan = self.node.latest_scan event_log = list(self.node.event_log) new_path = self.node.new_path_received new_costmap = self.node.new_costmap_received @@ -469,6 +485,29 @@ def update(self, _frame): self.lookahead_pt.set_data([], []) self.lookahead_line.set_data([], []) + # ── Laser scan hits ────────────────────────────────────────────────────── + # Convert scan from base_link polar coords → map Cartesian and scatter-plot. + # Assumes scan is in base_link frame (target_frame: base_link in p2l config). + if self.show_scan and latest_scan is not None and rx is not None: + sc = latest_scan + pts_x, pts_y = [], [] + for i, r in enumerate(sc.ranges): + if not math.isfinite(r) or r < sc.range_min or r > sc.range_max: + continue + alpha = sc.angle_min + i * sc.angle_increment # angle in base_link + map_x = rx + r * math.cos(yaw + alpha) + map_y = ry + r * math.sin(yaw + alpha) + pts_x.append(map_x) + pts_y.append(map_y) + if pts_x: + self.scan_scatter.set_offsets(np.column_stack([pts_x, pts_y])) + else: + self.scan_scatter.set_offsets(np.empty((0, 2))) + self.scan_scatter.set_visible(True) + else: + self.scan_scatter.set_offsets(np.empty((0, 2))) + self.scan_scatter.set_visible(self.show_scan) + # ── Viewport ───────────────────────────────────────────────────────────── if self.auto_fit and new_path: self._fit_viewport(path_poses, rx, ry) @@ -477,7 +516,8 @@ def update(self, _frame): # ── Status panel ───────────────────────────────────────────────────────── self._update_status(nav_status, nav_enabled, nav_mode, event_log, - heading_msg, costmap, path_poses, rx, ry) + heading_msg, costmap, path_poses, rx, ry, + latest_scan=latest_scan) # ── Costmap renderer ───────────────────────────────────────────────────── @@ -539,7 +579,7 @@ def _fit_viewport(self, path_poses, rx, ry): # ── Status panel text ──────────────────────────────────────────────────────── def _update_status(self, nav_status, nav_enabled, nav_mode, event_log, - heading_msg, costmap, path_poses, rx, ry): + heading_msg, costmap, path_poses, rx, ry, latest_scan=None): state = nav_status.state if nav_status else 'NO DATA' color = STATE_COLORS.get(state, DEFAULT_STATE_COLOR) @@ -611,6 +651,26 @@ def _update_status(self, nav_status, nav_enabled, nav_mode, event_log, lines.append(' (no robot pose)') lines.append('') + # Laser scan stats + lines.append('── Laser Scan ───────────────') + if latest_scan is not None: + sc = latest_scan + valid_ranges = [r for r in sc.ranges + if math.isfinite(r) and sc.range_min <= r <= sc.range_max] + close_ranges = [r for r in valid_ranges if r < 3.0] + min_r = min(valid_ranges) if valid_ranges else float('nan') + node_now = self.node.get_clock().now().to_msg() + stamp = sc.header.stamp + age_s = (node_now.sec - stamp.sec) + (node_now.nanosec - stamp.nanosec) * 1e-9 + lines.append(f' Frame : {sc.header.frame_id}') + lines.append(f' Age : {age_s:>6.2f}s') + lines.append(f' Beams : {len(sc.ranges)} total / {len(valid_ranges)} valid') + lines.append(f' Min rng : {min_r:>6.2f}m') + lines.append(f' <3m hits: {len(close_ranges)}') + else: + lines.append(' (no scan received)') + lines.append('') + # Event log lines.append('── Planner Events ───────────') if event_log: diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt index 57406adb..06c6db0e 100644 --- a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt +++ b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt @@ -7,4 +7,8 @@ install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) +install(PROGRAMS scripts/reframe_pointcloud.py + DESTINATION lib/${PROJECT_NAME} +) + ament_package() diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml index b0c00234..a170f50c 100644 --- a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml +++ b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml @@ -49,6 +49,26 @@ mission_executive: active_target_topic: "/active_target" nav_status_topic: "/nav_status" +# ── pointcloud_to_laserscan ─────────────────────────────────────────────────── +# Converts the ZED stereo camera's registered point cloud into a 2D laser scan +# used by the vector_field_planner for obstacle avoidance. +# Input topic remapped to /zed/zed_node/point_cloud/cloud_registered in nav.launch.py. +pointcloud_to_laserscan: + ros__parameters: + use_sim_time: true + target_frame: base_link # cloud arrives already reframed to base_link by reframe_pointcloud + transform_tolerance: 0.01 + min_height: 0.1 # ignore ground returns (m above sensor origin) + max_height: 1.5 # ignore returns above rover body + angle_min: -1.5708 # -90 deg (left side of forward arc) + angle_max: 1.5708 # 90 deg (right side of forward arc) + angle_increment: 0.00873 # 0.5 deg resolution + scan_time: 0.05 # 20 Hz to match planner + range_min: 0.3 # ignore very close returns (ZED blind zone) + range_max: 15.0 # maximum useful range for obstacle avoidance + use_inf: true # emit inf for beams with no return (not 0) + inf_epsilon: 1e-9 + # ── vector_field_planner ────────────────────────────────────────────────────── vector_field_planner: ros__parameters: @@ -61,7 +81,14 @@ vector_field_planner: max_steering_angle_rad: 0.5 # radians — clamp on angular.z output lookahead_dist_m: 3.0 # pure-pursuit lookahead distance k_p_steering: 1.5 # proportional gain: angular.z = k_p * heading_error - repulsion_gain: 0.0 # Phase 2: obstacle repulsion magnitude (0 = disabled) - repulsion_cutoff_m: 3.0 # Phase 2: distance at which repulsion activates + # ── Obstacle avoidance ────────────────────────────────────────────────── + obstacle_avoidance_enabled: true # set true or publish to /obstacle_avoidance_enabled + scan_topic: "/scan" # LaserScan source (from pointcloud_to_laserscan) + scan_max_age_s: 0.5 # reject scans older than this (seconds) + repulsion_gain: 2.5 # lateral force scale; tune up to ~1.5 for strong avoidance + repulsion_cutoff_m: 5.0 # obstacles beyond this range are ignored (meters) + obstacle_memory_time_s: 3.0 # how long (s) to remember obstacle hits after they leave the scan FoV + obstacle_max_points: 2000 # cap on buffered map-frame obstacle points + # ──────────────────────────────────────────────────────────────────────── goal_tolerance_m: 1.5 # distance to path end that counts as "arrived" publish_debug_markers: true diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py index 7cb292f3..c4dcf531 100644 --- a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py +++ b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py @@ -21,16 +21,18 @@ gps_pose_publisher : WGS84→ENU, /robot_pose, map→base_link TF dem_costmap_converter : DEM GeoTIFF → nav_msgs/OccupancyGrid /map global_planner : /goal_pose → /global_path (straight-line or A*) - vector_field_planner : /global_path → /cmd_vel (pure-pursuit) + vector_field_planner : /global_path → /cmd_vel (pure-pursuit + obstacle avoidance) mission_executive : state machine, action/service operator interface + pointcloud_to_laserscan : /zed/.../cloud_registered → /scan (for obstacle avoidance) Topic graph ─────────── - gps_pose_publisher → TF map→base_link - dem_costmap_converter → /map - mission_executive → /nav_enabled, /goal_pose - global_planner → /global_path - vector_field_planner → /cmd_vel, ~/debug_markers + gps_pose_publisher → TF map→base_link + dem_costmap_converter → /map + mission_executive → /nav_enabled, /goal_pose + global_planner → /global_path + pointcloud_to_laserscan → /scan + vector_field_planner → /cmd_vel, ~/debug_markers """ import os @@ -126,12 +128,44 @@ def generate_launch_description(): parameters=[nav_params_file], ) + # ── reframe_pointcloud (relabels ZED frame_id → base_link, sim only) ──────── + # The ZED driver in simulation publishes the cloud under + # athena/base_footprint/zed_depth_sensor. pointcloud_to_laserscan needs a + # consistent, simple frame name so it can produce a valid scan without any + # TF lookup. This relay just rewrites header.frame_id in-place. + reframe_pointcloud_node = Node( + package='minimal_nav_bringup', + executable='reframe_pointcloud.py', + name='reframe_pointcloud', + output='screen', + parameters=[{ + 'input_topic': '/zed/zed_node/point_cloud/cloud_registered', + 'output_topic': '/zed/cloud_base_link', + 'target_frame': 'base_link', + }], + ) + + # ── pointcloud_to_laserscan (ZED PointCloud2 → LaserScan for obstacle avoidance) ── + pointcloud_to_laserscan_node = Node( + package='pointcloud_to_laserscan', + executable='pointcloud_to_laserscan_node', + name='pointcloud_to_laserscan', + output='screen', + parameters=[nav_params_file], + remappings=[ + ('cloud_in', '/zed/cloud_base_link'), # reframed cloud, no TF needed + ('scan', '/scan'), + ], + ) + return LaunchDescription([ sim_arg, gps_launch, gps_pose_publisher_node, dem_costmap_converter_node, global_planner_node, + reframe_pointcloud_node, + pointcloud_to_laserscan_node, vector_field_planner_node, mission_executive_node, ]) diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml index 052734d8..7d12a733 100644 --- a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml +++ b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml @@ -15,6 +15,7 @@ vector_field_planner mission_executive athena_gps + pointcloud_to_laserscan ament_cmake diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/scripts/reframe_pointcloud.py b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/scripts/reframe_pointcloud.py new file mode 100755 index 00000000..662ab417 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/scripts/reframe_pointcloud.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 +# Copyright (c) 2025 UMD Loop +# +# 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. + +"""reframe_pointcloud.py + +Subscribes to a PointCloud2 topic and republishes each message with +header.frame_id replaced by a target frame name. + +This is used in simulation to relabel the ZED depth sensor frame +(athena/base_footprint/zed_depth_sensor) as base_link so that +pointcloud_to_laserscan needs no TF lookup. + +Parameters +---------- + input_topic (str, default /zed/zed_node/point_cloud/cloud_registered) + output_topic (str, default /zed/cloud_base_link) + target_frame (str, default base_link) +""" + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSPresetProfiles +from sensor_msgs.msg import PointCloud2 + + +class ReframePointcloud(Node): + def __init__(self): + super().__init__('reframe_pointcloud') + + self.declare_parameter('input_topic', '/zed/zed_node/point_cloud/cloud_registered') + self.declare_parameter('output_topic', '/zed/cloud_base_link') + self.declare_parameter('target_frame', 'base_link') + + input_topic = self.get_parameter('input_topic').get_parameter_value().string_value + output_topic = self.get_parameter('output_topic').get_parameter_value().string_value + self.target_frame = self.get_parameter('target_frame').get_parameter_value().string_value + + self.pub = self.create_publisher(PointCloud2, output_topic, + QoSPresetProfiles.SENSOR_DATA.value) + + self.sub = self.create_subscription(PointCloud2, input_topic, + self._cb, + QoSPresetProfiles.SENSOR_DATA.value) + + self.get_logger().info( + f'reframe_pointcloud: {input_topic} → {output_topic} ' + f'(frame_id → {self.target_frame})' + ) + + def _cb(self, msg: PointCloud2): + msg.header.frame_id = self.target_frame + self.pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = ReframePointcloud() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt index 81a6864a..ea2b8585 100644 --- a/src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt +++ b/src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -20,6 +21,7 @@ target_link_libraries(vector_field_planner_node rclcpp::rclcpp ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} ${std_msgs_TARGETS} ${visualization_msgs_TARGETS} tf2_ros::tf2_ros diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml b/src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml index 70bfa788..8a9f1d96 100644 --- a/src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml +++ b/src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml @@ -15,6 +15,7 @@ rclcpp geometry_msgs nav_msgs + sensor_msgs std_msgs visualization_msgs tf2_ros diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp b/src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp index 37c3c2a5..d456b03e 100644 --- a/src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp +++ b/src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp @@ -14,12 +14,14 @@ // vector_field_planner_node.cpp // -// Pure-pursuit path follower for an Ackermann rover. +// Pure-pursuit path follower for an Ackermann rover with optional obstacle avoidance. // // Inputs: -// /global_path (nav_msgs/Path, transient_local) — from global_planner -// /nav_enabled (std_msgs/Bool, reliable) — from mission_executive -// TF map→base_link — current robot pose +// /global_path (nav_msgs/Path, transient_local) — from global_planner +// /nav_enabled (std_msgs/Bool, reliable) — from mission_executive +// /obstacle_avoidance_enabled (std_msgs/Bool, reliable) — runtime avoidance toggle +// /scan (sensor_msgs/LaserScan) — from pointcloud_to_laserscan +// TF map→base_link — current robot pose // // Outputs: // /cmd_vel (geometry_msgs/Twist) — linear.x + angular.z @@ -32,22 +34,38 @@ // 3. Find the lookahead point: walk forward from the closest path pose // until a point is >= lookahead_dist_m from the robot. // 4. Compute heading error from current yaw to the lookahead direction. -// 5. angular.z = k_p_steering * heading_error, clamped to +-max_steering_angle_rad. -// 6. linear.x = max_speed_mps (constant; slow-down near goal is left for a later pass). +// 5. If obstacle_avoidance_enabled and repulsion_gain > 0: +// - On each scan callback, transform every hit within repulsion_cutoff_m +// into map frame and store it in obstacle_map_ with a timestamp. +// - Each tick, evict points older than obstacle_memory_time_s. +// - For each remaining point within repulsion_cutoff_m: +// repulsion direction = unit vector from obstacle to robot +// lateral component = project onto robot's left axis (-sin(yaw), cos(yaw)) +// weight = (cutoff - dist) / cutoff +// lateral_sum += lateral * weight +// - repulsion_steering = repulsion_gain * lateral_sum +// - Total steering = k_p_steering * heading_err + repulsion_steering +// 6. angular.z = clamped total steering, linear.x = max_speed_mps. // -// repulsion_gain / repulsion_cutoff_m are declared for completeness but not -// yet implemented — they will be used in a future phase when a local costmap -// or laser scan is available. - +// Obstacle memory design: +// Hits are stored in map frame, not sensor frame. This means the repulsion +// direction is recomputed from correct world-frame geometry every tick, +// regardless of how the robot has turned since the hit was recorded. +// The robot will not "forget" an obstacle just because it rotated away from it, +// and the steering magnitude correctly reflects the actual geometric clearance. + +#include #include #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/path.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" #include "std_msgs/msg/bool.hpp" #include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -55,6 +73,12 @@ #include "tf2_ros/transform_listener.h" #include "tf2/exceptions.h" +struct ObstaclePoint +{ + double x, y; // map frame + rclcpp::Time stamp; +}; + class VectorFieldPlanner : public rclcpp::Node { public: @@ -68,21 +92,33 @@ class VectorFieldPlanner : public rclcpp::Node declare_parameter("max_steering_angle_rad", 0.5); declare_parameter("lookahead_dist_m", 3.0); declare_parameter("k_p_steering", 1.5); - declare_parameter("repulsion_gain", 0.0); // reserved — Phase 2 - declare_parameter("repulsion_cutoff_m", 3.0); // reserved — Phase 2 - declare_parameter("goal_tolerance_m", 1.5); - declare_parameter("publish_debug_markers", true); - - map_frame_ = get_parameter("map_frame").as_string(); - base_frame_ = get_parameter("base_frame").as_string(); - cmd_vel_topic_ = get_parameter("cmd_vel_topic").as_string(); - tf_timeout_s_ = get_parameter("tf_timeout_s").as_double(); - max_speed_mps_ = get_parameter("max_speed_mps").as_double(); - max_steering_angle_rad_= get_parameter("max_steering_angle_rad").as_double(); - lookahead_dist_m_ = get_parameter("lookahead_dist_m").as_double(); - k_p_steering_ = get_parameter("k_p_steering").as_double(); - goal_tolerance_m_ = get_parameter("goal_tolerance_m").as_double(); - publish_debug_markers_ = get_parameter("publish_debug_markers").as_bool(); + declare_parameter("repulsion_gain", 0.5); + declare_parameter("repulsion_cutoff_m", 3.0); + declare_parameter("obstacle_memory_time_s", 3.0); + declare_parameter("obstacle_max_points", 2000); + declare_parameter("obstacle_avoidance_enabled", false); + declare_parameter("scan_topic", std::string("/scan")); + declare_parameter("scan_max_age_s", 0.5); + declare_parameter("goal_tolerance_m", 1.5); + declare_parameter("publish_debug_markers", true); + + map_frame_ = get_parameter("map_frame").as_string(); + base_frame_ = get_parameter("base_frame").as_string(); + cmd_vel_topic_ = get_parameter("cmd_vel_topic").as_string(); + tf_timeout_s_ = get_parameter("tf_timeout_s").as_double(); + max_speed_mps_ = get_parameter("max_speed_mps").as_double(); + max_steering_angle_rad_ = get_parameter("max_steering_angle_rad").as_double(); + lookahead_dist_m_ = get_parameter("lookahead_dist_m").as_double(); + k_p_steering_ = get_parameter("k_p_steering").as_double(); + repulsion_gain_ = get_parameter("repulsion_gain").as_double(); + repulsion_cutoff_m_ = get_parameter("repulsion_cutoff_m").as_double(); + obstacle_memory_time_s_ = get_parameter("obstacle_memory_time_s").as_double(); + obstacle_max_points_ = get_parameter("obstacle_max_points").as_int(); + obstacle_avoidance_enabled_ = get_parameter("obstacle_avoidance_enabled").as_bool(); + scan_topic_ = get_parameter("scan_topic").as_string(); + scan_max_age_s_ = get_parameter("scan_max_age_s").as_double(); + goal_tolerance_m_ = get_parameter("goal_tolerance_m").as_double(); + publish_debug_markers_ = get_parameter("publish_debug_markers").as_bool(); tf_buffer_ = std::make_shared(get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -101,6 +137,7 @@ class VectorFieldPlanner : public rclcpp::Node last_closest_idx_ = std::numeric_limits::max(); tick_count_ = 0; consecutive_clamped_ = 0; + obstacle_map_.clear(); // Log path quality — sparse paths (spacing > lookahead_dist_m) trigger the // findLookahead-behind-robot bug documented in the unit tests. @@ -136,18 +173,120 @@ class VectorFieldPlanner : public rclcpp::Node tick_count_ = 0; consecutive_clamped_ = 0; stuck_ticks_ = 0; + obstacle_map_.clear(); publishStop(); } else if (!was_enabled && nav_enabled_) { RCLCPP_INFO(get_logger(), "[NAV_ENABLED] starting navigation"); } }); + obstacle_avoidance_sub_ = create_subscription( + "/obstacle_avoidance_enabled", rclcpp::QoS(1).reliable(), + [this](const std_msgs::msg::Bool::SharedPtr msg) { + const bool was_enabled = obstacle_avoidance_enabled_; + obstacle_avoidance_enabled_ = msg->data; + if (was_enabled != obstacle_avoidance_enabled_) { + RCLCPP_INFO(get_logger(), "[AVOIDANCE_%s]", + obstacle_avoidance_enabled_ ? "ENABLED" : "DISABLED"); + } + }); + + scan_sub_ = create_subscription( + scan_topic_, rclcpp::SensorDataQoS(), + [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) { + ++scan_msg_count_; + // Log details on the first scan and every 100th thereafter + if (scan_msg_count_ == 1 || scan_msg_count_ % 100 == 0) { + float min_r = std::numeric_limits::infinity(); + int valid = 0; + for (float r : msg->ranges) { + if (r >= msg->range_min && r <= msg->range_max) { + min_r = std::min(min_r, r); + ++valid; + } + } + RCLCPP_INFO(get_logger(), + "[SCAN #%u] frame='%s' beams=%zu angle=[%.2f,%.2f]rad" + " range=[%.2f,%.2f]m valid_beams=%d min_range=%.2fm cutoff=%.2fm", + scan_msg_count_, + msg->header.frame_id.c_str(), + msg->ranges.size(), + msg->angle_min, msg->angle_max, + msg->range_min, msg->range_max, + valid, + std::isinf(min_r) ? -1.0f : min_r, + static_cast(repulsion_cutoff_m_)); + } + latest_scan_ = msg; + + // Transform hits into map frame and add to the obstacle buffer. + // Storing in map frame means repulsion geometry stays correct after + // the robot turns — we don't lose obstacles that have left the FoV. + if (!obstacle_avoidance_enabled_) return; + + geometry_msgs::msg::TransformStamped tf_to_map; + try { + tf_to_map = tf_buffer_->lookupTransform( + map_frame_, msg->header.frame_id, + tf2::TimePointZero, + tf2::durationFromSec(tf_timeout_s_)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "[SCAN_TF] %s→%s failed: %s — skipping buffer update", + msg->header.frame_id.c_str(), map_frame_.c_str(), ex.what()); + return; + } + + const double ox = tf_to_map.transform.translation.x; + const double oy = tf_to_map.transform.translation.y; + const auto & qm = tf_to_map.transform.rotation; + const double map_yaw = std::atan2( + 2.0 * (qm.w * qm.z + qm.x * qm.y), + 1.0 - 2.0 * (qm.y * qm.y + qm.z * qm.z)); + const double cos_my = std::cos(map_yaw); + const double sin_my = std::sin(map_yaw); + + const rclcpp::Time stamp = now(); + for (size_t i = 0; i < msg->ranges.size(); ++i) { + const float r = msg->ranges[i]; + if (r < msg->range_min || r > msg->range_max) continue; + if (static_cast(r) >= repulsion_cutoff_m_) continue; + const double alpha = msg->angle_min + + static_cast(i) * msg->angle_increment; + const double bx = r * std::cos(alpha); + const double by = r * std::sin(alpha); + obstacle_map_.push_back({ + ox + bx * cos_my - by * sin_my, + oy + bx * sin_my + by * cos_my, + stamp}); + } + + // Cap buffer size — evict oldest entries if over limit + if (static_cast(obstacle_map_.size()) > obstacle_max_points_) { + const size_t excess = + obstacle_map_.size() - static_cast(obstacle_max_points_); + obstacle_map_.erase(obstacle_map_.begin(), obstacle_map_.begin() + excess); + } + }); + // 20 Hz control loop timer_ = create_wall_timer( std::chrono::milliseconds(50), [this]() { controlLoop(); }); - RCLCPP_INFO(get_logger(), "VectorFieldPlanner ready"); + // ── Startup parameter summary ──────────────────────────────────────────── + RCLCPP_INFO(get_logger(), + "VectorFieldPlanner ready " + "speed=%.2f steer_max=%.3f lookahead=%.1fm kp=%.2f goal_tol=%.1fm", + max_speed_mps_, max_steering_angle_rad_, lookahead_dist_m_, + k_p_steering_, goal_tolerance_m_); + RCLCPP_INFO(get_logger(), + "[AVOIDANCE CONFIG] enabled=%d scan_topic='%s' cutoff=%.2fm gain=%.3f" + " memory_time=%.1fs max_points=%d — publish true/false to /obstacle_avoidance_enabled to toggle", + static_cast(obstacle_avoidance_enabled_), + scan_topic_.c_str(), + repulsion_cutoff_m_, repulsion_gain_, + obstacle_memory_time_s_, obstacle_max_points_); } private: @@ -244,8 +383,93 @@ class VectorFieldPlanner : public rclcpp::Node while (heading_err > M_PI) heading_err -= 2.0 * M_PI; while (heading_err < -M_PI) heading_err += 2.0 * M_PI; - // Proportional steering, clamped to physical limits - const double steering_unclamped = k_p_steering_ * heading_err; + // ── Obstacle avoidance (map-frame buffer) ───────────────────────────────── + // Evict obstacle points older than obstacle_memory_time_s_. + // This runs every tick regardless of avoidance toggle so the buffer + // doesn't grow unboundedly when avoidance is toggled off. + const rclcpp::Time now_time = now(); + obstacle_map_.erase( + std::remove_if(obstacle_map_.begin(), obstacle_map_.end(), + [&](const ObstaclePoint & p) { + return (now_time - p.stamp).seconds() > obstacle_memory_time_s_; + }), + obstacle_map_.end()); + + double repulsion_steering = 0.0; + + if (obstacle_avoidance_enabled_) { + if (repulsion_gain_ <= 0.0) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "[AVOIDANCE] avoidance enabled but repulsion_gain=%.3f — no effect. " + "Set repulsion_gain > 0 in nav_params.yaml.", repulsion_gain_); + } else if (!latest_scan_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "[AVOIDANCE] avoidance enabled but NO scan received yet on '%s'. " + "Check pointcloud_to_laserscan is running and publishing.", + scan_topic_.c_str()); + } else { + const double scan_age = (now_time - latest_scan_->header.stamp).seconds(); + if (scan_age > scan_max_age_s_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "[AVOIDANCE] scan stale: age=%.3fs > max=%.2fs — obstacle buffer may be outdated.", + scan_age, scan_max_age_s_); + } + + // Recompute repulsion from all buffered map-frame obstacle points. + // For each point within repulsion_cutoff_m: + // unit_repulsion = (robot - obstacle) / dist (push away from obstacle) + // lateral = unit_repulsion · (-sin(yaw), cos(yaw)) + // weight = (cutoff - dist) / cutoff + // lateral_sum += lateral * weight + double lateral_sum = 0.0; + double lateral_left = 0.0; + double lateral_right = 0.0; + int active_points = 0; + float closest_r = std::numeric_limits::infinity(); + + for (const auto & p : obstacle_map_) { + const double dx = rx - p.x; + const double dy = ry - p.y; + const double dist = std::hypot(dx, dy); + if (dist >= repulsion_cutoff_m_ || dist < 1e-6) continue; + closest_r = std::min(closest_r, static_cast(dist)); + const double weight = (repulsion_cutoff_m_ - dist) / repulsion_cutoff_m_; + // Project unit repulsion vector onto robot's left lateral axis + const double lateral = + (dx / dist) * (-std::sin(yaw)) + (dy / dist) * std::cos(yaw); + lateral_sum += lateral * weight; + ++active_points; + if (lateral > 0.0) lateral_left += lateral * weight; + else lateral_right += lateral * weight; + } + + repulsion_steering = repulsion_gain_ * lateral_sum; + + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, + "[AVOIDANCE age=%.3fs] buffered=%zu in_range=%d closest=%.2fm " + "lateral_sum=%.3f left=%.3f right=%.3f rep_steer=%.4f", + scan_age, + obstacle_map_.size(), active_points, + std::isinf(closest_r) ? -1.0f : closest_r, + lateral_sum, lateral_left, lateral_right, + repulsion_steering); + + if (active_points > 0) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 250, + "[AVOIDANCE ACTIVE] %d pts within %.1fm " + "left=%.3f right=%.3f rep_steer=%.4f path_steer=%.4f", + active_points, repulsion_cutoff_m_, + lateral_left, lateral_right, + repulsion_steering, k_p_steering_ * heading_err); + } + } + } else { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 10000, + "[AVOIDANCE OFF] publish 'true' to /obstacle_avoidance_enabled to activate"); + } + + // Proportional path-following steering + repulsion, clamped to physical limits + const double steering_unclamped = k_p_steering_ * heading_err + repulsion_steering; const double steering = std::clamp( steering_unclamped, -max_steering_angle_rad_, max_steering_angle_rad_); @@ -256,28 +480,28 @@ class VectorFieldPlanner : public rclcpp::Node if (consecutive_clamped_ == 40) { // 2 seconds saturated RCLCPP_WARN(get_logger(), "[STEER_SAT] steering saturated for %u consecutive ticks (%.1f s). " - "err=%.1fd unclamped=%.3f max=%.3f. " - "Consider increasing max_steering_angle_rad or k_p_steering.", + "err=%.1fd repulsion=%.3f unclamped=%.3f max=%.3f. " + "Consider increasing max_steering_angle_rad or reducing repulsion_gain.", consecutive_clamped_, consecutive_clamped_ / 20.0, heading_err * 180.0 / M_PI, - steering_unclamped, max_steering_angle_rad_); + repulsion_steering, steering_unclamped, max_steering_angle_rad_); } } else { consecutive_clamped_ = 0; } // ── Single structured log line (4 Hz) — paste directly into an LLM ────── - // Fields: tick pos yaw goal_dist path_idx lookahead lk_fwd_dot heading_err steer flags RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 250, "[NAV t=%u pos=(%.2f,%.2f) yaw=%.1fd goal=%.2fm idx=%zu/%zu " - "lk=(%.2f,%.2f) lk_dist=%.2fm fwd=%.2f err=%.1fd steer=%.3f%s%s]", + "lk=(%.2f,%.2f) lk_dist=%.2fm fwd=%.2f err=%.1fd rep=%.3f steer=%.3f%s%s%s]", tick_count_, rx, ry, yaw * 180.0 / M_PI, dist_to_goal, closest_idx, path_->poses.size() - 1, lx, ly, lookahead_dist, fwd_dot, - heading_err * 180.0 / M_PI, steering, + heading_err * 180.0 / M_PI, repulsion_steering, steering, clamped ? " CLAMPED" : "", - lk_behind ? " LK_BEHIND" : ""); + lk_behind ? " LK_BEHIND" : "", + (obstacle_avoidance_enabled_ && repulsion_gain_ > 0.0) ? " AVOID_ON" : ""); geometry_msgs::msg::TwistStamped cmd; cmd.header.stamp = now(); @@ -309,9 +533,6 @@ class VectorFieldPlanner : public rclcpp::Node } } - // Warn if the closest index jumped backwards — a common cause of - // oscillation on turns when the robot is equidistant to path segments - // before and after the apex. if (best < last_closest_idx_ && last_closest_idx_ != std::numeric_limits::max()) { RCLCPP_WARN(get_logger(), "[closest_idx] BACKWARDS JUMP: %zu → %zu (dist=%.3f m) — " @@ -401,6 +622,13 @@ class VectorFieldPlanner : public rclcpp::Node double max_steering_angle_rad_; double lookahead_dist_m_; double k_p_steering_; + double repulsion_gain_; + double repulsion_cutoff_m_; + double obstacle_memory_time_s_; + int obstacle_max_points_; + bool obstacle_avoidance_enabled_; + std::string scan_topic_; + double scan_max_age_s_; double goal_tolerance_m_; bool publish_debug_markers_; @@ -413,15 +641,20 @@ class VectorFieldPlanner : public rclcpp::Node rclcpp::Publisher::SharedPtr marker_pub_; rclcpp::Subscription::SharedPtr path_sub_; rclcpp::Subscription::SharedPtr nav_enabled_sub_; + rclcpp::Subscription::SharedPtr obstacle_avoidance_sub_; + rclcpp::Subscription::SharedPtr scan_sub_; rclcpp::TimerBase::SharedPtr timer_; // ── State ─────────────────────────────────────────────────────────────────── - nav_msgs::msg::Path::SharedPtr path_; - bool nav_enabled_{false}; - size_t last_closest_idx_{std::numeric_limits::max()}; - unsigned int tick_count_{0}; - unsigned int consecutive_clamped_{0}; - unsigned int stuck_ticks_{0}; + nav_msgs::msg::Path::SharedPtr path_; + sensor_msgs::msg::LaserScan::SharedPtr latest_scan_; + bool nav_enabled_{false}; + size_t last_closest_idx_{std::numeric_limits::max()}; + unsigned int tick_count_{0}; + unsigned int consecutive_clamped_{0}; + unsigned int stuck_ticks_{0}; + unsigned int scan_msg_count_{0}; + std::vector obstacle_map_; }; int main(int argc, char ** argv) From 0557e0a1e869a53bbf82bb06abf35f4c786f76a6 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Mon, 30 Mar 2026 19:02:34 -0400 Subject: [PATCH 05/21] spiral --- .../aruco_detection/__init__.py | 0 .../aruco_detection/aruco_node.py | 151 +++++++++++++ .../aruco_detection/correction_node.py | 103 +++++++++ .../aruco_detection/dummy_BB_node.py | 47 ++++ .../aruco_detection/package.xml | 23 ++ .../aruco_detection/resource/aruco_detection | 0 .../aruco_detection/setup.cfg | 4 + .../aruco_detection/setup.py | 28 +++ .../aruco_detection/test/test_copyright.py | 25 +++ .../aruco_detection/test/test_flake8.py | 25 +++ .../aruco_detection/test/test_pep257.py | 23 ++ .../mission_executive/CMakeLists.txt | 2 + .../mission_executive/package.xml | 1 + .../src/mission_executive_node.cpp | 203 +++++++++++++++++- .../yolo_ros/package.xml | 29 +++ .../yolo_ros/resource/yolo_ros | 0 .../even_more_minimal_nav/yolo_ros/setup.cfg | 4 + .../even_more_minimal_nav/yolo_ros/setup.py | 28 +++ .../yolo_ros/test/test_copyright.py | 25 +++ .../yolo_ros/test/test_flake8.py | 25 +++ .../yolo_ros/test/test_pep257.py | 23 ++ .../yolo_ros/yolo_ros/__init__.py | 0 .../yolo_ros/yolo_ros/image_publisher.py | 61 ++++++ .../yolo_ros/yolo_ros/img_inference.py | 120 +++++++++++ .../yolo_ros/yolo_ros/my_node.py | 6 + 25 files changed, 947 insertions(+), 9 deletions(-) create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/__init__.py create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/aruco_node.py create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/correction_node.py create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/dummy_BB_node.py create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/package.xml create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/resource/aruco_detection create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/setup.cfg create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/setup.py create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/test/test_copyright.py create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/test/test_flake8.py create mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/test/test_pep257.py create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/package.xml create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/resource/yolo_ros create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/setup.cfg create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/setup.py create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/test/test_copyright.py create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/test/test_flake8.py create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/test/test_pep257.py create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/__init__.py create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/image_publisher.py create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/img_inference.py create mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/my_node.py diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/__init__.py b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/aruco_node.py b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/aruco_node.py new file mode 100644 index 00000000..716bc96b --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/aruco_node.py @@ -0,0 +1,151 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import String +from cv_bridge import CvBridge +import cv2 +from vision_msgs.msg import Detection2D + + +class ZedArUcoNode(Node): + def __init__(self): + super().__init__('zed_aruco_node') + + self.declare_parameter("sim", False) + self.sim = self.get_parameter("sim").get_parameter_value().bool_value + + if self.sim: + image_topic = '/camera' + depth_topic = '/depth_camera' + else: + image_topic = '/zed/zed_node/left_gray/image_rect_gray' + depth_topic = '/zed/zed_node/depth/depth_registered' + + self.get_logger().info(f"sim={self.sim}") + self.get_logger().info(f"Subscribing to image feed: {image_topic}") + self.get_logger().info(f"Subscribing to depth image feed: {depth_topic}") + + # Initialize CvBridge to convert ROS images to OpenCV + self.bridge = CvBridge() + + # Subscribe to the ZED camera image and depth topics + self.image_sub = self.create_subscription( + Image, image_topic, self.image_callback, 10 + ) + self.depth_sub = self.create_subscription( + Image, depth_topic, self.depth_callback, 10 + ) + + self.image_pub = self.create_publisher(Image, 'aruco_annotated_img', 10) + self.tag_pub = self.create_publisher(Detection2D, 'aruco_loc', 10) + self.depth_pub = self.create_publisher(String, 'aruco_depth', 10) + + self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) + + try: + self.aruco_params = cv2.aruco.DetectorParameters() + except AttributeError: + self.aruco_params = cv2.aruco.DetectorParameters_create() + + self.corners = None + self.marker_id = None + self.latest_depth_map = None + + def image_callback(self, msg): + frame = self.bridge.imgmsg_to_cv2(msg, "mono8") + frame_color = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) + + # Process image (ArUco detection, etc.) + self.detect_aruco_markers(frame) + + # If we have a valid marker and a valid depth map, publish the information + if self.marker_id is not None and self.corners is not None: + + for i in range(4): + start_point = tuple(map(int, self.corners[i])) + end_point = tuple(map(int, self.corners[(i + 1) % 4])) # Connect to next corner + cv2.line(frame_color, start_point, end_point, (0, 255, 0), 2) # Draw green lines + + if self.corners is not None: + text_pos = tuple(map(int, self.corners[0])) + + cv2.putText( + frame_color, + f"id: {self.marker_id}", + (text_pos[0], text_pos[1] - 20), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (0, 0, 255), + 2, + cv2.LINE_AA + ) + + if not self.sim: + if self.latest_depth_map is not None: + depth_values = [ + self.latest_depth_map[int(self.corners[0][1]), int(self.corners[0][0])], # Top-left + self.latest_depth_map[int(self.corners[1][1]), int(self.corners[1][0])], # Top-right + self.latest_depth_map[int(self.corners[2][1]), int(self.corners[2][0])], # Bottom-right + self.latest_depth_map[int(self.corners[3][1]), int(self.corners[3][0])] # Bottom-left + ] + + depth_avg = sum(depth_values) / len(depth_values) + depth_message = String() + depth_message.data = str(depth_avg) + self.depth_pub.publish(depth_message) + else: + self.get_logger().warn("Depth map not received yet, skipping depth calc") + + # Publish tag bounding box as vision_msgs/Detection2D + message = Detection2D() + message.header = msg.header # keep same timestamp/frame as the image + + # Extract x and y coordinates from all four corners + x_values = [float(pt[0]) for pt in self.corners] + y_values = [float(pt[1]) for pt in self.corners] + + x_min = min(x_values) + y_min = min(y_values) + x_max = max(x_values) + y_max = max(y_values) + + # Detection2D bbox is defined by center pose (Pose2D) + size_x/size_y in pixels + message.bbox.center.position.x = (x_min + x_max) * 0.5 + message.bbox.center.position.x = (y_min + y_max) * 0.5 + message.bbox.center.theta = 0.0 # axis-aligned bbox + message.bbox.size_x = (x_max - x_min) + message.bbox.size_y = (y_max - y_min) + + # message.results can be left empty (CorrectionNode only needs bbox) + self.tag_pub.publish(message) + + + annotated_msg = self.bridge.cv2_to_imgmsg(frame_color, encoding="bgr8") + annotated_msg.header = msg.header + self.image_pub.publish(annotated_msg) + + + def depth_callback(self, msg): + # Convert ROS Image to depth map + self.latest_depth_map = self.bridge.imgmsg_to_cv2(msg, "32FC1") + + def detect_aruco_markers(self, frame): + corners, ids, _ = cv2.aruco.detectMarkers(frame, self.aruco_dict, parameters=self.aruco_params) + + if ids is not None: + for i in range(len(ids)): + self.marker_id = ids[i][0] + self.corners = corners[i][0] + else: + self.marker_id = None + self.corners = None + +def main(args=None): + rclpy.init(args=args) + node = ZedArUcoNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/correction_node.py b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/correction_node.py new file mode 100644 index 00000000..287d72e0 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/correction_node.py @@ -0,0 +1,103 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import TwistStamped +from vision_msgs.msg import Detection2D +from sensor_msgs.msg import Image + +class CorrectionNode(Node): + def __init__(self): + super().__init__('correction_node') + + self.declare_parameter("sim", False) + self.sim = self.get_parameter("sim").get_parameter_value().bool_value + + if self.sim: + twist_topic = '/ackermann_steering_controller/reference' + image_topic = '/camera' + else: + twist_topic = '/ackermann_steering_controller/reference' # TO-DO: add actual topic name + image_topic = '/zed/zed_node/left_gray/image_rect_gray' + + self.get_logger().info(f"sim={self.sim}") + self.get_logger().info(f"Publishing twist messages to topic: {twist_topic}") + + self.sub = self.create_subscription(Detection2D, 'aruco_loc', self.correction_callback, 10) + self.image_sub = self.create_subscription(Image, image_topic, self.image_callback, 10) + self.heading_pub = self.create_publisher(TwistStamped, twist_topic, 10) + + # PD coefficients, need tuning + self.kProp = 0.01 + self.kDer = 0.005 + + self.img_w = None + self.img_h = None + + # Error Tracking + self.currError = None + self.prevError = None + + def image_callback(self, msg: Image): + self.img_w = msg.width + self.img_h = msg.height + + + def correction_callback(self, msg: Detection2D): + # stop if tag fills >7% of frame + if self.img_w is None or self.img_h is None: + #self.get_logger().warn("No image size received yet; skipping area-based stop check") + return + else: + bbox_area_ratio = (msg.bbox.size_x * msg.bbox.size_y) / (float(self.img_w) * float(self.img_h)) + if bbox_area_ratio > 0.07: + stop_msg = TwistStamped() + stop_msg.header.stamp = self.get_clock().now().to_msg() + stop_msg.twist.linear.x = 0.0 + stop_msg.twist.angular.z = 0.0 + self.heading_pub.publish(stop_msg) + return + + if self.img_w is None: + return # no frame width yet + error = msg.bbox.center.position.x - (float(self.img_w) * 0.5) + + + # Initialize error values if first time + if self.currError is None: + self.currError = error + self.prevError = error # Avoid large derivative spikes + return # Skip first iteration + + self.prevError = self.currError + self.currError = error + + # Compute PD output + output = (self.kProp * self.currError) + (self.kDer * (self.currError - self.prevError)) + + # Limit output to a reasonable angular speed + max_angular_speed = 1.5 + output = max(-max_angular_speed, min(output, max_angular_speed)) + + # Create Twist message + correction_message = TwistStamped() + correction_message.header.stamp = self.get_clock().now().to_msg() + correction_message.twist.angular.z = output # Rotate based on PD output + + correction_message.twist.linear.x = 0.5 + + # Move forward if nearly aligned + if abs(error) < 15: + correction_message.twist.angular.z = 0.0 + correction_message.twist.linear.x = 1.0 + + # Publish correction + self.heading_pub.publish(correction_message) + +def main(args=None): + rclpy.init(args=args) + node = CorrectionNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/dummy_BB_node.py b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/dummy_BB_node.py new file mode 100644 index 00000000..1b235186 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/dummy_BB_node.py @@ -0,0 +1,47 @@ +import rclpy +from rclpy.node import Node +from interfaces.msg import BB + +class DummyBBNode(Node): + def __init__(self): + super().__init__('dummy_bb_node') + + self.bb_pub = self.create_publisher(BB, 'aruco_loc', 10) + self.timer = self.create_timer(0.1, self.timer_callback) + + self.width = 100 # Keep bounding box width constant + self.center_shift = 0 # Tracks shifting + self.direction = 1 # 1 for right, -1 for left + + def timer_callback(self): + msg = BB() + msg.img_width = 640 + msg.img_height = 320 + + # Calculate bounding box positions + msg.bb_top_left_x = 320 + self.center_shift - self.width // 2 + msg.bb_top_left_y = 160 + msg.bb_bottom_right_x = 320 + self.center_shift + self.width // 2 + msg.bb_bottom_right_y = 160 + + # Update shift for oscillation + self.center_shift += self.direction * 10 # Change 10 pixels per step + + # Reverse direction when reaching boundaries + if abs(self.center_shift) >= 100: # Max shift range + self.direction *= -1 + + self.bb_pub.publish(msg) + + + + +def main(args=None): + rclpy.init(args=args) + node = DummyBBNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/package.xml b/src/subsystems/even_more_minimal_nav/aruco_detection/package.xml new file mode 100644 index 00000000..208d0ea7 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/aruco_detection/package.xml @@ -0,0 +1,23 @@ + + + + aruco_detection + 0.0.0 + TODO: Package description + abhinavkota06 + TODO: License declaration + + + rclpy + cv_bridge + python3-opencv + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/resource/aruco_detection b/src/subsystems/even_more_minimal_nav/aruco_detection/resource/aruco_detection new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/setup.cfg b/src/subsystems/even_more_minimal_nav/aruco_detection/setup.cfg new file mode 100644 index 00000000..b829577d --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/aruco_detection/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/aruco_detection +[install] +install_scripts=$base/lib/aruco_detection diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/setup.py b/src/subsystems/even_more_minimal_nav/aruco_detection/setup.py new file mode 100644 index 00000000..6d0b311e --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/aruco_detection/setup.py @@ -0,0 +1,28 @@ +from setuptools import find_packages, setup + +package_name = 'aruco_detection' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools', 'opencv-python', 'cv-bridge', 'rclpy'], + zip_safe=True, + maintainer='abhinavkota06', + maintainer_email='abhinav.kota06@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "aruco_node = aruco_detection.aruco_node:main", + "correction_node = aruco_detection.correction_node:main", + "dummy_BB_node = aruco_detection.dummy_BB_node:main" + ], + }, +) diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_copyright.py b/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_flake8.py b/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_pep257.py b/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt index 166a36af..913ef8a3 100644 --- a/src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt +++ b/src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) +find_package(vision_msgs REQUIRED) find_package(msgs REQUIRED) add_executable(mission_executive_node src/mission_executive_node.cpp) @@ -29,6 +30,7 @@ target_link_libraries(mission_executive_node ${std_srvs_TARGETS} tf2::tf2 tf2_ros::tf2_ros + ${vision_msgs_TARGETS} ${msgs_TARGETS} ) diff --git a/src/subsystems/even_more_minimal_nav/mission_executive/package.xml b/src/subsystems/even_more_minimal_nav/mission_executive/package.xml index 358a2e7f..c115b8e0 100644 --- a/src/subsystems/even_more_minimal_nav/mission_executive/package.xml +++ b/src/subsystems/even_more_minimal_nav/mission_executive/package.xml @@ -18,6 +18,7 @@ std_srvs tf2 tf2_ros + vision_msgs msgs diff --git a/src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp b/src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp index 9f9594be..3e2112df 100644 --- a/src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp +++ b/src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp @@ -34,6 +34,7 @@ #include "std_msgs/msg/string.hpp" #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" +#include "vision_msgs/msg/detection2_d.hpp" #include "msgs/action/navigate_to_target.hpp" #include "msgs/msg/active_target.hpp" @@ -52,6 +53,8 @@ enum class State NAVIGATING, ARRIVING, STOPPED_AT_TARGET, + SPIRAL_COVERAGE, + SPIRAL_DONE, ABORTING, RETURNING, STOPPED_AT_RETURN, @@ -65,6 +68,8 @@ static std::string stateToStr(State s) case State::NAVIGATING: return "NAVIGATING"; case State::ARRIVING: return "ARRIVING"; case State::STOPPED_AT_TARGET: return "STOPPED_AT_TARGET"; + case State::SPIRAL_COVERAGE: return "SPIRAL_COVERAGE"; + case State::SPIRAL_DONE: return "SPIRAL_DONE"; case State::ABORTING: return "ABORTING"; case State::RETURNING: return "RETURNING"; case State::STOPPED_AT_RETURN: return "STOPPED_AT_RETURN"; @@ -104,6 +109,17 @@ class MissionExecutive : public rclcpp::Node declare_parameter("latlon_to_enu_service", std::string("/gps_pose_publisher/latlon_to_enu")); + // Spiral coverage parameters + declare_parameter("spiral_timeout_s", 120.0); + declare_parameter("spiral_radius_m", 15.0); + declare_parameter("spiral_spacing_m", 2.0); + declare_parameter("spiral_angular_step", 0.5); + declare_parameter("spiral_waypoint_tolerance_m", 2.0); + + // Detection topics + declare_parameter("aruco_detection_topic", std::string("/aruco_loc")); + declare_parameter("yolo_detection_topic", std::string("/yolo_detection")); + // Input topics declare_parameter("imu_topic", std::string("/imu")); declare_parameter("planner_event_topic", std::string("/planner_event")); @@ -116,10 +132,19 @@ class MissionExecutive : public rclcpp::Node declare_parameter("active_target_topic", std::string("/active_target")); declare_parameter("nav_status_topic", std::string("/nav_status")); - stop_angular_vel_threshold_ = get_parameter("stop_angular_vel_threshold").as_double(); - arrival_hold_time_ = get_parameter("arrival_hold_time").as_double(); - replan_distance_m_ = get_parameter("replan_distance_m").as_double(); - const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); + stop_angular_vel_threshold_ = get_parameter("stop_angular_vel_threshold").as_double(); + arrival_hold_time_ = get_parameter("arrival_hold_time").as_double(); + replan_distance_m_ = get_parameter("replan_distance_m").as_double(); + const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); + + spiral_timeout_s_ = get_parameter("spiral_timeout_s").as_double(); + spiral_radius_m_ = get_parameter("spiral_radius_m").as_double(); + spiral_spacing_m_ = get_parameter("spiral_spacing_m").as_double(); + spiral_angular_step_ = get_parameter("spiral_angular_step").as_double(); + spiral_waypoint_tolerance_m_ = get_parameter("spiral_waypoint_tolerance_m").as_double(); + + const auto aruco_detection_topic = get_parameter("aruco_detection_topic").as_string(); + const auto yolo_detection_topic = get_parameter("yolo_detection_topic").as_string(); const auto imu_topic = get_parameter("imu_topic").as_string(); const auto planner_event_topic = get_parameter("planner_event_topic").as_string(); @@ -179,7 +204,8 @@ class MissionExecutive : public rclcpp::Node std::lock_guard lk(mutex_); if (state_ == State::IDLE || state_ == State::TELEOP || state_ == State::ABORTING || - state_ == State::STOPPED_AT_TARGET || state_ == State::STOPPED_AT_RETURN) + state_ == State::STOPPED_AT_TARGET || state_ == State::STOPPED_AT_RETURN || + state_ == State::SPIRAL_DONE) { res->success = false; res->message = "Nothing to abort in state " + stateToStr(state_); @@ -258,6 +284,28 @@ class MissionExecutive : public rclcpp::Node global_path_ = *msg; }); + // Aruco detection — any message means a marker was found + aruco_sub_ = create_subscription( + aruco_detection_topic, 10, + [this](const vision_msgs::msg::Detection2D::SharedPtr) { + std::lock_guard lk(mutex_); + if (state_ == State::SPIRAL_COVERAGE) { + detection_received_ = true; + transition(State::SPIRAL_DONE, "aruco marker detected"); + } + }); + + // YOLO detection — Bool(True) signals an object was found + yolo_sub_ = create_subscription( + yolo_detection_topic, 10, + [this](const std_msgs::msg::Bool::SharedPtr msg) { + std::lock_guard lk(mutex_); + if (msg->data && state_ == State::SPIRAL_COVERAGE) { + detection_received_ = true; + transition(State::SPIRAL_DONE, "yolo object detected"); + } + }); + // ── 2 Hz status timer ──────────────────────────────────────────────────── // Also drives arrival detection and replan checks; 2 Hz is fine for both. status_timer_ = create_wall_timer( @@ -267,6 +315,7 @@ class MissionExecutive : public rclcpp::Node refreshPoseFromTF(); // updates robot_pose_ for all checks below checkArrival(); checkCrossTrackError(); + checkSpiralProgress(); publishStatus(); checkActionResult(); }); @@ -289,9 +338,10 @@ class MissionExecutive : public rclcpp::Node state_ = new_state; const bool nav_active = - state_ == State::NAVIGATING || - state_ == State::ARRIVING || - state_ == State::RETURNING; + state_ == State::NAVIGATING || + state_ == State::ARRIVING || + state_ == State::RETURNING || + state_ == State::SPIRAL_COVERAGE; publishNavEnabled(nav_active); publishNavMode(); publishStatus(); @@ -320,6 +370,7 @@ class MissionExecutive : public rclcpp::Node case State::NAVIGATING: case State::ARRIVING: case State::RETURNING: + case State::SPIRAL_COVERAGE: msg.data = "autonomous"; break; default: msg.data = "stopped"; break; @@ -508,6 +559,102 @@ class MissionExecutive : public rclcpp::Node } } + // ── Spiral coverage ─────────────────────────────────────────────────────── + + // Called with mutex_ held. + std::vector generateSpiralWaypoints( + const geometry_msgs::msg::PoseStamped & start) + { + std::vector waypoints; + const double a = spiral_spacing_m_ / (2.0 * M_PI); + if (a <= 0.0) return waypoints; + + const double max_angle = spiral_radius_m_ / a; + const double min_start_r = 1.5; + const double yaw0 = quaternionToYaw(start.pose.orientation); + const rclcpp::Time stamp = now(); + + double theta = 0.0; + while (theta <= max_angle) { + const double r = min_start_r + a * theta; + if (r > spiral_radius_m_) break; + + const double x = start.pose.position.x + r * std::cos(yaw0 + theta + M_PI_2); + const double y = start.pose.position.y + r * std::sin(yaw0 + theta + M_PI_2); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = stamp; + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.orientation.w = 1.0; + waypoints.push_back(pose); + + theta += spiral_angular_step_; + } + return waypoints; + } + + // Called with mutex_ held, immediately after transitioning to SPIRAL_COVERAGE. + void startSpiralCoverage() + { + if (!robot_pose_.has_value()) { + RCLCPP_WARN(get_logger(), "[mission_executive] No robot pose — skipping spiral"); + transition(State::SPIRAL_DONE, "no pose available for spiral"); + return; + } + + spiral_waypoints_ = generateSpiralWaypoints(*robot_pose_); + spiral_waypoint_idx_ = 0; + spiral_start_time_ = now(); + detection_received_ = false; + + if (spiral_waypoints_.empty()) { + RCLCPP_WARN(get_logger(), "[mission_executive] Empty spiral path — completing immediately"); + transition(State::SPIRAL_DONE, "empty spiral path"); + return; + } + + RCLCPP_INFO(get_logger(), + "[mission_executive] Spiral started: %zu waypoints, radius=%.1fm, timeout=%.1fs", + spiral_waypoints_.size(), spiral_radius_m_, spiral_timeout_s_); + + goal_pub_->publish(spiral_waypoints_[0]); + } + + // Called from the status timer with mutex_ held. + void checkSpiralProgress() + { + if (state_ != State::SPIRAL_COVERAGE) return; + + // Check timeout + const double elapsed = (now() - spiral_start_time_).seconds(); + if (elapsed >= spiral_timeout_s_) { + RCLCPP_INFO(get_logger(), + "[mission_executive] Spiral timeout after %.1fs", elapsed); + transition(State::SPIRAL_DONE, "spiral timeout"); + return; + } + + if (!robot_pose_.has_value() || spiral_waypoints_.empty()) return; + + // Advance to the next waypoint when within tolerance of the current one + const auto & wp = spiral_waypoints_[spiral_waypoint_idx_]; + const double dist = std::hypot( + robot_pose_->pose.position.x - wp.pose.position.x, + robot_pose_->pose.position.y - wp.pose.position.y); + + if (dist < spiral_waypoint_tolerance_m_) { + ++spiral_waypoint_idx_; + if (spiral_waypoint_idx_ >= spiral_waypoints_.size()) { + RCLCPP_INFO(get_logger(), "[mission_executive] All spiral waypoints visited"); + transition(State::SPIRAL_DONE, "spiral complete"); + return; + } + goal_pub_->publish(spiral_waypoints_[spiral_waypoint_idx_]); + } + } + // ── Action result dispatch (called from the 2 Hz status timer) ──────────── // Must be called with mutex_ held. @@ -529,7 +676,32 @@ class MissionExecutive : public rclcpp::Node } switch (state_) { - case State::STOPPED_AT_TARGET: + case State::STOPPED_AT_TARGET: { + // For targets that need spiral coverage, kick off the spiral instead of completing. + const bool needs_spiral = active_target_.has_value() && + (active_target_->target_type == msgs::srv::SetTarget::Request::ARUCO_POST || + active_target_->target_type == msgs::srv::SetTarget::Request::OBJECT); + if (needs_spiral) { + transition(State::SPIRAL_COVERAGE, "starting spiral coverage"); + startSpiralCoverage(); + return; + } + result->success = true; + result->message = "Arrived at target"; + active_goal_handle_->succeed(result); + active_goal_handle_ = nullptr; + return; + } + + case State::SPIRAL_DONE: + result->success = true; + result->message = detection_received_ + ? "Detection found during spiral coverage" + : "Spiral coverage complete (timeout or all waypoints visited)"; + active_goal_handle_->succeed(result); + active_goal_handle_ = nullptr; + return; + case State::STOPPED_AT_RETURN: result->success = true; result->message = "Arrived at target"; @@ -781,10 +953,21 @@ class MissionExecutive : public rclcpp::Node bool low_speed_tracking_{false}; uint8_t last_planner_event_{0}; + // Spiral state + std::vector spiral_waypoints_; + size_t spiral_waypoint_idx_{0}; + rclcpp::Time spiral_start_time_{0, 0, RCL_ROS_TIME}; + bool detection_received_{false}; + // Parameters double stop_angular_vel_threshold_{0.05}; double arrival_hold_time_{1.0}; double replan_distance_m_{3.0}; + double spiral_timeout_s_{120.0}; + double spiral_radius_m_{15.0}; + double spiral_spacing_m_{2.0}; + double spiral_angular_step_{0.5}; + double spiral_waypoint_tolerance_m_{2.0}; // ROS interfaces rclcpp::CallbackGroup::SharedPtr reentrant_group_; @@ -808,6 +991,8 @@ class MissionExecutive : public rclcpp::Node rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr planner_event_sub_; rclcpp::Subscription::SharedPtr global_path_sub_; + rclcpp::Subscription::SharedPtr aruco_sub_; + rclcpp::Subscription::SharedPtr yolo_sub_; rclcpp::Client::SharedPtr latlon_client_; diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/package.xml b/src/subsystems/even_more_minimal_nav/yolo_ros/package.xml new file mode 100644 index 00000000..7a3eca59 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/yolo_ros/package.xml @@ -0,0 +1,29 @@ + + + + yolo_ros + 0.0.0 + TODO: Package description + umdloop + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + sensor_msgs + std_msgs + python3-opencv + cv_bridge + python3-pip + + + + + ros2launch + + + ament_python + + diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/resource/yolo_ros b/src/subsystems/even_more_minimal_nav/yolo_ros/resource/yolo_ros new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/setup.cfg b/src/subsystems/even_more_minimal_nav/yolo_ros/setup.cfg new file mode 100644 index 00000000..4f9be5b8 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/yolo_ros/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/yolo_ros +[install] +install_scripts=$base/lib/yolo_ros diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/setup.py b/src/subsystems/even_more_minimal_nav/yolo_ros/setup.py new file mode 100644 index 00000000..a81b9b66 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/yolo_ros/setup.py @@ -0,0 +1,28 @@ +from setuptools import find_packages, setup + +package_name = 'yolo_ros' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools', 'yolo', 'ultralytics'], + zip_safe=True, + maintainer='umdloop', + maintainer_email='umdloop@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'my_node = yolo_ros.my_node:main', + 'yolo_ros = yolo_ros.image_publisher:main', + 'inference = yolo_ros.img_inference:main', + ], + }, +) diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_copyright.py b/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_flake8.py b/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_pep257.py b/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/__init__.py b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/image_publisher.py b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/image_publisher.py new file mode 100644 index 00000000..ad9a5ba3 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/image_publisher.py @@ -0,0 +1,61 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image as RosImage +from cv_bridge import CvBridge +import cv2 +import pyzed.sl as sl + +class ZedPublisher(Node): + def __init__(self): + super().__init__('zed_publisher') + self.publisher = self.create_publisher(RosImage, 'cam_in', 10) + self.bridge = CvBridge() + + self.zed = sl.Camera() + init_params = sl.InitParameters() + init_params.camera_resolution = sl.RESOLUTION.HD720 + init_params.camera_fps = 30 + init_params.depth_mode = sl.DEPTH_MODE.NONE + + err = self.zed.open(init_params) + if err != sl.ERROR_CODE.SUCCESS: + self.get_logger().error(f"Failed to open ZED camera: {err}") + return + + self.image = sl.Mat() + self.timer = self.create_timer(1 / 30.0, self.publish_frame) + + def publish_frame(self): + if self.zed.grab() == sl.ERROR_CODE.SUCCESS: + self.zed.retrieve_image(self.image, sl.VIEW.LEFT) + frame = self.image.get_data() + if frame.ndim == 2: + frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) + elif frame.shape[2] == 4: + frame = cv2.cvtColor(frame, cv2.COLOR_RGBA2BGR) + ros_image = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') + self.publisher.publish(ros_image) + cv2.imshow("ZED Camera feed", frame) + if (cv2.waitKey(1) and 0xFF == ord('q')): + self.get_logger().info("Exiting...") + cv2.destroyAllWindows() + else: + self.get_logger().warn("Failed to grab ZED frame") + + def __del__(self): + self.zed.close() + +def main(): + rclpy.init() + zed_publisher = ZedPublisher() + + executor = rclpy.executors.SingleThreadedExecutor() + executor.add_node(zed_publisher) + executor.spin() + + zed_publisher.destroy_node() + rclpy.shutdown() + cv2.destroyAllWindows() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/img_inference.py b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/img_inference.py new file mode 100644 index 00000000..607d63e3 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/img_inference.py @@ -0,0 +1,120 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image as RosImage +from std_msgs.msg import Bool +from cv_bridge import CvBridge +import cv2 +import threading +from ultralytics import YOLO +import numpy as np +import time + +class BoundingBox: + def __init__(self, img_width, img_height, x1, y1, x2, y2): + self.img_width = img_width + self.img_height = img_height + self.bb_top_left_x = x1 + self.bb_top_left_y = y1 + self.bb_bottom_right_x = x2 + self.bb_bottom_right_y = y2 + + def __repr__(self): + return (f"BoundingBox(img_width={self.img_width}, img_height={self.img_height}, " + f"x1={self.bb_top_left_x}, y1={self.bb_top_left_y}, " + f"x2={self.bb_bottom_right_x}, y2={self.bb_bottom_right_y})") + + def to_msg_format(self): + return (f"img_width: {self.img_width}, img_height: {self.img_height}, " + f"bb_top_left_x: {self.bb_top_left_x}, bb_top_left_y: {self.bb_top_left_y}, " + f"bb_bottom_right_x: {self.bb_bottom_right_x}, bb_bottom_right_y: {self.bb_bottom_right_y}\n") + +class InferenceNode(Node): + def __init__(self): + super().__init__('yolo_inference_node') + self.declare_parameter('confidence_threshold', 0.5) + self.conf_threshold = self.get_parameter('confidence_threshold').value + + self.subscription = self.create_subscription( + RosImage, + '/zed/zed_node/left_gray/image_rect_gray', + self.image_callback, + 10 + ) + self.detection_pub = self.create_publisher(Bool, '/yolo_detection', 10) + self.bridge = CvBridge() + self.frame_queue = [] + self.frame_lock = threading.Lock() + self.model = YOLO("best.pt") + self.last_inference_time = time.time() + self.fps = 0 + + self.inference_thread = threading.Thread(target=self.inference_loop, daemon=True) + self.inference_thread.start() + + def image_callback(self, msg): + frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + with self.frame_lock: + self.frame_queue.append((frame, msg.width, msg.height)) + if len(self.frame_queue) > 1: + self.frame_queue.pop(0) + + def inference_loop(self): + while rclpy.ok(): + if not self.frame_queue: + time.sleep(0.001) + continue + + with self.frame_lock: + if not self.frame_queue: + continue + frame, width, height = self.frame_queue.pop(0) + + results = self.model(frame) + + current_time = time.time() + self.fps = 1.0 / (current_time - self.last_inference_time) + self.last_inference_time = current_time + + self.process_results(frame, results, width, height) + + def process_results(self, frame, results, img_width, img_height): + cv2.putText(frame, f"FPS: {self.fps:.1f}", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + + detection_found = False + for result in results: + for box in result.boxes: + conf = float(box.conf[0]) + if conf < self.conf_threshold: + continue + x1, y1, x2, y2 = map(int, box.xyxy[0]) + cls = int(box.cls[0]) + label = f"{self.model.names[cls]}: {conf:.2f}" + + cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2) + cv2.putText(frame, label, (x1, y1 - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) + detection_found = True + + msg = Bool() + msg.data = detection_found + self.detection_pub.publish(msg) + + cv2.imshow("YOLO Output", frame) + cv2.waitKey(1) + +def main(): + rclpy.init() + yolo_node = InferenceNode() + + try: + rclpy.spin(yolo_node) + except KeyboardInterrupt: + pass + finally: + yolo_node.destroy_node() + rclpy.shutdown() + cv2.destroyAllWindows() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/my_node.py b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/my_node.py new file mode 100644 index 00000000..6980a95a --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/my_node.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from yolo_ros.') + + +if __name__ == '__main__': + main() From 14eb3579cf978868039d881932f9a0260594ec59 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Thu, 2 Apr 2026 21:26:08 -0400 Subject: [PATCH 06/21] script --- .../mission_executive/scripts/mission_cli.sh | 470 ++++++++++++++++++ 1 file changed, 470 insertions(+) create mode 100755 src/subsystems/even_more_minimal_nav/mission_executive/scripts/mission_cli.sh diff --git a/src/subsystems/even_more_minimal_nav/mission_executive/scripts/mission_cli.sh b/src/subsystems/even_more_minimal_nav/mission_executive/scripts/mission_cli.sh new file mode 100755 index 00000000..307eaa84 --- /dev/null +++ b/src/subsystems/even_more_minimal_nav/mission_executive/scripts/mission_cli.sh @@ -0,0 +1,470 @@ +#!/usr/bin/env bash +# mission_cli.sh — Interactive CLI for the MissionExecutive node +# +# Usage: +# ./mission_cli.sh [command] [args...] +# +# Commands: +# status Print current nav status once +# watch Stream nav status (Ctrl-C to stop) +# abort Abort the current mission +# teleop on|off Enable / disable teleop mode +# +# nav gps [tol] Navigate to GPS coords (GNSS_ONLY target) +# nav meter [tol] Navigate to ENU metre coords +# nav aruco [tol] Navigate + spiral for ArUco post +# nav object [tol] Navigate + spiral for OBJECT detection +# +# set-target gps [type] [tol] Register GPS target +# set-target meter [type] [tol] Register ENU target +# nav-by-id Navigate to a pre-registered target id +# +# menu Interactive menu (default if no args) +# +# target_type: 0=GNSS_ONLY 1=ARUCO_POST 2=OBJECT 3=LOCAL +# tol: arrival radius in metres (default 3.0) +# +# For ARUCO_POST and OBJECT targets the node automatically enters +# SPIRAL_COVERAGE after arrival; the action completes when a detection +# is received OR the spiral times out. The live status display shows +# the state transition so you can see when this happens. +# +# Ctrl-C during a nav command calls ~/abort before exiting. + +set -euo pipefail + +# ── Constants ───────────────────────────────────────────────────────────────── + +NODE_NS="/mission_executive" +STATUS_TOPIC="/nav_status" +STATUS_TYPE="msgs/msg/NavStatus" + +RED='\033[0;31m' +GRN='\033[0;32m' +YLW='\033[1;33m' +CYN='\033[0;36m' +BLD='\033[1m' +DIM='\033[2m' +RST='\033[0m' + +# ── Helpers ─────────────────────────────────────────────────────────────────── + +info() { echo -e "${GRN}[✓]${RST} $*"; } +warn() { echo -e "${YLW}[!]${RST} $*"; } +error() { echo -e "${RED}[✗]${RST} $*" >&2; } +title() { echo -e "\n${BLD}$*${RST}"; } + +require_ros() { + if ! command -v ros2 &>/dev/null; then + error "ros2 not found. Source your ROS 2 workspace first." + exit 1 + fi +} + +target_type_name() { + case "$1" in + 0) echo "GNSS_ONLY" ;; + 1) echo "ARUCO_POST" ;; + 2) echo "OBJECT" ;; + 3) echo "LOCAL" ;; + *) echo "UNKNOWN($1)" ;; + esac +} + +# Return one field from the next /nav_status message, or "?" on timeout. +_status_field() { + local field="$1" + timeout 2 ros2 topic echo --once --field "$field" "$STATUS_TOPIC" 2>/dev/null \ + | tr -d ' \n' || echo "?" +} + +# ── cmd_status / cmd_watch ──────────────────────────────────────────────────── + +cmd_status() { + title "Nav Status" + local raw + raw=$(timeout 2 ros2 topic echo --once "$STATUS_TOPIC" "$STATUS_TYPE" 2>/dev/null) || { + warn "No message received — is mission_executive running?" + return + } + local state dist xte speed active + state=$(echo "$raw" | awk '/^state:/{print $2}') + active=$(echo "$raw" | awk '/^active_target_id:/{print $2}') + dist=$(echo "$raw" | awk '/^distance_to_goal_m:/{print $2}') + xte=$(echo "$raw" | awk '/^cross_track_error_m:/{print $2}') + speed=$(echo "$raw" | awk '/^robot_speed_mps:/{print $2}') + + printf " %-22s %s\n" "State:" "${BLD}${state}${RST}" + printf " %-22s %s\n" "Active target:" "${active:-(none)}" + printf " %-22s %s m\n" "Dist to goal:" "${dist:--}" + printf " %-22s %s m\n" "Cross-track err:" "${xte:--}" + printf " %-22s %s rad/s\n" "Angular vel:" "${speed:--}" +} + +cmd_watch() { + title "Watching $STATUS_TOPIC (Ctrl-C to stop)" + # Pretty-print each message as a single status line + ros2 topic echo "$STATUS_TOPIC" "$STATUS_TYPE" | \ + awk ' + /^state:/ { state = $2 } + /^active_target_id:/ { tgt = $2 } + /^distance_to_goal_m:/ { dist = $2 } + /^cross_track_error_m:/{ xte = $2 } + /^---/ { + printf "state=%-20s tgt=%-12s dist=%6s m xte=%6s m\n", + state, tgt, dist, xte + state=""; tgt=""; dist=""; xte="" + } + ' +} + +# ── cmd_abort / cmd_teleop ──────────────────────────────────────────────────── + +cmd_abort() { + title "Abort Mission" + ros2 service call "${NODE_NS}/abort" std_srvs/srv/Trigger '{}' +} + +cmd_teleop() { + local mode="${1:-}" + case "$mode" in + on|1|true) + title "Teleop ON" + ros2 service call "${NODE_NS}/teleop" std_srvs/srv/SetBool '{data: true}' + ;; + off|0|false) + title "Teleop OFF" + ros2 service call "${NODE_NS}/teleop" std_srvs/srv/SetBool '{data: false}' + ;; + *) + error "Usage: teleop on|off" + exit 1 + ;; + esac +} + +# ── Live-status display while an action runs ────────────────────────────────── +# +# Runs the action goal in the background, streams /nav_status to the terminal +# as a single updating line, and waits for the action to finish. +# Ctrl-C calls ~/abort and cancels the background job. + +_run_nav_with_status() { + local goal_yaml="$1" + local target_type="$2" + local result_file + result_file=$(mktemp /tmp/mission_result.XXXXXX) + + # Warn about spiral phases for detection targets + if [[ "$target_type" == "1" || "$target_type" == "2" ]]; then + echo -e " ${CYN}Note: After arrival the node will enter SPIRAL_COVERAGE${RST}" + echo -e " ${CYN} and complete when a detection is received or it times out.${RST}" + echo + fi + + # --feedback keeps the process alive until the action completes; + # without it ros2 action send_goal exits as soon as the goal is accepted. + ros2 action send_goal \ + "${NODE_NS}/navigate_to_target" \ + msgs/action/NavigateToTarget \ + "$goal_yaml" \ + --feedback \ + > "$result_file" 2>&1 & + local action_pid=$! + + # Ctrl-C: abort mission then clean up + local interrupted=false + trap ' + interrupted=true + echo + warn "Interrupted — calling abort..." + ros2 service call "${NODE_NS}/abort" std_srvs/srv/Trigger "{}" >/dev/null 2>&1 || true + kill "$action_pid" 2>/dev/null || true + ' INT + + echo -e " ${DIM}Waiting for first status message...${RST}" + + local prev_state="" + local start_s=$SECONDS + + while kill -0 "$action_pid" 2>/dev/null; do + local raw + raw=$(timeout 1.2 ros2 topic echo --once "$STATUS_TOPIC" "$STATUS_TYPE" 2>/dev/null) || true + + if [[ -n "$raw" ]]; then + local state dist xte elapsed + state=$(echo "$raw" | awk '/^state:/{print $2}') + dist=$(echo "$raw" | awk '/^distance_to_goal_m:/{print $2}') + xte=$(echo "$raw" | awk '/^cross_track_error_m:/{print $2}') + elapsed=$(( SECONDS - start_s )) + + # Format to 2 decimal places if numeric, else show "-" + local dist_s="-" xte_s="-" + [[ -n "$dist" ]] && dist_s=$(printf "%.2f" "$dist" 2>/dev/null) || true + [[ -n "$xte" ]] && xte_s=$(printf "%.2f" "$xte" 2>/dev/null) || true + + # Highlight state transitions on their own line + if [[ "$state" != "$prev_state" && -n "$state" ]]; then + printf "\n ${YLW}→ State: ${BLD}%s${RST}\n" "$state" + prev_state="$state" + fi + + printf "\r [%3ds] dist=%8s m xte=%8s m " \ + "$elapsed" "$dist_s" "$xte_s" + fi + + sleep 0.5 + done + + # Restore default INT handler + trap - INT + + # Print the action result + echo -e "\n" + if [[ "$interrupted" == false ]]; then + echo -e "${BLD}── Action Result ──────────────────────────────${RST}" + # Show just the result block (last few lines after "Result:") + grep -A5 "Result:" "$result_file" 2>/dev/null || cat "$result_file" + echo -e "${BLD}───────────────────────────────────────────────${RST}" + fi + + rm -f "$result_file" +} + +# ── _send_nav_goal (builds YAML and dispatches) ─────────────────────────────── +# $1: goal_type (0=GPS 1=METER) +# $2: target_id (may be empty string for inline) +# $3: lat or x_m +# $4: lon or y_m +# $5: target_type (0-3) +# $6: tolerance_m +# $7: is_return (true|false) + +_send_nav_goal() { + local goal_type="$1" target_id="$2" a="$3" b="$4" + local target_type="${5:-0}" tolerance="${6:-3.0}" is_return="${7:-false}" + + local lat=0.0 lon=0.0 x=0.0 y=0.0 + [[ "$goal_type" == "0" ]] && { lat="$a"; lon="$b"; } || { x="$a"; y="$b"; } + + title "navigate_to_target" + [[ -n "$target_id" ]] && echo " target_id : $target_id" || echo " target_id : (inline)" + echo " goal_type : $([ "$goal_type" = "0" ] && echo GPS || echo METER)" + [[ "$goal_type" == "0" ]] \ + && echo " lat / lon : $lat / $lon" \ + || echo " x_m / y_m : $x / $y" + echo " target_type : $(target_type_name "$target_type")" + echo " tolerance_m : $tolerance" + echo " is_return : $is_return" + echo + + local yaml="{target_id: '$target_id', lat: $lat, lon: $lon, x_m: $x, y_m: $y, \ +goal_type: $goal_type, target_type: $target_type, \ +tolerance_m: $tolerance, is_return: $is_return}" + + _run_nav_with_status "$yaml" "$target_type" +} + +# ── cmd_nav ─────────────────────────────────────────────────────────────────── + +cmd_nav() { + local coord_type="${1:-}" + case "$coord_type" in + gps|GPS) + local lat="${2:?lat required}" lon="${3:?lon required}" + local tol="${4:-3.0}" + _send_nav_goal 0 "" "$lat" "$lon" 0 "$tol" false + ;; + meter|METER|m) + local x="${2:?x_m required}" y="${3:?y_m required}" + local tol="${4:-3.0}" + _send_nav_goal 1 "" "$x" "$y" 0 "$tol" false + ;; + aruco|ARUCO) + # ARUCO_POST (type=1) — navigate then auto-spiral, done on detection or timeout + local lat="${2:?lat required}" lon="${3:?lon required}" + local tol="${4:-3.0}" + _send_nav_goal 0 "" "$lat" "$lon" 1 "$tol" false + ;; + object|OBJECT|obj) + # OBJECT (type=2) — same spiral flow as aruco but for YOLO detections + local lat="${2:?lat required}" lon="${3:?lon required}" + local tol="${4:-3.0}" + _send_nav_goal 0 "" "$lat" "$lon" 2 "$tol" false + ;; + *) + error "Usage:" + error " nav gps [tol] — GNSS_ONLY target" + error " nav meter [tol] — ENU metre target" + error " nav aruco [tol] — navigate + spiral (ArUco)" + error " nav object [tol] — navigate + spiral (YOLO object)" + exit 1 + ;; + esac +} + +# Navigate to a pre-registered target by ID +cmd_nav_by_id() { + local id="${1:?target_id required}" + local is_return="${2:-false}" + title "navigate_to_target (id=$id is_return=$is_return)" + + local yaml="{target_id: '$id', lat: 0.0, lon: 0.0, x_m: 0.0, y_m: 0.0, \ +goal_type: 0, target_type: 0, tolerance_m: 3.0, is_return: $is_return}" + + # target_type unknown here — pass 0 (spiral check uses what's in the registry) + _run_nav_with_status "$yaml" 0 +} + +# ── cmd_set_target ──────────────────────────────────────────────────────────── + +_call_set_target() { + local goal_type="$1" target_id="$2" a="$3" b="$4" + local target_type="${5:-0}" tolerance="${6:-3.0}" + + local lat=0.0 lon=0.0 x=0.0 y=0.0 + [[ "$goal_type" == "0" ]] && { lat="$a"; lon="$b"; } || { x="$a"; y="$b"; } + + title "set_target (id=$target_id type=$(target_type_name "$target_type"))" + ros2 service call "${NODE_NS}/set_target" msgs/srv/SetTarget \ + "{target_id: '$target_id', lat: $lat, lon: $lon, x_m: $x, y_m: $y, \ +goal_type: $goal_type, target_type: $target_type, tolerance_m: $tolerance}" +} + +cmd_set_target() { + local coord_type="${1:-}" + case "$coord_type" in + gps|GPS) + local id="${2:?id}" lat="${3:?lat}" lon="${4:?lon}" + local ttype="${5:-0}" tol="${6:-3.0}" + _call_set_target 0 "$id" "$lat" "$lon" "$ttype" "$tol" + ;; + meter|METER|m) + local id="${2:?id}" x="${3:?x}" y="${4:?y}" + local ttype="${5:-0}" tol="${6:-3.0}" + _call_set_target 1 "$id" "$x" "$y" "$ttype" "$tol" + ;; + *) + error "Usage: set-target gps [type 0-3] [tol]" + error " set-target meter [type 0-3] [tol]" + exit 1 + ;; + esac +} + +# ── Interactive menu ────────────────────────────────────────────────────────── + +_menu_nav_goal() { + echo + echo "Navigation type:" + echo " 1) GPS point (GNSS_ONLY)" + echo " 2) GPS point (ArUco post — will spiral after arrival)" + echo " 3) GPS point (YOLO object — will spiral after arrival)" + echo " 4) ENU metres (GNSS_ONLY)" + echo " 5) By registered target ID" + read -rp "Choice: " ct + + case "$ct" in + 5) + read -rp "Target ID: " tid + read -rp "Is return? [y/N]: " ret + [[ "$ret" =~ ^[Yy]$ ]] && cmd_nav_by_id "$tid" true || cmd_nav_by_id "$tid" false + return + ;; + esac + + local goal_type=0 target_type=0 a b tol + case "$ct" in + 1) target_type=0 ;; + 2) target_type=1 ;; + 3) target_type=2 ;; + 4) goal_type=1 ;; + esac + + if [[ "$goal_type" == "0" ]]; then + read -rp "Latitude: " a + read -rp "Longitude: " b + else + read -rp "x_m: " a + read -rp "y_m: " b + fi + + read -rp "Arrival tolerance in metres [3.0]: " tol + tol="${tol:-3.0}" + + _send_nav_goal "$goal_type" "" "$a" "$b" "$target_type" "$tol" false +} + +_menu_set_target() { + echo + echo "Coordinate type:" + echo " 1) GPS (lat/lon) 2) ENU metres (x/y)" + read -rp "Choice: " ct + read -rp "Target ID: " tid + + local goal_type=0 a b ttype tol + if [[ "$ct" == "1" ]]; then + read -rp "Latitude: " a; read -rp "Longitude: " b; goal_type=0 + else + read -rp "x_m: " a; read -rp "y_m: " b; goal_type=1 + fi + + echo "Target type: 0=GNSS_ONLY 1=ARUCO_POST 2=OBJECT 3=LOCAL" + read -rp "Target type [0]: " ttype; ttype="${ttype:-0}" + read -rp "Arrival tolerance [3.0]: " tol; tol="${tol:-3.0}" + + _call_set_target "$goal_type" "$tid" "$a" "$b" "$ttype" "$tol" +} + +cmd_menu() { + while true; do + echo + echo -e "${BLD}╔══ Mission Executive CLI ════════════════════╗${RST}" + echo -e "${BLD}║${RST} 1) Status (snapshot) ${BLD}║${RST}" + echo -e "${BLD}║${RST} 2) Watch status (stream) ${BLD}║${RST}" + echo -e "${BLD}║${RST} 3) Navigate (GPS / meter / aruco / object) ${BLD}║${RST}" + echo -e "${BLD}║${RST} 4) Register target (set_target service) ${BLD}║${RST}" + echo -e "${BLD}║${RST} 5) Abort mission ${BLD}║${RST}" + echo -e "${BLD}║${RST} 6) Teleop ON ${BLD}║${RST}" + echo -e "${BLD}║${RST} 7) Teleop OFF ${BLD}║${RST}" + echo -e "${BLD}║${RST} q) Quit ${BLD}║${RST}" + echo -e "${BLD}╚═════════════════════════════════════════════╝${RST}" + read -rp "Choice: " choice + + case "$choice" in + 1) cmd_status ;; + 2) cmd_watch ;; + 3) _menu_nav_goal ;; + 4) _menu_set_target ;; + 5) cmd_abort ;; + 6) cmd_teleop on ;; + 7) cmd_teleop off ;; + q|Q) info "Bye."; exit 0 ;; + *) warn "Unknown option '$choice'" ;; + esac + done +} + +# ── Entry point ─────────────────────────────────────────────────────────────── + +require_ros + +CMD="${1:-menu}" +shift || true + +case "$CMD" in + status) cmd_status ;; + watch) cmd_watch ;; + abort) cmd_abort ;; + teleop) cmd_teleop "$@" ;; + nav) cmd_nav "$@" ;; + nav-by-id) cmd_nav_by_id "$@" ;; + set-target) cmd_set_target "$@" ;; + menu) cmd_menu ;; + -h|--help|help) sed -n '3,33p' "$0" ;; + *) + error "Unknown command: $CMD" + echo "Run '$0 help' for usage." + exit 1 + ;; +esac From dc607ac55b3354f39d1908fa35d5694f0b780899 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Fri, 3 Apr 2026 18:03:44 -0400 Subject: [PATCH 07/21] cleanup --- .../aruco_detection/__init__.py | 0 .../aruco_detection/aruco_node.py | 151 --- .../aruco_detection/correction_node.py | 103 -- .../aruco_detection/dummy_BB_node.py | 47 - .../aruco_detection/package.xml | 23 - .../aruco_detection/resource/aruco_detection | 0 .../aruco_detection/setup.cfg | 4 - .../aruco_detection/setup.py | 28 - .../aruco_detection/test/test_copyright.py | 25 - .../aruco_detection/test/test_flake8.py | 25 - .../aruco_detection/test/test_pep257.py | 23 - .../dem_costmap/CMakeLists.txt | 65 -- .../even_more_minimal_nav/dem_costmap/LICENSE | 202 ---- .../dem_costmap/config/dem_costmap.yaml | 8 - .../dem_costmap/launch/dem_costmap.launch.py | 30 - .../dem_costmap/maps/north_site800m.tif | 3 - .../dem_costmap/package.xml | 22 - .../dem_costmap/src/map_node.cpp | 236 ---- .../emmn_visualizer/__init__.py | 0 .../emmn_visualizer/emmn_visualizer_node.py | 711 ------------ .../emmn_visualizer/package.xml | 27 - .../emmn_visualizer/resource/emmn_visualizer | 0 .../emmn_visualizer/setup.cfg | 4 - .../emmn_visualizer/setup.py | 26 - .../src/global_planner_node.cpp | 330 ------ .../minimal_nav_bringup/CMakeLists.txt | 14 - .../minimal_nav_bringup/package.xml | 23 - .../scripts/reframe_pointcloud.py | 75 -- .../mission_executive/CMakeLists.txt | 41 - .../src/mission_executive_node.cpp | 1013 ----------------- src/subsystems/even_more_minimal_nav/plan.md | 101 -- .../yolo_ros/package.xml | 29 - .../yolo_ros/resource/yolo_ros | 0 .../even_more_minimal_nav/yolo_ros/setup.cfg | 4 - .../even_more_minimal_nav/yolo_ros/setup.py | 28 - .../yolo_ros/test/test_copyright.py | 25 - .../yolo_ros/test/test_flake8.py | 25 - .../yolo_ros/test/test_pep257.py | 23 - .../yolo_ros/yolo_ros/__init__.py | 0 .../yolo_ros/yolo_ros/image_publisher.py | 61 - .../yolo_ros/yolo_ros/img_inference.py | 120 -- .../yolo_ros/yolo_ros/my_node.py | 6 - .../navigation/athena_map/src/map_node.cpp | 10 +- .../global_planner/CMakeLists.txt | 22 +- .../global_planner/global_planner_algo.hpp | 63 + .../global_planner/package.xml | 0 .../src/global_planner_algo.cpp | 153 +++ .../src/global_planner_node.cpp | 173 +++ .../gps_pose_publisher/CMakeLists.txt | 0 .../gps_pose_publisher/package.xml | 0 .../src/gps_pose_publisher_node.cpp | 0 .../mission_executive/CMakeLists.txt | 82 ++ .../mission_executive_algo.hpp | 161 +++ .../mission_executive/package.xml | 0 .../mission_executive/scripts/mission_cli.sh | 0 .../src/mission_executive_algo.cpp | 366 ++++++ .../src/mission_executive_node.cpp | 576 ++++++++++ .../test/test_mission_executive.cpp | 272 +++++ .../test/test_mock_pipeline.cpp | 341 ++++++ .../navigation/nav_bringup/CMakeLists.txt | 2 +- .../nav_bringup/config/nav_params_real.yaml | 92 ++ .../nav_bringup/config/nav_params_sim.yaml} | 18 +- .../nav_bringup/launch/emmn.launch.py} | 82 +- .../navigation/nav_bringup/package.xml | 2 +- .../vector_field_planner/CMakeLists.txt | 21 +- .../vector_field_planner_algo.hpp | 101 ++ .../vector_field_planner/package.xml | 0 .../src/vector_field_planner_algo.cpp | 153 +++ .../src/vector_field_planner_node.cpp | 435 +++---- .../test/test_pure_pursuit_sim.cpp | 331 +++--- .../navigation/yolo_ros/package.xml | 1 + .../yolo_ros/yolo_ros/img_inference.py | 42 +- 72 files changed, 2958 insertions(+), 4222 deletions(-) delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/__init__.py delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/aruco_node.py delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/correction_node.py delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/dummy_BB_node.py delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/package.xml delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/resource/aruco_detection delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/setup.cfg delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/setup.py delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/test/test_copyright.py delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/test/test_flake8.py delete mode 100644 src/subsystems/even_more_minimal_nav/aruco_detection/test/test_pep257.py delete mode 100644 src/subsystems/even_more_minimal_nav/dem_costmap/CMakeLists.txt delete mode 100644 src/subsystems/even_more_minimal_nav/dem_costmap/LICENSE delete mode 100644 src/subsystems/even_more_minimal_nav/dem_costmap/config/dem_costmap.yaml delete mode 100644 src/subsystems/even_more_minimal_nav/dem_costmap/launch/dem_costmap.launch.py delete mode 100644 src/subsystems/even_more_minimal_nav/dem_costmap/maps/north_site800m.tif delete mode 100644 src/subsystems/even_more_minimal_nav/dem_costmap/package.xml delete mode 100644 src/subsystems/even_more_minimal_nav/dem_costmap/src/map_node.cpp delete mode 100644 src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/__init__.py delete mode 100644 src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py delete mode 100644 src/subsystems/even_more_minimal_nav/emmn_visualizer/package.xml delete mode 100644 src/subsystems/even_more_minimal_nav/emmn_visualizer/resource/emmn_visualizer delete mode 100644 src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.cfg delete mode 100644 src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.py delete mode 100644 src/subsystems/even_more_minimal_nav/global_planner/src/global_planner_node.cpp delete mode 100644 src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt delete mode 100644 src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml delete mode 100755 src/subsystems/even_more_minimal_nav/minimal_nav_bringup/scripts/reframe_pointcloud.py delete mode 100644 src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt delete mode 100644 src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp delete mode 100644 src/subsystems/even_more_minimal_nav/plan.md delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/package.xml delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/resource/yolo_ros delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/setup.cfg delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/setup.py delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/test/test_copyright.py delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/test/test_flake8.py delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/test/test_pep257.py delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/__init__.py delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/image_publisher.py delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/img_inference.py delete mode 100644 src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/my_node.py rename src/subsystems/{even_more_minimal_nav => navigation}/global_planner/CMakeLists.txt (53%) create mode 100644 src/subsystems/navigation/global_planner/include/global_planner/global_planner_algo.hpp rename src/subsystems/{even_more_minimal_nav => navigation}/global_planner/package.xml (100%) create mode 100644 src/subsystems/navigation/global_planner/src/global_planner_algo.cpp create mode 100644 src/subsystems/navigation/global_planner/src/global_planner_node.cpp rename src/subsystems/{even_more_minimal_nav => navigation}/gps_pose_publisher/CMakeLists.txt (100%) rename src/subsystems/{even_more_minimal_nav => navigation}/gps_pose_publisher/package.xml (100%) rename src/subsystems/{even_more_minimal_nav => navigation}/gps_pose_publisher/src/gps_pose_publisher_node.cpp (100%) create mode 100644 src/subsystems/navigation/mission_executive/CMakeLists.txt create mode 100644 src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp rename src/subsystems/{even_more_minimal_nav => navigation}/mission_executive/package.xml (100%) rename src/subsystems/{even_more_minimal_nav => navigation}/mission_executive/scripts/mission_cli.sh (100%) create mode 100644 src/subsystems/navigation/mission_executive/src/mission_executive_algo.cpp create mode 100644 src/subsystems/navigation/mission_executive/src/mission_executive_node.cpp create mode 100644 src/subsystems/navigation/mission_executive/test/test_mission_executive.cpp create mode 100644 src/subsystems/navigation/mission_executive/test/test_mock_pipeline.cpp create mode 100644 src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml rename src/subsystems/{even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml => navigation/nav_bringup/config/nav_params_sim.yaml} (74%) rename src/subsystems/{even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py => navigation/nav_bringup/launch/emmn.launch.py} (65%) rename src/subsystems/{even_more_minimal_nav => navigation}/vector_field_planner/CMakeLists.txt (60%) create mode 100644 src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp rename src/subsystems/{even_more_minimal_nav => navigation}/vector_field_planner/package.xml (100%) create mode 100644 src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp rename src/subsystems/{even_more_minimal_nav => navigation}/vector_field_planner/src/vector_field_planner_node.cpp (50%) rename src/subsystems/{even_more_minimal_nav => navigation}/vector_field_planner/test/test_pure_pursuit_sim.cpp (60%) diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/__init__.py b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/aruco_node.py b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/aruco_node.py deleted file mode 100644 index 716bc96b..00000000 --- a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/aruco_node.py +++ /dev/null @@ -1,151 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image -from std_msgs.msg import String -from cv_bridge import CvBridge -import cv2 -from vision_msgs.msg import Detection2D - - -class ZedArUcoNode(Node): - def __init__(self): - super().__init__('zed_aruco_node') - - self.declare_parameter("sim", False) - self.sim = self.get_parameter("sim").get_parameter_value().bool_value - - if self.sim: - image_topic = '/camera' - depth_topic = '/depth_camera' - else: - image_topic = '/zed/zed_node/left_gray/image_rect_gray' - depth_topic = '/zed/zed_node/depth/depth_registered' - - self.get_logger().info(f"sim={self.sim}") - self.get_logger().info(f"Subscribing to image feed: {image_topic}") - self.get_logger().info(f"Subscribing to depth image feed: {depth_topic}") - - # Initialize CvBridge to convert ROS images to OpenCV - self.bridge = CvBridge() - - # Subscribe to the ZED camera image and depth topics - self.image_sub = self.create_subscription( - Image, image_topic, self.image_callback, 10 - ) - self.depth_sub = self.create_subscription( - Image, depth_topic, self.depth_callback, 10 - ) - - self.image_pub = self.create_publisher(Image, 'aruco_annotated_img', 10) - self.tag_pub = self.create_publisher(Detection2D, 'aruco_loc', 10) - self.depth_pub = self.create_publisher(String, 'aruco_depth', 10) - - self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) - - try: - self.aruco_params = cv2.aruco.DetectorParameters() - except AttributeError: - self.aruco_params = cv2.aruco.DetectorParameters_create() - - self.corners = None - self.marker_id = None - self.latest_depth_map = None - - def image_callback(self, msg): - frame = self.bridge.imgmsg_to_cv2(msg, "mono8") - frame_color = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) - - # Process image (ArUco detection, etc.) - self.detect_aruco_markers(frame) - - # If we have a valid marker and a valid depth map, publish the information - if self.marker_id is not None and self.corners is not None: - - for i in range(4): - start_point = tuple(map(int, self.corners[i])) - end_point = tuple(map(int, self.corners[(i + 1) % 4])) # Connect to next corner - cv2.line(frame_color, start_point, end_point, (0, 255, 0), 2) # Draw green lines - - if self.corners is not None: - text_pos = tuple(map(int, self.corners[0])) - - cv2.putText( - frame_color, - f"id: {self.marker_id}", - (text_pos[0], text_pos[1] - 20), - cv2.FONT_HERSHEY_SIMPLEX, - 0.7, - (0, 0, 255), - 2, - cv2.LINE_AA - ) - - if not self.sim: - if self.latest_depth_map is not None: - depth_values = [ - self.latest_depth_map[int(self.corners[0][1]), int(self.corners[0][0])], # Top-left - self.latest_depth_map[int(self.corners[1][1]), int(self.corners[1][0])], # Top-right - self.latest_depth_map[int(self.corners[2][1]), int(self.corners[2][0])], # Bottom-right - self.latest_depth_map[int(self.corners[3][1]), int(self.corners[3][0])] # Bottom-left - ] - - depth_avg = sum(depth_values) / len(depth_values) - depth_message = String() - depth_message.data = str(depth_avg) - self.depth_pub.publish(depth_message) - else: - self.get_logger().warn("Depth map not received yet, skipping depth calc") - - # Publish tag bounding box as vision_msgs/Detection2D - message = Detection2D() - message.header = msg.header # keep same timestamp/frame as the image - - # Extract x and y coordinates from all four corners - x_values = [float(pt[0]) for pt in self.corners] - y_values = [float(pt[1]) for pt in self.corners] - - x_min = min(x_values) - y_min = min(y_values) - x_max = max(x_values) - y_max = max(y_values) - - # Detection2D bbox is defined by center pose (Pose2D) + size_x/size_y in pixels - message.bbox.center.position.x = (x_min + x_max) * 0.5 - message.bbox.center.position.x = (y_min + y_max) * 0.5 - message.bbox.center.theta = 0.0 # axis-aligned bbox - message.bbox.size_x = (x_max - x_min) - message.bbox.size_y = (y_max - y_min) - - # message.results can be left empty (CorrectionNode only needs bbox) - self.tag_pub.publish(message) - - - annotated_msg = self.bridge.cv2_to_imgmsg(frame_color, encoding="bgr8") - annotated_msg.header = msg.header - self.image_pub.publish(annotated_msg) - - - def depth_callback(self, msg): - # Convert ROS Image to depth map - self.latest_depth_map = self.bridge.imgmsg_to_cv2(msg, "32FC1") - - def detect_aruco_markers(self, frame): - corners, ids, _ = cv2.aruco.detectMarkers(frame, self.aruco_dict, parameters=self.aruco_params) - - if ids is not None: - for i in range(len(ids)): - self.marker_id = ids[i][0] - self.corners = corners[i][0] - else: - self.marker_id = None - self.corners = None - -def main(args=None): - rclpy.init(args=args) - node = ZedArUcoNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/correction_node.py b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/correction_node.py deleted file mode 100644 index 287d72e0..00000000 --- a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/correction_node.py +++ /dev/null @@ -1,103 +0,0 @@ -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import TwistStamped -from vision_msgs.msg import Detection2D -from sensor_msgs.msg import Image - -class CorrectionNode(Node): - def __init__(self): - super().__init__('correction_node') - - self.declare_parameter("sim", False) - self.sim = self.get_parameter("sim").get_parameter_value().bool_value - - if self.sim: - twist_topic = '/ackermann_steering_controller/reference' - image_topic = '/camera' - else: - twist_topic = '/ackermann_steering_controller/reference' # TO-DO: add actual topic name - image_topic = '/zed/zed_node/left_gray/image_rect_gray' - - self.get_logger().info(f"sim={self.sim}") - self.get_logger().info(f"Publishing twist messages to topic: {twist_topic}") - - self.sub = self.create_subscription(Detection2D, 'aruco_loc', self.correction_callback, 10) - self.image_sub = self.create_subscription(Image, image_topic, self.image_callback, 10) - self.heading_pub = self.create_publisher(TwistStamped, twist_topic, 10) - - # PD coefficients, need tuning - self.kProp = 0.01 - self.kDer = 0.005 - - self.img_w = None - self.img_h = None - - # Error Tracking - self.currError = None - self.prevError = None - - def image_callback(self, msg: Image): - self.img_w = msg.width - self.img_h = msg.height - - - def correction_callback(self, msg: Detection2D): - # stop if tag fills >7% of frame - if self.img_w is None or self.img_h is None: - #self.get_logger().warn("No image size received yet; skipping area-based stop check") - return - else: - bbox_area_ratio = (msg.bbox.size_x * msg.bbox.size_y) / (float(self.img_w) * float(self.img_h)) - if bbox_area_ratio > 0.07: - stop_msg = TwistStamped() - stop_msg.header.stamp = self.get_clock().now().to_msg() - stop_msg.twist.linear.x = 0.0 - stop_msg.twist.angular.z = 0.0 - self.heading_pub.publish(stop_msg) - return - - if self.img_w is None: - return # no frame width yet - error = msg.bbox.center.position.x - (float(self.img_w) * 0.5) - - - # Initialize error values if first time - if self.currError is None: - self.currError = error - self.prevError = error # Avoid large derivative spikes - return # Skip first iteration - - self.prevError = self.currError - self.currError = error - - # Compute PD output - output = (self.kProp * self.currError) + (self.kDer * (self.currError - self.prevError)) - - # Limit output to a reasonable angular speed - max_angular_speed = 1.5 - output = max(-max_angular_speed, min(output, max_angular_speed)) - - # Create Twist message - correction_message = TwistStamped() - correction_message.header.stamp = self.get_clock().now().to_msg() - correction_message.twist.angular.z = output # Rotate based on PD output - - correction_message.twist.linear.x = 0.5 - - # Move forward if nearly aligned - if abs(error) < 15: - correction_message.twist.angular.z = 0.0 - correction_message.twist.linear.x = 1.0 - - # Publish correction - self.heading_pub.publish(correction_message) - -def main(args=None): - rclpy.init(args=args) - node = CorrectionNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/dummy_BB_node.py b/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/dummy_BB_node.py deleted file mode 100644 index 1b235186..00000000 --- a/src/subsystems/even_more_minimal_nav/aruco_detection/aruco_detection/dummy_BB_node.py +++ /dev/null @@ -1,47 +0,0 @@ -import rclpy -from rclpy.node import Node -from interfaces.msg import BB - -class DummyBBNode(Node): - def __init__(self): - super().__init__('dummy_bb_node') - - self.bb_pub = self.create_publisher(BB, 'aruco_loc', 10) - self.timer = self.create_timer(0.1, self.timer_callback) - - self.width = 100 # Keep bounding box width constant - self.center_shift = 0 # Tracks shifting - self.direction = 1 # 1 for right, -1 for left - - def timer_callback(self): - msg = BB() - msg.img_width = 640 - msg.img_height = 320 - - # Calculate bounding box positions - msg.bb_top_left_x = 320 + self.center_shift - self.width // 2 - msg.bb_top_left_y = 160 - msg.bb_bottom_right_x = 320 + self.center_shift + self.width // 2 - msg.bb_bottom_right_y = 160 - - # Update shift for oscillation - self.center_shift += self.direction * 10 # Change 10 pixels per step - - # Reverse direction when reaching boundaries - if abs(self.center_shift) >= 100: # Max shift range - self.direction *= -1 - - self.bb_pub.publish(msg) - - - - -def main(args=None): - rclpy.init(args=args) - node = DummyBBNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/package.xml b/src/subsystems/even_more_minimal_nav/aruco_detection/package.xml deleted file mode 100644 index 208d0ea7..00000000 --- a/src/subsystems/even_more_minimal_nav/aruco_detection/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - aruco_detection - 0.0.0 - TODO: Package description - abhinavkota06 - TODO: License declaration - - - rclpy - cv_bridge - python3-opencv - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/resource/aruco_detection b/src/subsystems/even_more_minimal_nav/aruco_detection/resource/aruco_detection deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/setup.cfg b/src/subsystems/even_more_minimal_nav/aruco_detection/setup.cfg deleted file mode 100644 index b829577d..00000000 --- a/src/subsystems/even_more_minimal_nav/aruco_detection/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/aruco_detection -[install] -install_scripts=$base/lib/aruco_detection diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/setup.py b/src/subsystems/even_more_minimal_nav/aruco_detection/setup.py deleted file mode 100644 index 6d0b311e..00000000 --- a/src/subsystems/even_more_minimal_nav/aruco_detection/setup.py +++ /dev/null @@ -1,28 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'aruco_detection' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools', 'opencv-python', 'cv-bridge', 'rclpy'], - zip_safe=True, - maintainer='abhinavkota06', - maintainer_email='abhinav.kota06@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - "aruco_node = aruco_detection.aruco_node:main", - "correction_node = aruco_detection.correction_node:main", - "dummy_BB_node = aruco_detection.dummy_BB_node:main" - ], - }, -) diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_copyright.py b/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_copyright.py deleted file mode 100644 index 97a39196..00000000 --- a/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_copyright.py +++ /dev/null @@ -1,25 +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 - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_flake8.py b/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_flake8.py deleted file mode 100644 index 27ee1078..00000000 --- a/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_flake8.py +++ /dev/null @@ -1,25 +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(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_pep257.py b/src/subsystems/even_more_minimal_nav/aruco_detection/test/test_pep257.py deleted file mode 100644 index b234a384..00000000 --- a/src/subsystems/even_more_minimal_nav/aruco_detection/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(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/src/subsystems/even_more_minimal_nav/dem_costmap/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/dem_costmap/CMakeLists.txt deleted file mode 100644 index 8b0d98b2..00000000 --- a/src/subsystems/even_more_minimal_nav/dem_costmap/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(dem_costmap) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(OpenCV REQUIRED) - -set(dependencies - rclcpp - nav_msgs - sensor_msgs - cv_bridge - OpenCV -) - -add_executable(map_node src/map_node.cpp) -target_include_directories(map_node PUBLIC - $ - $ - ${OpenCV_INCLUDE_DIRS}) - -target_compile_features(map_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies( - map_node SYSTEM - ${dependencies} -) - -target_link_libraries(map_node ${OpenCV_LIBRARIES}) - -install(TARGETS map_node - DESTINATION lib/${PROJECT_NAME}) - -# Install maps directory -install(DIRECTORY maps/ - DESTINATION share/${PROJECT_NAME}/maps) - -# Install launch directory -install(DIRECTORY launch/ - DESTINATION share/${PROJECT_NAME}/launch) - -# Install config directory -install(DIRECTORY config/ - DESTINATION share/${PROJECT_NAME}/config) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/subsystems/even_more_minimal_nav/dem_costmap/LICENSE b/src/subsystems/even_more_minimal_nav/dem_costmap/LICENSE deleted file mode 100644 index d6456956..00000000 --- a/src/subsystems/even_more_minimal_nav/dem_costmap/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/src/subsystems/even_more_minimal_nav/dem_costmap/config/dem_costmap.yaml b/src/subsystems/even_more_minimal_nav/dem_costmap/config/dem_costmap.yaml deleted file mode 100644 index 60bafdb3..00000000 --- a/src/subsystems/even_more_minimal_nav/dem_costmap/config/dem_costmap.yaml +++ /dev/null @@ -1,8 +0,0 @@ -dem_costmap_converter: - ros__parameters: - dem_file_path: "" - map_resolution: 1.0 - max_passable_slope_degrees: 15.0 - output_frame: map - origin_x: -400.0 - origin_y: -400.0 \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/dem_costmap/launch/dem_costmap.launch.py b/src/subsystems/even_more_minimal_nav/dem_costmap/launch/dem_costmap.launch.py deleted file mode 100644 index 8b03a15d..00000000 --- a/src/subsystems/even_more_minimal_nav/dem_costmap/launch/dem_costmap.launch.py +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env python3 - -import os -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - pkg_dir = get_package_share_directory('dem_costmap') - config_file = os.path.join(pkg_dir, 'config', 'dem_costmap.yaml') - dem_file = os.path.join(pkg_dir, 'maps', 'north_site800m.tif') - - dem_node = Node( - package='dem_costmap', - executable='map_node', - name='dem_costmap_converter', - output='screen', - parameters=[ - config_file, - { - 'dem_file_path': dem_file - } - ] - ) - - return LaunchDescription([ - dem_node - ]) diff --git a/src/subsystems/even_more_minimal_nav/dem_costmap/maps/north_site800m.tif b/src/subsystems/even_more_minimal_nav/dem_costmap/maps/north_site800m.tif deleted file mode 100644 index ec4e9fca..00000000 --- a/src/subsystems/even_more_minimal_nav/dem_costmap/maps/north_site800m.tif +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:122777571140e3b8484bc334cdbdf3247eacf3a9c06a6624ec7cdc340476a3a3 -size 2559156 diff --git a/src/subsystems/even_more_minimal_nav/dem_costmap/package.xml b/src/subsystems/even_more_minimal_nav/dem_costmap/package.xml deleted file mode 100644 index 00fe8e66..00000000 --- a/src/subsystems/even_more_minimal_nav/dem_costmap/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - dem_costmap - 0.0.0 - DEM to Costmap converter for terrain-based navigation - mdurrani - Apache-2.0 - - ament_cmake - rclcpp - nav_msgs - sensor_msgs - cv_bridge - libopencv-dev - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/subsystems/even_more_minimal_nav/dem_costmap/src/map_node.cpp b/src/subsystems/even_more_minimal_nav/dem_costmap/src/map_node.cpp deleted file mode 100644 index 637faf71..00000000 --- a/src/subsystems/even_more_minimal_nav/dem_costmap/src/map_node.cpp +++ /dev/null @@ -1,236 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -class DEMCostmapConverter : public rclcpp::Node -{ -public: - DEMCostmapConverter() : Node("dem_costmap_converter") - { - // Declare parameters - this->declare_parameter("dem_file_path", ""); - this->declare_parameter("map_resolution", 1.0); // meters per pixel - this->declare_parameter("max_passable_slope_degrees", 15.0); - this->declare_parameter("output_frame", "map"); - this->declare_parameter("origin_x", 0.0); - this->declare_parameter("origin_y", 0.0); - - // Get parameters - std::string dem_file_path = this->get_parameter("dem_file_path").as_string(); - map_resolution_ = this->get_parameter("map_resolution").as_double(); - max_passable_slope_degrees_ = this->get_parameter("max_passable_slope_degrees").as_double(); - output_frame_ = this->get_parameter("output_frame").as_string(); - origin_x_ = this->get_parameter("origin_x").as_double(); - origin_y_ = this->get_parameter("origin_y").as_double(); - - // Create publisher with transient_local QoS for static maps - costmap_publisher_ = this->create_publisher( - "map", rclcpp::QoS(1).transient_local()); - - RCLCPP_INFO(this->get_logger(), "DEM Costmap Converter initialized"); - RCLCPP_INFO(this->get_logger(), "DEM file: %s", dem_file_path.c_str()); - RCLCPP_INFO(this->get_logger(), "Resolution: %.2f m/pixel", map_resolution_); - RCLCPP_INFO(this->get_logger(), "Max passable slope: %.1f degrees", max_passable_slope_degrees_); - - // Load and process DEM - costmap_ready_ = false; - if (!dem_file_path.empty()) { - loadAndProcessDEM(dem_file_path); - } else { - RCLCPP_ERROR(this->get_logger(), "No DEM file path provided. Use param 'dem_file_path'"); - RCLCPP_ERROR(this->get_logger(), "Node will not publish costmap without valid DEM file"); - } - - // Publish at 1Hz - timer_ = this->create_wall_timer( - std::chrono::seconds(1), - std::bind(&DEMCostmapConverter::publishCostmap, this)); - } - -private: - void loadAndProcessDEM(const std::string& file_path) - { - RCLCPP_INFO(this->get_logger(), "Loading DEM from: %s", file_path.c_str()); - - // Check if file exists - if (!std::filesystem::exists(file_path)) { - RCLCPP_ERROR(this->get_logger(), "DEM file does not exist: %s", file_path.c_str()); - return; - } - - // Load DEM using OpenCV (supports TIFF) - cv::Mat dem = cv::imread(file_path, cv::IMREAD_ANYDEPTH | cv::IMREAD_GRAYSCALE); - - if (dem.empty()) { - RCLCPP_ERROR(this->get_logger(), "Failed to load DEM file: %s", file_path.c_str()); - return; - } - - RCLCPP_INFO(this->get_logger(), "DEM loaded successfully: %dx%d pixels", dem.cols, dem.rows); - - // Convert to float for calculations - cv::Mat dem_float; - dem.convertTo(dem_float, CV_32F); - - // Calculate slope map - cv::Mat slope_map = calculateSlope(dem_float); - - // Convert slope to costmap - costmap_msg_ = createCostmapFromSlope(slope_map, dem.cols, dem.rows); - costmap_ready_ = true; - - RCLCPP_INFO(this->get_logger(), "Costmap generated successfully"); - } - - cv::Mat calculateSlope(const cv::Mat& dem) - { - RCLCPP_INFO(this->get_logger(), "Calculating slope map..."); - - cv::Mat grad_x, grad_y; - cv::Mat slope_radians = cv::Mat::zeros(dem.size(), CV_32F); - - // Calculate gradients using Sobel operator - cv::Sobel(dem, grad_x, CV_32F, 1, 0, 3); - cv::Sobel(dem, grad_y, CV_32F, 0, 1, 3); - - // Scale gradients by resolution to get proper slope - grad_x = grad_x / (8.0 * map_resolution_); // Sobel scale factor is 8 - grad_y = grad_y / (8.0 * map_resolution_); - - // Calculate slope magnitude - for (int i = 0; i < dem.rows; ++i) { - for (int j = 0; j < dem.cols; ++j) { - float dx = grad_x.at(i, j); - float dy = grad_y.at(i, j); - float slope_rad = std::atan(std::sqrt(dx*dx + dy*dy)); - slope_radians.at(i, j) = slope_rad; - } - } - - // Convert to degrees - cv::Mat slope_degrees; - slope_radians *= (180.0 / M_PI); - slope_degrees = slope_radians; - - return slope_degrees; - } - - nav_msgs::msg::OccupancyGrid createCostmapFromSlope(const cv::Mat& slope_map, int width, int height) - { - RCLCPP_INFO(this->get_logger(), "Converting slope to costmap..."); - - nav_msgs::msg::OccupancyGrid costmap; - costmap.header.frame_id = output_frame_; - costmap.header.stamp = this->get_clock()->now(); - - // Set map info - costmap.info.resolution = map_resolution_; - costmap.info.width = width; - costmap.info.height = height; - costmap.info.origin.position.x = origin_x_; - costmap.info.origin.position.y = origin_y_; - costmap.info.origin.position.z = 0.0; - costmap.info.origin.orientation.w = 1.0; - - // Calculate and log costmap bounds - double costmap_max_x = origin_x_ + (width * map_resolution_); - double costmap_max_y = origin_y_ + (height * map_resolution_); - - RCLCPP_INFO(this->get_logger(), "Costmap bounds:"); - RCLCPP_INFO(this->get_logger(), " Origin: (%.2f, %.2f)", origin_x_, origin_y_); - RCLCPP_INFO(this->get_logger(), " Size: %dx%d pixels", width, height); - RCLCPP_INFO(this->get_logger(), " Resolution: %.2f m/pixel", map_resolution_); - RCLCPP_INFO(this->get_logger(), " Bounds: X[%.2f, %.2f], Y[%.2f, %.2f]", - origin_x_, costmap_max_x, origin_y_, costmap_max_y); - RCLCPP_INFO(this->get_logger(), " Total coverage: %.2f x %.2f meters", - width * map_resolution_, height * map_resolution_); - - // Resize data array - costmap.data.resize(width * height); - - // Convert slope to cost values following nav2 costmap2d architecture - int lethal_count = 0; - for (int i = 0; i < height; ++i) { - for (int j = 0; j < width; ++j) { - float slope_deg = slope_map.at(i, j); - int cost = slopeToCost(slope_deg); - - // OpenCV uses (row, col) but OccupancyGrid uses (x, y) - // Need to flip Y coordinate for ROS convention - int ros_i = height - 1 - i; - costmap.data[ros_i * width + j] = cost; - - if (cost >= 99) lethal_count++; - } - } - - RCLCPP_INFO(this->get_logger(), - "Costmap conversion complete. Lethal/high-cost cells: %d/%d (%.1f%%)", - lethal_count, width * height, - 100.0 * lethal_count / (width * height)); - - return costmap; - } - - int slopeToCost(float slope_degrees) - { - // Nav2 costmap2d cost values: - // 0: Free space - // 1-252: Scaled costs - // 253: Possibly circumscribed - // 254: Lethal obstacle (impassable) - // 255: No information / unknown - - if (slope_degrees >= max_passable_slope_degrees_) { - return 254; // Lethal - impassable - } else if (slope_degrees >= 10.0) { - // High cost: linear scale from 150-252 for 10-15 degrees - float ratio = (slope_degrees - 10.0) / (max_passable_slope_degrees_ - 10.0); - return static_cast(150 + ratio * 102); - } else if (slope_degrees >= 5.0) { - // Medium cost: linear scale from 50-149 for 5-10 degrees - float ratio = (slope_degrees - 5.0) / 5.0; - return static_cast(50 + ratio * 99); - } else { - // Low cost: linear scale from 0-49 for 0-5 degrees - float ratio = slope_degrees / 5.0; - return static_cast(ratio * 49); - } - } - - - void publishCostmap() - { - if (costmap_ready_) { - costmap_msg_.header.stamp = this->get_clock()->now(); - costmap_publisher_->publish(costmap_msg_); - } else { - RCLCPP_DEBUG(this->get_logger(), "Costmap not ready, skipping publish"); - } - } - - // Member variables - rclcpp::Publisher::SharedPtr costmap_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - nav_msgs::msg::OccupancyGrid costmap_msg_; - bool costmap_ready_; - - double map_resolution_; - double max_passable_slope_degrees_; - std::string output_frame_; - double origin_x_; - double origin_y_; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/__init__.py b/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py b/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py deleted file mode 100644 index 64eb9c4b..00000000 --- a/src/subsystems/even_more_minimal_nav/emmn_visualizer/emmn_visualizer/emmn_visualizer_node.py +++ /dev/null @@ -1,711 +0,0 @@ -import math -import threading -from collections import deque - -import matplotlib -matplotlib.use('TkAgg') -import matplotlib.pyplot as plt -import matplotlib.gridspec as gridspec -from matplotlib.animation import FuncAnimation -from matplotlib.widgets import CheckButtons -import numpy as np -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, QoSPresetProfiles -from geometry_msgs.msg import PoseStamped -from nav_msgs.msg import Path, OccupancyGrid -from sensor_msgs.msg import LaserScan, NavSatFix -from std_msgs.msg import Bool, String - -from msgs.msg import NavStatus, Heading, PlannerEvent - -# ─── Constants ──────────────────────────────────────────────────────────────── - -STATE_COLORS = { - 'IDLE': '#888888', - 'NAVIGATING': '#00cc55', - 'ARRIVING': '#ffaa00', - 'STOPPED_AT_TARGET': '#44aaff', - 'STOPPED_AT_RETURN': '#44aaff', - 'ABORTING': '#ff3333', - 'RETURNING': '#aa44ff', - 'TELEOP': '#ff8800', -} -DEFAULT_STATE_COLOR = '#444444' - -PLANNER_EVENT_NAMES = { - PlannerEvent.NEW_GOAL: 'NEW_GOAL', - PlannerEvent.PLANNING: 'PLANNING', - PlannerEvent.PLAN_SUCCEEDED: 'PLAN_OK', - PlannerEvent.PLAN_FAILED: 'PLAN_FAILED', -} -PLAN_FAILED_COLOR = '#ff4444' -PLAN_OK_COLOR = '#44ff88' - -# ─── Coordinate utilities ────────────────────────────────────────────────────── - -def quat_to_yaw(q): - siny_cosp = 2 * (q.w * q.z + q.x * q.y) - cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z) - return math.atan2(siny_cosp, cosy_cosp) - - -class CoordinateConverter: - """WGS-84 ENU ↔ LLA conversion for hover coordinate display.""" - - def __init__(self): - self.origin_lat = None - self.a = 6378137.0 - self.f = 1 / 298.257223563 - self.b = self.a * (1 - self.f) - self.e2 = (self.a**2 - self.b**2) / self.a**2 - - def set_origin(self, lat, lon, alt): - self.origin_lat = lat - lr, lo = math.radians(lat), math.radians(lon) - self.origin_lat_rad, self.origin_lon_rad = lr, lo - self.x0, self.y0, self.z0 = self._lla_to_ecef(lat, lon, alt) - self.s_lat, self.c_lat = math.sin(lr), math.cos(lr) - self.s_lon, self.c_lon = math.sin(lo), math.cos(lo) - - def _lla_to_ecef(self, lat, lon, alt): - lr, lo = math.radians(lat), math.radians(lon) - N = self.a / math.sqrt(1 - self.e2 * math.sin(lr)**2) - return ( - (N + alt) * math.cos(lr) * math.cos(lo), - (N + alt) * math.cos(lr) * math.sin(lo), - (N * (1 - self.e2) + alt) * math.sin(lr), - ) - - def enu_to_lla(self, e, n, u=0.0): - if self.origin_lat is None: - return None, None - dx = -self.s_lon * e - self.c_lon * self.s_lat * n + self.c_lon * self.c_lat * u - dy = self.c_lon * e - self.s_lon * self.s_lat * n + self.s_lon * self.c_lat * u - dz = self.c_lat * n + self.s_lat * u - x, y, z = dx + self.x0, dy + self.y0, dz + self.z0 - lon = math.atan2(y, x) - p = math.sqrt(x**2 + y**2) - lat = math.atan2(z, p * (1 - self.e2)) - for _ in range(5): - N = self.a / math.sqrt(1 - self.e2 * math.sin(lat)**2) - lat = math.atan2(z + self.e2 * N * math.sin(lat), p) - return math.degrees(lat), math.degrees(lon) - - -# ─── ROS node (data collection only) ───────────────────────────────────────── - -class EMMNVisualizerNode(Node): - def __init__(self): - super().__init__('emmn_visualizer') - - self.lock = threading.Lock() - - # Data store - self.robot_pose = None - self.global_path = None - self.goal_pose = None - self.nav_status = None - self.nav_enabled = False - self.nav_mode = 'stopped' - self.heading_msg = None - self.costmap = None - self.latest_scan = None # sensor_msgs/LaserScan - self.converter = CoordinateConverter() - self.event_log = deque(maxlen=10) # (timestamp_str, event_name, is_failure) - self.new_path_received = False - self.new_costmap_received = False - - # QoS profiles - transient_qos = QoSProfile( - depth=1, - durability=DurabilityPolicy.TRANSIENT_LOCAL, - reliability=ReliabilityPolicy.RELIABLE, - ) - reliable_qos = QoSProfile( - depth=1, - reliability=ReliabilityPolicy.RELIABLE, - ) - - self.create_subscription(PoseStamped, '/robot_pose', self._pose_cb, 10) - self.create_subscription(Path, '/global_path', self._path_cb, transient_qos) - self.create_subscription(OccupancyGrid, '/map', self._map_cb, transient_qos) - self.create_subscription(PoseStamped, '/goal_pose', self._goal_cb, 10) - self.create_subscription(NavStatus, '/nav_status', self._status_cb, 10) - self.create_subscription(Bool, '/nav_enabled', self._enabled_cb, reliable_qos) - self.create_subscription(String, '/nav_mode', self._mode_cb, 10) - self.create_subscription(PlannerEvent, '/planner_event', self._event_cb, 10) - self.create_subscription(NavSatFix, '/gps/fix', self._gps_cb, 10) - self.create_subscription(Heading, '/gps/heading', self._heading_cb, 10) - self.create_subscription(LaserScan, '/scan', self._scan_cb, - QoSPresetProfiles.SENSOR_DATA.value) - - # ── Callbacks ────────────────────────────────────────────────────────────── - - def _pose_cb(self, msg): - with self.lock: - self.robot_pose = msg - - def _path_cb(self, msg): - with self.lock: - self.global_path = msg - self.new_path_received = True - - def _map_cb(self, msg): - with self.lock: - self.costmap = msg - self.new_costmap_received = True - - def _goal_cb(self, msg): - with self.lock: - self.goal_pose = msg - - def _status_cb(self, msg): - with self.lock: - self.nav_status = msg - - def _enabled_cb(self, msg): - with self.lock: - self.nav_enabled = msg.data - - def _mode_cb(self, msg): - with self.lock: - self.nav_mode = msg.data - - def _event_cb(self, msg): - now = self.get_clock().now().to_msg() - ts = f'{now.sec % 86400 // 3600:02d}:{now.sec % 3600 // 60:02d}:{now.sec % 60:02d}' - name = PLANNER_EVENT_NAMES.get(msg.event, f'EVENT_{msg.event}') - failed = (msg.event == PlannerEvent.PLAN_FAILED) - with self.lock: - self.event_log.append((ts, name, failed)) - - def _gps_cb(self, msg): - with self.lock: - if self.converter.origin_lat is None and msg.status.status >= 0: - self.converter.set_origin(msg.latitude, msg.longitude, msg.altitude) - - def _heading_cb(self, msg): - with self.lock: - self.heading_msg = msg - - def _scan_cb(self, msg): - with self.lock: - self.latest_scan = msg - - -# ─── Plotting ───────────────────────────────────────────────────────────────── - -def _find_lookahead(path_poses, rx, ry, lookahead_m): - """Approximate the VFP lookahead point: closest path index + walk forward.""" - if not path_poses: - return None - best_i, best_d2 = 0, float('inf') - for i, p in enumerate(path_poses): - dx, dy = p.pose.position.x - rx, p.pose.position.y - ry - d2 = dx*dx + dy*dy - if d2 < best_d2: - best_d2, best_i = d2, i - for i in range(best_i, len(path_poses)): - dx = path_poses[i].pose.position.x - rx - dy = path_poses[i].pose.position.y - ry - if math.hypot(dx, dy) >= lookahead_m: - return path_poses[i].pose.position.x, path_poses[i].pose.position.y - last = path_poses[-1].pose.position - return last.x, last.y - - -def _closest_path_point(path_poses, rx, ry): - """Return the closest point on the path (segment-level) and its distance.""" - if not path_poses or len(path_poses) < 2: - return None - min_dist, best_pt = float('inf'), None - for i in range(len(path_poses) - 1): - ax, ay = path_poses[i].pose.position.x, path_poses[i].pose.position.y - bx, by = path_poses[i+1].pose.position.x, path_poses[i+1].pose.position.y - dx, dy = bx - ax, by - ay - len2 = dx*dx + dy*dy - if len2 < 1e-10: - cx, cy = ax, ay - else: - t = max(0.0, min(1.0, ((rx-ax)*dx + (ry-ay)*dy) / len2)) - cx, cy = ax + t*dx, ay + t*dy - d = math.hypot(rx - cx, ry - cy) - if d < min_dist: - min_dist, best_pt = d, (cx, cy) - return best_pt - - -class VisualizerPlot: - LOOKAHEAD_M = 3.0 # default — matches nav_params.yaml lookahead_dist_m - - def __init__(self, node: EMMNVisualizerNode): - self.node = node - - # ── Figure layout ────────────────────────────────────────────────────── - self.fig = plt.figure(figsize=(15, 9)) - self.fig.patch.set_facecolor('#1a1a2e') - - gs = gridspec.GridSpec( - 1, 2, - width_ratios=[3, 1], - left=0.04, right=0.98, - top=0.96, bottom=0.10, - wspace=0.04, - ) - - self.ax_map = self.fig.add_subplot(gs[0, 0]) - self.ax_status = self.fig.add_subplot(gs[0, 1]) - - self._style_map_axes() - self._style_status_axes() - - # ── Map elements ──────────────────────────────────────────────────────── - # Costmap (imshow) - self.costmap_img = None # created lazily on first costmap - - # Global path - self.path_line, = self.ax_map.plot([], [], '--', color='#5588ff', - linewidth=1.8, label='Global Path', zorder=2) - - # Cross-track error: line from robot to closest path point - self.xte_line, = self.ax_map.plot([], [], '-', color='#ffff00', - linewidth=1.5, label='XTE', zorder=5, - linestyle='dashed') - self.xte_dot, = self.ax_map.plot([], [], 'o', color='#ffff00', - markersize=6, zorder=6) - - # Lookahead circle + line + point - self.lookahead_circle = plt.Circle((0, 0), self.LOOKAHEAD_M, - color='#88ccff', fill=False, linewidth=1.2, - linestyle='dotted', zorder=4) - self.ax_map.add_patch(self.lookahead_circle) - self.lookahead_line, = self.ax_map.plot([], [], '-', color='#88ccff', - linewidth=1.2, zorder=5, linestyle='dotted') - self.lookahead_pt, = self.ax_map.plot([], [], 'D', color='#88ccff', - markersize=7, zorder=7, label='Lookahead') - - # Robot marker + heading arrow - self.robot_dot, = self.ax_map.plot([], [], 'o', color='#ff4444', - markersize=10, zorder=10, label='Robot') - self.heading_line,= self.ax_map.plot([], [], '-', color='#ffaa00', - linewidth=2.5, zorder=11) - - # Goal marker - self.goal_marker, = self.ax_map.plot([], [], 'x', color='white', - markersize=12, markeredgewidth=2.5, - zorder=9, label='Goal') - - # Laser scan hits — scatter of obstacle points in map frame - self.scan_scatter = self.ax_map.scatter( - [], [], s=4, c='#ff6633', alpha=0.65, zorder=8, label='Scan hits') - - self.ax_map.legend(loc='lower left', framealpha=0.6, - facecolor='#1a1a2e', labelcolor='white', - fontsize=9) - - # Hover text - self.hover_text = self.ax_map.text( - 0.02, 0.02, '', transform=self.ax_map.transAxes, - fontsize=8, color='#cccccc', - bbox=dict(facecolor='#1a1a2e', alpha=0.8, edgecolor='#555555'), - verticalalignment='bottom', - ) - - # ── Status panel elements ─────────────────────────────────────────────── - self.status_text = self.ax_status.text( - 0.05, 0.98, 'Waiting for data...', - transform=self.ax_status.transAxes, - fontsize=9, color='white', family='monospace', - verticalalignment='top', - bbox=dict(facecolor='#0d0d1a', alpha=0.0, edgecolor='none'), - ) - - # ── Checkboxes (bottom strip) ─────────────────────────────────────────── - cb_labels = ['Auto-Fit', 'Show Costmap', 'Show Lookahead', 'Show XTE', 'Show Scan'] - cb_states = [True, True, True, True, True] - rax = self.fig.add_axes([0.04, 0.01, 0.38, 0.07]) - rax.set_facecolor('#1a1a2e') - self.check = CheckButtons(rax, cb_labels, cb_states) - for txt in self.check.labels: - txt.set_color('white') - txt.set_fontsize(9) - self.auto_fit = True - self.show_costmap = True - self.show_lookahead = True - self.show_xte = True - self.show_scan = True - self.check.on_clicked(self._on_toggle) - - # ── Events ───────────────────────────────────────────────────────────── - self.fig.canvas.mpl_connect('motion_notify_event', self._on_hover) - - # ── Axis styling ─────────────────────────────────────────────────────────── - - def _style_map_axes(self): - ax = self.ax_map - ax.set_facecolor('#0d0d1a') - ax.tick_params(colors='#888888', labelsize=8) - for spine in ax.spines.values(): - spine.set_edgecolor('#333355') - ax.grid(True, linestyle=':', alpha=0.3, color='#444466') - ax.set_aspect('equal') - ax.set_xlabel('East (m)', color='#888888', fontsize=9) - ax.set_ylabel('North (m)', color='#888888', fontsize=9) - ax.set_title('Even More Minimal Nav — Live Visualizer', color='#aaaacc', fontsize=11) - - def _style_status_axes(self): - ax = self.ax_status - ax.set_facecolor('#0d0d1a') - ax.set_xlim(0, 1) - ax.set_ylim(0, 1) - ax.axis('off') - for spine in ax.spines.values(): - spine.set_edgecolor('#333355') - - # ── Toggles ──────────────────────────────────────────────────────────────── - - def _on_toggle(self, label): - if label == 'Auto-Fit': - self.auto_fit = not self.auto_fit - if self.auto_fit: - with self.node.lock: - self.node.new_path_received = True - elif label == 'Show Costmap': - self.show_costmap = not self.show_costmap - if self.costmap_img is not None: - self.costmap_img.set_visible(self.show_costmap) - elif label == 'Show Lookahead': - self.show_lookahead = not self.show_lookahead - elif label == 'Show XTE': - self.show_xte = not self.show_xte - elif label == 'Show Scan': - self.show_scan = not self.show_scan - self.scan_scatter.set_visible(self.show_scan) - - # ── Hover ────────────────────────────────────────────────────────────────── - - def _on_hover(self, event): - if event.inaxes != self.ax_map or event.xdata is None: - return - with self.node.lock: - lat, lon = self.node.converter.enu_to_lla(event.xdata, event.ydata) - if lat is not None: - self.hover_text.set_text( - f'E:{event.xdata:+.1f}m N:{event.ydata:+.1f}m\n' - f'Lat:{lat:.6f} Lon:{lon:.6f}' - ) - else: - self.hover_text.set_text(f'E:{event.xdata:+.1f}m N:{event.ydata:+.1f}m') - - # ── Main update (called at ~10 Hz by FuncAnimation) ─────────────────────── - - def update(self, _frame): - with self.node.lock: - robot_pose = self.node.robot_pose - global_path = self.node.global_path - goal_pose = self.node.goal_pose - nav_status = self.node.nav_status - nav_enabled = self.node.nav_enabled - nav_mode = self.node.nav_mode - heading_msg = self.node.heading_msg - costmap = self.node.costmap - latest_scan = self.node.latest_scan - event_log = list(self.node.event_log) - new_path = self.node.new_path_received - new_costmap = self.node.new_costmap_received - - if new_costmap: - self.node.new_costmap_received = False - - path_poses = global_path.poses if global_path else [] - - rx = ry = yaw = None - if robot_pose: - rx, ry = robot_pose.pose.position.x, robot_pose.pose.position.y - if heading_msg: - yaw = math.radians(heading_msg.heading) - else: - yaw = quat_to_yaw(robot_pose.pose.orientation) - - # ── Costmap ───────────────────────────────────────────────────────────── - if new_costmap and costmap is not None: - self._render_costmap(costmap) - - # ── Global path ───────────────────────────────────────────────────────── - if path_poses: - xs = [p.pose.position.x for p in path_poses] - ys = [p.pose.position.y for p in path_poses] - self.path_line.set_data(xs, ys) - else: - self.path_line.set_data([], []) - - # ── Robot + heading ───────────────────────────────────────────────────── - if rx is not None: - self.robot_dot.set_data([rx], [ry]) - L = 2.5 - self.heading_line.set_data( - [rx, rx + L * math.cos(yaw)], - [ry, ry + L * math.sin(yaw)], - ) - self.lookahead_circle.center = (rx, ry) - else: - self.robot_dot.set_data([], []) - self.heading_line.set_data([], []) - - # ── Goal ──────────────────────────────────────────────────────────────── - if goal_pose: - self.goal_marker.set_data( - [goal_pose.pose.position.x], [goal_pose.pose.position.y]) - else: - self.goal_marker.set_data([], []) - - # ── XTE indicator ─────────────────────────────────────────────────────── - if self.show_xte and rx is not None and len(path_poses) >= 2: - cp = _closest_path_point(path_poses, rx, ry) - if cp: - self.xte_line.set_data([rx, cp[0]], [ry, cp[1]]) - self.xte_dot.set_data([cp[0]], [cp[1]]) - else: - self.xte_line.set_data([], []) - self.xte_dot.set_data([], []) - else: - self.xte_line.set_data([], []) - self.xte_dot.set_data([], []) - - # ── Lookahead ──────────────────────────────────────────────────────────── - lp = None - if self.show_lookahead and rx is not None and path_poses: - lp = _find_lookahead(path_poses, rx, ry, self.LOOKAHEAD_M) - self.lookahead_circle.set_visible(self.show_lookahead and rx is not None) - if lp and self.show_lookahead: - self.lookahead_pt.set_data([lp[0]], [lp[1]]) - self.lookahead_line.set_data([rx, lp[0]], [ry, lp[1]]) - else: - self.lookahead_pt.set_data([], []) - self.lookahead_line.set_data([], []) - - # ── Laser scan hits ────────────────────────────────────────────────────── - # Convert scan from base_link polar coords → map Cartesian and scatter-plot. - # Assumes scan is in base_link frame (target_frame: base_link in p2l config). - if self.show_scan and latest_scan is not None and rx is not None: - sc = latest_scan - pts_x, pts_y = [], [] - for i, r in enumerate(sc.ranges): - if not math.isfinite(r) or r < sc.range_min or r > sc.range_max: - continue - alpha = sc.angle_min + i * sc.angle_increment # angle in base_link - map_x = rx + r * math.cos(yaw + alpha) - map_y = ry + r * math.sin(yaw + alpha) - pts_x.append(map_x) - pts_y.append(map_y) - if pts_x: - self.scan_scatter.set_offsets(np.column_stack([pts_x, pts_y])) - else: - self.scan_scatter.set_offsets(np.empty((0, 2))) - self.scan_scatter.set_visible(True) - else: - self.scan_scatter.set_offsets(np.empty((0, 2))) - self.scan_scatter.set_visible(self.show_scan) - - # ── Viewport ───────────────────────────────────────────────────────────── - if self.auto_fit and new_path: - self._fit_viewport(path_poses, rx, ry) - with self.node.lock: - self.node.new_path_received = False - - # ── Status panel ───────────────────────────────────────────────────────── - self._update_status(nav_status, nav_enabled, nav_mode, event_log, - heading_msg, costmap, path_poses, rx, ry, - latest_scan=latest_scan) - - # ── Costmap renderer ───────────────────────────────────────────────────── - - def _render_costmap(self, costmap): - info = costmap.info - W, H = info.width, info.height - res = info.resolution - ox = info.origin.position.x - oy = info.origin.position.y - - arr = np.array(costmap.data, dtype=np.int16).reshape(H, W).astype(np.float32) - arr[arr < 0] = np.nan # unknown cells → transparent - arr = np.clip(arr, 0, 254) / 254.0 - - extent = [ox, ox + W * res, oy, oy + H * res] - cmap = plt.cm.RdYlGn_r.copy() - cmap.set_bad(color='none') - - if self.costmap_img is None: - self.costmap_img = self.ax_map.imshow( - arr, origin='lower', extent=extent, - cmap=cmap, vmin=0.0, vmax=1.0, - alpha=0.45, zorder=1, - interpolation='nearest', - ) - self.fig.colorbar( - self.costmap_img, ax=self.ax_map, - fraction=0.025, pad=0.01, - label='Slope Cost (0=safe, 1=lethal)', - ).ax.yaxis.label.set_color('#888888') - else: - self.costmap_img.set_data(arr) - self.costmap_img.set_extent(extent) - - self.costmap_img.set_visible(self.show_costmap) - - # ── Viewport fit ──────────────────────────────────────────────────────────── - - def _fit_viewport(self, path_poses, rx, ry): - pts_x, pts_y = [], [] - for p in path_poses: - pts_x.append(p.pose.position.x) - pts_y.append(p.pose.position.y) - if rx is not None: - pts_x.append(rx) - pts_y.append(ry) - if not pts_x: - return - mn_x, mx_x = min(pts_x), max(pts_x) - mn_y, mx_y = min(pts_y), max(pts_y) - dx = max(mx_x - mn_x, 15.0) - dy = max(mx_y - mn_y, 15.0) - cx = (mn_x + mx_x) / 2.0 - cy = (mn_y + mx_y) / 2.0 - pad = 0.65 - self.ax_map.set_xlim(cx - dx * pad, cx + dx * pad) - self.ax_map.set_ylim(cy - dy * pad, cy + dy * pad) - - # ── Status panel text ──────────────────────────────────────────────────────── - - def _update_status(self, nav_status, nav_enabled, nav_mode, event_log, - heading_msg, costmap, path_poses, rx, ry, latest_scan=None): - state = nav_status.state if nav_status else 'NO DATA' - color = STATE_COLORS.get(state, DEFAULT_STATE_COLOR) - - lines = [] - lines.append(f'{"═"*26}') - lines.append(f' {state:^24s}') - lines.append(f'{"═"*26}') - - # Target info - if nav_status and nav_status.active_target_id: - ttype = nav_status.active_target_type - lines.append(f' TARGET : {nav_status.active_target_id}') - lines.append(f' Type : {ttype}') - lines.append(f' Return : {"Yes" if nav_status.is_return else "No"}') - else: - lines.append(' TARGET : (none)') - lines.append('') - - # Metrics - lines.append('── Metrics ──────────────────') - if nav_status: - dist = nav_status.distance_to_goal_m - xte = nav_status.cross_track_error_m - herr = math.degrees(nav_status.heading_error_rad) - avel = nav_status.robot_speed_mps # actually imu angular vel - dist_str = f'{dist:.1f}m' if dist >= 0 else 'N/A' - xte_str = f'{xte:.2f}m' if xte >= 0 else 'N/A' - lines.append(f' Dist to Goal : {dist_str:>8s}') - lines.append(f' Cross-Track : {xte_str:>8s}') - lines.append(f' Heading Err : {herr:>+7.1f}°') - lines.append(f' AngVel (IMU) : {avel:>7.3f} r/s') - else: - lines.append(' (no nav_status)') - lines.append('') - - # Heading - lines.append('── Heading ──────────────────') - if heading_msg: - lines.append(f' ENU heading : {heading_msg.heading:>+7.1f}°') - lines.append(f' Compass : {heading_msg.compass_bearing:>+7.1f}°') - else: - lines.append(' (no heading msg)') - lines.append('') - - # System state - lines.append('── System ───────────────────') - enabled_str = 'YES' if nav_enabled else 'NO' - lines.append(f' Nav Enabled : {enabled_str}') - lines.append(f' Nav Mode : {nav_mode}') - if costmap is not None: - W = costmap.info.width - H = costmap.info.height - lines.append(f' Costmap : {W}×{H}') - else: - lines.append(' Costmap : not received') - lines.append(f' Path poses : {len(path_poses)}') - lines.append('') - - # Robot position - lines.append('── Robot Position ───────────') - if rx is not None: - lat, lon = self.node.converter.enu_to_lla(rx, ry) - lines.append(f' E: {rx:>+8.2f}m') - lines.append(f' N: {ry:>+8.2f}m') - if lat is not None: - lines.append(f' Lat: {lat:.6f}') - lines.append(f' Lon: {lon:.6f}') - else: - lines.append(' (no robot pose)') - lines.append('') - - # Laser scan stats - lines.append('── Laser Scan ───────────────') - if latest_scan is not None: - sc = latest_scan - valid_ranges = [r for r in sc.ranges - if math.isfinite(r) and sc.range_min <= r <= sc.range_max] - close_ranges = [r for r in valid_ranges if r < 3.0] - min_r = min(valid_ranges) if valid_ranges else float('nan') - node_now = self.node.get_clock().now().to_msg() - stamp = sc.header.stamp - age_s = (node_now.sec - stamp.sec) + (node_now.nanosec - stamp.nanosec) * 1e-9 - lines.append(f' Frame : {sc.header.frame_id}') - lines.append(f' Age : {age_s:>6.2f}s') - lines.append(f' Beams : {len(sc.ranges)} total / {len(valid_ranges)} valid') - lines.append(f' Min rng : {min_r:>6.2f}m') - lines.append(f' <3m hits: {len(close_ranges)}') - else: - lines.append(' (no scan received)') - lines.append('') - - # Event log - lines.append('── Planner Events ───────────') - if event_log: - for ts, name, failed in reversed(event_log): - marker = '!!' if failed else ' ' - lines.append(f' {marker}[{ts}] {name}') - else: - lines.append(' (none yet)') - - text = '\n'.join(lines) - self.status_text.set_text(text) - - # Color the state line background dynamically via the text bbox - self.status_text.set_bbox(dict( - facecolor=color + '33', # hex color + 20% alpha - edgecolor=color, - linewidth=0.8, - )) - - -# ─── Entry point ────────────────────────────────────────────────────────────── - -def main(args=None): - rclpy.init(args=args) - node = EMMNVisualizerNode() - - spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) - spin_thread.start() - - viz = VisualizerPlot(node) - ani = FuncAnimation(viz.fig, viz.update, interval=100, blit=False, cache_frame_data=False) - - plt.show() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/subsystems/even_more_minimal_nav/emmn_visualizer/package.xml b/src/subsystems/even_more_minimal_nav/emmn_visualizer/package.xml deleted file mode 100644 index 70a5d095..00000000 --- a/src/subsystems/even_more_minimal_nav/emmn_visualizer/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - emmn_visualizer - 0.0.0 - Debug visualizer for even_more_minimal_nav - ros - Apache-2.0 - - rclpy - nav_msgs - geometry_msgs - sensor_msgs - std_msgs - msgs - python3-matplotlib - python3-numpy - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/subsystems/even_more_minimal_nav/emmn_visualizer/resource/emmn_visualizer b/src/subsystems/even_more_minimal_nav/emmn_visualizer/resource/emmn_visualizer deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.cfg b/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.cfg deleted file mode 100644 index 234583bb..00000000 --- a/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/emmn_visualizer -[install] -install_scripts=$base/lib/emmn_visualizer diff --git a/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.py b/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.py deleted file mode 100644 index aa39ff14..00000000 --- a/src/subsystems/even_more_minimal_nav/emmn_visualizer/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'emmn_visualizer' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools', 'matplotlib', 'numpy'], - zip_safe=True, - maintainer='ros', - maintainer_email='mdurrani808@gmail.com', - description='Debug visualizer for even_more_minimal_nav', - license='Apache-2.0', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'emmn_visualizer_node = emmn_visualizer.emmn_visualizer_node:main', - ], - }, -) diff --git a/src/subsystems/even_more_minimal_nav/global_planner/src/global_planner_node.cpp b/src/subsystems/even_more_minimal_nav/global_planner/src/global_planner_node.cpp deleted file mode 100644 index 3baed4fd..00000000 --- a/src/subsystems/even_more_minimal_nav/global_planner/src/global_planner_node.cpp +++ /dev/null @@ -1,330 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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. - -// global_planner_node.cpp -// -// Converts a /goal_pose into a /global_path. -// -// Phase 1 (use_costmap: false): -// Straight-line interpolation from current TF position to goal. -// Runs immediately on every new goal with no external dependencies. -// -// Phase 2 (use_costmap: true, costmap received): -// A* on the OccupancyGrid from dem_costmap_converter. -// Cost to enter a cell: dist_to_neighbor + slope_weight * (grid_value / 254). -// Cells with grid_value >= 254 are lethal and impassable. -// Falls back to straight-line if A* cannot find a path. - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav_msgs/msg/path.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "tf2/exceptions.h" - -class GlobalPlanner : public rclcpp::Node -{ -public: - GlobalPlanner() : Node("global_planner") - { - declare_parameter("path_resolution_m", 1.0); - declare_parameter("use_costmap", false); - declare_parameter("slope_weight", 10.0); - - path_resolution_m_ = get_parameter("path_resolution_m").as_double(); - use_costmap_ = get_parameter("use_costmap").as_bool(); - slope_weight_ = get_parameter("slope_weight").as_double(); - - tf_buffer_ = std::make_shared(get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - path_pub_ = create_publisher( - "/global_path", rclcpp::QoS(1).transient_local()); - - goal_sub_ = create_subscription( - "/goal_pose", 10, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { onGoal(msg); }); - - // Costmap is optional — published by dem_costmap_converter with transient_local QoS. - costmap_sub_ = create_subscription( - "/map", rclcpp::QoS(1).transient_local(), - [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { - costmap_ = *msg; - }); - - RCLCPP_INFO(get_logger(), "GlobalPlanner ready (use_costmap=%s)", - use_costmap_ ? "true" : "false"); - } - -private: - // ── Goal callback ────────────────────────────────────────────────────────── - - void onGoal(const geometry_msgs::msg::PoseStamped::SharedPtr goal) - { - // Look up current robot position in map frame - geometry_msgs::msg::TransformStamped tf; - try { - tf = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(get_logger(), "TF map→base_link unavailable: %s — cannot plan", ex.what()); - return; - } - - const double sx = tf.transform.translation.x; - const double sy = tf.transform.translation.y; - const double gx = goal->pose.position.x; - const double gy = goal->pose.position.y; - - RCLCPP_INFO(get_logger(), "Planning (%.2f,%.2f) → (%.2f,%.2f)", sx, sy, gx, gy); - - // Phase 2: A* if enabled and costmap is available - if (use_costmap_ && costmap_.has_value()) { - auto maybe_path = planAstar(sx, sy, gx, gy); - if (maybe_path.has_value()) { - path_pub_->publish(*maybe_path); - RCLCPP_INFO(get_logger(), "A* path published (%zu poses)", maybe_path->poses.size()); - return; - } - RCLCPP_WARN(get_logger(), "A* failed — falling back to straight-line"); - } - - // Phase 1: straight-line - auto path = planStraightLine(sx, sy, gx, gy); - path_pub_->publish(path); - RCLCPP_INFO(get_logger(), "Straight-line path published (%zu poses)", path.poses.size()); - } - - // ── Phase 1: straight-line interpolation ────────────────────────────────── - - nav_msgs::msg::Path planStraightLine( - double sx, double sy, double gx, double gy) const - { - nav_msgs::msg::Path path; - path.header.frame_id = "map"; - path.header.stamp = now(); - - const double dist = std::hypot(gx - sx, gy - sy); - - if (dist < 1e-6) { - // Already at goal — single-pose path - geometry_msgs::msg::PoseStamped p; - p.header = path.header; - p.pose.position.x = gx; - p.pose.position.y = gy; - p.pose.orientation.w = 1.0; - path.poses.push_back(p); - return path; - } - - // Number of samples including both endpoints - const int n = std::max(2, static_cast(std::ceil(dist / path_resolution_m_)) + 1); - - // Heading quaternion: constant along the line - const double yaw = std::atan2(gy - sy, gx - sx); - const double half = yaw * 0.5; - const double qz = std::sin(half); - const double qw = std::cos(half); - - path.poses.reserve(n); - for (int i = 0; i < n; ++i) { - const double t = static_cast(i) / (n - 1); - geometry_msgs::msg::PoseStamped p; - p.header = path.header; - p.pose.position.x = sx + t * (gx - sx); - p.pose.position.y = sy + t * (gy - sy); - p.pose.orientation.z = qz; - p.pose.orientation.w = qw; - path.poses.push_back(p); - } - - return path; - } - - // ── Phase 2: A* on OccupancyGrid ────────────────────────────────────────── - // - // g(cell) = dist_to_neighbor + slope_weight_ * (grid_value / 254) - // - // OccupancyGrid.data is int8[], but dem_costmap_converter writes values in - // the nav2 range 0–254. Cast to uint8_t before comparing to avoid sign bugs - // (254 as int8_t = -2, which would incorrectly pass a < 254 guard). - - std::optional planAstar( - double sx, double sy, double gx, double gy) const - { - const auto & info = costmap_->info; - const int W = static_cast(info.width); - const int H = static_cast(info.height); - const double res = info.resolution; - const double ox = info.origin.position.x; - const double oy = info.origin.position.y; - - // World → grid cell (col, row) - auto toGrid = [&](double wx, double wy, int & col, int & row) -> bool { - col = static_cast((wx - ox) / res); - row = static_cast((wy - oy) / res); - return col >= 0 && col < W && row >= 0 && row < H; - }; - - // Grid cell center → world - auto toWorld = [&](int col, int row, double & wx, double & wy) { - wx = ox + (col + 0.5) * res; - wy = oy + (row + 0.5) * res; - }; - - int sc, sr, gc, gr; - if (!toGrid(sx, sy, sc, sr)) { - RCLCPP_WARN(get_logger(), "Start (%.2f,%.2f) is outside costmap bounds", sx, sy); - return std::nullopt; - } - if (!toGrid(gx, gy, gc, gr)) { - RCLCPP_WARN(get_logger(), "Goal (%.2f,%.2f) is outside costmap bounds", gx, gy); - return std::nullopt; - } - - // Reject lethal goal cell - const auto goal_val = static_cast(costmap_->data[gr * W + gc]); - if (goal_val >= 254) { - RCLCPP_WARN(get_logger(), "Goal cell is lethal (cost=%u) — A* cannot plan", goal_val); - return std::nullopt; - } - - // A* with 8-connected grid - // Open set: (f_cost, col, row) - using State = std::tuple; - std::priority_queue, std::greater> open; - - std::vector g_cost(W * H, std::numeric_limits::infinity()); - std::vector parent(W * H, -1); - - const int s_idx = sr * W + sc; - g_cost[s_idx] = 0.0; - open.emplace(0.0, sc, sr); - - // 8-directional moves: (dcol, drow, euclidean_distance) - constexpr int dcol[8] = { 1, -1, 0, 0, 1, 1, -1, -1}; - constexpr int drow[8] = { 0, 0, 1, -1, 1, -1, 1, -1}; - constexpr double step[8] = { 1, 1, 1, 1, M_SQRT2, M_SQRT2, M_SQRT2, M_SQRT2}; - - bool found = false; - while (!open.empty()) { - auto [f, cx, cy] = open.top(); open.pop(); - - const int idx = cy * W + cx; - if (f > g_cost[idx] + 1e-9) continue; // stale entry - if (cx == gc && cy == gr) { found = true; break; } - - for (int d = 0; d < 8; ++d) { - const int nx = cx + dcol[d]; - const int ny = cy + drow[d]; - if (nx < 0 || nx >= W || ny < 0 || ny >= H) continue; - - const auto cell_val = static_cast(costmap_->data[ny * W + nx]); - if (cell_val >= 254) continue; // lethal — cannot enter - - const double move_cost = - step[d] * res + slope_weight_ * static_cast(cell_val) / 254.0; - - const double ng = g_cost[idx] + move_cost; - const int n_idx = ny * W + nx; - if (ng < g_cost[n_idx]) { - g_cost[n_idx] = ng; - parent[n_idx] = idx; - // Euclidean heuristic (admissible) - const double h = std::hypot(nx - gc, ny - gr) * res; - open.emplace(ng + h, nx, ny); - } - } - } - - if (!found) { - RCLCPP_WARN(get_logger(), "A* exhausted open set — no path found"); - return std::nullopt; - } - - // Trace path from goal back to start, then reverse - std::vector> cells; // (col, row) - for (int cur = gr * W + gc; cur != -1; cur = parent[cur]) { - cells.emplace_back(cur % W, cur / W); - } - std::reverse(cells.begin(), cells.end()); - - nav_msgs::msg::Path path; - path.header.frame_id = "map"; - path.header.stamp = now(); - path.poses.reserve(cells.size()); - - for (size_t i = 0; i < cells.size(); ++i) { - double wx, wy; - toWorld(cells[i].first, cells[i].second, wx, wy); - - geometry_msgs::msg::PoseStamped p; - p.header = path.header; - p.pose.position.x = wx; - p.pose.position.y = wy; - - // Point toward the next cell; last pose inherits previous heading - if (i + 1 < cells.size()) { - double nwx, nwy; - toWorld(cells[i + 1].first, cells[i + 1].second, nwx, nwy); - const double yaw = std::atan2(nwy - wy, nwx - wx); - const double half = yaw * 0.5; - p.pose.orientation.z = std::sin(half); - p.pose.orientation.w = std::cos(half); - } else if (!path.poses.empty()) { - p.pose.orientation = path.poses.back().pose.orientation; - } else { - p.pose.orientation.w = 1.0; - } - - path.poses.push_back(p); - } - - return path; - } - - // ── Parameters ───────────────────────────────────────────────────────────── - double path_resolution_m_{1.0}; - bool use_costmap_{false}; - double slope_weight_{10.0}; - - // ── TF ───────────────────────────────────────────────────────────────────── - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - // ── ROS interfaces ────────────────────────────────────────────────────────── - rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Subscription::SharedPtr goal_sub_; - rclcpp::Subscription::SharedPtr costmap_sub_; - - // ── State ─────────────────────────────────────────────────────────────────── - std::optional costmap_; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt deleted file mode 100644 index 06c6db0e..00000000 --- a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(minimal_nav_bringup) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY launch config - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS scripts/reframe_pointcloud.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml deleted file mode 100644 index 7d12a733..00000000 --- a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - minimal_nav_bringup - 0.1.0 - Launch and configuration files for the Athena navigation stack (GPS-only, Nav2-free) - UMD Loop - Apache-2.0 - - ament_cmake - - gps_pose_publisher - dem_costmap - global_planner - vector_field_planner - mission_executive - athena_gps - pointcloud_to_laserscan - - - ament_cmake - - diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/scripts/reframe_pointcloud.py b/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/scripts/reframe_pointcloud.py deleted file mode 100755 index 662ab417..00000000 --- a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/scripts/reframe_pointcloud.py +++ /dev/null @@ -1,75 +0,0 @@ -#!/usr/bin/env python3 -# Copyright (c) 2025 UMD Loop -# -# 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. - -"""reframe_pointcloud.py - -Subscribes to a PointCloud2 topic and republishes each message with -header.frame_id replaced by a target frame name. - -This is used in simulation to relabel the ZED depth sensor frame -(athena/base_footprint/zed_depth_sensor) as base_link so that -pointcloud_to_laserscan needs no TF lookup. - -Parameters ----------- - input_topic (str, default /zed/zed_node/point_cloud/cloud_registered) - output_topic (str, default /zed/cloud_base_link) - target_frame (str, default base_link) -""" - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSPresetProfiles -from sensor_msgs.msg import PointCloud2 - - -class ReframePointcloud(Node): - def __init__(self): - super().__init__('reframe_pointcloud') - - self.declare_parameter('input_topic', '/zed/zed_node/point_cloud/cloud_registered') - self.declare_parameter('output_topic', '/zed/cloud_base_link') - self.declare_parameter('target_frame', 'base_link') - - input_topic = self.get_parameter('input_topic').get_parameter_value().string_value - output_topic = self.get_parameter('output_topic').get_parameter_value().string_value - self.target_frame = self.get_parameter('target_frame').get_parameter_value().string_value - - self.pub = self.create_publisher(PointCloud2, output_topic, - QoSPresetProfiles.SENSOR_DATA.value) - - self.sub = self.create_subscription(PointCloud2, input_topic, - self._cb, - QoSPresetProfiles.SENSOR_DATA.value) - - self.get_logger().info( - f'reframe_pointcloud: {input_topic} → {output_topic} ' - f'(frame_id → {self.target_frame})' - ) - - def _cb(self, msg: PointCloud2): - msg.header.frame_id = self.target_frame - self.pub.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - node = ReframePointcloud() - rclpy.spin(node) - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt b/src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt deleted file mode 100644 index 913ef8a3..00000000 --- a/src/subsystems/even_more_minimal_nav/mission_executive/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(mission_executive) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(vision_msgs REQUIRED) -find_package(msgs REQUIRED) - -add_executable(mission_executive_node src/mission_executive_node.cpp) -target_compile_features(mission_executive_node PUBLIC cxx_std_17) -target_link_libraries(mission_executive_node - rclcpp::rclcpp - rclcpp_action::rclcpp_action - ${geometry_msgs_TARGETS} - ${nav_msgs_TARGETS} - ${sensor_msgs_TARGETS} - ${std_msgs_TARGETS} - ${std_srvs_TARGETS} - tf2::tf2 - tf2_ros::tf2_ros - ${vision_msgs_TARGETS} - ${msgs_TARGETS} -) - -install(TARGETS mission_executive_node - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp b/src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp deleted file mode 100644 index 3e2112df..00000000 --- a/src/subsystems/even_more_minimal_nav/mission_executive/src/mission_executive_node.cpp +++ /dev/null @@ -1,1013 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" -#include "sensor_msgs/msg/imu.hpp" -#include "tf2/exceptions.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/string.hpp" -#include "std_srvs/srv/set_bool.hpp" -#include "std_srvs/srv/trigger.hpp" -#include "vision_msgs/msg/detection2_d.hpp" - -#include "msgs/action/navigate_to_target.hpp" -#include "msgs/msg/active_target.hpp" -#include "msgs/msg/nav_status.hpp" -#include "msgs/msg/planner_event.hpp" -#include "msgs/srv/lat_lon_to_enu.hpp" -#include "msgs/srv/set_target.hpp" - -using namespace std::chrono_literals; - -// ── State ────────────────────────────────────────────────────────────────── - -enum class State -{ - IDLE, - NAVIGATING, - ARRIVING, - STOPPED_AT_TARGET, - SPIRAL_COVERAGE, - SPIRAL_DONE, - ABORTING, - RETURNING, - STOPPED_AT_RETURN, - TELEOP -}; - -static std::string stateToStr(State s) -{ - switch (s) { - case State::IDLE: return "IDLE"; - case State::NAVIGATING: return "NAVIGATING"; - case State::ARRIVING: return "ARRIVING"; - case State::STOPPED_AT_TARGET: return "STOPPED_AT_TARGET"; - case State::SPIRAL_COVERAGE: return "SPIRAL_COVERAGE"; - case State::SPIRAL_DONE: return "SPIRAL_DONE"; - case State::ABORTING: return "ABORTING"; - case State::RETURNING: return "RETURNING"; - case State::STOPPED_AT_RETURN: return "STOPPED_AT_RETURN"; - case State::TELEOP: return "TELEOP"; - default: return "UNKNOWN"; - } -} - -// ── Node ─────────────────────────────────────────────────────────────────── - -class MissionExecutive : public rclcpp::Node -{ -public: - using NavAction = msgs::action::NavigateToTarget; - using GoalHandle = rclcpp_action::ServerGoalHandle; - - // Target entry stored in the registry and as the active target - struct TargetEntry - { - std::string id; - double x_m{0.0}; - double y_m{0.0}; - uint8_t target_type{0}; - uint8_t goal_source{0}; // GPS=0, METER=1 - double tolerance_m{3.0}; - bool visited{false}; - geometry_msgs::msg::PoseStamped goal_enu; - }; - - explicit MissionExecutive(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("mission_executive", options) - { - // Behaviour parameters - declare_parameter("stop_angular_vel_threshold", 0.05); // rad/s — IMU gyro magnitude - declare_parameter("arrival_hold_time", 1.0); - declare_parameter("replan_distance_m", 3.0); - declare_parameter("latlon_to_enu_service", - std::string("/gps_pose_publisher/latlon_to_enu")); - - // Spiral coverage parameters - declare_parameter("spiral_timeout_s", 120.0); - declare_parameter("spiral_radius_m", 15.0); - declare_parameter("spiral_spacing_m", 2.0); - declare_parameter("spiral_angular_step", 0.5); - declare_parameter("spiral_waypoint_tolerance_m", 2.0); - - // Detection topics - declare_parameter("aruco_detection_topic", std::string("/aruco_loc")); - declare_parameter("yolo_detection_topic", std::string("/yolo_detection")); - - // Input topics - declare_parameter("imu_topic", std::string("/imu")); - declare_parameter("planner_event_topic", std::string("/planner_event")); - declare_parameter("global_path_topic", std::string("/global_path")); - - // Output topics - declare_parameter("goal_pose_topic", std::string("/goal_pose")); - declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); - declare_parameter("nav_mode_topic", std::string("/nav_mode")); - declare_parameter("active_target_topic", std::string("/active_target")); - declare_parameter("nav_status_topic", std::string("/nav_status")); - - stop_angular_vel_threshold_ = get_parameter("stop_angular_vel_threshold").as_double(); - arrival_hold_time_ = get_parameter("arrival_hold_time").as_double(); - replan_distance_m_ = get_parameter("replan_distance_m").as_double(); - const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); - - spiral_timeout_s_ = get_parameter("spiral_timeout_s").as_double(); - spiral_radius_m_ = get_parameter("spiral_radius_m").as_double(); - spiral_spacing_m_ = get_parameter("spiral_spacing_m").as_double(); - spiral_angular_step_ = get_parameter("spiral_angular_step").as_double(); - spiral_waypoint_tolerance_m_ = get_parameter("spiral_waypoint_tolerance_m").as_double(); - - const auto aruco_detection_topic = get_parameter("aruco_detection_topic").as_string(); - const auto yolo_detection_topic = get_parameter("yolo_detection_topic").as_string(); - - const auto imu_topic = get_parameter("imu_topic").as_string(); - const auto planner_event_topic = get_parameter("planner_event_topic").as_string(); - const auto global_path_topic = get_parameter("global_path_topic").as_string(); - const auto goal_pose_topic = get_parameter("goal_pose_topic").as_string(); - const auto nav_enabled_topic = get_parameter("nav_enabled_topic").as_string(); - const auto nav_mode_topic = get_parameter("nav_mode_topic").as_string(); - const auto active_target_topic = get_parameter("active_target_topic").as_string(); - const auto nav_status_topic = get_parameter("nav_status_topic").as_string(); - - // Reentrant group — allows the action server / service client to block - // inside handleAccepted() while other callbacks still run on other threads. - reentrant_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - // TF — used to look up map→base_link for arrival and cross-track checks - tf_buffer_ = std::make_shared(get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - // ── Publishers ────────────────────────────────────────────────────────── - goal_pub_ = create_publisher(goal_pose_topic, 10); - nav_enabled_pub_ = create_publisher( - nav_enabled_topic, rclcpp::QoS(1).reliable()); - nav_mode_pub_ = create_publisher(nav_mode_topic, 10); - active_target_pub_ = create_publisher(active_target_topic, 10); - nav_status_pub_ = create_publisher( - nav_status_topic, rclcpp::QoS(1).reliable()); - - // ── Service client ────────────────────────────────────────────────────── - latlon_client_ = create_client( - latlon_svc, - rmw_qos_profile_services_default, - reentrant_group_); - - // ── Action server ─────────────────────────────────────────────────────── - action_server_ = rclcpp_action::create_server( - this, - "~/navigate_to_target", - [this](const rclcpp_action::GoalUUID &, std::shared_ptr goal) { - return handleGoal(goal); - }, - [this](std::shared_ptr gh) { - return handleCancel(gh); - }, - [this](std::shared_ptr gh) { - handleAccepted(gh); - }, - rcl_action_server_get_default_options(), - reentrant_group_); - - // ── Services ──────────────────────────────────────────────────────────── - abort_srv_ = create_service( - "~/abort", - [this]( - const std_srvs::srv::Trigger::Request::SharedPtr, - std_srvs::srv::Trigger::Response::SharedPtr res) - { - std::lock_guard lk(mutex_); - if (state_ == State::IDLE || state_ == State::TELEOP || - state_ == State::ABORTING || - state_ == State::STOPPED_AT_TARGET || state_ == State::STOPPED_AT_RETURN || - state_ == State::SPIRAL_DONE) - { - res->success = false; - res->message = "Nothing to abort in state " + stateToStr(state_); - return; - } - transition(State::ABORTING, "~/abort service called"); - res->success = true; - res->message = "Aborting"; - }); - - set_target_srv_ = create_service( - "~/set_target", - [this]( - const msgs::srv::SetTarget::Request::SharedPtr req, - msgs::srv::SetTarget::Response::SharedPtr res) - { - onSetTarget(req, res); - }, - rmw_qos_profile_services_default, - reentrant_group_); - - teleop_srv_ = create_service( - "~/teleop", - [this]( - const std_srvs::srv::SetBool::Request::SharedPtr req, - std_srvs::srv::SetBool::Response::SharedPtr res) - { - std::lock_guard lk(mutex_); - if (req->data) { - // TELEOP_ON — valid from any state - transition(State::TELEOP, "teleop enabled"); - res->success = true; - res->message = "Teleop ON"; - } else { - // TELEOP_OFF — only from TELEOP - if (state_ == State::TELEOP) { - transition(State::IDLE, "teleop disabled"); - res->success = true; - res->message = "Teleop OFF"; - } else { - res->success = false; - res->message = "Not in TELEOP state"; - } - } - }); - - // ── Subscriptions ──────────────────────────────────────────────────────── - - // IMU-based stop detection: use angular velocity magnitude from the gyro. - // A stopped rover has near-zero rotation rate; stop_angular_vel_threshold (rad/s) - // controls how small it must be to count as "stopped". - imu_sub_ = create_subscription( - imu_topic, rclcpp::SensorDataQoS(), - [this](const sensor_msgs::msg::Imu::SharedPtr msg) { - std::lock_guard lk(mutex_); - const auto & w = msg->angular_velocity; - imu_angular_vel_ = std::sqrt(w.x * w.x + w.y * w.y + w.z * w.z); - checkStopDetection(); - }); - - planner_event_sub_ = create_subscription( - planner_event_topic, 10, - [this](const msgs::msg::PlannerEvent::SharedPtr msg) { - std::lock_guard lk(mutex_); - last_planner_event_ = msg->event; - if (msg->event == msgs::msg::PlannerEvent::PLAN_FAILED) { - onPlanFailed(); - } - }); - - auto transient_qos = rclcpp::QoS(1).transient_local(); - global_path_sub_ = create_subscription( - global_path_topic, transient_qos, - [this](const nav_msgs::msg::Path::SharedPtr msg) { - std::lock_guard lk(mutex_); - global_path_ = *msg; - }); - - // Aruco detection — any message means a marker was found - aruco_sub_ = create_subscription( - aruco_detection_topic, 10, - [this](const vision_msgs::msg::Detection2D::SharedPtr) { - std::lock_guard lk(mutex_); - if (state_ == State::SPIRAL_COVERAGE) { - detection_received_ = true; - transition(State::SPIRAL_DONE, "aruco marker detected"); - } - }); - - // YOLO detection — Bool(True) signals an object was found - yolo_sub_ = create_subscription( - yolo_detection_topic, 10, - [this](const std_msgs::msg::Bool::SharedPtr msg) { - std::lock_guard lk(mutex_); - if (msg->data && state_ == State::SPIRAL_COVERAGE) { - detection_received_ = true; - transition(State::SPIRAL_DONE, "yolo object detected"); - } - }); - - // ── 2 Hz status timer ──────────────────────────────────────────────────── - // Also drives arrival detection and replan checks; 2 Hz is fine for both. - status_timer_ = create_wall_timer( - 500ms, - [this]() { - std::lock_guard lk(mutex_); - refreshPoseFromTF(); // updates robot_pose_ for all checks below - checkArrival(); - checkCrossTrackError(); - checkSpiralProgress(); - publishStatus(); - checkActionResult(); - }); - - // Publish initial safe state - publishNavEnabled(false); - publishNavMode(); - - RCLCPP_INFO(get_logger(), "MissionExecutive ready — state: IDLE"); - } - -private: - // ── State transitions ───────────────────────────────────────────────────── - - // Must be called with mutex_ held. - void transition(State new_state, const std::string & reason) - { - RCLCPP_INFO(get_logger(), "[mission_executive] %s → %s: %s", - stateToStr(state_).c_str(), stateToStr(new_state).c_str(), reason.c_str()); - state_ = new_state; - - const bool nav_active = - state_ == State::NAVIGATING || - state_ == State::ARRIVING || - state_ == State::RETURNING || - state_ == State::SPIRAL_COVERAGE; - publishNavEnabled(nav_active); - publishNavMode(); - publishStatus(); - - if (state_ == State::TELEOP || !nav_active) { - // Clear stop-detection tracking on any non-navigating transition - low_speed_tracking_ = false; - } - } - - // ── Internal publish helpers (no lock needed — caller holds mutex) ───────── - - void publishNavEnabled(bool enabled) - { - std_msgs::msg::Bool msg; - msg.data = enabled; - nav_enabled_pub_->publish(msg); - } - - void publishNavMode() - { - std_msgs::msg::String msg; - switch (state_) { - case State::TELEOP: - msg.data = "teleop"; break; - case State::NAVIGATING: - case State::ARRIVING: - case State::RETURNING: - case State::SPIRAL_COVERAGE: - msg.data = "autonomous"; break; - default: - msg.data = "stopped"; break; - } - nav_mode_pub_->publish(msg); - } - - void publishActiveTarget() - { - if (!active_target_.has_value()) return; - msgs::msg::ActiveTarget at; - at.target_id = active_target_->id; - at.target_type = active_target_->target_type; - at.tolerance_m = active_target_->tolerance_m; - at.goal_enu = active_target_->goal_enu; - at.goal_source = active_target_->goal_source; - at.status = stateToStr(state_); - active_target_pub_->publish(at); - } - - void publishStatus() - { - msgs::msg::NavStatus s; - s.state = stateToStr(state_); - if (active_target_.has_value()) { - s.active_target_id = active_target_->id; - s.active_target_type = active_target_->target_type; - s.goal_source = active_target_->goal_source; - } - s.distance_to_goal_m = distToGoal(); - s.cross_track_error_m = computeCrossTrackError(); - s.heading_error_rad = computeHeadingError(); - s.robot_speed_mps = imu_angular_vel_; // IMU angular velocity magnitude (rad/s) - s.is_return = is_return_; - s.last_planner_event = last_planner_event_; - nav_status_pub_->publish(s); - } - - // ── Geometry helpers ────────────────────────────────────────────────────── - - // Both return -1.0 if data is unavailable. Called with mutex_ held. - - static double quaternionToYaw(const geometry_msgs::msg::Quaternion & q) - { - const double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); - const double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); - return std::atan2(siny_cosp, cosy_cosp); - } - - double computeHeadingError() const - { - if (!robot_pose_.has_value() || !active_target_.has_value()) return 0.0; - const double yaw = quaternionToYaw(robot_pose_->pose.orientation); - const double dx = active_target_->goal_enu.pose.position.x - robot_pose_->pose.position.x; - const double dy = active_target_->goal_enu.pose.position.y - robot_pose_->pose.position.y; - const double bearing = std::atan2(dy, dx); - double err = bearing - yaw; - while (err > M_PI) err -= 2.0 * M_PI; - while (err < -M_PI) err += 2.0 * M_PI; - return err; - } - - double distToGoal() const - { - if (!robot_pose_.has_value() || !active_target_.has_value()) return -1.0; - const double dx = - robot_pose_->pose.position.x - active_target_->goal_enu.pose.position.x; - const double dy = - robot_pose_->pose.position.y - active_target_->goal_enu.pose.position.y; - return std::hypot(dx, dy); - } - - double computeCrossTrackError() const - { - if (!robot_pose_.has_value() || !global_path_.has_value()) return -1.0; - const auto & poses = global_path_->poses; - if (poses.size() < 2) return -1.0; - - const double rx = robot_pose_->pose.position.x; - const double ry = robot_pose_->pose.position.y; - double min_dist = std::numeric_limits::max(); - - for (size_t i = 0; i < poses.size() - 1; ++i) { - const double ax = poses[i].pose.position.x, ay = poses[i].pose.position.y; - const double bx = poses[i+1].pose.position.x, by = poses[i+1].pose.position.y; - const double dx = bx - ax, dy = by - ay; - const double len2 = dx*dx + dy*dy; - double dist; - if (len2 < 1e-10) { - dist = std::hypot(rx - ax, ry - ay); - } else { - const double t = std::clamp(((rx-ax)*dx + (ry-ay)*dy) / len2, 0.0, 1.0); - dist = std::hypot(rx - (ax + t*dx), ry - (ay + t*dy)); - } - min_dist = std::min(min_dist, dist); - } - return min_dist; - } - - // ── Subscription callbacks (mutex_ already held by caller) ──────────────── - - // Look up map→base_link and cache the result in robot_pose_. - // Called from the status timer; silently does nothing if the TF is not yet available. - void refreshPoseFromTF() - { - try { - const auto tf = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero); - geometry_msgs::msg::PoseStamped p; - p.header.frame_id = "map"; - p.header.stamp = tf.header.stamp; - p.pose.position.x = tf.transform.translation.x; - p.pose.position.y = tf.transform.translation.y; - p.pose.position.z = tf.transform.translation.z; - p.pose.orientation = tf.transform.rotation; - robot_pose_ = p; - } catch (const tf2::TransformException &) { - // TF not yet available — robot_pose_ stays as-is (nullopt at startup) - } - } - - void checkCrossTrackError() - { - if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; - if (!active_target_.has_value()) return; - - const double xte = computeCrossTrackError(); - if (xte < 0.0 || xte <= replan_distance_m_) return; - - RCLCPP_INFO(get_logger(), - "[mission_executive] replan triggered: xtrack=%.2fm", xte); - goal_pub_->publish(active_target_->goal_enu); - } - - void checkArrival() - { - // Transition NAVIGATING/RETURNING → ARRIVING when within tolerance - if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; - if (!active_target_.has_value()) return; - - const double d = distToGoal(); - if (d < 0.0 || d >= active_target_->tolerance_m) return; - - transition(State::ARRIVING, - "within tolerance (d=" + std::to_string(d) + "m)"); - } - - void checkStopDetection() - { - // Transition ARRIVING → STOPPED_AT_TARGET / STOPPED_AT_RETURN - if (state_ != State::ARRIVING) return; - - if (imu_angular_vel_ < stop_angular_vel_threshold_) { - if (!low_speed_tracking_) { - low_speed_start_ = now(); - low_speed_tracking_ = true; - } else { - const double held = (now() - low_speed_start_).seconds(); - if (held >= arrival_hold_time_) { - low_speed_tracking_ = false; - if (is_return_) { - transition(State::STOPPED_AT_RETURN, "velocity held < threshold"); - } else { - // Mark target as visited so RETURN is permitted later - if (active_target_.has_value()) { - auto it = target_registry_.find(active_target_->id); - if (it != target_registry_.end()) { - it->second.visited = true; - } - } - transition(State::STOPPED_AT_TARGET, "velocity held < threshold"); - } - } - } - } else { - low_speed_tracking_ = false; - } - } - - void onPlanFailed() - { - // Called with mutex_ held - if (state_ == State::NAVIGATING || state_ == State::RETURNING) { - RCLCPP_WARN(get_logger(), - "[mission_executive] PLAN_FAILED — transitioning to ABORTING"); - transition(State::ABORTING, "PLAN_FAILED received"); - } - } - - // ── Spiral coverage ─────────────────────────────────────────────────────── - - // Called with mutex_ held. - std::vector generateSpiralWaypoints( - const geometry_msgs::msg::PoseStamped & start) - { - std::vector waypoints; - const double a = spiral_spacing_m_ / (2.0 * M_PI); - if (a <= 0.0) return waypoints; - - const double max_angle = spiral_radius_m_ / a; - const double min_start_r = 1.5; - const double yaw0 = quaternionToYaw(start.pose.orientation); - const rclcpp::Time stamp = now(); - - double theta = 0.0; - while (theta <= max_angle) { - const double r = min_start_r + a * theta; - if (r > spiral_radius_m_) break; - - const double x = start.pose.position.x + r * std::cos(yaw0 + theta + M_PI_2); - const double y = start.pose.position.y + r * std::sin(yaw0 + theta + M_PI_2); - - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = "map"; - pose.header.stamp = stamp; - pose.pose.position.x = x; - pose.pose.position.y = y; - pose.pose.orientation.w = 1.0; - waypoints.push_back(pose); - - theta += spiral_angular_step_; - } - return waypoints; - } - - // Called with mutex_ held, immediately after transitioning to SPIRAL_COVERAGE. - void startSpiralCoverage() - { - if (!robot_pose_.has_value()) { - RCLCPP_WARN(get_logger(), "[mission_executive] No robot pose — skipping spiral"); - transition(State::SPIRAL_DONE, "no pose available for spiral"); - return; - } - - spiral_waypoints_ = generateSpiralWaypoints(*robot_pose_); - spiral_waypoint_idx_ = 0; - spiral_start_time_ = now(); - detection_received_ = false; - - if (spiral_waypoints_.empty()) { - RCLCPP_WARN(get_logger(), "[mission_executive] Empty spiral path — completing immediately"); - transition(State::SPIRAL_DONE, "empty spiral path"); - return; - } - - RCLCPP_INFO(get_logger(), - "[mission_executive] Spiral started: %zu waypoints, radius=%.1fm, timeout=%.1fs", - spiral_waypoints_.size(), spiral_radius_m_, spiral_timeout_s_); - - goal_pub_->publish(spiral_waypoints_[0]); - } - - // Called from the status timer with mutex_ held. - void checkSpiralProgress() - { - if (state_ != State::SPIRAL_COVERAGE) return; - - // Check timeout - const double elapsed = (now() - spiral_start_time_).seconds(); - if (elapsed >= spiral_timeout_s_) { - RCLCPP_INFO(get_logger(), - "[mission_executive] Spiral timeout after %.1fs", elapsed); - transition(State::SPIRAL_DONE, "spiral timeout"); - return; - } - - if (!robot_pose_.has_value() || spiral_waypoints_.empty()) return; - - // Advance to the next waypoint when within tolerance of the current one - const auto & wp = spiral_waypoints_[spiral_waypoint_idx_]; - const double dist = std::hypot( - robot_pose_->pose.position.x - wp.pose.position.x, - robot_pose_->pose.position.y - wp.pose.position.y); - - if (dist < spiral_waypoint_tolerance_m_) { - ++spiral_waypoint_idx_; - if (spiral_waypoint_idx_ >= spiral_waypoints_.size()) { - RCLCPP_INFO(get_logger(), "[mission_executive] All spiral waypoints visited"); - transition(State::SPIRAL_DONE, "spiral complete"); - return; - } - goal_pub_->publish(spiral_waypoints_[spiral_waypoint_idx_]); - } - } - - // ── Action result dispatch (called from the 2 Hz status timer) ──────────── - // Must be called with mutex_ held. - - void checkActionResult() - { - if (!active_goal_handle_) return; - - auto result = std::make_shared(); - - // Cancel requested externally — result already sent, go straight to IDLE. - // (ABORTING is only for the case where the result hasn't been sent yet.) - if (active_goal_handle_->is_canceling()) { - result->success = false; - result->message = "Cancelled"; - active_goal_handle_->canceled(result); - active_goal_handle_ = nullptr; - transition(State::IDLE, "action cancelled"); - return; - } - - switch (state_) { - case State::STOPPED_AT_TARGET: { - // For targets that need spiral coverage, kick off the spiral instead of completing. - const bool needs_spiral = active_target_.has_value() && - (active_target_->target_type == msgs::srv::SetTarget::Request::ARUCO_POST || - active_target_->target_type == msgs::srv::SetTarget::Request::OBJECT); - if (needs_spiral) { - transition(State::SPIRAL_COVERAGE, "starting spiral coverage"); - startSpiralCoverage(); - return; - } - result->success = true; - result->message = "Arrived at target"; - active_goal_handle_->succeed(result); - active_goal_handle_ = nullptr; - return; - } - - case State::SPIRAL_DONE: - result->success = true; - result->message = detection_received_ - ? "Detection found during spiral coverage" - : "Spiral coverage complete (timeout or all waypoints visited)"; - active_goal_handle_->succeed(result); - active_goal_handle_ = nullptr; - return; - - case State::STOPPED_AT_RETURN: - result->success = true; - result->message = "Arrived at target"; - active_goal_handle_->succeed(result); - active_goal_handle_ = nullptr; - return; - - case State::ABORTING: - result->success = false; - result->message = "Aborted"; - active_goal_handle_->abort(result); - active_goal_handle_ = nullptr; - if (queued_goal_handle_) { - // Promote queued goal — go directly to NAVIGATING/RETURNING (§3.2.2) - active_target_ = queued_entry_; - is_return_ = queued_is_return_; - active_goal_handle_ = queued_goal_handle_; - queued_goal_handle_ = nullptr; - transition(is_return_ ? State::RETURNING : State::NAVIGATING, - "queued goal dequeued after abort"); - goal_pub_->publish(active_target_->goal_enu); - publishActiveTarget(); - } else { - transition(State::IDLE, "abort complete"); - } - return; - - default: - break; - } - - // Still executing — push feedback - auto fb = std::make_shared(); - fb->distance_to_goal_m = distToGoal(); - fb->cross_track_error_m = computeCrossTrackError(); - fb->state = stateToStr(state_); - active_goal_handle_->publish_feedback(fb); - } - - // ── Action server callbacks ─────────────────────────────────────────────── - - rclcpp_action::GoalResponse handleGoal( - std::shared_ptr /*goal*/) - { - // Accept all goals; teleop/state guards are applied in handleAccepted. - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - rclcpp_action::CancelResponse handleCancel(std::shared_ptr /*gh*/) - { - return rclcpp_action::CancelResponse::ACCEPT; - } - - // Runs on a thread from the MultiThreadedExecutor (reentrant group). - // May block briefly for the latlon_to_enu service call. - void handleAccepted(std::shared_ptr goal_handle) - { - const auto goal = goal_handle->get_goal(); - - // ── Resolve ENU coordinates ──────────────────────────────────────────── - TargetEntry entry; - const bool is_ret = goal->is_return; - - if (!goal->target_id.empty()) { - // Look up pre-registered target - std::lock_guard lk(mutex_); - auto it = target_registry_.find(goal->target_id); - if (it == target_registry_.end()) { - auto res = std::make_shared(); - res->success = false; - res->message = "Unknown target_id: " + goal->target_id; - goal_handle->abort(res); - return; - } - if (is_ret && !it->second.visited) { - auto res = std::make_shared(); - res->success = false; - res->message = "Target not yet visited — cannot RETURN"; - goal_handle->abort(res); - return; - } - entry = it->second; - } else { - // Inline goal — no target_id - entry.id = ""; - entry.target_type = goal->target_type; - entry.tolerance_m = goal->tolerance_m; - entry.goal_source = goal->goal_type; - - if (goal->goal_type == NavAction::Goal::GPS) { - if (!latlon_client_->wait_for_service(3s)) { - auto res = std::make_shared(); - res->success = false; - res->message = "latlon_to_enu service not available"; - goal_handle->abort(res); - return; - } - auto req = std::make_shared(); - req->lat = goal->lat; - req->lon = goal->lon; - auto future = latlon_client_->async_send_request(req); - // Blocking wait is safe here: MultiThreadedExecutor + reentrant group - // means other callbacks continue on other threads. - if (future.wait_for(5s) != std::future_status::ready) { - auto res = std::make_shared(); - res->success = false; - res->message = "latlon_to_enu service timeout"; - goal_handle->abort(res); - return; - } - auto resp = future.get(); - entry.x_m = resp->x; - entry.y_m = resp->y; - } else { - entry.x_m = goal->x_m; - entry.y_m = goal->y_m; - } - - entry.goal_enu.header.frame_id = "map"; - entry.goal_enu.header.stamp = now(); - entry.goal_enu.pose.position.x = entry.x_m; - entry.goal_enu.pose.position.y = entry.y_m; - entry.goal_enu.pose.orientation.w = 1.0; - } - - // ── Apply state transition ───────────────────────────────────────────── - { - std::lock_guard lk(mutex_); - - if (state_ == State::TELEOP) { - auto res = std::make_shared(); - res->success = false; - res->message = "Cannot navigate — currently in TELEOP mode"; - goal_handle->abort(res); - return; - } - - // §3.2.2: During ABORTING, queue the goal rather than preempting the abort - if (state_ == State::ABORTING) { - // Preempt any previously queued goal - if (queued_goal_handle_ && queued_goal_handle_->is_active()) { - auto old_res = std::make_shared(); - old_res->success = false; - old_res->message = "Preempted by newer queued goal"; - queued_goal_handle_->abort(old_res); - } - queued_goal_handle_ = goal_handle; - queued_entry_ = entry; - queued_is_return_ = is_ret; - RCLCPP_INFO(get_logger(), - "[mission_executive] goal queued — currently ABORTING"); - return; - } - - // Preempt any existing active goal (guard: abort() throws if not active) - if (active_goal_handle_ && active_goal_handle_->is_active()) { - auto old_res = std::make_shared(); - old_res->success = false; - old_res->message = "Preempted by new goal"; - active_goal_handle_->abort(old_res); - active_goal_handle_ = nullptr; - } - - active_target_ = entry; - is_return_ = is_ret; - active_goal_handle_ = goal_handle; - - transition(is_ret ? State::RETURNING : State::NAVIGATING, - is_ret ? "RETURN action accepted" : "GO_TO action accepted"); - - // Trigger global planner - goal_pub_->publish(active_target_->goal_enu); - publishActiveTarget(); - } - } - - // ── SetTarget service ───────────────────────────────────────────────────── - - void onSetTarget( - const msgs::srv::SetTarget::Request::SharedPtr req, - msgs::srv::SetTarget::Response::SharedPtr res) - { - TargetEntry entry; - entry.id = req->target_id; - entry.target_type = req->target_type; - entry.tolerance_m = req->tolerance_m; - entry.goal_source = req->goal_type; - - if (req->goal_type == msgs::srv::SetTarget::Request::GPS) { - if (!latlon_client_->wait_for_service(3s)) { - res->success = false; - res->message = "latlon_to_enu service not available"; - return; - } - auto conv = std::make_shared(); - conv->lat = req->lat; - conv->lon = req->lon; - auto future = latlon_client_->async_send_request(conv); - if (future.wait_for(5s) != std::future_status::ready) { - res->success = false; - res->message = "latlon_to_enu service timeout"; - return; - } - auto resp = future.get(); - entry.x_m = resp->x; - entry.y_m = resp->y; - } else { - entry.x_m = req->x_m; - entry.y_m = req->y_m; - } - - entry.goal_enu.header.frame_id = "map"; - entry.goal_enu.header.stamp = now(); - entry.goal_enu.pose.position.x = entry.x_m; - entry.goal_enu.pose.position.y = entry.y_m; - entry.goal_enu.pose.orientation.w = 1.0; - - { - std::lock_guard lk(mutex_); - target_registry_[entry.id] = entry; - } - - res->success = true; - res->message = "Target '" + entry.id + "' registered"; - RCLCPP_INFO(get_logger(), - "Target '%s' registered: (%.2f, %.2f) tol=%.2fm", - entry.id.c_str(), entry.x_m, entry.y_m, entry.tolerance_m); - } - - // ── Member data ─────────────────────────────────────────────────────────── - - std::mutex mutex_; - State state_{State::IDLE}; - - std::unordered_map target_registry_; - std::optional active_target_; - bool is_return_{false}; - - // Goal queued while ABORTING (§3.2.2 — GO_TO/RETURN during ABORTING are "queued") - std::shared_ptr queued_goal_handle_; - TargetEntry queued_entry_; - bool queued_is_return_{false}; - - // Sensor cache - std::optional robot_pose_; - std::optional global_path_; - double imu_angular_vel_{0.0}; // rad/s - rclcpp::Time low_speed_start_{0, 0, RCL_ROS_TIME}; - bool low_speed_tracking_{false}; - uint8_t last_planner_event_{0}; - - // Spiral state - std::vector spiral_waypoints_; - size_t spiral_waypoint_idx_{0}; - rclcpp::Time spiral_start_time_{0, 0, RCL_ROS_TIME}; - bool detection_received_{false}; - - // Parameters - double stop_angular_vel_threshold_{0.05}; - double arrival_hold_time_{1.0}; - double replan_distance_m_{3.0}; - double spiral_timeout_s_{120.0}; - double spiral_radius_m_{15.0}; - double spiral_spacing_m_{2.0}; - double spiral_angular_step_{0.5}; - double spiral_waypoint_tolerance_m_{2.0}; - - // ROS interfaces - rclcpp::CallbackGroup::SharedPtr reentrant_group_; - - rclcpp::Publisher::SharedPtr goal_pub_; - rclcpp::Publisher::SharedPtr nav_enabled_pub_; - rclcpp::Publisher::SharedPtr nav_mode_pub_; - rclcpp::Publisher::SharedPtr active_target_pub_; - rclcpp::Publisher::SharedPtr nav_status_pub_; - - rclcpp_action::Server::SharedPtr action_server_; - std::shared_ptr active_goal_handle_; - - rclcpp::Service::SharedPtr abort_srv_; - rclcpp::Service::SharedPtr set_target_srv_; - rclcpp::Service::SharedPtr teleop_srv_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr planner_event_sub_; - rclcpp::Subscription::SharedPtr global_path_sub_; - rclcpp::Subscription::SharedPtr aruco_sub_; - rclcpp::Subscription::SharedPtr yolo_sub_; - - rclcpp::Client::SharedPtr latlon_client_; - - rclcpp::TimerBase::SharedPtr status_timer_; -}; - -// ── main ────────────────────────────────────────────────────────────────── - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/even_more_minimal_nav/plan.md b/src/subsystems/even_more_minimal_nav/plan.md deleted file mode 100644 index c0bbdc7b..00000000 --- a/src/subsystems/even_more_minimal_nav/plan.md +++ /dev/null @@ -1,101 +0,0 @@ -### Nodes (updated) - -**`gps_pose_publisher`** — unchanged -**`mission_executive`** — unchanged -**`dem_costmap_converter`** — exists, no changes needed -**`global_planner`** — new, described below -**`vector_field_planner`** — unchanged - ---- - -## `dem_costmap_converter` role - -Runs independently at startup. Loads the GeoTIFF, computes a slope map via Sobel gradients, converts slope degrees to nav2-convention cost values (0–254), and republishes the `OccupancyGrid` at 1 Hz with transient_local QoS. The `global_planner` subscribes to this and treats it as a static cost lookup table — nothing else changes in `dem_costmap_converter`. - ---- - -## `global_planner` node - -**Inputs:** -- `/goal_pose` (`geometry_msgs/PoseStamped`) — from `mission_executive` -- `map → base_link` TF — start position -- `/map` (`nav_msgs/OccupancyGrid`, transient_local) — from `dem_costmap_converter`, optional - -**Output:** -- `/global_path` (`nav_msgs/Path`, transient_local) - -### Phase 1 (now): straight-line interpolation - -When a goal arrives and either `use_costmap` is false or no costmap has been received yet, sample evenly-spaced poses along the straight line from robot to goal at `path_resolution_m` spacing. Publish immediately. - -### Phase 2 (later): costmap-weighted A\* - -When `use_costmap: true` and a costmap has been received, run A\* on the occupancy grid. The cost to enter a cell is read directly from the grid value published by `dem_costmap_converter` — cells at 254 are treated as impassable walls, everything below scales traversal cost. A `slope_weight` parameter scales how much the cost value penalizes the path versus raw distance, giving field-tunable behavior between "shortest path" and "flattest path": - -$$g(cell) = \text{dist\_to\_neighbor} + \alpha \cdot \frac{\text{grid\_value}(cell)}{254}$$ - -where $\alpha$ is `slope_weight`. - -The fallback is always available: if A\* fails to find a path (e.g. goal is in a lethal cell), the planner logs a warning and falls back to straight-line, which `mission_executive` will eventually replan anyway via its cross-track error monitor. - -### Transition between phases - -The planner checks `use_costmap` and costmap availability on every new `/goal_pose`. Switching from Phase 1 to Phase 2 in the field requires only: -1. Setting `use_costmap: true` in the YAML -2. Setting `dem_file_path` and launching `dem_costmap_converter` - -No code changes, no interface changes. - ---- - -## Updated `nav_params.yaml` - -``` -dem_costmap_converter: dem_file_path, map_resolution, - max_passable_slope_degrees, - output_frame, origin_x, origin_y - -gps_pose_publisher: heading_topic, heading_offset_deg, - use_start_gate_ref, origin_lat/lon/alt - -mission_executive: velocity_zero_threshold, arrival_hold_time, - replan_distance_m, latlon_to_enu_service, odom_topic - -global_planner: path_resolution_m, - use_costmap, - slope_weight - -vector_field_planner: map_frame, base_frame, tf_timeout_s, - max_speed_mps, max_steering_angle_rad, - lookahead_dist_m, k_p_steering, - repulsion_gain, repulsion_cutoff_m, - goal_tolerance_m, publish_debug_markers -``` - -`elevation_map_path` is gone — that's now entirely owned by `dem_costmap_converter` via `dem_file_path`. The `global_planner` never touches the GeoTIFF directly. - ---- - -## Updated topic graph - -| Publisher | Topic | Subscriber(s) | -|---|---|---| -| `gps_pose_publisher` | TF `map→base_link` | `global_planner`, `vector_field_planner`, `mission_executive` | -| `dem_costmap_converter` | `/map` | `global_planner` | -| `mission_executive` | `/nav_enabled` | `vector_field_planner` | -| `mission_executive` | `/goal_pose` | `global_planner` | -| `global_planner` | `/global_path` | `mission_executive`, `vector_field_planner` | -| `vector_field_planner` | `/cmd_vel` | chassis driver | -| `vector_field_planner` | `~/debug_markers` | RViz | - ---- - -## What changes when moving to Phase 2 - -1. Launch `dem_costmap_converter` with `dem_file_path` pointing at your GeoTIFF -2. Set `use_costmap: true` and tune `slope_weight` in the YAML -3. Nothing else — same topics, same interfaces, `mission_executive` and `vector_field_planner` are completely unaware of the change - - -minimal_nav_bringup is an old package that oyu will have to update the params and launch file for, but a similar pattern should be followed. - diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/package.xml b/src/subsystems/even_more_minimal_nav/yolo_ros/package.xml deleted file mode 100644 index 7a3eca59..00000000 --- a/src/subsystems/even_more_minimal_nav/yolo_ros/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - yolo_ros - 0.0.0 - TODO: Package description - umdloop - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - sensor_msgs - std_msgs - python3-opencv - cv_bridge - python3-pip - - - - - ros2launch - - - ament_python - - diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/resource/yolo_ros b/src/subsystems/even_more_minimal_nav/yolo_ros/resource/yolo_ros deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/setup.cfg b/src/subsystems/even_more_minimal_nav/yolo_ros/setup.cfg deleted file mode 100644 index 4f9be5b8..00000000 --- a/src/subsystems/even_more_minimal_nav/yolo_ros/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/yolo_ros -[install] -install_scripts=$base/lib/yolo_ros diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/setup.py b/src/subsystems/even_more_minimal_nav/yolo_ros/setup.py deleted file mode 100644 index a81b9b66..00000000 --- a/src/subsystems/even_more_minimal_nav/yolo_ros/setup.py +++ /dev/null @@ -1,28 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'yolo_ros' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools', 'yolo', 'ultralytics'], - zip_safe=True, - maintainer='umdloop', - maintainer_email='umdloop@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'my_node = yolo_ros.my_node:main', - 'yolo_ros = yolo_ros.image_publisher:main', - 'inference = yolo_ros.img_inference:main', - ], - }, -) diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_copyright.py b/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_copyright.py deleted file mode 100644 index 97a39196..00000000 --- a/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_copyright.py +++ /dev/null @@ -1,25 +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 - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_flake8.py b/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_flake8.py deleted file mode 100644 index 27ee1078..00000000 --- a/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_flake8.py +++ /dev/null @@ -1,25 +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(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_pep257.py b/src/subsystems/even_more_minimal_nav/yolo_ros/test/test_pep257.py deleted file mode 100644 index b234a384..00000000 --- a/src/subsystems/even_more_minimal_nav/yolo_ros/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(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/__init__.py b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/image_publisher.py b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/image_publisher.py deleted file mode 100644 index ad9a5ba3..00000000 --- a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/image_publisher.py +++ /dev/null @@ -1,61 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image as RosImage -from cv_bridge import CvBridge -import cv2 -import pyzed.sl as sl - -class ZedPublisher(Node): - def __init__(self): - super().__init__('zed_publisher') - self.publisher = self.create_publisher(RosImage, 'cam_in', 10) - self.bridge = CvBridge() - - self.zed = sl.Camera() - init_params = sl.InitParameters() - init_params.camera_resolution = sl.RESOLUTION.HD720 - init_params.camera_fps = 30 - init_params.depth_mode = sl.DEPTH_MODE.NONE - - err = self.zed.open(init_params) - if err != sl.ERROR_CODE.SUCCESS: - self.get_logger().error(f"Failed to open ZED camera: {err}") - return - - self.image = sl.Mat() - self.timer = self.create_timer(1 / 30.0, self.publish_frame) - - def publish_frame(self): - if self.zed.grab() == sl.ERROR_CODE.SUCCESS: - self.zed.retrieve_image(self.image, sl.VIEW.LEFT) - frame = self.image.get_data() - if frame.ndim == 2: - frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) - elif frame.shape[2] == 4: - frame = cv2.cvtColor(frame, cv2.COLOR_RGBA2BGR) - ros_image = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') - self.publisher.publish(ros_image) - cv2.imshow("ZED Camera feed", frame) - if (cv2.waitKey(1) and 0xFF == ord('q')): - self.get_logger().info("Exiting...") - cv2.destroyAllWindows() - else: - self.get_logger().warn("Failed to grab ZED frame") - - def __del__(self): - self.zed.close() - -def main(): - rclpy.init() - zed_publisher = ZedPublisher() - - executor = rclpy.executors.SingleThreadedExecutor() - executor.add_node(zed_publisher) - executor.spin() - - zed_publisher.destroy_node() - rclpy.shutdown() - cv2.destroyAllWindows() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/img_inference.py b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/img_inference.py deleted file mode 100644 index 607d63e3..00000000 --- a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/img_inference.py +++ /dev/null @@ -1,120 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image as RosImage -from std_msgs.msg import Bool -from cv_bridge import CvBridge -import cv2 -import threading -from ultralytics import YOLO -import numpy as np -import time - -class BoundingBox: - def __init__(self, img_width, img_height, x1, y1, x2, y2): - self.img_width = img_width - self.img_height = img_height - self.bb_top_left_x = x1 - self.bb_top_left_y = y1 - self.bb_bottom_right_x = x2 - self.bb_bottom_right_y = y2 - - def __repr__(self): - return (f"BoundingBox(img_width={self.img_width}, img_height={self.img_height}, " - f"x1={self.bb_top_left_x}, y1={self.bb_top_left_y}, " - f"x2={self.bb_bottom_right_x}, y2={self.bb_bottom_right_y})") - - def to_msg_format(self): - return (f"img_width: {self.img_width}, img_height: {self.img_height}, " - f"bb_top_left_x: {self.bb_top_left_x}, bb_top_left_y: {self.bb_top_left_y}, " - f"bb_bottom_right_x: {self.bb_bottom_right_x}, bb_bottom_right_y: {self.bb_bottom_right_y}\n") - -class InferenceNode(Node): - def __init__(self): - super().__init__('yolo_inference_node') - self.declare_parameter('confidence_threshold', 0.5) - self.conf_threshold = self.get_parameter('confidence_threshold').value - - self.subscription = self.create_subscription( - RosImage, - '/zed/zed_node/left_gray/image_rect_gray', - self.image_callback, - 10 - ) - self.detection_pub = self.create_publisher(Bool, '/yolo_detection', 10) - self.bridge = CvBridge() - self.frame_queue = [] - self.frame_lock = threading.Lock() - self.model = YOLO("best.pt") - self.last_inference_time = time.time() - self.fps = 0 - - self.inference_thread = threading.Thread(target=self.inference_loop, daemon=True) - self.inference_thread.start() - - def image_callback(self, msg): - frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') - with self.frame_lock: - self.frame_queue.append((frame, msg.width, msg.height)) - if len(self.frame_queue) > 1: - self.frame_queue.pop(0) - - def inference_loop(self): - while rclpy.ok(): - if not self.frame_queue: - time.sleep(0.001) - continue - - with self.frame_lock: - if not self.frame_queue: - continue - frame, width, height = self.frame_queue.pop(0) - - results = self.model(frame) - - current_time = time.time() - self.fps = 1.0 / (current_time - self.last_inference_time) - self.last_inference_time = current_time - - self.process_results(frame, results, width, height) - - def process_results(self, frame, results, img_width, img_height): - cv2.putText(frame, f"FPS: {self.fps:.1f}", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - - detection_found = False - for result in results: - for box in result.boxes: - conf = float(box.conf[0]) - if conf < self.conf_threshold: - continue - x1, y1, x2, y2 = map(int, box.xyxy[0]) - cls = int(box.cls[0]) - label = f"{self.model.names[cls]}: {conf:.2f}" - - cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2) - cv2.putText(frame, label, (x1, y1 - 10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) - detection_found = True - - msg = Bool() - msg.data = detection_found - self.detection_pub.publish(msg) - - cv2.imshow("YOLO Output", frame) - cv2.waitKey(1) - -def main(): - rclpy.init() - yolo_node = InferenceNode() - - try: - rclpy.spin(yolo_node) - except KeyboardInterrupt: - pass - finally: - yolo_node.destroy_node() - rclpy.shutdown() - cv2.destroyAllWindows() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/my_node.py b/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/my_node.py deleted file mode 100644 index 6980a95a..00000000 --- a/src/subsystems/even_more_minimal_nav/yolo_ros/yolo_ros/my_node.py +++ /dev/null @@ -1,6 +0,0 @@ -def main(): - print('Hi from yolo_ros.') - - -if __name__ == '__main__': - main() diff --git a/src/subsystems/navigation/athena_map/src/map_node.cpp b/src/subsystems/navigation/athena_map/src/map_node.cpp index b59e5675..637faf71 100644 --- a/src/subsystems/navigation/athena_map/src/map_node.cpp +++ b/src/subsystems/navigation/athena_map/src/map_node.cpp @@ -182,12 +182,12 @@ class DEMCostmapConverter : public rclcpp::Node // Nav2 costmap2d cost values: // 0: Free space // 1-252: Scaled costs - // 253: Possibly circumscribed - // 254: Unknown - // 255: Lethal obstacle (impassable) - + // 253: Possibly circumscribed + // 254: Lethal obstacle (impassable) + // 255: No information / unknown + if (slope_degrees >= max_passable_slope_degrees_) { - return 255; // Lethal - impassable + return 254; // Lethal - impassable } else if (slope_degrees >= 10.0) { // High cost: linear scale from 150-252 for 10-15 degrees float ratio = (slope_degrees - 10.0) / (max_passable_slope_degrees_ - 10.0); diff --git a/src/subsystems/even_more_minimal_nav/global_planner/CMakeLists.txt b/src/subsystems/navigation/global_planner/CMakeLists.txt similarity index 53% rename from src/subsystems/even_more_minimal_nav/global_planner/CMakeLists.txt rename to src/subsystems/navigation/global_planner/CMakeLists.txt index b0834978..bfcb7534 100644 --- a/src/subsystems/even_more_minimal_nav/global_planner/CMakeLists.txt +++ b/src/subsystems/navigation/global_planner/CMakeLists.txt @@ -12,9 +12,20 @@ find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) +add_library(global_planner_algo src/global_planner_algo.cpp) +target_include_directories(global_planner_algo PUBLIC + $ + $ +) +target_compile_features(global_planner_algo PUBLIC cxx_std_17) + add_executable(global_planner_node src/global_planner_node.cpp) +target_include_directories(global_planner_node PUBLIC + $ +) target_compile_features(global_planner_node PUBLIC cxx_std_17) target_link_libraries(global_planner_node + global_planner_algo rclcpp::rclcpp ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -22,8 +33,17 @@ target_link_libraries(global_planner_node tf2::tf2 ) -install(TARGETS global_planner_node +install(TARGETS global_planner_algo global_planner_node DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + # Add tests here when they exist +endif() + ament_package() diff --git a/src/subsystems/navigation/global_planner/include/global_planner/global_planner_algo.hpp b/src/subsystems/navigation/global_planner/include/global_planner/global_planner_algo.hpp new file mode 100644 index 00000000..c94c7a20 --- /dev/null +++ b/src/subsystems/navigation/global_planner/include/global_planner/global_planner_algo.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2025 UMD Loop +// +// 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. + +#pragma once + +#include +#include +#include + +namespace global_planner { + +struct Pose2D { + double x; + double y; + double yaw; +}; + +struct Costmap { + uint32_t width = 0; + uint32_t height = 0; + double resolution = 0.0; + double origin_x = 0.0; + double origin_y = 0.0; + std::vector data; +}; + +struct PlannerParams { + double path_resolution_m = 1.0; + bool use_costmap = false; + double slope_weight = 10.0; +}; + +class GlobalPlannerAlgo { +public: + GlobalPlannerAlgo() = default; + + void setParams(const PlannerParams& params) { params_ = params; } + const PlannerParams& getParams() const { return params_; } + + void setCostmap(const Costmap& costmap) { costmap_ = costmap; } + void clearCostmap() { costmap_ = std::nullopt; } + bool hasCostmap() const { return costmap_.has_value(); } + + std::vector planStraightLine(double sx, double sy, double gx, double gy) const; + std::optional> planAstar(double sx, double sy, double gx, double gy) const; + +private: + PlannerParams params_; + std::optional costmap_; +}; + +} // namespace global_planner diff --git a/src/subsystems/even_more_minimal_nav/global_planner/package.xml b/src/subsystems/navigation/global_planner/package.xml similarity index 100% rename from src/subsystems/even_more_minimal_nav/global_planner/package.xml rename to src/subsystems/navigation/global_planner/package.xml diff --git a/src/subsystems/navigation/global_planner/src/global_planner_algo.cpp b/src/subsystems/navigation/global_planner/src/global_planner_algo.cpp new file mode 100644 index 00000000..40ba5276 --- /dev/null +++ b/src/subsystems/navigation/global_planner/src/global_planner_algo.cpp @@ -0,0 +1,153 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 "global_planner/global_planner_algo.hpp" + +#include +#include +#include +#include +#include + +namespace global_planner { + +std::vector GlobalPlannerAlgo::planStraightLine( + double sx, double sy, double gx, double gy) const { + std::vector path; + + const double dist = std::hypot(gx - sx, gy - sy); + const double yaw = std::atan2(gy - sy, gx - sx); + + if (dist < 1e-6) { + path.push_back({gx, gy, yaw}); + return path; + } + + const int n = std::max(2, static_cast(std::ceil(dist / params_.path_resolution_m)) + 1); + + path.reserve(n); + for (int i = 0; i < n; ++i) { + const double t = static_cast(i) / (n - 1); + path.push_back({sx + t * (gx - sx), sy + t * (gy - sy), yaw}); + } + + return path; +} + +std::optional> GlobalPlannerAlgo::planAstar( + double sx, double sy, double gx, double gy) const { + if (!costmap_.has_value()) { + return std::nullopt; + } + + const auto& cmap = costmap_.value(); + const int W = static_cast(cmap.width); + const int H = static_cast(cmap.height); + const double res = cmap.resolution; + const double ox = cmap.origin_x; + const double oy = cmap.origin_y; + + auto toGrid = [&](double wx, double wy, int& col, int& row) -> bool { + col = static_cast((wx - ox) / res); + row = static_cast((wy - oy) / res); + return col >= 0 && col < W && row >= 0 && row < H; + }; + + auto toWorld = [&](int col, int row, double& wx, double& wy) { + wx = ox + (col + 0.5) * res; + wy = oy + (row + 0.5) * res; + }; + + int sc, sr, gc, gr; + if (!toGrid(sx, sy, sc, sr)) return std::nullopt; + if (!toGrid(gx, gy, gc, gr)) return std::nullopt; + + const auto goal_val = static_cast(cmap.data[gr * W + gc]); + if (goal_val >= 254) return std::nullopt; + + using State = std::tuple; + std::priority_queue, std::greater> open; + + std::vector g_cost(W * H, std::numeric_limits::infinity()); + std::vector parent(W * H, -1); + + const int s_idx = sr * W + sc; + g_cost[s_idx] = 0.0; + open.emplace(0.0, sc, sr); + + constexpr int dcol[8] = {1, -1, 0, 0, 1, 1, -1, -1}; + constexpr int drow[8] = {0, 0, 1, -1, 1, -1, 1, -1}; + constexpr double step[8] = {1, 1, 1, 1, M_SQRT2, M_SQRT2, M_SQRT2, M_SQRT2}; + + bool found = false; + while (!open.empty()) { + auto [f, cx, cy] = open.top(); + open.pop(); + + const int idx = cy * W + cx; + if (f > g_cost[idx] + 1e-9) continue; + if (cx == gc && cy == gr) { found = true; break; } + + for (int d = 0; d < 8; ++d) { + const int nx = cx + dcol[d]; + const int ny = cy + drow[d]; + if (nx < 0 || nx >= W || ny < 0 || ny >= H) continue; + + const auto cell_val = static_cast(cmap.data[ny * W + nx]); + if (cell_val >= 254) continue; + + const double move_cost = step[d] * res + params_.slope_weight * static_cast(cell_val) / 254.0; + const double ng = g_cost[idx] + move_cost; + const int n_idx = ny * W + nx; + + if (ng < g_cost[n_idx]) { + g_cost[n_idx] = ng; + parent[n_idx] = idx; + const double h = std::hypot(nx - gc, ny - gr) * res; + open.emplace(ng + h, nx, ny); + } + } + } + + if (!found) return std::nullopt; + + std::vector> cells; + for (int cur = gr * W + gc; cur != -1; cur = parent[cur]) { + cells.emplace_back(cur % W, cur / W); + } + std::reverse(cells.begin(), cells.end()); + + std::vector path; + path.reserve(cells.size()); + + for (size_t i = 0; i < cells.size(); ++i) { + double wx, wy; + toWorld(cells[i].first, cells[i].second, wx, wy); + + double yaw = 0.0; + if (i + 1 < cells.size()) { + double nwx, nwy; + toWorld(cells[i + 1].first, cells[i + 1].second, nwx, nwy); + yaw = std::atan2(nwy - wy, nwx - wx); + } else if (!path.empty()) { + yaw = path.back().yaw; + } + + path.push_back({wx, wy, yaw}); + } + + return path; +} + +} // namespace global_planner \ No newline at end of file diff --git a/src/subsystems/navigation/global_planner/src/global_planner_node.cpp b/src/subsystems/navigation/global_planner/src/global_planner_node.cpp new file mode 100644 index 00000000..2e2b7b2f --- /dev/null +++ b/src/subsystems/navigation/global_planner/src/global_planner_node.cpp @@ -0,0 +1,173 @@ +// Copyright (c) 2025 UMD Loop +// +// 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. + +// global_planner_node.cpp +// +// Converts a /goal_pose into a /global_path. +// +// Phase 1 (use_costmap: false): +// Straight-line interpolation from current TF position to goal. +// Runs immediately on every new goal with no external dependencies. +// +// Phase 2 (use_costmap: true, costmap received): +// A* on the OccupancyGrid from dem_costmap_converter. +// Cost to enter a cell: dist_to_neighbor + slope_weight * (grid_value / 254). +// Cells with grid_value >= 254 are lethal and impassable. +// Falls back to straight-line if A* cannot find a path. + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/path.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/exceptions.h" + +#include "global_planner/global_planner_algo.hpp" + +class GlobalPlanner : public rclcpp::Node +{ +public: + GlobalPlanner() : Node("global_planner") + { + declare_parameter("path_resolution_m", 1.0); + declare_parameter("use_costmap", false); + declare_parameter("slope_weight", 10.0); + + global_planner::PlannerParams p; + p.path_resolution_m = get_parameter("path_resolution_m").as_double(); + p.use_costmap = get_parameter("use_costmap").as_bool(); + p.slope_weight = get_parameter("slope_weight").as_double(); + + algo_.setParams(p); + + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + path_pub_ = create_publisher( + "/global_path", rclcpp::QoS(1).transient_local()); + + goal_sub_ = create_subscription( + "/goal_pose", 10, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { onGoal(msg); }); + + // Costmap is optional — published by dem_costmap_converter with transient_local QoS. + costmap_sub_ = create_subscription( + "/map", rclcpp::QoS(1).transient_local(), + [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + global_planner::Costmap cmap; + cmap.width = msg->info.width; + cmap.height = msg->info.height; + cmap.resolution = msg->info.resolution; + cmap.origin_x = msg->info.origin.position.x; + cmap.origin_y = msg->info.origin.position.y; + cmap.data = msg->data; + algo_.setCostmap(cmap); + }); + + RCLCPP_INFO(get_logger(), "GlobalPlanner ready (use_costmap=%s)", + p.use_costmap ? "true" : "false"); + } + +private: + // ── Goal callback ────────────────────────────────────────────────────────── + + void onGoal(const geometry_msgs::msg::PoseStamped::SharedPtr goal) + { + // Look up current robot position in map frame + geometry_msgs::msg::TransformStamped tf; + try { + tf = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "TF map→base_link unavailable: %s — cannot plan", ex.what()); + return; + } + + const double sx = tf.transform.translation.x; + const double sy = tf.transform.translation.y; + const double gx = goal->pose.position.x; + const double gy = goal->pose.position.y; + + RCLCPP_INFO(get_logger(), "Planning (%.2f,%.2f) → (%.2f,%.2f)", sx, sy, gx, gy); + + const auto& p = algo_.getParams(); + + // Phase 2: A* if enabled and costmap is available + if (p.use_costmap && algo_.hasCostmap()) { + auto maybe_algo_path = algo_.planAstar(sx, sy, gx, gy); + if (maybe_algo_path.has_value()) { + auto msg = toPathMsg(maybe_algo_path.value()); + path_pub_->publish(msg); + RCLCPP_INFO(get_logger(), "A* path published (%zu poses)", msg.poses.size()); + return; + } + RCLCPP_WARN(get_logger(), "A* failed — falling back to straight-line"); + } + + // Phase 1: straight-line + auto algo_path = algo_.planStraightLine(sx, sy, gx, gy); + auto msg = toPathMsg(algo_path); + path_pub_->publish(msg); + RCLCPP_INFO(get_logger(), "Straight-line path published (%zu poses)", msg.poses.size()); + } + + nav_msgs::msg::Path toPathMsg(const std::vector& algo_path) const { + nav_msgs::msg::Path path; + path.header.frame_id = "map"; + path.header.stamp = now(); + path.poses.reserve(algo_path.size()); + + for (const auto& pt : algo_path) { + geometry_msgs::msg::PoseStamped p; + p.header = path.header; + p.pose.position.x = pt.x; + p.pose.position.y = pt.y; + + const double half = pt.yaw * 0.5; + p.pose.orientation.z = std::sin(half); + p.pose.orientation.w = std::cos(half); + + path.poses.push_back(p); + } + + return path; + } + + // ── TF ───────────────────────────────────────────────────────────────────── + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // ── ROS interfaces ────────────────────────────────────────────────────────── + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr costmap_sub_; + + // ── State ─────────────────────────────────────────────────────────────────── + global_planner::GlobalPlannerAlgo algo_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/even_more_minimal_nav/gps_pose_publisher/CMakeLists.txt b/src/subsystems/navigation/gps_pose_publisher/CMakeLists.txt similarity index 100% rename from src/subsystems/even_more_minimal_nav/gps_pose_publisher/CMakeLists.txt rename to src/subsystems/navigation/gps_pose_publisher/CMakeLists.txt diff --git a/src/subsystems/even_more_minimal_nav/gps_pose_publisher/package.xml b/src/subsystems/navigation/gps_pose_publisher/package.xml similarity index 100% rename from src/subsystems/even_more_minimal_nav/gps_pose_publisher/package.xml rename to src/subsystems/navigation/gps_pose_publisher/package.xml diff --git a/src/subsystems/even_more_minimal_nav/gps_pose_publisher/src/gps_pose_publisher_node.cpp b/src/subsystems/navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp similarity index 100% rename from src/subsystems/even_more_minimal_nav/gps_pose_publisher/src/gps_pose_publisher_node.cpp rename to src/subsystems/navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp diff --git a/src/subsystems/navigation/mission_executive/CMakeLists.txt b/src/subsystems/navigation/mission_executive/CMakeLists.txt new file mode 100644 index 00000000..caf34217 --- /dev/null +++ b/src/subsystems/navigation/mission_executive/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.8) +project(mission_executive) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(msgs REQUIRED) + +add_library(mission_executive_algo src/mission_executive_algo.cpp) +target_include_directories(mission_executive_algo PUBLIC + $ + $ +) +target_compile_features(mission_executive_algo PUBLIC cxx_std_17) + +add_executable(mission_executive_node src/mission_executive_node.cpp) +target_include_directories(mission_executive_node PUBLIC + $ +) +target_compile_features(mission_executive_node PUBLIC cxx_std_17) +target_link_libraries(mission_executive_node + mission_executive_algo + rclcpp::rclcpp + rclcpp_action::rclcpp_action + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${std_msgs_TARGETS} + ${std_srvs_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros + ${vision_msgs_TARGETS} + ${msgs_TARGETS} +) + +install(TARGETS mission_executive_algo mission_executive_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_mission_executive + test/test_mission_executive.cpp + ) + target_include_directories(test_mission_executive PUBLIC + $ + ) + target_compile_features(test_mission_executive PUBLIC cxx_std_17) + target_link_libraries(test_mission_executive mission_executive_algo) + + ament_add_gtest(test_mock_pipeline + test/test_mock_pipeline.cpp + ../global_planner/src/global_planner_algo.cpp + ../vector_field_planner/src/vector_field_planner_algo.cpp + ) + target_include_directories(test_mock_pipeline PUBLIC + $ + $ + $ + ) + target_compile_features(test_mock_pipeline PUBLIC cxx_std_17) + target_link_libraries(test_mock_pipeline mission_executive_algo) +endif() + +ament_package() diff --git a/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp b/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp new file mode 100644 index 00000000..290f264d --- /dev/null +++ b/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp @@ -0,0 +1,161 @@ +// Copyright (c) 2025 UMD Loop +// +// 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. + +#pragma once + +#include +#include +#include +#include +#include + +namespace mission_executive { + +enum class State { + IDLE, + NAVIGATING, + ARRIVING, + STOPPED_AT_TARGET, + SPIRAL_COVERAGE, + SPIRAL_DONE, + ABORTING, + RETURNING, + STOPPED_AT_RETURN, + TELEOP +}; + +std::string stateToStr(State s); + +struct Pose2D { + double x{0.0}; + double y{0.0}; + double yaw{0.0}; +}; + +struct TargetEntry { + std::string id; + double x_m{0.0}; + double y_m{0.0}; + uint8_t target_type{0}; + uint8_t goal_source{0}; + double tolerance_m{3.0}; + bool visited{false}; +}; + +struct MissionParams { + double stop_angular_vel_threshold = 0.05; + double arrival_hold_time = 1.0; + double replan_distance_m = 3.0; + double spiral_timeout_s = 120.0; + double spiral_radius_m = 15.0; + double spiral_spacing_m = 2.0; + double spiral_angular_step = 0.5; + double spiral_waypoint_tolerance_m = 2.0; +}; + +struct CommandResult { + bool success; + std::string message; +}; + +struct StartNavResult { + bool accepted = false; + std::string message; + bool preempted_old = false; + bool publish_goal = false; + Pose2D goal_to_publish; +}; + +struct TickResult { + bool publish_goal = false; + Pose2D goal_to_publish; + + bool action_finished = false; + bool action_success = false; + std::string action_message; + + bool start_queued_goal = false; +}; + +class MissionExecutiveAlgo { +public: + MissionExecutiveAlgo() = default; + + void setParams(const MissionParams& p) { params_ = p; } + + // Inputs + void updateRobotPose(const Pose2D& pose) { robot_pose_ = pose; } + void updateGlobalPath(const std::vector& path) { global_path_ = path; } + void updateImu(double angular_vel_mag, double current_time_s); + void updatePlannerEvent(uint8_t event) { last_planner_event_ = event; } + + void onPlanFailed(); + void onDetection(); + + // Commands + CommandResult setTarget(const TargetEntry& entry); + StartNavResult startNav(const std::string& target_id, const std::optional& inline_target, bool is_return); + CommandResult abort(); + CommandResult setTeleop(bool enable); + CommandResult cancelNav(); + + TickResult tick(double current_time_s); + + // Getters + State getState() const { return state_; } + bool isNavEnabled() const { + return state_ == State::NAVIGATING || state_ == State::ARRIVING || + state_ == State::RETURNING || state_ == State::SPIRAL_COVERAGE; + } + std::string getNavMode() const; + std::optional getActiveTarget() const { return active_target_; } + double getDistToGoal() const; + double getCrossTrackError() const; + double getHeadingError() const; + double getImuAngularVel() const { return imu_angular_vel_; } + bool isReturn() const { return is_return_; } + uint8_t getLastPlannerEvent() const { return last_planner_event_; } + bool hasQueuedGoal() const { return queued_active_; } + +private: + void transition(State new_state); + void startSpiralCoverage(double current_time_s); + std::vector generateSpiralWaypoints(const Pose2D& start); + + State state_{State::IDLE}; + MissionParams params_; + + std::unordered_map target_registry_; + std::optional active_target_; + bool is_return_{false}; + + bool queued_active_{false}; + TargetEntry queued_entry_; + bool queued_is_return_{false}; + + std::optional robot_pose_; + std::optional> global_path_; + + double imu_angular_vel_{0.0}; + double low_speed_start_s_{0.0}; + bool low_speed_tracking_{false}; + uint8_t last_planner_event_{0}; + + std::vector spiral_waypoints_; + size_t spiral_waypoint_idx_{0}; + double spiral_start_time_s_{0.0}; + bool detection_received_{false}; +}; + +} // namespace mission_executive diff --git a/src/subsystems/even_more_minimal_nav/mission_executive/package.xml b/src/subsystems/navigation/mission_executive/package.xml similarity index 100% rename from src/subsystems/even_more_minimal_nav/mission_executive/package.xml rename to src/subsystems/navigation/mission_executive/package.xml diff --git a/src/subsystems/even_more_minimal_nav/mission_executive/scripts/mission_cli.sh b/src/subsystems/navigation/mission_executive/scripts/mission_cli.sh similarity index 100% rename from src/subsystems/even_more_minimal_nav/mission_executive/scripts/mission_cli.sh rename to src/subsystems/navigation/mission_executive/scripts/mission_cli.sh diff --git a/src/subsystems/navigation/mission_executive/src/mission_executive_algo.cpp b/src/subsystems/navigation/mission_executive/src/mission_executive_algo.cpp new file mode 100644 index 00000000..8622e3b1 --- /dev/null +++ b/src/subsystems/navigation/mission_executive/src/mission_executive_algo.cpp @@ -0,0 +1,366 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 "mission_executive/mission_executive_algo.hpp" +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 +#endif + +namespace mission_executive { + +std::string stateToStr(State s) { + switch (s) { + case State::IDLE: return "IDLE"; + case State::NAVIGATING: return "NAVIGATING"; + case State::ARRIVING: return "ARRIVING"; + case State::STOPPED_AT_TARGET: return "STOPPED_AT_TARGET"; + case State::SPIRAL_COVERAGE: return "SPIRAL_COVERAGE"; + case State::SPIRAL_DONE: return "SPIRAL_DONE"; + case State::ABORTING: return "ABORTING"; + case State::RETURNING: return "RETURNING"; + case State::STOPPED_AT_RETURN: return "STOPPED_AT_RETURN"; + case State::TELEOP: return "TELEOP"; + default: return "UNKNOWN"; + } +} + +void MissionExecutiveAlgo::transition(State new_state) { + state_ = new_state; + if (state_ == State::TELEOP || !isNavEnabled()) { + low_speed_tracking_ = false; + } +} + +std::string MissionExecutiveAlgo::getNavMode() const { + switch (state_) { + case State::TELEOP: return "teleop"; + case State::NAVIGATING: + case State::ARRIVING: + case State::RETURNING: + case State::SPIRAL_COVERAGE: return "autonomous"; + default: return "stopped"; + } +} + +double MissionExecutiveAlgo::getHeadingError() const { + if (!robot_pose_.has_value() || !active_target_.has_value()) return 0.0; + const double dx = active_target_->x_m - robot_pose_->x; + const double dy = active_target_->y_m - robot_pose_->y; + const double bearing = std::atan2(dy, dx); + double err = bearing - robot_pose_->yaw; + while (err > M_PI) err -= 2.0 * M_PI; + while (err < -M_PI) err += 2.0 * M_PI; + return err; +} + +double MissionExecutiveAlgo::getDistToGoal() const { + if (!robot_pose_.has_value() || !active_target_.has_value()) return -1.0; + return std::hypot(robot_pose_->x - active_target_->x_m, robot_pose_->y - active_target_->y_m); +} + +double MissionExecutiveAlgo::getCrossTrackError() const { + if (!robot_pose_.has_value() || !global_path_.has_value()) return -1.0; + const auto & poses = global_path_.value(); + if (poses.size() < 2) return -1.0; + + const double rx = robot_pose_->x; + const double ry = robot_pose_->y; + double min_dist = std::numeric_limits::max(); + + for (size_t i = 0; i < poses.size() - 1; ++i) { + const double ax = poses[i].x, ay = poses[i].y; + const double bx = poses[i+1].x, by = poses[i+1].y; + const double dx = bx - ax, dy = by - ay; + const double len2 = dx*dx + dy*dy; + double dist; + if (len2 < 1e-10) { + dist = std::hypot(rx - ax, ry - ay); + } else { + const double t = std::clamp(((rx-ax)*dx + (ry-ay)*dy) / len2, 0.0, 1.0); + dist = std::hypot(rx - (ax + t*dx), ry - (ay + t*dy)); + } + min_dist = std::min(min_dist, dist); + } + return min_dist; +} + +void MissionExecutiveAlgo::updateImu(double angular_vel_mag, double current_time_s) { + imu_angular_vel_ = angular_vel_mag; + if (state_ == State::ARRIVING) { + if (imu_angular_vel_ < params_.stop_angular_vel_threshold) { + if (!low_speed_tracking_) { + low_speed_start_s_ = current_time_s; + low_speed_tracking_ = true; + } else { + if (current_time_s - low_speed_start_s_ >= params_.arrival_hold_time) { + low_speed_tracking_ = false; + if (is_return_) { + transition(State::STOPPED_AT_RETURN); + } else { + if (active_target_.has_value()) { + auto it = target_registry_.find(active_target_->id); + if (it != target_registry_.end()) { + it->second.visited = true; + } + } + transition(State::STOPPED_AT_TARGET); + } + } + } + } else { + low_speed_tracking_ = false; + } + } +} + +void MissionExecutiveAlgo::onPlanFailed() { + if (state_ == State::NAVIGATING || state_ == State::RETURNING) { + transition(State::ABORTING); + } +} + +void MissionExecutiveAlgo::onDetection() { + if (state_ == State::SPIRAL_COVERAGE) { + detection_received_ = true; + transition(State::SPIRAL_DONE); + } +} + +CommandResult MissionExecutiveAlgo::setTarget(const TargetEntry& entry) { + target_registry_[entry.id] = entry; + return {true, "Target '" + entry.id + "' registered"}; +} + +StartNavResult MissionExecutiveAlgo::startNav(const std::string& target_id, const std::optional& inline_target, bool is_return) { + StartNavResult res; + + TargetEntry entry; + if (!target_id.empty()) { + auto it = target_registry_.find(target_id); + if (it == target_registry_.end()) { + res.accepted = false; + res.message = "Unknown target_id: " + target_id; + return res; + } + if (is_return && !it->second.visited) { + res.accepted = false; + res.message = "Target not yet visited — cannot RETURN"; + return res; + } + entry = it->second; + } else if (inline_target.has_value()) { + entry = inline_target.value(); + } else { + res.accepted = false; + res.message = "No target provided"; + return res; + } + + if (state_ == State::TELEOP) { + res.accepted = false; + res.message = "Cannot navigate — currently in TELEOP mode"; + return res; + } + + if (state_ == State::ABORTING) { + if (queued_active_) { + res.preempted_old = true; + } + queued_active_ = true; + queued_entry_ = entry; + queued_is_return_ = is_return; + res.accepted = true; + res.message = "Goal queued — currently ABORTING"; + return res; + } + + bool is_active_nav = (state_ != State::IDLE && state_ != State::STOPPED_AT_TARGET && state_ != State::STOPPED_AT_RETURN && state_ != State::SPIRAL_DONE); + if (is_active_nav) { + res.preempted_old = true; + } + + active_target_ = entry; + is_return_ = is_return; + transition(is_return ? State::RETURNING : State::NAVIGATING); + + res.accepted = true; + res.publish_goal = true; + res.goal_to_publish = {entry.x_m, entry.y_m, 0.0}; + return res; +} + +CommandResult MissionExecutiveAlgo::abort() { + if (state_ == State::IDLE || state_ == State::TELEOP || + state_ == State::ABORTING || + state_ == State::STOPPED_AT_TARGET || state_ == State::STOPPED_AT_RETURN || + state_ == State::SPIRAL_DONE) { + return {false, "Nothing to abort in state " + stateToStr(state_)}; + } + transition(State::ABORTING); + return {true, "Aborting"}; +} + +CommandResult MissionExecutiveAlgo::setTeleop(bool enable) { + if (enable) { + transition(State::TELEOP); + return {true, "Teleop ON"}; + } else { + if (state_ == State::TELEOP) { + transition(State::IDLE); + return {true, "Teleop OFF"}; + } else { + return {false, "Not in TELEOP state"}; + } + } +} + +CommandResult MissionExecutiveAlgo::cancelNav() { + transition(State::IDLE); + return {true, "Cancelled"}; +} + +std::vector MissionExecutiveAlgo::generateSpiralWaypoints(const Pose2D& start) { + std::vector waypoints; + const double a = params_.spiral_spacing_m / (2.0 * M_PI); + if (a <= 0.0) return waypoints; + + const double max_angle = params_.spiral_radius_m / a; + const double min_start_r = 1.5; + const double yaw0 = start.yaw; + + double theta = 0.0; + while (theta <= max_angle) { + const double r = min_start_r + a * theta; + if (r > params_.spiral_radius_m) break; + + const double x = start.x + r * std::cos(yaw0 + theta + M_PI_2); + const double y = start.y + r * std::sin(yaw0 + theta + M_PI_2); + + waypoints.push_back({x, y, 0.0}); + theta += params_.spiral_angular_step; + } + return waypoints; +} + +void MissionExecutiveAlgo::startSpiralCoverage(double current_time_s) { + if (!robot_pose_.has_value()) { + transition(State::SPIRAL_DONE); + return; + } + + spiral_waypoints_ = generateSpiralWaypoints(robot_pose_.value()); + spiral_waypoint_idx_ = 0; + spiral_start_time_s_ = current_time_s; + detection_received_ = false; + + if (spiral_waypoints_.empty()) { + transition(State::SPIRAL_DONE); + } +} + +TickResult MissionExecutiveAlgo::tick(double current_time_s) { + TickResult res; + + // 1. checkArrival + if ((state_ == State::NAVIGATING || state_ == State::RETURNING) && active_target_.has_value()) { + double d = getDistToGoal(); + if (d >= 0.0 && d < active_target_->tolerance_m) { + transition(State::ARRIVING); + } + } + + // 2. checkCrossTrackError + if ((state_ == State::NAVIGATING || state_ == State::RETURNING) && active_target_.has_value()) { + double xte = getCrossTrackError(); + if (xte >= 0.0 && xte > params_.replan_distance_m) { + res.publish_goal = true; + res.goal_to_publish = {active_target_->x_m, active_target_->y_m, 0.0}; + } + } + + // 3. checkSpiralProgress + if (state_ == State::SPIRAL_COVERAGE) { + if (current_time_s - spiral_start_time_s_ >= params_.spiral_timeout_s) { + transition(State::SPIRAL_DONE); + } else if (robot_pose_.has_value() && !spiral_waypoints_.empty()) { + const auto & wp = spiral_waypoints_[spiral_waypoint_idx_]; + const double dist = std::hypot(robot_pose_->x - wp.x, robot_pose_->y - wp.y); + if (dist < params_.spiral_waypoint_tolerance_m) { + ++spiral_waypoint_idx_; + if (spiral_waypoint_idx_ >= spiral_waypoints_.size()) { + transition(State::SPIRAL_DONE); + } else { + res.publish_goal = true; + res.goal_to_publish = spiral_waypoints_[spiral_waypoint_idx_]; + } + } + } + } + + // 4. Action result resolution + if (state_ == State::STOPPED_AT_TARGET) { + // ARUCO_POST = 1, OBJECT = 2 + const bool needs_spiral = active_target_.has_value() && + (active_target_->target_type == 1 || active_target_->target_type == 2); + + if (needs_spiral) { + transition(State::SPIRAL_COVERAGE); + startSpiralCoverage(current_time_s); + if (state_ == State::SPIRAL_COVERAGE && !spiral_waypoints_.empty()) { + res.publish_goal = true; + res.goal_to_publish = spiral_waypoints_[0]; + } + } else { + res.action_finished = true; + res.action_success = true; + res.action_message = "Arrived at target"; + } + } else if (state_ == State::SPIRAL_DONE) { + res.action_finished = true; + res.action_success = true; + res.action_message = detection_received_ ? "Detection found during spiral coverage" : "Spiral coverage complete (timeout or all waypoints visited)"; + } else if (state_ == State::STOPPED_AT_RETURN) { + res.action_finished = true; + res.action_success = true; + res.action_message = "Arrived at target"; + } else if (state_ == State::ABORTING) { + res.action_finished = true; + res.action_success = false; + res.action_message = "Aborted"; + + if (queued_active_) { + active_target_ = queued_entry_; + is_return_ = queued_is_return_; + queued_active_ = false; + transition(is_return_ ? State::RETURNING : State::NAVIGATING); + res.start_queued_goal = true; + res.publish_goal = true; + res.goal_to_publish = {active_target_->x_m, active_target_->y_m, 0.0}; + } else { + transition(State::IDLE); + } + } + + return res; +} + +} // namespace mission_executive diff --git a/src/subsystems/navigation/mission_executive/src/mission_executive_node.cpp b/src/subsystems/navigation/mission_executive/src/mission_executive_node.cpp new file mode 100644 index 00000000..1a5c8444 --- /dev/null +++ b/src/subsystems/navigation/mission_executive/src/mission_executive_node.cpp @@ -0,0 +1,576 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "tf2/exceptions.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/string.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "vision_msgs/msg/detection2_d.hpp" + +#include "msgs/action/navigate_to_target.hpp" +#include "msgs/msg/active_target.hpp" +#include "msgs/msg/nav_status.hpp" +#include "msgs/msg/planner_event.hpp" +#include "msgs/srv/lat_lon_to_enu.hpp" +#include "msgs/srv/set_target.hpp" + +#include "mission_executive/mission_executive_algo.hpp" + +using namespace std::chrono_literals; + +class MissionExecutive : public rclcpp::Node +{ +public: + using NavAction = msgs::action::NavigateToTarget; + using GoalHandle = rclcpp_action::ServerGoalHandle; + + explicit MissionExecutive(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("mission_executive", options) + { + // Behaviour parameters + declare_parameter("stop_angular_vel_threshold", 0.05); // rad/s — IMU gyro magnitude + declare_parameter("arrival_hold_time", 1.0); + declare_parameter("replan_distance_m", 3.0); + declare_parameter("latlon_to_enu_service", + std::string("/gps_pose_publisher/latlon_to_enu")); + + // Spiral coverage parameters + declare_parameter("spiral_timeout_s", 120.0); + declare_parameter("spiral_radius_m", 15.0); + declare_parameter("spiral_spacing_m", 2.0); + declare_parameter("spiral_angular_step", 0.5); + declare_parameter("spiral_waypoint_tolerance_m", 2.0); + + // Detection topics + declare_parameter("aruco_detection_topic", std::string("/aruco_loc")); + declare_parameter("yolo_detection_topic", std::string("/yolo_detection")); + + // Input topics + declare_parameter("imu_topic", std::string("/imu")); + declare_parameter("planner_event_topic", std::string("/planner_event")); + declare_parameter("global_path_topic", std::string("/global_path")); + + // Output topics + declare_parameter("goal_pose_topic", std::string("/goal_pose")); + declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); + declare_parameter("nav_mode_topic", std::string("/nav_mode")); + declare_parameter("active_target_topic", std::string("/active_target")); + declare_parameter("nav_status_topic", std::string("/nav_status")); + + mission_executive::MissionParams p; + p.stop_angular_vel_threshold = get_parameter("stop_angular_vel_threshold").as_double(); + p.arrival_hold_time = get_parameter("arrival_hold_time").as_double(); + p.replan_distance_m = get_parameter("replan_distance_m").as_double(); + p.spiral_timeout_s = get_parameter("spiral_timeout_s").as_double(); + p.spiral_radius_m = get_parameter("spiral_radius_m").as_double(); + p.spiral_spacing_m = get_parameter("spiral_spacing_m").as_double(); + p.spiral_angular_step = get_parameter("spiral_angular_step").as_double(); + p.spiral_waypoint_tolerance_m= get_parameter("spiral_waypoint_tolerance_m").as_double(); + + algo_.setParams(p); + + const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); + + const auto aruco_detection_topic = get_parameter("aruco_detection_topic").as_string(); + const auto yolo_detection_topic = get_parameter("yolo_detection_topic").as_string(); + + const auto imu_topic = get_parameter("imu_topic").as_string(); + const auto planner_event_topic = get_parameter("planner_event_topic").as_string(); + const auto global_path_topic = get_parameter("global_path_topic").as_string(); + const auto goal_pose_topic = get_parameter("goal_pose_topic").as_string(); + const auto nav_enabled_topic = get_parameter("nav_enabled_topic").as_string(); + const auto nav_mode_topic = get_parameter("nav_mode_topic").as_string(); + const auto active_target_topic = get_parameter("active_target_topic").as_string(); + const auto nav_status_topic = get_parameter("nav_status_topic").as_string(); + + reentrant_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + goal_pub_ = create_publisher(goal_pose_topic, 10); + nav_enabled_pub_ = create_publisher( + nav_enabled_topic, rclcpp::QoS(1).reliable()); + nav_mode_pub_ = create_publisher(nav_mode_topic, 10); + active_target_pub_ = create_publisher(active_target_topic, 10); + nav_status_pub_ = create_publisher( + nav_status_topic, rclcpp::QoS(1).reliable()); + + latlon_client_ = create_client( + latlon_svc, + rmw_qos_profile_services_default, + reentrant_group_); + + action_server_ = rclcpp_action::create_server( + this, + "~/navigate_to_target", + [this](const rclcpp_action::GoalUUID &, std::shared_ptr goal) { + return handleGoal(goal); + }, + [this](std::shared_ptr gh) { + return handleCancel(gh); + }, + [this](std::shared_ptr gh) { + handleAccepted(gh); + }, + rcl_action_server_get_default_options(), + reentrant_group_); + + abort_srv_ = create_service( + "~/abort", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr res) + { + std::lock_guard lk(mutex_); + auto cmd_res = algo_.abort(); + res->success = cmd_res.success; + res->message = cmd_res.message; + if (cmd_res.success) { + publishNavEnabled(algo_.isNavEnabled()); + publishNavMode(algo_.getNavMode()); + publishStatus(); + } + }); + + set_target_srv_ = create_service( + "~/set_target", + [this]( + const msgs::srv::SetTarget::Request::SharedPtr req, + msgs::srv::SetTarget::Response::SharedPtr res) + { + onSetTarget(req, res); + }, + rmw_qos_profile_services_default, + reentrant_group_); + + teleop_srv_ = create_service( + "~/teleop", + [this]( + const std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res) + { + std::lock_guard lk(mutex_); + auto cmd_res = algo_.setTeleop(req->data); + res->success = cmd_res.success; + res->message = cmd_res.message; + if (cmd_res.success) { + publishNavEnabled(algo_.isNavEnabled()); + publishNavMode(algo_.getNavMode()); + publishStatus(); + } + }); + + imu_sub_ = create_subscription( + imu_topic, rclcpp::SensorDataQoS(), + [this](const sensor_msgs::msg::Imu::SharedPtr msg) { + std::lock_guard lk(mutex_); + const auto & w = msg->angular_velocity; + double angular_vel = std::sqrt(w.x * w.x + w.y * w.y + w.z * w.z); + double current_time = now().seconds(); + algo_.updateImu(angular_vel, current_time); + + // This could trigger a transition to STOPPED_AT_TARGET, which then needs action server response + // but we handle action results in tick() + }); + + planner_event_sub_ = create_subscription( + planner_event_topic, 10, + [this](const msgs::msg::PlannerEvent::SharedPtr msg) { + std::lock_guard lk(mutex_); + algo_.updatePlannerEvent(msg->event); + if (msg->event == msgs::msg::PlannerEvent::PLAN_FAILED) { + algo_.onPlanFailed(); + publishNavEnabled(algo_.isNavEnabled()); + publishNavMode(algo_.getNavMode()); + publishStatus(); + } + }); + + auto transient_qos = rclcpp::QoS(1).transient_local(); + global_path_sub_ = create_subscription( + global_path_topic, transient_qos, + [this](const nav_msgs::msg::Path::SharedPtr msg) { + std::lock_guard lk(mutex_); + std::vector path; + path.reserve(msg->poses.size()); + for (const auto& p : msg->poses) { + path.push_back({p.pose.position.x, p.pose.position.y, quaternionToYaw(p.pose.orientation)}); + } + algo_.updateGlobalPath(path); + }); + + aruco_sub_ = create_subscription( + aruco_detection_topic, 10, + [this](const vision_msgs::msg::Detection2D::SharedPtr) { + std::lock_guard lk(mutex_); + algo_.onDetection(); + }); + + yolo_sub_ = create_subscription( + yolo_detection_topic, 10, + [this](const std_msgs::msg::Bool::SharedPtr msg) { + std::lock_guard lk(mutex_); + if (msg->data) { + algo_.onDetection(); + } + }); + + status_timer_ = create_wall_timer( + 500ms, + [this]() { + std::lock_guard lk(mutex_); + refreshPoseFromTF(); + + auto res = algo_.tick(now().seconds()); + + if (res.publish_goal) { + geometry_msgs::msg::PoseStamped p; + p.header.frame_id = "map"; + p.header.stamp = now(); + p.pose.position.x = res.goal_to_publish.x; + p.pose.position.y = res.goal_to_publish.y; + p.pose.orientation.w = 1.0; + goal_pub_->publish(p); + } + + checkActionResult(res); + + publishNavEnabled(algo_.isNavEnabled()); + publishNavMode(algo_.getNavMode()); + publishStatus(); + }); + + publishNavEnabled(false); + publishNavMode("stopped"); + + RCLCPP_INFO(get_logger(), "MissionExecutive ready — state: IDLE"); + } + +private: + void publishNavEnabled(bool enabled) { + std_msgs::msg::Bool msg; + msg.data = enabled; + nav_enabled_pub_->publish(msg); + } + + void publishNavMode(const std::string& mode) { + std_msgs::msg::String msg; + msg.data = mode; + nav_mode_pub_->publish(msg); + } + + void publishActiveTarget() { + auto target = algo_.getActiveTarget(); + if (!target.has_value()) return; + msgs::msg::ActiveTarget at; + at.target_id = target->id; + at.target_type = target->target_type; + at.tolerance_m = target->tolerance_m; + + at.goal_enu.header.frame_id = "map"; + at.goal_enu.header.stamp = now(); + at.goal_enu.pose.position.x = target->x_m; + at.goal_enu.pose.position.y = target->y_m; + at.goal_enu.pose.orientation.w = 1.0; + + at.goal_source = target->goal_source; + at.status = mission_executive::stateToStr(algo_.getState()); + active_target_pub_->publish(at); + } + + void publishStatus() { + msgs::msg::NavStatus s; + s.state = mission_executive::stateToStr(algo_.getState()); + auto target = algo_.getActiveTarget(); + if (target.has_value()) { + s.active_target_id = target->id; + s.active_target_type = target->target_type; + s.goal_source = target->goal_source; + } + s.distance_to_goal_m = algo_.getDistToGoal(); + s.cross_track_error_m = algo_.getCrossTrackError(); + s.heading_error_rad = algo_.getHeadingError(); + s.robot_speed_mps = algo_.getImuAngularVel(); + s.is_return = algo_.isReturn(); + s.last_planner_event = algo_.getLastPlannerEvent(); + nav_status_pub_->publish(s); + } + + static double quaternionToYaw(const geometry_msgs::msg::Quaternion & q) { + const double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); + const double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); + return std::atan2(siny_cosp, cosy_cosp); + } + + void refreshPoseFromTF() { + try { + const auto tf = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero); + mission_executive::Pose2D pose; + pose.x = tf.transform.translation.x; + pose.y = tf.transform.translation.y; + pose.yaw = quaternionToYaw(tf.transform.rotation); + algo_.updateRobotPose(pose); + } catch (const tf2::TransformException &) { + } + } + + void checkActionResult(const mission_executive::TickResult& res) { + if (!active_goal_handle_) return; + + if (active_goal_handle_->is_canceling()) { + auto result = std::make_shared(); + result->success = false; + result->message = "Cancelled"; + active_goal_handle_->canceled(result); + active_goal_handle_ = nullptr; + algo_.cancelNav(); + return; + } + + if (res.action_finished) { + auto result = std::make_shared(); + result->success = res.action_success; + result->message = res.action_message; + + if (res.action_success) { + active_goal_handle_->succeed(result); + } else { + active_goal_handle_->abort(result); + } + active_goal_handle_ = nullptr; + } else { + auto fb = std::make_shared(); + fb->distance_to_goal_m = algo_.getDistToGoal(); + fb->cross_track_error_m = algo_.getCrossTrackError(); + fb->state = mission_executive::stateToStr(algo_.getState()); + active_goal_handle_->publish_feedback(fb); + } + + if (res.start_queued_goal) { + active_goal_handle_ = queued_goal_handle_; + queued_goal_handle_ = nullptr; + publishActiveTarget(); + } + } + + rclcpp_action::GoalResponse handleGoal( + std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handleCancel(std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handleAccepted(std::shared_ptr goal_handle) { + const auto goal = goal_handle->get_goal(); + + std::optional inline_target; + + if (goal->target_id.empty()) { + mission_executive::TargetEntry entry; + entry.id = ""; + entry.target_type = goal->target_type; + entry.tolerance_m = goal->tolerance_m; + entry.goal_source = goal->goal_type; + + if (goal->goal_type == NavAction::Goal::GPS) { + if (!latlon_client_->wait_for_service(3s)) { + auto res = std::make_shared(); + res->success = false; + res->message = "latlon_to_enu service not available"; + goal_handle->abort(res); + return; + } + auto req = std::make_shared(); + req->lat = goal->lat; + req->lon = goal->lon; + auto future = latlon_client_->async_send_request(req); + + if (future.wait_for(5s) != std::future_status::ready) { + auto res = std::make_shared(); + res->success = false; + res->message = "latlon_to_enu service timeout"; + goal_handle->abort(res); + return; + } + auto resp = future.get(); + entry.x_m = resp->x; + entry.y_m = resp->y; + } else { + entry.x_m = goal->x_m; + entry.y_m = goal->y_m; + } + inline_target = entry; + } + + { + std::lock_guard lk(mutex_); + + auto cmd_res = algo_.startNav(goal->target_id, inline_target, goal->is_return); + + if (!cmd_res.accepted) { + auto res = std::make_shared(); + res->success = false; + res->message = cmd_res.message; + goal_handle->abort(res); + return; + } + + if (algo_.hasQueuedGoal() && algo_.getState() == mission_executive::State::ABORTING) { + if (queued_goal_handle_ && queued_goal_handle_->is_active()) { + auto old_res = std::make_shared(); + old_res->success = false; + old_res->message = "Preempted by newer queued goal"; + queued_goal_handle_->abort(old_res); + } + queued_goal_handle_ = goal_handle; + return; + } + + if (cmd_res.preempted_old) { + if (active_goal_handle_ && active_goal_handle_->is_active()) { + auto old_res = std::make_shared(); + old_res->success = false; + old_res->message = "Preempted by new goal"; + active_goal_handle_->abort(old_res); + active_goal_handle_ = nullptr; + } + } + + active_goal_handle_ = goal_handle; + + if (cmd_res.publish_goal) { + geometry_msgs::msg::PoseStamped p; + p.header.frame_id = "map"; + p.header.stamp = now(); + p.pose.position.x = cmd_res.goal_to_publish.x; + p.pose.position.y = cmd_res.goal_to_publish.y; + p.pose.orientation.w = 1.0; + goal_pub_->publish(p); + publishActiveTarget(); + } + + publishNavEnabled(algo_.isNavEnabled()); + publishNavMode(algo_.getNavMode()); + publishStatus(); + } + } + + void onSetTarget( + const msgs::srv::SetTarget::Request::SharedPtr req, + msgs::srv::SetTarget::Response::SharedPtr res) + { + mission_executive::TargetEntry entry; + entry.id = req->target_id; + entry.target_type = req->target_type; + entry.tolerance_m = req->tolerance_m; + entry.goal_source = req->goal_type; + + if (req->goal_type == msgs::srv::SetTarget::Request::GPS) { + if (!latlon_client_->wait_for_service(3s)) { + res->success = false; + res->message = "latlon_to_enu service not available"; + return; + } + auto conv = std::make_shared(); + conv->lat = req->lat; + conv->lon = req->lon; + auto future = latlon_client_->async_send_request(conv); + if (future.wait_for(5s) != std::future_status::ready) { + res->success = false; + res->message = "latlon_to_enu service timeout"; + return; + } + auto resp = future.get(); + entry.x_m = resp->x; + entry.y_m = resp->y; + } else { + entry.x_m = req->x_m; + entry.y_m = req->y_m; + } + + { + std::lock_guard lk(mutex_); + auto cmd_res = algo_.setTarget(entry); + res->success = cmd_res.success; + res->message = cmd_res.message; + } + } + + std::mutex mutex_; + mission_executive::MissionExecutiveAlgo algo_; + + std::shared_ptr queued_goal_handle_; + std::shared_ptr active_goal_handle_; + + rclcpp::CallbackGroup::SharedPtr reentrant_group_; + + rclcpp::Publisher::SharedPtr goal_pub_; + rclcpp::Publisher::SharedPtr nav_enabled_pub_; + rclcpp::Publisher::SharedPtr nav_mode_pub_; + rclcpp::Publisher::SharedPtr active_target_pub_; + rclcpp::Publisher::SharedPtr nav_status_pub_; + + rclcpp_action::Server::SharedPtr action_server_; + + rclcpp::Service::SharedPtr abort_srv_; + rclcpp::Service::SharedPtr set_target_srv_; + rclcpp::Service::SharedPtr teleop_srv_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr planner_event_sub_; + rclcpp::Subscription::SharedPtr global_path_sub_; + rclcpp::Subscription::SharedPtr aruco_sub_; + rclcpp::Subscription::SharedPtr yolo_sub_; + + rclcpp::Client::SharedPtr latlon_client_; + + rclcpp::TimerBase::SharedPtr status_timer_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/navigation/mission_executive/test/test_mission_executive.cpp b/src/subsystems/navigation/mission_executive/test/test_mission_executive.cpp new file mode 100644 index 00000000..18126282 --- /dev/null +++ b/src/subsystems/navigation/mission_executive/test/test_mission_executive.cpp @@ -0,0 +1,272 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 "mission_executive/mission_executive_algo.hpp" + +using namespace mission_executive; + +class MissionExecutiveAlgoTest : public ::testing::Test { +protected: + void SetUp() override { + MissionParams p; + p.stop_angular_vel_threshold = 0.05; + p.arrival_hold_time = 1.0; + p.replan_distance_m = 2.0; + p.spiral_timeout_s = 60.0; + p.spiral_radius_m = 10.0; + p.spiral_spacing_m = 2.0; + p.spiral_angular_step = 0.5; + p.spiral_waypoint_tolerance_m = 1.0; + algo_.setParams(p); + } + + MissionExecutiveAlgo algo_; +}; + +TEST_F(MissionExecutiveAlgoTest, InitialStateIsIdle) { + EXPECT_EQ(algo_.getState(), State::IDLE); + EXPECT_FALSE(algo_.isNavEnabled()); + EXPECT_EQ(algo_.getNavMode(), "stopped"); + EXPECT_FALSE(algo_.hasQueuedGoal()); +} + +TEST_F(MissionExecutiveAlgoTest, RegisterTarget) { + TargetEntry entry{"t1", 10.0, 5.0, 0, 0, 3.0, false}; + auto res = algo_.setTarget(entry); + EXPECT_TRUE(res.success); +} + +TEST_F(MissionExecutiveAlgoTest, StartNavWithoutTargetFails) { + auto res = algo_.startNav("", std::nullopt, false); + EXPECT_FALSE(res.accepted); + EXPECT_EQ(algo_.getState(), State::IDLE); +} + +TEST_F(MissionExecutiveAlgoTest, StartNavWithUnknownTargetFails) { + auto res = algo_.startNav("unknown", std::nullopt, false); + EXPECT_FALSE(res.accepted); + EXPECT_EQ(algo_.getState(), State::IDLE); +} + +TEST_F(MissionExecutiveAlgoTest, StartNavValidTargetTransitionsToNavigating) { + TargetEntry entry{"gnss1", 10.0, 10.0, 0, 0, 2.0, false}; + algo_.setTarget(entry); + + auto res = algo_.startNav("gnss1", std::nullopt, false); + EXPECT_TRUE(res.accepted); + EXPECT_TRUE(res.publish_goal); + EXPECT_EQ(res.goal_to_publish.x, 10.0); + EXPECT_EQ(res.goal_to_publish.y, 10.0); + EXPECT_EQ(algo_.getState(), State::NAVIGATING); + EXPECT_TRUE(algo_.isNavEnabled()); + EXPECT_EQ(algo_.getNavMode(), "autonomous"); +} + +TEST_F(MissionExecutiveAlgoTest, CrossTrackErrorReplan) { + TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; + algo_.setTarget(entry); + algo_.startNav("gnss1", std::nullopt, false); + + // Set a path from (0,0) to (10,0) + std::vector path = {{0.0, 0.0, 0.0}, {10.0, 0.0, 0.0}}; + algo_.updateGlobalPath(path); + + // Robot is at (5, 3), which is 3m away from the path y=0. Replan threshold is 2m. + algo_.updateRobotPose({5.0, 3.0, 0.0}); + + auto res = algo_.tick(0.0); + EXPECT_TRUE(res.publish_goal); + EXPECT_EQ(res.goal_to_publish.x, 10.0); +} + +TEST_F(MissionExecutiveAlgoTest, ArrivalStateTransition) { + TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; + algo_.setTarget(entry); + algo_.startNav("gnss1", std::nullopt, false); + + // Robot moves within tolerance (1.5m < 2.0m) + algo_.updateRobotPose({8.5, 0.0, 0.0}); + + algo_.tick(0.0); + EXPECT_EQ(algo_.getState(), State::ARRIVING); +} + +TEST_F(MissionExecutiveAlgoTest, ArrivalHoldTimeAndStop) { + TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; + algo_.setTarget(entry); + algo_.startNav("gnss1", std::nullopt, false); + + algo_.updateRobotPose({9.0, 0.0, 0.0}); + algo_.tick(0.0); // Transitions to ARRIVING + EXPECT_EQ(algo_.getState(), State::ARRIVING); + + // First IMU reading below threshold starts the timer + algo_.updateImu(0.01, 10.0); + algo_.tick(10.0); + EXPECT_EQ(algo_.getState(), State::ARRIVING); + + // Tick before hold time expires + algo_.updateImu(0.01, 10.5); + algo_.tick(10.5); + EXPECT_EQ(algo_.getState(), State::ARRIVING); + + // Tick after hold time expires + algo_.updateImu(0.01, 11.1); + auto tick_res = algo_.tick(11.1); + EXPECT_EQ(algo_.getState(), State::STOPPED_AT_TARGET); + EXPECT_TRUE(tick_res.action_finished); + EXPECT_TRUE(tick_res.action_success); +} + +TEST_F(MissionExecutiveAlgoTest, ArrivalWithSpiralCoverage) { + // Target type 1 is ARUCO_POST, which triggers spiral coverage after stopping + TargetEntry entry{"post1", 10.0, 0.0, 1, 0, 2.0, false}; + algo_.setTarget(entry); + algo_.startNav("post1", std::nullopt, false); + + algo_.updateRobotPose({9.0, 0.0, 0.0}); + algo_.tick(0.0); // ARRIVING + + algo_.updateImu(0.01, 1.0); + algo_.tick(1.0); + algo_.updateImu(0.01, 2.1); + auto tick_res = algo_.tick(2.1); + + // Since it's type 1, it transitions to SPIRAL_COVERAGE + EXPECT_EQ(algo_.getState(), State::SPIRAL_COVERAGE); + EXPECT_FALSE(tick_res.action_finished); // Action is not finished yet + EXPECT_TRUE(tick_res.publish_goal); // Should publish first spiral waypoint +} + +TEST_F(MissionExecutiveAlgoTest, SpiralCoverageCompletesOnDetection) { + TargetEntry entry{"post1", 10.0, 0.0, 1, 0, 2.0, false}; + algo_.setTarget(entry); + algo_.startNav("post1", std::nullopt, false); + algo_.updateRobotPose({9.0, 0.0, 0.0}); + algo_.tick(0.0); // ARRIVING + algo_.updateImu(0.01, 1.0); + algo_.updateImu(0.01, 2.1); + algo_.tick(2.1); // SPIRAL_COVERAGE + + EXPECT_EQ(algo_.getState(), State::SPIRAL_COVERAGE); + + // Detection occurs! + algo_.onDetection(); + + auto tick_res = algo_.tick(3.0); + EXPECT_EQ(algo_.getState(), State::SPIRAL_DONE); + EXPECT_TRUE(tick_res.action_finished); + EXPECT_TRUE(tick_res.action_success); +} + +TEST_F(MissionExecutiveAlgoTest, SpiralCoverageTimeout) { + TargetEntry entry{"post1", 10.0, 0.0, 1, 0, 2.0, false}; + algo_.setTarget(entry); + algo_.startNav("post1", std::nullopt, false); + algo_.updateRobotPose({9.0, 0.0, 0.0}); + algo_.tick(0.0); // ARRIVING + algo_.updateImu(0.01, 1.0); + algo_.updateImu(0.01, 2.1); + algo_.tick(2.1); // SPIRAL_COVERAGE + + // Jump time forward past the timeout + auto tick_res = algo_.tick(2.1 + 61.0); + + EXPECT_EQ(algo_.getState(), State::SPIRAL_DONE); + EXPECT_TRUE(tick_res.action_finished); + EXPECT_TRUE(tick_res.action_success); // Timeout is considered completion +} + +TEST_F(MissionExecutiveAlgoTest, ReturnFailsIfUnvisited) { + TargetEntry entry{"post1", 10.0, 0.0, 1, 0, 2.0, false}; + algo_.setTarget(entry); + + // Attempt to return to an unvisited target + auto res = algo_.startNav("post1", std::nullopt, true); + EXPECT_FALSE(res.accepted); +} + +TEST_F(MissionExecutiveAlgoTest, ReturnSucceedsIfVisited) { + TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, true}; + algo_.setTarget(entry); + + auto res = algo_.startNav("gnss1", std::nullopt, true); + EXPECT_TRUE(res.accepted); + EXPECT_EQ(algo_.getState(), State::RETURNING); +} + +TEST_F(MissionExecutiveAlgoTest, AbortTransitionsToAborting) { + TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; + algo_.setTarget(entry); + algo_.startNav("gnss1", std::nullopt, false); + EXPECT_EQ(algo_.getState(), State::NAVIGATING); + + auto res = algo_.abort(); + EXPECT_TRUE(res.success); + EXPECT_EQ(algo_.getState(), State::ABORTING); + + // Ticking should complete the abort + auto tick_res = algo_.tick(0.0); + EXPECT_TRUE(tick_res.action_finished); + EXPECT_FALSE(tick_res.action_success); + EXPECT_EQ(algo_.getState(), State::IDLE); +} + +TEST_F(MissionExecutiveAlgoTest, GoalQueuedDuringAbort) { + TargetEntry entry1{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; + algo_.setTarget(entry1); + algo_.startNav("gnss1", std::nullopt, false); + + algo_.abort(); + EXPECT_EQ(algo_.getState(), State::ABORTING); + + TargetEntry entry2{"gnss2", 20.0, 0.0, 0, 0, 2.0, false}; + algo_.setTarget(entry2); + + // Send new goal while aborting + auto res = algo_.startNav("gnss2", std::nullopt, false); + EXPECT_TRUE(res.accepted); + EXPECT_TRUE(algo_.hasQueuedGoal()); + + // Tick finishes abort, promotes queued goal + auto tick_res = algo_.tick(0.0); + EXPECT_TRUE(tick_res.action_finished); + EXPECT_TRUE(tick_res.start_queued_goal); + EXPECT_EQ(algo_.getState(), State::NAVIGATING); + EXPECT_EQ(algo_.getActiveTarget()->id, "gnss2"); +} + +TEST_F(MissionExecutiveAlgoTest, PlanFailedTriggersAbort) { + TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; + algo_.setTarget(entry); + algo_.startNav("gnss1", std::nullopt, false); + + algo_.onPlanFailed(); + EXPECT_EQ(algo_.getState(), State::ABORTING); +} + +TEST_F(MissionExecutiveAlgoTest, TeleopPreventsNav) { + algo_.setTeleop(true); + EXPECT_EQ(algo_.getState(), State::TELEOP); + + TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; + algo_.setTarget(entry); + + auto res = algo_.startNav("gnss1", std::nullopt, false); + EXPECT_FALSE(res.accepted); + + algo_.setTeleop(false); + EXPECT_EQ(algo_.getState(), State::IDLE); +} diff --git a/src/subsystems/navigation/mission_executive/test/test_mock_pipeline.cpp b/src/subsystems/navigation/mission_executive/test/test_mock_pipeline.cpp new file mode 100644 index 00000000..8c73436c --- /dev/null +++ b/src/subsystems/navigation/mission_executive/test/test_mock_pipeline.cpp @@ -0,0 +1,341 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 +#include + +#include "mission_executive/mission_executive_algo.hpp" +#include "global_planner/global_planner_algo.hpp" +#include "vector_field_planner/vector_field_planner_algo.hpp" + +// ============================================================================ +// Replica of FrontAckermannController logic (from test_pure_pursuit_sim.cpp) +// ============================================================================ + +struct AckermannParams { + double wheelbase = 0.8382; + double track_width = 0.6604; + double wheel_radius = 0.254; + double max_speed = 1.27; + double max_steer_angle = 0.785; +}; + +struct WheelState { + double fl_steer = 0.0, fr_steer = 0.0; + double fl_vel = 0.0, fr_vel = 0.0, rl_vel = 0.0, rr_vel = 0.0; + double linear_vel = 0.0; + double angular_vel = 0.0; +}; + +static WheelState controllerStep( + double linear_vel_cmd, double angular_z_cmd, + const AckermannParams & p) +{ + WheelState ws; + linear_vel_cmd = std::clamp(linear_vel_cmd, -p.max_speed, p.max_speed); + + double steer_cmd = 0.0; + if (std::abs(linear_vel_cmd) > 1e-4) { + steer_cmd = std::atan(angular_z_cmd * p.wheelbase / linear_vel_cmd); + } + steer_cmd = std::clamp(steer_cmd, -p.max_steer_angle, p.max_steer_angle); + + ws.fl_vel = ws.fr_vel = ws.rl_vel = ws.rr_vel = linear_vel_cmd; + + if (std::abs(steer_cmd) > 1e-4) { + const double turn_radius = p.wheelbase / std::tan(steer_cmd); + double angular_vel = std::abs(linear_vel_cmd) / std::abs(turn_radius); + if (linear_vel_cmd < 0.0) angular_vel = -angular_vel; + + const double inner_angle = std::atan(p.wheelbase / (std::abs(turn_radius) - p.track_width / 2.0)); + const double outer_angle = std::atan(p.wheelbase / (std::abs(turn_radius) + p.track_width / 2.0)); + + const double inner_rear = angular_vel * (std::abs(turn_radius) - p.track_width / 2.0); + const double outer_rear = angular_vel * (std::abs(turn_radius) + p.track_width / 2.0); + const double inner_front = angular_vel * std::sqrt(p.wheelbase * p.wheelbase + std::pow(std::abs(turn_radius) - p.track_width / 2.0, 2)); + const double outer_front = angular_vel * std::sqrt(p.wheelbase * p.wheelbase + std::pow(std::abs(turn_radius) + p.track_width / 2.0, 2)); + + if (steer_cmd > 0.0) { + ws.fl_steer = inner_angle; ws.fr_steer = outer_angle; + ws.fl_vel = inner_front; ws.fr_vel = outer_front; + ws.rl_vel = inner_rear; ws.rr_vel = outer_rear; + } else { + ws.fl_steer = -outer_angle; ws.fr_steer = -inner_angle; + ws.fl_vel = outer_front; ws.fr_vel = inner_front; + ws.rl_vel = outer_rear; ws.rr_vel = inner_rear; + } + } + + ws.linear_vel = (ws.rl_vel + ws.rr_vel) / 2.0; + const double avg_steer = (ws.fl_steer + ws.fr_steer) / 2.0; + if (std::abs(p.wheelbase) > 1e-6) { + ws.angular_vel = ws.linear_vel * std::tan(avg_steer) / p.wheelbase; + } + + return ws; +} + +// ============================================================================ +// Pipeline Mock +// ============================================================================ + +struct MockPipeline { + mission_executive::MissionExecutiveAlgo mission; + global_planner::GlobalPlannerAlgo global; + vector_field_planner::VectorFieldPlannerAlgo local; + AckermannParams ackermann; + + double x = 0.0, y = 0.0, yaw = 0.0; + double time_s = 0.0; + double dt = 0.05; // 20 Hz + + std::vector current_global_path; + + MockPipeline() { + // Setup parameters + mission_executive::MissionParams mp; + mp.stop_angular_vel_threshold = 0.05; + mp.arrival_hold_time = 1.0; + mp.replan_distance_m = 3.0; + mp.spiral_radius_m = 10.0; + mission.setParams(mp); + + global_planner::PlannerParams gp; + gp.path_resolution_m = 1.0; + gp.use_costmap = false; // Just use straight line for mock + global.setParams(gp); + + vector_field_planner::PlannerParams vp; + vp.max_speed_mps = 0.8; + vp.lookahead_dist_m = 3.0; + vp.goal_tolerance_m = 1.5; + vp.obstacle_avoidance_enabled = false; + local.setParams(vp); + } + + void setPose(double px, double py, double pyaw) { + x = px; y = py; yaw = pyaw; + } + + void planGlobal(double gx, double gy) { + current_global_path = global.planStraightLine(x, y, gx, gy); + + // Convert to mission and local types + std::vector mp; + std::vector lp; + for (auto& p : current_global_path) { + mp.push_back({p.x, p.y, p.yaw}); + lp.push_back({p.x, p.y}); + } + + mission.updateGlobalPath(mp); + local.setPath(lp); + } + + bool step(int max_steps = 10000) { + bool goal_reached = false; + + for (int i = 0; i < max_steps; ++i) { + mission.updateRobotPose({x, y, yaw}); + + auto tick_res = mission.tick(time_s); + + if (tick_res.publish_goal) { + planGlobal(tick_res.goal_to_publish.x, tick_res.goal_to_publish.y); + } + + if (tick_res.action_finished) { + goal_reached = tick_res.action_success; + if (!tick_res.start_queued_goal) { + break; // Action is complete + } + } + + if (mission.getState() == mission_executive::State::IDLE) { + break; + } + + // If Navigating, get command from Local Planner + double linear_cmd = 0.0; + double angular_cmd = 0.0; + + if (mission.isNavEnabled() && mission.getState() != mission_executive::State::ARRIVING) { + auto local_res = local.compute(x, y, yaw); + if (!local_res.goal_reached) { + linear_cmd = local_res.linear_vel; + angular_cmd = local_res.angular_vel; + } + } else if (mission.getState() == mission_executive::State::ARRIVING) { + // Slow down or stop if arriving + linear_cmd = 0.0; + angular_cmd = 0.0; + } + + // Step Ackermann + auto ws = controllerStep(linear_cmd, angular_cmd, ackermann); + + // Update Pose + const double heading_mid = yaw + ws.angular_vel * dt / 2.0; + x += ws.linear_vel * std::cos(heading_mid) * dt; + y += ws.linear_vel * std::sin(heading_mid) * dt; + yaw += ws.angular_vel * dt; + + while (yaw > M_PI) yaw -= 2.0 * M_PI; + while (yaw < -M_PI) yaw += 2.0 * M_PI; + + // Update Mission IMU + mission.updateImu(std::abs(ws.angular_vel), time_s); + + time_s += dt; + } + + return goal_reached; + } +}; + +TEST(MockPipelineTest, FullAutonomousGNSSNavigation) { + MockPipeline sim; + sim.setPose(0.0, 0.0, 0.0); + + // 1.f.iii: 2 GNSS-only locations. Stopping within 3m is considered successful. + // The local planner uses 1.5m goal tolerance by default, which is safely < 3m. + + mission_executive::TargetEntry gnss1{"gnss1", 20.0, 10.0, 0, 0, 2.5, false}; + sim.mission.setTarget(gnss1); + + auto res = sim.mission.startNav("gnss1", std::nullopt, false); + ASSERT_TRUE(res.accepted); + if (res.publish_goal) sim.planGlobal(res.goal_to_publish.x, res.goal_to_publish.y); + + bool success = sim.step(); + + EXPECT_TRUE(success) << "Failed to reach GNSS target 1"; + EXPECT_EQ(sim.mission.getState(), mission_executive::State::STOPPED_AT_TARGET); + + double dist = std::hypot(sim.x - 20.0, sim.y - 10.0); + EXPECT_LE(dist, 3.0) << "Final distance > 3m limit for GNSS point"; +} + +TEST(MockPipelineTest, ArucoPostWithSpiralSearch) { + MockPipeline sim; + sim.setPose(0.0, 0.0, 0.0); + + // 1.f.iv: Posts with AR markers. + // Target type 1 is ARUCO_POST + mission_executive::TargetEntry post1{"post1", 10.0, 0.0, 1, 0, 2.0, false}; + sim.mission.setTarget(post1); + + auto res = sim.mission.startNav("post1", std::nullopt, false); + ASSERT_TRUE(res.accepted); + if (res.publish_goal) sim.planGlobal(res.goal_to_publish.x, res.goal_to_publish.y); + + // We need to inject a detection while it's spiraling. + // Run until it enters spiral coverage + for (int i=0; i<5000; i++) { + sim.mission.updateRobotPose({sim.x, sim.y, sim.yaw}); + auto tick_res = sim.mission.tick(sim.time_s); + if (tick_res.publish_goal) sim.planGlobal(tick_res.goal_to_publish.x, tick_res.goal_to_publish.y); + + if (sim.mission.getState() == mission_executive::State::SPIRAL_COVERAGE) { + break; + } + + double linear_cmd = 0.0, angular_cmd = 0.0; + if (sim.mission.isNavEnabled() && sim.mission.getState() != mission_executive::State::ARRIVING) { + auto local_res = sim.local.compute(sim.x, sim.y, sim.yaw); + if (!local_res.goal_reached) { + linear_cmd = local_res.linear_vel; + angular_cmd = local_res.angular_vel; + } + } + + auto ws = controllerStep(linear_cmd, angular_cmd, sim.ackermann); + const double heading_mid = sim.yaw + ws.angular_vel * sim.dt / 2.0; + sim.x += ws.linear_vel * std::cos(heading_mid) * sim.dt; + sim.y += ws.linear_vel * std::sin(heading_mid) * sim.dt; + sim.yaw += ws.angular_vel * sim.dt; + sim.mission.updateImu(std::abs(ws.angular_vel), sim.time_s); + sim.time_s += sim.dt; + } + + EXPECT_EQ(sim.mission.getState(), mission_executive::State::SPIRAL_COVERAGE) << "Did not transition to spiral coverage"; + + // Simulate CV detection + sim.mission.onDetection(); + + // Finish step + bool success = sim.step(); + EXPECT_TRUE(success); + EXPECT_EQ(sim.mission.getState(), mission_executive::State::SPIRAL_DONE); +} + +TEST(MockPipelineTest, AbortAndReturnToPreviousGNSS) { + MockPipeline sim; + sim.setPose(0.0, 0.0, 0.0); + + // 1.f.viii: Operators may abort and return to any *previous* GNSS coordinate. + mission_executive::TargetEntry start_gate{"start", 0.0, 0.0, 0, 0, 2.0, true}; + sim.mission.setTarget(start_gate); + + mission_executive::TargetEntry gnss1{"gnss1", 30.0, 0.0, 0, 0, 2.0, false}; + sim.mission.setTarget(gnss1); + + // Start moving to gnss1 + auto res = sim.mission.startNav("gnss1", std::nullopt, false); + ASSERT_TRUE(res.accepted); + if (res.publish_goal) sim.planGlobal(res.goal_to_publish.x, res.goal_to_publish.y); + + // Step 200 times (10 seconds), rover is moving + for(int i=0; i<200; i++) { + sim.mission.updateRobotPose({sim.x, sim.y, sim.yaw}); + auto tick_res = sim.mission.tick(sim.time_s); + if (tick_res.publish_goal) sim.planGlobal(tick_res.goal_to_publish.x, tick_res.goal_to_publish.y); + + auto local_res = sim.local.compute(sim.x, sim.y, sim.yaw); + auto ws = controllerStep(local_res.linear_vel, local_res.angular_vel, sim.ackermann); + const double heading_mid = sim.yaw + ws.angular_vel * sim.dt / 2.0; + sim.x += ws.linear_vel * std::cos(heading_mid) * sim.dt; + sim.y += ws.linear_vel * std::sin(heading_mid) * sim.dt; + sim.yaw += ws.angular_vel * sim.dt; + sim.mission.updateImu(std::abs(ws.angular_vel), sim.time_s); + sim.time_s += sim.dt; + } + + // Rover is now somewhere around X=8.0 + EXPECT_GT(sim.x, 2.0); + EXPECT_LT(sim.x, 15.0); + EXPECT_EQ(sim.mission.getState(), mission_executive::State::NAVIGATING); + + // Trigger ABORT + sim.mission.abort(); + EXPECT_EQ(sim.mission.getState(), mission_executive::State::ABORTING); + + // While aborting, queue a RETURN to start_gate + auto ret_res = sim.mission.startNav("start", std::nullopt, true); + EXPECT_TRUE(ret_res.accepted); + + // Step to completion + bool success = sim.step(); + + EXPECT_TRUE(success) << "Failed to return to start gate"; + EXPECT_EQ(sim.mission.getState(), mission_executive::State::STOPPED_AT_RETURN); + + double dist = std::hypot(sim.x - 0.0, sim.y - 0.0); + EXPECT_LE(dist, 3.0) << "Did not successfully return within tolerance"; +} diff --git a/src/subsystems/navigation/nav_bringup/CMakeLists.txt b/src/subsystems/navigation/nav_bringup/CMakeLists.txt index 9648c1b0..14c7f50f 100644 --- a/src/subsystems/navigation/nav_bringup/CMakeLists.txt +++ b/src/subsystems/navigation/nav_bringup/CMakeLists.txt @@ -3,7 +3,7 @@ project(nav_bringup) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml new file mode 100644 index 00000000..5c3b5be0 --- /dev/null +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml @@ -0,0 +1,92 @@ +# nav_params_real.yaml + +gps_pose_publisher: + ros__parameters: + use_sim_time: false + heading_topic: "/gps/heading" + heading_offset_deg: 0.0 # set to 180 if sensor is mounted backwards + use_start_gate_ref: false # if true, waits for /start_gate_ref NavSatFix to set origin + origin_lat: .nan # set to a fixed lat/lon to skip first-fix origin logic + origin_lon: .nan + origin_alt: 0.0 + +dem_costmap_converter: + ros__parameters: + use_sim_time: false + dem_file_path: "" # set by launch file at runtime + map_resolution: 1.0 # meters per pixel + max_passable_slope_degrees: 15.0 + output_frame: map + origin_x: -400.0 + origin_y: -400.0 + +global_planner: + ros__parameters: + use_sim_time: false + path_resolution_m: 1.0 # spacing between path poses + use_costmap: false # set to true to enable A* on dem_costmap_converter output + slope_weight: 10.0 # Phase 2: alpha in g = dist + alpha * (cell_cost / 254) + +mission_executive: + ros__parameters: + use_sim_time: false + stop_angular_vel_threshold: 0.05 # rad/s — IMU gyro magnitude below which robot is "stopped" + arrival_hold_time: 1.5 # slightly longer in real life to filter IMU noise + replan_distance_m: 5.0 # cross-track error (m) that triggers a replan + latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" + # Input topics + imu_topic: "/imu" + planner_event_topic: "/planner_event" + global_path_topic: "/global_path" + # Output topics + goal_pose_topic: "/goal_pose" + nav_enabled_topic: "/nav_enabled" + nav_mode_topic: "/nav_mode" + active_target_topic: "/active_target" + nav_status_topic: "/nav_status" + +reframe_pointcloud: + ros__parameters: + use_sim_time: false + input_topic: "/zed/zed_node/point_cloud/cloud_registered" + output_topic: "/zed/cloud_base_link" + target_frame: "base_link" + +pointcloud_to_laserscan: + ros__parameters: + use_sim_time: false + target_frame: base_link # cloud arrives already reframed to base_link by reframe_pointcloud + transform_tolerance: 0.05 # slightly relaxed for real TF trees + min_height: 0.1 # ignore ground returns (m above sensor origin) + max_height: 1.5 # ignore returns above rover body + angle_min: -1.5708 # -90 deg (left side of forward arc) + angle_max: 1.5708 # 90 deg (right side of forward arc) + angle_increment: 0.00873 # 0.5 deg resolution + scan_time: 0.05 # 20 Hz to match planner + range_min: 0.3 # ignore very close returns (ZED blind zone) + range_max: 15.0 # maximum useful range for obstacle avoidance + use_inf: true # emit inf for beams with no return (not 0) + inf_epsilon: 1e-9 + +vector_field_planner: + ros__parameters: + use_sim_time: false + map_frame: map + base_frame: base_link + cmd_vel_topic: "/front_ackermann_controller/reference" + tf_timeout_s: 0.2 # slightly longer to handle wireless / CPU spikes + max_speed_mps: 0.5 # LOWERED FOR REAL LIFE SAFETY - adjust up to 1.0 once tested + max_steering_angle_rad: 0.5 # radians — clamp on angular.z output + lookahead_dist_m: 3.0 # pure-pursuit lookahead distance + k_p_steering: 1.5 # proportional gain: angular.z = k_p * heading_error + # ── Obstacle avoidance ────────────────────────────────────────────────── + obstacle_avoidance_enabled: true # set true or publish to /obstacle_avoidance_enabled + scan_topic: "/scan" # LaserScan source (from pointcloud_to_laserscan) + scan_max_age_s: 1.0 # slightly older accepted to handle real sensor jitter + repulsion_gain: 2.5 # lateral force scale; tune up to ~1.5 for strong avoidance + repulsion_cutoff_m: 5.0 # obstacles beyond this range are ignored (meters) + obstacle_memory_time_s: 3.0 # how long (s) to remember obstacle hits after they leave the scan FoV + obstacle_max_points: 2000 # cap on buffered map-frame obstacle points + # ──────────────────────────────────────────────────────────────────────── + goal_tolerance_m: 1.5 # distance to path end that counts as "arrived" (compliant with <3m rules) + publish_debug_markers: true diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml b/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml similarity index 74% rename from src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml rename to src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml index a170f50c..64ec74c2 100644 --- a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/config/nav_params.yaml +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml @@ -1,6 +1,5 @@ -# nav_params.yaml — single parameter file passed to all navigation nodes. +# nav_params_sim.yaml -# ── gps_pose_publisher ─────────────────────────────────────────────────────── gps_pose_publisher: ros__parameters: use_sim_time: true @@ -11,7 +10,6 @@ gps_pose_publisher: origin_lon: .nan origin_alt: 0.0 -# ── dem_costmap_converter ──────────────────────────────────────────────────── dem_costmap_converter: ros__parameters: use_sim_time: true @@ -22,7 +20,6 @@ dem_costmap_converter: origin_x: -400.0 origin_y: -400.0 -# ── global_planner ─────────────────────────────────────────────────────────── global_planner: ros__parameters: use_sim_time: true @@ -30,7 +27,6 @@ global_planner: use_costmap: false # set to true to enable A* on dem_costmap_converter output slope_weight: 10.0 # Phase 2: alpha in g = dist + alpha * (cell_cost / 254) -# ── mission_executive ──────────────────────────────────────────────────────── mission_executive: ros__parameters: use_sim_time: true @@ -49,10 +45,13 @@ mission_executive: active_target_topic: "/active_target" nav_status_topic: "/nav_status" -# ── pointcloud_to_laserscan ─────────────────────────────────────────────────── -# Converts the ZED stereo camera's registered point cloud into a 2D laser scan -# used by the vector_field_planner for obstacle avoidance. -# Input topic remapped to /zed/zed_node/point_cloud/cloud_registered in nav.launch.py. +reframe_pointcloud: + ros__parameters: + use_sim_time: true + input_topic: "/zed/zed_node/point_cloud/cloud_registered" + output_topic: "/zed/cloud_base_link" + target_frame: "base_link" + pointcloud_to_laserscan: ros__parameters: use_sim_time: true @@ -69,7 +68,6 @@ pointcloud_to_laserscan: use_inf: true # emit inf for beams with no return (not 0) inf_epsilon: 1e-9 -# ── vector_field_planner ────────────────────────────────────────────────────── vector_field_planner: ros__parameters: use_sim_time: true diff --git a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py similarity index 65% rename from src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py rename to src/subsystems/navigation/nav_bringup/launch/emmn.launch.py index c4dcf531..989ad533 100644 --- a/src/subsystems/even_more_minimal_nav/minimal_nav_bringup/launch/nav.launch.py +++ b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py @@ -41,9 +41,10 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, IfElseSubstitution, Command, FindExecutable, PathJoinSubstitution +from launch.conditions import IfCondition from launch_ros.actions import Node - +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): # ── Arguments ──────────────────────────────────────────────────────────── @@ -57,12 +58,70 @@ def generate_launch_description(): sim = LaunchConfiguration('sim') # ── Config ─────────────────────────────────────────────────────────────── - nav_bringup_dir = get_package_share_directory('minimal_nav_bringup') - nav_params_file = os.path.join(nav_bringup_dir, 'config', 'nav_params.yaml') + nav_bringup_dir = get_package_share_directory('nav_bringup') + + from launch.substitutions import PythonExpression + nav_params_file = PythonExpression([ + "'", os.path.join(nav_bringup_dir, 'config', 'nav_params_sim.yaml'), "' if '", sim, "' == 'true' else '", + os.path.join(nav_bringup_dir, 'config', 'nav_params_real.yaml'), "'" + ]) dem_costmap_dir = get_package_share_directory('dem_costmap') dem_file = os.path.join(dem_costmap_dir, 'maps', 'north_site800m.tif') + # ── Robot State Publisher ──────────────────────────────────────────────── + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([ + FindPackageShare('description'), 'urdf', 'athena_drive.urdf.xacro' + ]), + ]) + + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[{ + 'robot_description': robot_description_content, + 'use_sim_time': sim, + }], + ) + + # ── Sensors Bringup ────────────────────────────────────────────────────── + sensors_share = get_package_share_directory('athena_sensors') + sensors_launch_file = os.path.join(sensors_share, 'launch', 'sensors.launch.py') + + sensors_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(sensors_launch_file), + launch_arguments={ + 'sim': sim, + 'enable_gnss': 'false', + }.items(), + ) + + # ── Aruco Detection ────────────────────────────────────────────────────── + aruco_bt_share = get_package_share_directory('aruco_bt') + aruco_launch_file = os.path.join(aruco_bt_share, 'launch', 'aruco.launch.py') + + aruco_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(aruco_launch_file), + launch_arguments={'use_sim_time': sim, 'marker_size': '0.20'}.items() + ) + + # ── YOLO Detection ─────────────────────────────────────────────────────── + # yolo_ros expects /zed/zed_node/left_gray/image_rect_gray and publishes to /yolo_detection + yolo_inference_node = Node( + package='yolo_ros', + executable='inference', + name='yolo_inference', + output='screen', + parameters=[{'use_sim_time': sim}], + # Provide compatible remapping if ZED color is needed instead: + # remappings=[('/zed/zed_node/left_gray/image_rect_gray', '/zed/zed_node/left/image_rect_color')], + ) + # ── GPS hardware / sim bridge ───────────────────────────────────────────── gps_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -134,15 +193,12 @@ def generate_launch_description(): # consistent, simple frame name so it can produce a valid scan without any # TF lookup. This relay just rewrites header.frame_id in-place. reframe_pointcloud_node = Node( - package='minimal_nav_bringup', + package='nav_bringup', executable='reframe_pointcloud.py', name='reframe_pointcloud', output='screen', - parameters=[{ - 'input_topic': '/zed/zed_node/point_cloud/cloud_registered', - 'output_topic': '/zed/cloud_base_link', - 'target_frame': 'base_link', - }], + parameters=[nav_params_file], + condition=IfCondition(sim) ) # ── pointcloud_to_laserscan (ZED PointCloud2 → LaserScan for obstacle avoidance) ── @@ -153,13 +209,17 @@ def generate_launch_description(): output='screen', parameters=[nav_params_file], remappings=[ - ('cloud_in', '/zed/cloud_base_link'), # reframed cloud, no TF needed + ('cloud_in', IfElseSubstitution(sim, if_value='/zed/cloud_base_link', else_value='/zed/zed_node/point_cloud/cloud_registered')), ('scan', '/scan'), ], ) return LaunchDescription([ sim_arg, + robot_state_publisher_node, + sensors_launch, + aruco_launch, + yolo_inference_node, gps_launch, gps_pose_publisher_node, dem_costmap_converter_node, diff --git a/src/subsystems/navigation/nav_bringup/package.xml b/src/subsystems/navigation/nav_bringup/package.xml index 9a8331ae..bf012da3 100644 --- a/src/subsystems/navigation/nav_bringup/package.xml +++ b/src/subsystems/navigation/nav_bringup/package.xml @@ -1,7 +1,7 @@ - nav_bringup + minimal_nav_bringup 0.1.0 Top-level bringup package for the navigation stack Maintainer diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt b/src/subsystems/navigation/vector_field_planner/CMakeLists.txt similarity index 60% rename from src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt rename to src/subsystems/navigation/vector_field_planner/CMakeLists.txt index ea2b8585..ffdecc1a 100644 --- a/src/subsystems/even_more_minimal_nav/vector_field_planner/CMakeLists.txt +++ b/src/subsystems/navigation/vector_field_planner/CMakeLists.txt @@ -15,9 +15,20 @@ find_package(visualization_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) +add_library(vector_field_planner_algo src/vector_field_planner_algo.cpp) +target_include_directories(vector_field_planner_algo PUBLIC + $ + $ +) +target_compile_features(vector_field_planner_algo PUBLIC cxx_std_17) + add_executable(vector_field_planner_node src/vector_field_planner_node.cpp) +target_include_directories(vector_field_planner_node PUBLIC + $ +) target_compile_features(vector_field_planner_node PUBLIC cxx_std_17) target_link_libraries(vector_field_planner_node + vector_field_planner_algo rclcpp::rclcpp ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} @@ -28,17 +39,25 @@ target_link_libraries(vector_field_planner_node tf2::tf2 ) -install(TARGETS vector_field_planner_node +install(TARGETS vector_field_planner_algo vector_field_planner_node DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY include/ + DESTINATION include +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_pure_pursuit_sim test/test_pure_pursuit_sim.cpp ) + target_include_directories(test_pure_pursuit_sim PUBLIC + $ + ) target_compile_features(test_pure_pursuit_sim PUBLIC cxx_std_17) + target_link_libraries(test_pure_pursuit_sim vector_field_planner_algo) endif() ament_package() diff --git a/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp b/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp new file mode 100644 index 00000000..039a1456 --- /dev/null +++ b/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2025 UMD Loop +// +// 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. + +#pragma once + +#include +#include +#include + +namespace vector_field_planner { + +struct Pose2D { + double x; + double y; +}; + +struct ObstaclePoint { + double x; + double y; + // We can add a timestamp if needed, but eviction is currently handled + // by the ROS node before passing to the algorithm. +}; + +struct PlannerParams { + double lookahead_dist_m = 3.0; + double k_p_steering = 1.5; + double max_steering_angle_rad = 0.5; + double max_speed_mps = 1.5; + double goal_tolerance_m = 1.5; + + bool obstacle_avoidance_enabled = false; + double repulsion_gain = 0.5; + double repulsion_cutoff_m = 3.0; +}; + +struct PlannerResult { + double linear_vel = 0.0; + double angular_vel = 0.0; + bool goal_reached = false; + double lookahead_x = 0.0; + double lookahead_y = 0.0; + size_t closest_idx = 0; + double heading_err = 0.0; + double repulsion_steering = 0.0; + double steering_unclamped = 0.0; + bool clamped = false; + bool lookahead_behind = false; + + // Debug info for obstacle avoidance + double lateral_sum = 0.0; + double lateral_left = 0.0; + double lateral_right = 0.0; + int active_points = 0; + double closest_r = -1.0; +}; + +class VectorFieldPlannerAlgo { +public: + VectorFieldPlannerAlgo() = default; + + void setParams(const PlannerParams& params) { + params_ = params; + } + + const PlannerParams& getParams() const { + return params_; + } + + void setPath(const std::vector& path) { + path_ = path; + } + + // obstacle_map should only contain valid (non-stale) points in map frame + void updateObstacles(const std::vector& obstacles) { + obstacles_ = obstacles; + } + + PlannerResult compute(double rx, double ry, double yaw); + + // Expose for testing and debugging + size_t findClosestIndex(double rx, double ry) const; + std::pair findLookahead(double rx, double ry, size_t closest_idx) const; + +private: + PlannerParams params_; + std::vector path_; + std::vector obstacles_; +}; + +} // namespace vector_field_planner diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml b/src/subsystems/navigation/vector_field_planner/package.xml similarity index 100% rename from src/subsystems/even_more_minimal_nav/vector_field_planner/package.xml rename to src/subsystems/navigation/vector_field_planner/package.xml diff --git a/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp new file mode 100644 index 00000000..83825f92 --- /dev/null +++ b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp @@ -0,0 +1,153 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 "vector_field_planner/vector_field_planner_algo.hpp" + +#include +#include +#include + +namespace vector_field_planner { + +PlannerResult VectorFieldPlannerAlgo::compute(double rx, double ry, double yaw) { + PlannerResult res; + res.closest_r = std::numeric_limits::infinity(); + + if (path_.empty()) { + return res; + } + + const auto& goal_pos = path_.back(); + const double dist_to_goal = std::hypot(goal_pos.x - rx, goal_pos.y - ry); + if (dist_to_goal < params_.goal_tolerance_m) { + res.goal_reached = true; + res.linear_vel = 0.0; + res.angular_vel = 0.0; + return res; + } + + res.closest_idx = findClosestIndex(rx, ry); + const auto [lx, ly] = findLookahead(rx, ry, res.closest_idx); + res.lookahead_x = lx; + res.lookahead_y = ly; + + const double fwd_dot = (lx - rx) * std::cos(yaw) + (ly - ry) * std::sin(yaw); + res.lookahead_behind = (fwd_dot < 0.0); + + res.heading_err = std::atan2(ly - ry, lx - rx) - yaw; + while (res.heading_err > M_PI) res.heading_err -= 2.0 * M_PI; + while (res.heading_err < -M_PI) res.heading_err += 2.0 * M_PI; + + if (params_.obstacle_avoidance_enabled && params_.repulsion_gain > 0.0) { + for (const auto& p : obstacles_) { + const double dx = rx - p.x; + const double dy = ry - p.y; + const double dist = std::hypot(dx, dy); + + if (dist >= params_.repulsion_cutoff_m || dist < 1e-6) continue; + + res.closest_r = std::min(res.closest_r, dist); + + const double weight = (params_.repulsion_cutoff_m - dist) / params_.repulsion_cutoff_m; + // Project unit repulsion vector onto robot's left lateral axis + const double lateral = (dx / dist) * (-std::sin(yaw)) + (dy / dist) * std::cos(yaw); + res.lateral_sum += lateral * weight; + + ++res.active_points; + + if (lateral > 0.0) { + res.lateral_left += lateral * weight; + } else { + res.lateral_right += lateral * weight; + } + } + res.repulsion_steering = params_.repulsion_gain * res.lateral_sum; + } + + res.steering_unclamped = params_.k_p_steering * res.heading_err + res.repulsion_steering; + res.angular_vel = std::clamp(res.steering_unclamped, + -params_.max_steering_angle_rad, + params_.max_steering_angle_rad); + res.clamped = std::abs(res.steering_unclamped) > params_.max_steering_angle_rad; + res.linear_vel = params_.max_speed_mps; + + if (std::isinf(res.closest_r)) { + res.closest_r = -1.0; + } + + return res; +} + +size_t VectorFieldPlannerAlgo::findClosestIndex(double rx, double ry) const { + size_t best = 0; + double best_dist2 = std::numeric_limits::infinity(); + + for (size_t i = 0; i < path_.size(); ++i) { + const double dx = path_[i].x - rx; + const double dy = path_[i].y - ry; + const double d2 = dx * dx + dy * dy; + if (d2 < best_dist2) { + best_dist2 = d2; + best = i; + } + } + + return best; +} + +std::pair VectorFieldPlannerAlgo::findLookahead( + double rx, double ry, size_t closest_idx) const { + if (path_.empty()) { + return {rx, ry}; + } + + for (size_t i = closest_idx; i < path_.size(); ++i) { + const double dx = path_[i].x - rx; + const double dy = path_[i].y - ry; + if (std::hypot(dx, dy) >= params_.lookahead_dist_m) { + // Fix for "lookahead behind robot" bug: + // When the path spacing is larger than lookahead_dist_m, the closest point + // may be behind the robot (e.g. pose 0 that it already passed). + // We check if this point `i` is behind the robot relative to the path segment `[i, i+1]`. + // If it is, and we have a next point, we skip it and look ahead. + if (i + 1 < path_.size()) { + const double path_dx = path_[i+1].x - path_[i].x; + const double path_dy = path_[i+1].y - path_[i].y; + + // Vector from robot to point `i` + const double to_pt_dx = path_[i].x - rx; + const double to_pt_dy = path_[i].y - ry; + + // Dot product of (robot -> point i) and (path direction at point i) + const double dot = to_pt_dx * path_dx + to_pt_dy * path_dy; + + // If dot < 0, the point is behind the robot along the path direction. + // We only do this check for `i == closest_idx` to prevent skipping points + // that naturally turn backwards, but we definitely want to skip the closest point + // if we just passed it. + if (dot < 0.0 && i == closest_idx) { + continue; // Skip this point, check the next one + } + } + + return {path_[i].x, path_[i].y}; + } + } + + // If no point is far enough ahead (e.g. nearing goal), return the goal. + const auto& last = path_.back(); + return {last.x, last.y}; +} + +} // namespace vector_field_planner \ No newline at end of file diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp similarity index 50% rename from src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp rename to src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp index d456b03e..f2c2e675 100644 --- a/src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp +++ b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp @@ -15,44 +15,6 @@ // vector_field_planner_node.cpp // // Pure-pursuit path follower for an Ackermann rover with optional obstacle avoidance. -// -// Inputs: -// /global_path (nav_msgs/Path, transient_local) — from global_planner -// /nav_enabled (std_msgs/Bool, reliable) — from mission_executive -// /obstacle_avoidance_enabled (std_msgs/Bool, reliable) — runtime avoidance toggle -// /scan (sensor_msgs/LaserScan) — from pointcloud_to_laserscan -// TF map→base_link — current robot pose -// -// Outputs: -// /cmd_vel (geometry_msgs/Twist) — linear.x + angular.z -// ~/debug_markers (visualization_msgs/MarkerArray) — RViz visualization -// -// Algorithm: -// On each 20 Hz tick, if nav_enabled and a path is loaded: -// 1. Look up current pose via TF. -// 2. Stop if within goal_tolerance_m of the last path pose. -// 3. Find the lookahead point: walk forward from the closest path pose -// until a point is >= lookahead_dist_m from the robot. -// 4. Compute heading error from current yaw to the lookahead direction. -// 5. If obstacle_avoidance_enabled and repulsion_gain > 0: -// - On each scan callback, transform every hit within repulsion_cutoff_m -// into map frame and store it in obstacle_map_ with a timestamp. -// - Each tick, evict points older than obstacle_memory_time_s. -// - For each remaining point within repulsion_cutoff_m: -// repulsion direction = unit vector from obstacle to robot -// lateral component = project onto robot's left axis (-sin(yaw), cos(yaw)) -// weight = (cutoff - dist) / cutoff -// lateral_sum += lateral * weight -// - repulsion_steering = repulsion_gain * lateral_sum -// - Total steering = k_p_steering * heading_err + repulsion_steering -// 6. angular.z = clamped total steering, linear.x = max_speed_mps. -// -// Obstacle memory design: -// Hits are stored in map frame, not sensor frame. This means the repulsion -// direction is recomputed from correct world-frame geometry every tick, -// regardless of how the robot has turned since the hit was recorded. -// The robot will not "forget" an obstacle just because it rotated away from it, -// and the steering magnitude correctly reflects the actual geometric clearance. #include #include @@ -73,7 +35,9 @@ #include "tf2_ros/transform_listener.h" #include "tf2/exceptions.h" -struct ObstaclePoint +#include "vector_field_planner/vector_field_planner_algo.hpp" + +struct StampedObstaclePoint { double x, y; // map frame rclcpp::Time stamp; @@ -106,18 +70,23 @@ class VectorFieldPlanner : public rclcpp::Node base_frame_ = get_parameter("base_frame").as_string(); cmd_vel_topic_ = get_parameter("cmd_vel_topic").as_string(); tf_timeout_s_ = get_parameter("tf_timeout_s").as_double(); - max_speed_mps_ = get_parameter("max_speed_mps").as_double(); - max_steering_angle_rad_ = get_parameter("max_steering_angle_rad").as_double(); - lookahead_dist_m_ = get_parameter("lookahead_dist_m").as_double(); - k_p_steering_ = get_parameter("k_p_steering").as_double(); - repulsion_gain_ = get_parameter("repulsion_gain").as_double(); - repulsion_cutoff_m_ = get_parameter("repulsion_cutoff_m").as_double(); + + vector_field_planner::PlannerParams p; + p.max_speed_mps = get_parameter("max_speed_mps").as_double(); + p.max_steering_angle_rad = get_parameter("max_steering_angle_rad").as_double(); + p.lookahead_dist_m = get_parameter("lookahead_dist_m").as_double(); + p.k_p_steering = get_parameter("k_p_steering").as_double(); + p.repulsion_gain = get_parameter("repulsion_gain").as_double(); + p.repulsion_cutoff_m = get_parameter("repulsion_cutoff_m").as_double(); + p.obstacle_avoidance_enabled = get_parameter("obstacle_avoidance_enabled").as_bool(); + p.goal_tolerance_m = get_parameter("goal_tolerance_m").as_double(); + + algo_.setParams(p); + obstacle_memory_time_s_ = get_parameter("obstacle_memory_time_s").as_double(); obstacle_max_points_ = get_parameter("obstacle_max_points").as_int(); - obstacle_avoidance_enabled_ = get_parameter("obstacle_avoidance_enabled").as_bool(); scan_topic_ = get_parameter("scan_topic").as_string(); scan_max_age_s_ = get_parameter("scan_max_age_s").as_double(); - goal_tolerance_m_ = get_parameter("goal_tolerance_m").as_double(); publish_debug_markers_ = get_parameter("publish_debug_markers").as_bool(); tf_buffer_ = std::make_shared(get_clock()); @@ -139,27 +108,34 @@ class VectorFieldPlanner : public rclcpp::Node consecutive_clamped_ = 0; obstacle_map_.clear(); - // Log path quality — sparse paths (spacing > lookahead_dist_m) trigger the - // findLookahead-behind-robot bug documented in the unit tests. + std::vector algo_path; + algo_path.reserve(msg->poses.size()); + double total_len = 0.0; double max_gap = 0.0; - for (size_t i = 1; i < msg->poses.size(); ++i) { - const double gap = std::hypot( - msg->poses[i].pose.position.x - msg->poses[i-1].pose.position.x, - msg->poses[i].pose.position.y - msg->poses[i-1].pose.position.y); - total_len += gap; - max_gap = std::max(max_gap, gap); + for (size_t i = 0; i < msg->poses.size(); ++i) { + algo_path.push_back({msg->poses[i].pose.position.x, msg->poses[i].pose.position.y}); + if (i > 0) { + const double gap = std::hypot( + msg->poses[i].pose.position.x - msg->poses[i-1].pose.position.x, + msg->poses[i].pose.position.y - msg->poses[i-1].pose.position.y); + total_len += gap; + max_gap = std::max(max_gap, gap); + } } + + algo_.setPath(algo_path); + + const double lookahead_dist_m = algo_.getParams().lookahead_dist_m; RCLCPP_INFO(get_logger(), "[PATH_RECV] poses=%zu total_len=%.1fm max_gap=%.2fm lookahead=%.1fm goal=(%.2f,%.2f)", - msg->poses.size(), total_len, max_gap, lookahead_dist_m_, + msg->poses.size(), total_len, max_gap, lookahead_dist_m, msg->poses.back().pose.position.x, msg->poses.back().pose.position.y); - if (max_gap > lookahead_dist_m_) { + if (max_gap > lookahead_dist_m) { RCLCPP_WARN(get_logger(), - "[PATH_RECV] max_gap %.2fm > lookahead %.1fm — findLookahead will return " - "behind-robot points once robot passes a sparse waypoint. " - "Reduce path_resolution_m or increase lookahead_dist_m.", - max_gap, lookahead_dist_m_); + "[PATH_RECV] max_gap %.2fm > lookahead %.1fm — consider reducing path_resolution_m " + "or increasing lookahead_dist_m.", + max_gap, lookahead_dist_m); } }); @@ -183,11 +159,14 @@ class VectorFieldPlanner : public rclcpp::Node obstacle_avoidance_sub_ = create_subscription( "/obstacle_avoidance_enabled", rclcpp::QoS(1).reliable(), [this](const std_msgs::msg::Bool::SharedPtr msg) { - const bool was_enabled = obstacle_avoidance_enabled_; - obstacle_avoidance_enabled_ = msg->data; - if (was_enabled != obstacle_avoidance_enabled_) { + vector_field_planner::PlannerParams p = algo_.getParams(); + const bool was_enabled = p.obstacle_avoidance_enabled; + p.obstacle_avoidance_enabled = msg->data; + algo_.setParams(p); + + if (was_enabled != p.obstacle_avoidance_enabled) { RCLCPP_INFO(get_logger(), "[AVOIDANCE_%s]", - obstacle_avoidance_enabled_ ? "ENABLED" : "DISABLED"); + p.obstacle_avoidance_enabled ? "ENABLED" : "DISABLED"); } }); @@ -195,7 +174,7 @@ class VectorFieldPlanner : public rclcpp::Node scan_topic_, rclcpp::SensorDataQoS(), [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) { ++scan_msg_count_; - // Log details on the first scan and every 100th thereafter + const auto p = algo_.getParams(); if (scan_msg_count_ == 1 || scan_msg_count_ % 100 == 0) { float min_r = std::numeric_limits::infinity(); int valid = 0; @@ -215,14 +194,11 @@ class VectorFieldPlanner : public rclcpp::Node msg->range_min, msg->range_max, valid, std::isinf(min_r) ? -1.0f : min_r, - static_cast(repulsion_cutoff_m_)); + static_cast(p.repulsion_cutoff_m)); } latest_scan_ = msg; - // Transform hits into map frame and add to the obstacle buffer. - // Storing in map frame means repulsion geometry stays correct after - // the robot turns — we don't lose obstacles that have left the FoV. - if (!obstacle_avoidance_enabled_) return; + if (!p.obstacle_avoidance_enabled) return; geometry_msgs::msg::TransformStamped tf_to_map; try { @@ -250,7 +226,7 @@ class VectorFieldPlanner : public rclcpp::Node for (size_t i = 0; i < msg->ranges.size(); ++i) { const float r = msg->ranges[i]; if (r < msg->range_min || r > msg->range_max) continue; - if (static_cast(r) >= repulsion_cutoff_m_) continue; + if (static_cast(r) >= p.repulsion_cutoff_m) continue; const double alpha = msg->angle_min + static_cast(i) * msg->angle_increment; const double bx = r * std::cos(alpha); @@ -261,7 +237,6 @@ class VectorFieldPlanner : public rclcpp::Node stamp}); } - // Cap buffer size — evict oldest entries if over limit if (static_cast(obstacle_map_.size()) > obstacle_max_points_) { const size_t excess = obstacle_map_.size() - static_cast(obstacle_max_points_); @@ -269,29 +244,26 @@ class VectorFieldPlanner : public rclcpp::Node } }); - // 20 Hz control loop timer_ = create_wall_timer( std::chrono::milliseconds(50), [this]() { controlLoop(); }); - // ── Startup parameter summary ──────────────────────────────────────────── + const auto current_params = algo_.getParams(); RCLCPP_INFO(get_logger(), "VectorFieldPlanner ready " "speed=%.2f steer_max=%.3f lookahead=%.1fm kp=%.2f goal_tol=%.1fm", - max_speed_mps_, max_steering_angle_rad_, lookahead_dist_m_, - k_p_steering_, goal_tolerance_m_); + current_params.max_speed_mps, current_params.max_steering_angle_rad, current_params.lookahead_dist_m, + current_params.k_p_steering, current_params.goal_tolerance_m); RCLCPP_INFO(get_logger(), "[AVOIDANCE CONFIG] enabled=%d scan_topic='%s' cutoff=%.2fm gain=%.3f" " memory_time=%.1fs max_points=%d — publish true/false to /obstacle_avoidance_enabled to toggle", - static_cast(obstacle_avoidance_enabled_), + static_cast(current_params.obstacle_avoidance_enabled), scan_topic_.c_str(), - repulsion_cutoff_m_, repulsion_gain_, + current_params.repulsion_cutoff_m, current_params.repulsion_gain, obstacle_memory_time_s_, obstacle_max_points_); } private: - // ── Control loop (20 Hz) ─────────────────────────────────────────────────── - void controlLoop() { if (!nav_enabled_ || !path_ || path_->poses.empty()) { @@ -303,7 +275,6 @@ class VectorFieldPlanner : public rclcpp::Node ++tick_count_; - // Look up current robot pose geometry_msgs::msg::TransformStamped tf; try { tf = tf_buffer_->lookupTransform( @@ -320,147 +291,33 @@ class VectorFieldPlanner : public rclcpp::Node const double rx = tf.transform.translation.x; const double ry = tf.transform.translation.y; - // Yaw from quaternion (z-rotation only — planar robot) const auto & q = tf.transform.rotation; const double yaw = std::atan2( 2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); - // Check arrival at the last pose in the path - const auto & goal_pos = path_->poses.back().pose.position; - const double dist_to_goal = std::hypot(goal_pos.x - rx, goal_pos.y - ry); - if (dist_to_goal < goal_tolerance_m_) { - RCLCPP_INFO(get_logger(), - "[GOAL_REACHED] dist=%.3fm tol=%.3fm ticks=%u", - dist_to_goal, goal_tolerance_m_, tick_count_); - publishStop(); - return; - } - - // Find lookahead point - const size_t closest_idx = findClosestIndex(rx, ry); - const auto [lx, ly] = findLookahead(rx, ry, closest_idx); - - const double lookahead_dist = std::hypot(lx - rx, ly - ry); - - // Detect lookahead-behind-robot: dot(robot_forward, robot→lookahead) < 0 - const double fwd_dot = (lx - rx) * std::cos(yaw) + (ly - ry) * std::sin(yaw); - const bool lk_behind = fwd_dot < 0.0; - - if (lk_behind) { - RCLCPP_WARN(get_logger(), - "[LK_BEHIND] lookahead=(%.2f,%.2f) is BEHIND robot at (%.2f,%.2f) yaw=%.1fd " - "fwd_dot=%.3f closest_idx=%zu — robot will steer backward. " - "Likely cause: path spacing (%.2fm) > lookahead_dist (%.2fm).", - lx, ly, rx, ry, yaw * 180.0 / M_PI, fwd_dot, closest_idx, - [&]() -> double { - if (closest_idx + 1 < path_->poses.size()) { - return std::hypot( - path_->poses[closest_idx+1].pose.position.x - path_->poses[closest_idx].pose.position.x, - path_->poses[closest_idx+1].pose.position.y - path_->poses[closest_idx].pose.position.y); - } - return 0.0; - }(), - lookahead_dist_m_); - } - - // Warn if closest_idx has not advanced for many ticks (robot stuck or looping) - if (closest_idx == last_closest_idx_ && tick_count_ > 1) { - ++stuck_ticks_; - if (stuck_ticks_ == 20) { // 1 second at 20 Hz - RCLCPP_WARN(get_logger(), - "[STUCK] closest_idx=%zu has not advanced for %u ticks. " - "pos=(%.2f,%.2f) goal_dist=%.2fm. Robot may be off-path or looping.", - closest_idx, stuck_ticks_, rx, ry, dist_to_goal); - } - } else { - stuck_ticks_ = 0; - } - - // Heading error to the lookahead - double heading_err = std::atan2(ly - ry, lx - rx) - yaw; - // Normalise to [-pi, pi] - while (heading_err > M_PI) heading_err -= 2.0 * M_PI; - while (heading_err < -M_PI) heading_err += 2.0 * M_PI; - - // ── Obstacle avoidance (map-frame buffer) ───────────────────────────────── - // Evict obstacle points older than obstacle_memory_time_s_. - // This runs every tick regardless of avoidance toggle so the buffer - // doesn't grow unboundedly when avoidance is toggled off. const rclcpp::Time now_time = now(); obstacle_map_.erase( std::remove_if(obstacle_map_.begin(), obstacle_map_.end(), - [&](const ObstaclePoint & p) { + [&](const StampedObstaclePoint & p) { return (now_time - p.stamp).seconds() > obstacle_memory_time_s_; }), obstacle_map_.end()); - double repulsion_steering = 0.0; - - if (obstacle_avoidance_enabled_) { - if (repulsion_gain_ <= 0.0) { + const auto p = algo_.getParams(); + + if (p.obstacle_avoidance_enabled) { + if (p.repulsion_gain <= 0.0) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, - "[AVOIDANCE] avoidance enabled but repulsion_gain=%.3f — no effect. " - "Set repulsion_gain > 0 in nav_params.yaml.", repulsion_gain_); + "[AVOIDANCE] avoidance enabled but repulsion_gain=%.3f — no effect.", p.repulsion_gain); } else if (!latest_scan_) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, - "[AVOIDANCE] avoidance enabled but NO scan received yet on '%s'. " - "Check pointcloud_to_laserscan is running and publishing.", - scan_topic_.c_str()); + "[AVOIDANCE] avoidance enabled but NO scan received yet on '%s'.", scan_topic_.c_str()); } else { const double scan_age = (now_time - latest_scan_->header.stamp).seconds(); if (scan_age > scan_max_age_s_) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, - "[AVOIDANCE] scan stale: age=%.3fs > max=%.2fs — obstacle buffer may be outdated.", - scan_age, scan_max_age_s_); - } - - // Recompute repulsion from all buffered map-frame obstacle points. - // For each point within repulsion_cutoff_m: - // unit_repulsion = (robot - obstacle) / dist (push away from obstacle) - // lateral = unit_repulsion · (-sin(yaw), cos(yaw)) - // weight = (cutoff - dist) / cutoff - // lateral_sum += lateral * weight - double lateral_sum = 0.0; - double lateral_left = 0.0; - double lateral_right = 0.0; - int active_points = 0; - float closest_r = std::numeric_limits::infinity(); - - for (const auto & p : obstacle_map_) { - const double dx = rx - p.x; - const double dy = ry - p.y; - const double dist = std::hypot(dx, dy); - if (dist >= repulsion_cutoff_m_ || dist < 1e-6) continue; - closest_r = std::min(closest_r, static_cast(dist)); - const double weight = (repulsion_cutoff_m_ - dist) / repulsion_cutoff_m_; - // Project unit repulsion vector onto robot's left lateral axis - const double lateral = - (dx / dist) * (-std::sin(yaw)) + (dy / dist) * std::cos(yaw); - lateral_sum += lateral * weight; - ++active_points; - if (lateral > 0.0) lateral_left += lateral * weight; - else lateral_right += lateral * weight; - } - - repulsion_steering = repulsion_gain_ * lateral_sum; - - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, - "[AVOIDANCE age=%.3fs] buffered=%zu in_range=%d closest=%.2fm " - "lateral_sum=%.3f left=%.3f right=%.3f rep_steer=%.4f", - scan_age, - obstacle_map_.size(), active_points, - std::isinf(closest_r) ? -1.0f : closest_r, - lateral_sum, lateral_left, lateral_right, - repulsion_steering); - - if (active_points > 0) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 250, - "[AVOIDANCE ACTIVE] %d pts within %.1fm " - "left=%.3f right=%.3f rep_steer=%.4f path_steer=%.4f", - active_points, repulsion_cutoff_m_, - lateral_left, lateral_right, - repulsion_steering, k_p_steering_ * heading_err); + "[AVOIDANCE] scan stale: age=%.3fs > max=%.2fs", scan_age, scan_max_age_s_); } } } else { @@ -468,100 +325,107 @@ class VectorFieldPlanner : public rclcpp::Node "[AVOIDANCE OFF] publish 'true' to /obstacle_avoidance_enabled to activate"); } - // Proportional path-following steering + repulsion, clamped to physical limits - const double steering_unclamped = k_p_steering_ * heading_err + repulsion_steering; - const double steering = std::clamp( - steering_unclamped, - -max_steering_angle_rad_, max_steering_angle_rad_); + std::vector algo_obstacles; + algo_obstacles.reserve(obstacle_map_.size()); + for (const auto& op : obstacle_map_) { + algo_obstacles.push_back({op.x, op.y}); + } + algo_.updateObstacles(algo_obstacles); + + const auto res = algo_.compute(rx, ry, yaw); + + if (res.goal_reached) { + const auto & goal_pos = path_->poses.back().pose.position; + const double dist_to_goal = std::hypot(goal_pos.x - rx, goal_pos.y - ry); + RCLCPP_INFO(get_logger(), + "[GOAL_REACHED] dist=%.3fm tol=%.3fm ticks=%u", + dist_to_goal, p.goal_tolerance_m, tick_count_); + publishStop(); + return; + } + + if (res.closest_idx < last_closest_idx_ && last_closest_idx_ != std::numeric_limits::max()) { + RCLCPP_WARN(get_logger(), + "[closest_idx] BACKWARDS JUMP: %zu → %zu — robot may be closer to an earlier path segment", + last_closest_idx_, res.closest_idx); + } + last_closest_idx_ = res.closest_idx; + + if (res.lookahead_behind) { + RCLCPP_WARN(get_logger(), + "[LK_BEHIND] lookahead=(%.2f,%.2f) is BEHIND robot at (%.2f,%.2f) yaw=%.1fd closest_idx=%zu", + res.lookahead_x, res.lookahead_y, rx, ry, yaw * 180.0 / M_PI, res.closest_idx); + } + + const auto & goal_pos = path_->poses.back().pose.position; + const double dist_to_goal = std::hypot(goal_pos.x - rx, goal_pos.y - ry); + + if (res.closest_idx == last_closest_idx_ && tick_count_ > 1) { + ++stuck_ticks_; + if (stuck_ticks_ == 20) { + RCLCPP_WARN(get_logger(), + "[STUCK] closest_idx=%zu has not advanced for %u ticks. " + "pos=(%.2f,%.2f) goal_dist=%.2fm.", res.closest_idx, stuck_ticks_, rx, ry, dist_to_goal); + } + } else { + stuck_ticks_ = 0; + } + + if (p.obstacle_avoidance_enabled && p.repulsion_gain > 0.0 && latest_scan_) { + const double scan_age = (now_time - latest_scan_->header.stamp).seconds(); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, + "[AVOIDANCE age=%.3fs] buffered=%zu in_range=%d closest=%.2fm " + "lateral_sum=%.3f left=%.3f right=%.3f rep_steer=%.4f", + scan_age, obstacle_map_.size(), res.active_points, res.closest_r, + res.lateral_sum, res.lateral_left, res.lateral_right, res.repulsion_steering); + + if (res.active_points > 0) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 250, + "[AVOIDANCE ACTIVE] %d pts within %.1fm left=%.3f right=%.3f rep_steer=%.4f path_steer=%.4f", + res.active_points, p.repulsion_cutoff_m, res.lateral_left, res.lateral_right, + res.repulsion_steering, p.k_p_steering * res.heading_err); + } + } - const bool clamped = std::abs(steering_unclamped) > max_steering_angle_rad_; - if (clamped) { + if (res.clamped) { ++consecutive_clamped_; - if (consecutive_clamped_ == 40) { // 2 seconds saturated + if (consecutive_clamped_ == 40) { RCLCPP_WARN(get_logger(), "[STEER_SAT] steering saturated for %u consecutive ticks (%.1f s). " - "err=%.1fd repulsion=%.3f unclamped=%.3f max=%.3f. " - "Consider increasing max_steering_angle_rad or reducing repulsion_gain.", - consecutive_clamped_, - consecutive_clamped_ / 20.0, - heading_err * 180.0 / M_PI, - repulsion_steering, steering_unclamped, max_steering_angle_rad_); + "err=%.1fd repulsion=%.3f unclamped=%.3f max=%.3f.", + consecutive_clamped_, consecutive_clamped_ / 20.0, + res.heading_err * 180.0 / M_PI, res.repulsion_steering, + res.steering_unclamped, p.max_steering_angle_rad); } } else { consecutive_clamped_ = 0; } - // ── Single structured log line (4 Hz) — paste directly into an LLM ────── + const double lookahead_dist = std::hypot(res.lookahead_x - rx, res.lookahead_y - ry); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 250, "[NAV t=%u pos=(%.2f,%.2f) yaw=%.1fd goal=%.2fm idx=%zu/%zu " - "lk=(%.2f,%.2f) lk_dist=%.2fm fwd=%.2f err=%.1fd rep=%.3f steer=%.3f%s%s%s]", + "lk=(%.2f,%.2f) lk_dist=%.2fm err=%.1fd rep=%.3f steer=%.3f%s%s%s]", tick_count_, rx, ry, yaw * 180.0 / M_PI, dist_to_goal, - closest_idx, path_->poses.size() - 1, - lx, ly, lookahead_dist, fwd_dot, - heading_err * 180.0 / M_PI, repulsion_steering, steering, - clamped ? " CLAMPED" : "", - lk_behind ? " LK_BEHIND" : "", - (obstacle_avoidance_enabled_ && repulsion_gain_ > 0.0) ? " AVOID_ON" : ""); + res.closest_idx, path_->poses.size() - 1, + res.lookahead_x, res.lookahead_y, lookahead_dist, + res.heading_err * 180.0 / M_PI, res.repulsion_steering, res.angular_vel, + res.clamped ? " CLAMPED" : "", + res.lookahead_behind ? " LK_BEHIND" : "", + (p.obstacle_avoidance_enabled && p.repulsion_gain > 0.0) ? " AVOID_ON" : ""); geometry_msgs::msg::TwistStamped cmd; cmd.header.stamp = now(); cmd.header.frame_id = base_frame_; - cmd.twist.linear.x = max_speed_mps_; - cmd.twist.angular.z = steering; + cmd.twist.linear.x = res.linear_vel; + cmd.twist.angular.z = res.angular_vel; cmd_pub_->publish(cmd); if (publish_debug_markers_) { - publishDebugMarkers(rx, ry, lx, ly); - } - } - - // ── Helpers ──────────────────────────────────────────────────────────────── - - // Index of the path pose closest to (rx, ry). - size_t findClosestIndex(double rx, double ry) - { - size_t best = 0; - double best_dist2 = std::numeric_limits::infinity(); - - for (size_t i = 0; i < path_->poses.size(); ++i) { - const double dx = path_->poses[i].pose.position.x - rx; - const double dy = path_->poses[i].pose.position.y - ry; - const double d2 = dx * dx + dy * dy; - if (d2 < best_dist2) { - best_dist2 = d2; - best = i; - } - } - - if (best < last_closest_idx_ && last_closest_idx_ != std::numeric_limits::max()) { - RCLCPP_WARN(get_logger(), - "[closest_idx] BACKWARDS JUMP: %zu → %zu (dist=%.3f m) — " - "robot may be closer to an earlier path segment", - last_closest_idx_, best, std::sqrt(best_dist2)); - } - last_closest_idx_ = best; - return best; - } - - // Starting from closest_idx, walk forward along the path until a pose is - // >= lookahead_dist_m from the robot. If the path is shorter, return its - // last pose so the robot drives toward the goal even at close range. - std::pair findLookahead( - double rx, double ry, size_t closest_idx) const - { - for (size_t i = closest_idx; i < path_->poses.size(); ++i) { - const double dx = path_->poses[i].pose.position.x - rx; - const double dy = path_->poses[i].pose.position.y - ry; - if (std::hypot(dx, dy) >= lookahead_dist_m_) { - return {path_->poses[i].pose.position.x, - path_->poses[i].pose.position.y}; - } + publishDebugMarkers(rx, ry, res.lookahead_x, res.lookahead_y); } - const auto & last = path_->poses.back().pose.position; - return {last.x, last.y}; } - // Zero-velocity command — always safe to call. void publishStop() { geometry_msgs::msg::TwistStamped stop_cmd; @@ -570,14 +434,11 @@ class VectorFieldPlanner : public rclcpp::Node cmd_pub_->publish(stop_cmd); } - // Two RViz markers: a sphere at the lookahead point, and a line from the - // robot to that point. void publishDebugMarkers(double rx, double ry, double lx, double ly) { visualization_msgs::msg::MarkerArray arr; const auto stamp = now(); - // Lookahead sphere visualization_msgs::msg::Marker sphere; sphere.header.frame_id = map_frame_; sphere.header.stamp = stamp; @@ -594,7 +455,6 @@ class VectorFieldPlanner : public rclcpp::Node sphere.color.a = 1.0f; arr.markers.push_back(sphere); - // Line from robot to lookahead visualization_msgs::msg::Marker line; line.header = sphere.header; line.ns = "vector_field_planner"; @@ -613,30 +473,19 @@ class VectorFieldPlanner : public rclcpp::Node marker_pub_->publish(arr); } - // ── Parameters ───────────────────────────────────────────────────────────── std::string map_frame_; std::string base_frame_; std::string cmd_vel_topic_; double tf_timeout_s_; - double max_speed_mps_; - double max_steering_angle_rad_; - double lookahead_dist_m_; - double k_p_steering_; - double repulsion_gain_; - double repulsion_cutoff_m_; double obstacle_memory_time_s_; int obstacle_max_points_; - bool obstacle_avoidance_enabled_; std::string scan_topic_; double scan_max_age_s_; - double goal_tolerance_m_; bool publish_debug_markers_; - // ── TF ───────────────────────────────────────────────────────────────────── std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - // ── ROS interfaces ────────────────────────────────────────────────────────── rclcpp::Publisher::SharedPtr cmd_pub_; rclcpp::Publisher::SharedPtr marker_pub_; rclcpp::Subscription::SharedPtr path_sub_; @@ -645,7 +494,6 @@ class VectorFieldPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr scan_sub_; rclcpp::TimerBase::SharedPtr timer_; - // ── State ─────────────────────────────────────────────────────────────────── nav_msgs::msg::Path::SharedPtr path_; sensor_msgs::msg::LaserScan::SharedPtr latest_scan_; bool nav_enabled_{false}; @@ -654,7 +502,8 @@ class VectorFieldPlanner : public rclcpp::Node unsigned int consecutive_clamped_{0}; unsigned int stuck_ticks_{0}; unsigned int scan_msg_count_{0}; - std::vector obstacle_map_; + std::vector obstacle_map_; + vector_field_planner::VectorFieldPlannerAlgo algo_; }; int main(int argc, char ** argv) diff --git a/src/subsystems/even_more_minimal_nav/vector_field_planner/test/test_pure_pursuit_sim.cpp b/src/subsystems/navigation/vector_field_planner/test/test_pure_pursuit_sim.cpp similarity index 60% rename from src/subsystems/even_more_minimal_nav/vector_field_planner/test/test_pure_pursuit_sim.cpp rename to src/subsystems/navigation/vector_field_planner/test/test_pure_pursuit_sim.cpp index 644ace6a..a1da6440 100644 --- a/src/subsystems/even_more_minimal_nav/vector_field_planner/test/test_pure_pursuit_sim.cpp +++ b/src/subsystems/navigation/vector_field_planner/test/test_pure_pursuit_sim.cpp @@ -12,16 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// test_pure_pursuit_sim.cpp -// -// Closed-loop simulation tests for the VectorFieldPlanner + FrontAckermannController pipeline. -// No ROS runtime required — all logic is replicated inline from the production sources: -// src/subsystems/even_more_minimal_nav/vector_field_planner/src/vector_field_planner_node.cpp -// src/subsystems/drive/drive_controllers/src/front_ackermann_controller.cpp -// -// Each replica mirrors the production code exactly. If you change the production code, -// update the corresponding replica here so the simulation stays faithful. - #include #include @@ -33,77 +23,26 @@ #include #include +#include "vector_field_planner/vector_field_planner_algo.hpp" + // ============================================================================ -// Replica of VectorFieldPlanner logic +// Wrapper for Planner logic (using extracted algorithm) // ============================================================================ -struct Pose2D { double x, y; }; - -struct SimPath { std::vector poses; }; - -/// Mirrors VectorFieldPlanner::findClosestIndex — full-scan, no index caching. -static size_t findClosestIndex(const SimPath & path, double rx, double ry) -{ - size_t best = 0; - double best_dist2 = std::numeric_limits::infinity(); - for (size_t i = 0; i < path.poses.size(); ++i) { - const double dx = path.poses[i].x - rx; - const double dy = path.poses[i].y - ry; - const double d2 = dx * dx + dy * dy; - if (d2 < best_dist2) { best_dist2 = d2; best = i; } - } - return best; -} - -/// Mirrors VectorFieldPlanner::findLookahead — scans forward from closest_idx. -static std::pair findLookahead( - const SimPath & path, double rx, double ry, - size_t closest_idx, double lookahead_dist_m) -{ - for (size_t i = closest_idx; i < path.poses.size(); ++i) { - const double dx = path.poses[i].x - rx; - const double dy = path.poses[i].y - ry; - if (std::hypot(dx, dy) >= lookahead_dist_m) { - return {path.poses[i].x, path.poses[i].y}; - } - } - return {path.poses.back().x, path.poses.back().y}; -} - -struct PlannerParams { - double lookahead_dist_m = 3.0; - double k_p_steering = 1.5; - double max_steering_angle_rad = 0.5; - double max_speed_mps = 0.8; - double goal_tolerance_m = 1.5; -}; +using SimPath = std::vector; -/// Mirrors VectorFieldPlanner::controlLoop (pure math portion — no TF). -/// Returns {linear_x, angular_z} that would be published as TwistStamped. +/// Calls the extracted planner algorithm to get the velocity commands. static std::pair plannerStep( const SimPath & path, double rx, double ry, double yaw, - const PlannerParams & p, bool & goal_reached) + const vector_field_planner::PlannerParams & p, bool & goal_reached) { - const auto & goal_pos = path.poses.back(); - const double dist_to_goal = std::hypot(goal_pos.x - rx, goal_pos.y - ry); - if (dist_to_goal < p.goal_tolerance_m) { - goal_reached = true; - return {0.0, 0.0}; - } - goal_reached = false; - - const size_t closest_idx = findClosestIndex(path, rx, ry); - const auto [lx, ly] = findLookahead(path, rx, ry, closest_idx, p.lookahead_dist_m); - - double heading_err = std::atan2(ly - ry, lx - rx) - yaw; - while (heading_err > M_PI) heading_err -= 2.0 * M_PI; - while (heading_err < -M_PI) heading_err += 2.0 * M_PI; + vector_field_planner::VectorFieldPlannerAlgo algo; + algo.setParams(p); + algo.setPath(path); - const double steering = std::clamp( - p.k_p_steering * heading_err, - -p.max_steering_angle_rad, p.max_steering_angle_rad); - - return {p.max_speed_mps, steering}; + auto res = algo.compute(rx, ry, yaw); + goal_reached = res.goal_reached; + return {res.linear_vel, res.angular_vel}; } // ============================================================================ @@ -126,13 +65,6 @@ struct WheelState { double angular_vel = 0.0; }; -/// Mirrors FrontAckermannController::update (the wheel command + odom portion). -/// linear_vel_cmd = twist.linear.x, angular_z_cmd = twist.angular.z. -/// -/// KEY: angular_z_cmd is treated by the controller as omega (rad/s) in the -/// bicycle model: steer_angle = atan(omega * L / v) -/// The planner, however, outputs angular.z = k_p * heading_err intending it -/// to represent a desired steering angle in radians. static WheelState controllerStep( double linear_vel_cmd, double angular_z_cmd, const AckermannParams & p) @@ -202,9 +134,9 @@ static SimPath makeStraightPath( const int n = static_cast(std::ceil(dist / resolution_m)) + 1; for (int i = 0; i < n; ++i) { const double t = std::min(1.0, (i * resolution_m) / dist); - path.poses.push_back({x0 + t * dx, y0 + t * dy}); + path.push_back({x0 + t * dx, y0 + t * dy}); } - path.poses.back() = {x1, y1}; // ensure exact endpoint + path.back() = {x1, y1}; // ensure exact endpoint return path; } @@ -224,7 +156,7 @@ struct SimResult { static SimResult runSimulation( const SimPath & path, double start_x, double start_y, double start_yaw, - const PlannerParams & planner, const AckermannParams & ackermann, + const vector_field_planner::PlannerParams & planner, const AckermannParams & ackermann, double dt = 0.05, int max_steps = 2000) { SimResult result; @@ -260,7 +192,7 @@ static SimResult runSimulation( result.final_y = y; result.final_yaw = yaw; result.final_dist_to_goal = std::hypot( - path.poses.back().x - x, path.poses.back().y - y); + path.back().x - x, path.back().y - y); return result; } @@ -271,15 +203,20 @@ static SimResult runSimulation( TEST(FindClosestIndex, StartOfPath) { const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - EXPECT_EQ(findClosestIndex(path, 0.0, 0.0), 0u); + vector_field_planner::VectorFieldPlannerAlgo algo; + algo.setPath(path); + EXPECT_EQ(algo.findClosestIndex(0.0, 0.0), 0u); } TEST(FindClosestIndex, AdvancesAsRobotMoves) { const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); + vector_field_planner::VectorFieldPlannerAlgo algo; + algo.setPath(path); + // Path direction: (10,-10)/sqrt(200) = (0.707,-0.707) // A robot at (3.5, -3.5) should map to the closest waypoint index ~5 - const size_t idx = findClosestIndex(path, 3.5, -3.5); + const size_t idx = algo.findClosestIndex(3.5, -3.5); EXPECT_GE(idx, 4u); EXPECT_LE(idx, 6u); } @@ -287,8 +224,11 @@ TEST(FindClosestIndex, AdvancesAsRobotMoves) TEST(FindClosestIndex, NearGoal) { const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const size_t last = path.poses.size() - 1; - EXPECT_EQ(findClosestIndex(path, 10.0, -10.0), last); + vector_field_planner::VectorFieldPlannerAlgo algo; + algo.setPath(path); + + const size_t last = path.size() - 1; + EXPECT_EQ(algo.findClosestIndex(10.0, -10.0), last); } // --------------------------------------------------------------------------- @@ -296,125 +236,134 @@ TEST(FindClosestIndex, NearGoal) TEST(FindLookahead, ReturnsPointAtLeastLookaheadAway) { const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const double lookahead = 3.0; - const auto [lx, ly] = findLookahead(path, 0.0, 0.0, 0, lookahead); - EXPECT_GE(std::hypot(lx, ly), lookahead) - << "Lookahead point must be at least " << lookahead << " m from robot"; + vector_field_planner::VectorFieldPlannerAlgo algo; + algo.setPath(path); + + vector_field_planner::PlannerParams params; + params.lookahead_dist_m = 3.0; + algo.setParams(params); + + const auto [lx, ly] = algo.findLookahead(0.0, 0.0, 0); + EXPECT_GE(std::hypot(lx, ly), params.lookahead_dist_m) + << "Lookahead point must be at least " << params.lookahead_dist_m << " m from robot"; } TEST(FindLookahead, DirectionIsSoutheast) { const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const auto [lx, ly] = findLookahead(path, 0.0, 0.0, 0, 3.0); + vector_field_planner::VectorFieldPlannerAlgo algo; + algo.setPath(path); + + vector_field_planner::PlannerParams params; + params.lookahead_dist_m = 3.0; + algo.setParams(params); + + const auto [lx, ly] = algo.findLookahead(0.0, 0.0, 0); EXPECT_GT(lx, 0.0) << "Lookahead x should be positive (east)"; EXPECT_LT(ly, 0.0) << "Lookahead y should be negative (south)"; } TEST(FindLookahead, FallsBackToGoalWhenPathShorterThanLookahead) { - // Robot very close to the goal — remaining path < lookahead distance const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const size_t near_end = path.poses.size() - 2; - const double rx = path.poses[near_end].x; - const double ry = path.poses[near_end].y; - - const auto [lx, ly] = findLookahead(path, rx, ry, near_end, 3.0); + vector_field_planner::VectorFieldPlannerAlgo algo; + algo.setPath(path); + + vector_field_planner::PlannerParams params; + params.lookahead_dist_m = 3.0; + algo.setParams(params); + + const size_t near_end = path.size() - 2; + const double rx = path[near_end].x; + const double ry = path[near_end].y; + + const auto [lx, ly] = algo.findLookahead(rx, ry, near_end); EXPECT_NEAR(lx, 10.0, 1e-6) << "Should fall back to goal x"; EXPECT_NEAR(ly, -10.0, 1e-6) << "Should fall back to goal y"; } TEST(FindLookahead, DoesNotSearchBeforeClosestIndex) { - // If closest_idx is near the end, the lookahead must not wrap around - // to the beginning of the path. const SimPath path = makeStraightPath(0.0, 0.0, 20.0, 0.0, 1.0); // east-only path + vector_field_planner::VectorFieldPlannerAlgo algo; + algo.setPath(path); + + vector_field_planner::PlannerParams params; + params.lookahead_dist_m = 3.0; + algo.setParams(params); + // Robot is at x=18, y=0 — very close to end - const size_t closest = findClosestIndex(path, 18.0, 0.0); - const auto [lx, ly] = findLookahead(path, 18.0, 0.0, closest, 3.0); + const size_t closest = algo.findClosestIndex(18.0, 0.0); + const auto [lx, ly] = algo.findLookahead(18.0, 0.0, closest); EXPECT_GE(lx, 18.0) << "Lookahead must be ahead of the robot, not behind"; } -// BUG DOCUMENTED: findLookahead returns a point BEHIND the robot when path spacing -// exceeds lookahead_dist_m. -// -// Root cause: findLookahead scans from closest_idx and returns the first pose that is -// >= lookahead_dist_m from the robot. When path spacing > lookahead_dist_m (e.g. a -// 2-pose path), closest_idx=0 may be the START pose that the robot has already passed. -// If the robot is now > lookahead_dist_m from pose[0], the scan immediately returns -// pose[0] — which is behind the robot. The robot then steers backward, circles, -// and never reaches the goal. -// -// This does NOT trigger with the production path (path_resolution_m=1.0, lookahead=3.0) -// because the 1 m-spaced poses ensure closest_idx quickly advances past the robot. -// It WOULD trigger if the global planner ever emits a coarse path (spacing > 3 m) -// or if lookahead_dist_m is reduced below the path spacing. -TEST(FindLookahead, BugSparsePathReturnsPointBehindRobot) +TEST(FindLookahead, SkipBehindRobotWhenSparse) { - // Two-pose path: start=(0,0), goal=(10,-10). Spacing ≈ 14.1 m >> lookahead=3.0 m. + vector_field_planner::VectorFieldPlannerAlgo algo; SimPath path; - path.poses.push_back({0.0, 0.0}); - path.poses.push_back({10.0, -10.0}); + path.push_back({0.0, 0.0}); + path.push_back({10.0, -10.0}); + algo.setPath(path); + + vector_field_planner::PlannerParams p; + p.lookahead_dist_m = 3.0; + algo.setParams(p); - // Robot has moved ~4 m along the path direction (past lookahead radius from pose[0]). - const double rx = 4.0 * 0.7071, ry = 4.0 * -0.7071; // (2.83, -2.83) + const double rx = 4.0 * 0.7071, ry = 4.0 * -0.7071; - // closest_idx is 0 (robot is still in the first half of the path) - const size_t closest = findClosestIndex(path, rx, ry); + const size_t closest = algo.findClosestIndex(rx, ry); EXPECT_EQ(closest, 0u); - // findLookahead starts at i=0 (pose (0,0)). - // dist from robot (2.83,-2.83) to pose[0] (0,0) = 4.0 m >= lookahead 3.0 m. - // So it immediately returns (0,0) — the point the robot already passed. - const auto [lx, ly] = findLookahead(path, rx, ry, closest, 3.0); - - // This assertion DOCUMENTS the bug: the returned lookahead is pose[0] = (0,0), - // which is BEHIND the robot. - EXPECT_NEAR(lx, 0.0, 1e-9) << "BUG: findLookahead returned the already-passed start pose"; - EXPECT_NEAR(ly, 0.0, 1e-9) << "BUG: findLookahead returned the already-passed start pose"; - - // Consequence: the heading error points backward, causing the robot to u-turn. - const double yaw = std::atan2(-1.0, 1.0); // robot facing SE (correct heading) - double heading_err = std::atan2(ly - ry, lx - rx) - yaw; - while (heading_err > M_PI) heading_err -= 2.0 * M_PI; - while (heading_err < -M_PI) heading_err += 2.0 * M_PI; - - // heading_err should be ~±π (opposite direction) — the robot steers away from goal - EXPECT_GT(std::abs(heading_err), M_PI / 2.0) - << "BUG consequence: heading error is > 90 deg because lookahead is behind the robot"; + const auto [lx, ly] = algo.findLookahead(rx, ry, closest); + + // With the bug fix, it should return (10, -10) instead of the start point + EXPECT_NEAR(lx, 10.0, 1e-9); + EXPECT_NEAR(ly, -10.0, 1e-9); } // --------------------------------------------------------------------------- TEST(HeadingError, DiagonalGoalFromEastFacing) { + vector_field_planner::VectorFieldPlannerAlgo algo; + vector_field_planner::PlannerParams p; + p.k_p_steering = 1.0; + p.lookahead_dist_m = 0.5; // very small so it picks the goal + p.goal_tolerance_m = 0.1; // so it doesn't immediately return goal_reached + algo.setParams(p); + + SimPath path = {{1.0, -1.0}}; + algo.setPath(path); + // Robot at origin facing east (yaw=0); lookahead at (1,-1) → bearing = -45° - const double rx = 0.0, ry = 0.0, yaw = 0.0; - const double lx = 1.0, ly = -1.0; - double err = std::atan2(ly - ry, lx - rx) - yaw; - while (err > M_PI) err -= 2 * M_PI; - while (err < -M_PI) err += 2 * M_PI; - EXPECT_NEAR(err, -M_PI / 4.0, 1e-9); + auto res = algo.compute(0.0, 0.0, 0.0); + + // heading error is returned in res + EXPECT_NEAR(res.heading_err, -M_PI / 4.0, 1e-9); } TEST(HeadingError, NormalisedThroughPi) { + vector_field_planner::VectorFieldPlannerAlgo algo; + vector_field_planner::PlannerParams p; + p.k_p_steering = 1.0; + p.goal_tolerance_m = 0.1; // so it doesn't immediately return goal_reached + algo.setParams(p); + + SimPath path = {{1.0, 0.0}}; + algo.setPath(path); + // Facing west (-π), lookahead directly east → raw error ≈ +2π, must wrap to 0 - const double yaw = M_PI; // facing west - const double lx = 1.0, ly = 0.0; // lookahead due east → bearing = 0 - double err = std::atan2(ly, lx) - yaw; - while (err > M_PI) err -= 2 * M_PI; - while (err < -M_PI) err += 2 * M_PI; - EXPECT_NEAR(std::abs(err), M_PI, 1e-9); // ±π, both are correct + auto res = algo.compute(0.0, 0.0, M_PI); + + EXPECT_NEAR(std::abs(res.heading_err), M_PI, 1e-9); // ±π, both are correct } // ============================================================================ // Unit tests — controller steer conversion // ============================================================================ -// This test documents the planner/controller interface mismatch. -// The planner sends angular.z = k_p * heading_err, intending it as a steering angle. -// The controller converts it via: steer = atan(angular_z * L / v), as if it's omega. -// The two interpretations diverge for non-trivial angular_z values. TEST(AckermannController, AngularZInterpretationMismatch) { const AckermannParams p; @@ -473,10 +422,9 @@ TEST(AckermannController, ZeroAngularZGoeseStraight) // Closed-loop simulation: (0,0) → (10,-10) // ============================================================================ -// Default parameters, drawn directly from nav_params.yaml and front_ackermann_controller.yaml -static PlannerParams defaultPlannerParams() +static vector_field_planner::PlannerParams defaultPlannerParams() { - PlannerParams p; + vector_field_planner::PlannerParams p; p.lookahead_dist_m = 3.0; p.k_p_steering = 1.5; p.max_steering_angle_rad = 0.5; @@ -487,7 +435,7 @@ static PlannerParams defaultPlannerParams() static AckermannParams defaultAckermannParams() { - return AckermannParams{}; // all defaults from struct definition + return AckermannParams{}; } TEST(ClosedLoopSim, GoalIsReached) @@ -511,8 +459,6 @@ TEST(ClosedLoopSim, GoalReachedWithinReasonableTime) path, 0.0, 0.0, 0.0, defaultPlannerParams(), defaultAckermannParams()); - // At 0.8 m/s the Euclidean distance is ~14.1 m → ideal time ~17.7 s → ~354 steps. - // Allow 3x margin for curvature, but not infinite spinning. const int reasonable_steps = 1100; // ~55 s at 20 Hz EXPECT_LE(result.steps, reasonable_steps) << "Simulation used " << result.steps << " steps (>" << reasonable_steps @@ -521,10 +467,8 @@ TEST(ClosedLoopSim, GoalReachedWithinReasonableTime) TEST(ClosedLoopSim, RobotSteerRightInitially) { - // (0,0) → (10,-10): the robot starts facing east and must turn right (south-east). - // Verify the very first command steers the robot in the correct direction. const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const PlannerParams p = defaultPlannerParams(); + const auto p = defaultPlannerParams(); bool goal_reached = false; const auto [lin_x, ang_z] = plannerStep(path, 0.0, 0.0, 0.0, p, goal_reached); @@ -533,7 +477,6 @@ TEST(ClosedLoopSim, RobotSteerRightInitially) EXPECT_GT(lin_x, 0.0) << "Speed should be positive"; EXPECT_LT(ang_z, 0.0) << "angular.z should be negative (right turn toward (10,-10))"; - // Verify the controller also produces a right-turning angular velocity const WheelState ws = controllerStep(lin_x, ang_z, defaultAckermannParams()); EXPECT_LT(ws.angular_vel, 0.0) << "Controller must produce negative angular velocity for initial right turn"; @@ -541,22 +484,21 @@ TEST(ClosedLoopSim, RobotSteerRightInitially) TEST(ClosedLoopSim, LookaheadPointIsAlwaysForward) { - // During the entire trajectory, the lookahead point should always be ahead of - // (or at) the robot's current path progress — never behind. const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const PlannerParams p = defaultPlannerParams(); - const AckermannParams a = defaultAckermannParams(); + const auto p = defaultPlannerParams(); + const auto a = defaultAckermannParams(); const SimResult result = runSimulation(path, 0.0, 0.0, 0.0, p, a); - // Replay trajectory and verify the lookahead is never further BEHIND the path - // than the closest index. + vector_field_planner::VectorFieldPlannerAlgo algo; + algo.setPath(path); + algo.setParams(p); + size_t prev_closest = 0; for (const auto & [x, y, yaw] : result.trajectory) { - const size_t closest = findClosestIndex(path, x, y); - const auto [lx, ly] = findLookahead(path, x, y, closest, p.lookahead_dist_m); + const size_t closest = algo.findClosestIndex(x, y); + const auto [lx, ly] = algo.findLookahead(x, y, closest); - // The lookahead's closest index must be >= the robot's current closest index - const size_t lookahead_path_idx = findClosestIndex(path, lx, ly); + const size_t lookahead_path_idx = algo.findClosestIndex(lx, ly); EXPECT_GE(lookahead_path_idx, closest) << "Lookahead point is behind the robot's current closest path index at (" << x << ", " << y << ")"; @@ -568,9 +510,6 @@ TEST(ClosedLoopSim, LookaheadPointIsAlwaysForward) TEST(ClosedLoopSim, DistanceToGoalMonotonicallyDecreasesOnAverage) { - // The robot must make net progress toward the goal. We check that the - // distance to goal at the midpoint of the simulation is less than at the start, - // and at the end is less than at the midpoint. const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); const SimResult result = runSimulation( path, 0.0, 0.0, 0.0, @@ -604,7 +543,7 @@ TEST(ClosedLoopSim, DistanceToGoalMonotonicallyDecreasesOnAverage) TEST(EdgeCase, AlreadyAtGoal) { const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const PlannerParams p = defaultPlannerParams(); + const auto p = defaultPlannerParams(); bool goal_reached = false; const auto [lin_x, ang_z] = plannerStep(path, 10.0, -10.0, 0.0, p, goal_reached); @@ -616,40 +555,32 @@ TEST(EdgeCase, AlreadyAtGoal) TEST(EdgeCase, SinglePosePath) { SimPath path; - path.poses.push_back({5.0, -5.0}); + path.push_back({5.0, -5.0}); - const PlannerParams p = defaultPlannerParams(); + const auto p = defaultPlannerParams(); bool goal_reached = false; - // Robot well outside tolerance + const auto [lin_x, ang_z] = plannerStep(path, 0.0, 0.0, 0.0, p, goal_reached); EXPECT_FALSE(goal_reached); EXPECT_GT(lin_x, 0.0); } -// BUG REGRESSION: a 2-pose path causes the robot to loop and never reach the goal. -// See FindLookahead.BugSparsePathReturnsPointBehindRobot for the root cause. -// Once the planner bug is fixed this test should be inverted to EXPECT true. -TEST(EdgeCase, PathWithOnlyTwoPosesFailsDueToLookaheadBug) +TEST(EdgeCase, PathWithOnlyTwoPosesReachesGoal) { SimPath path; - path.poses.push_back({0.0, 0.0}); - path.poses.push_back({10.0, -10.0}); + path.push_back({0.0, 0.0}); + path.push_back({10.0, -10.0}); const SimResult result = runSimulation( path, 0.0, 0.0, 0.0, defaultPlannerParams(), defaultAckermannParams()); - // This currently FAILS to reach the goal because findLookahead returns the - // already-passed start pose once the robot is > lookahead_dist_m from it. - // When this is fixed, flip the expectation to EXPECT_TRUE. - EXPECT_FALSE(result.goal_reached) - << "BUG FIXED: update this test to EXPECT_TRUE(goal_reached)"; + EXPECT_TRUE(result.goal_reached) + << "BUG FIXED: The robot successfully reaches the goal even with a 2-pose sparse path."; } TEST(EdgeCase, RobotStartsFacingAwayFromGoal) { - // Robot faces west (yaw = ±π); goal is at (10,-10) (south-east). - // The controller must still steer the robot around and eventually reach the goal. const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); const SimResult result = runSimulation( path, 0.0, 0.0, M_PI, // facing west diff --git a/src/subsystems/navigation/yolo_ros/package.xml b/src/subsystems/navigation/yolo_ros/package.xml index 4c157465..7a3eca59 100644 --- a/src/subsystems/navigation/yolo_ros/package.xml +++ b/src/subsystems/navigation/yolo_ros/package.xml @@ -13,6 +13,7 @@ python3-pytest sensor_msgs + std_msgs python3-opencv cv_bridge python3-pip diff --git a/src/subsystems/navigation/yolo_ros/yolo_ros/img_inference.py b/src/subsystems/navigation/yolo_ros/yolo_ros/img_inference.py index e367d4c6..607d63e3 100644 --- a/src/subsystems/navigation/yolo_ros/yolo_ros/img_inference.py +++ b/src/subsystems/navigation/yolo_ros/yolo_ros/img_inference.py @@ -1,6 +1,7 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import Image as RosImage +from std_msgs.msg import Bool from cv_bridge import CvBridge import cv2 import threading @@ -30,23 +31,24 @@ def to_msg_format(self): class InferenceNode(Node): def __init__(self): super().__init__('yolo_inference_node') + self.declare_parameter('confidence_threshold', 0.5) + self.conf_threshold = self.get_parameter('confidence_threshold').value + self.subscription = self.create_subscription( RosImage, '/zed/zed_node/left_gray/image_rect_gray', self.image_callback, 10 ) + self.detection_pub = self.create_publisher(Bool, '/yolo_detection', 10) self.bridge = CvBridge() self.frame_queue = [] self.frame_lock = threading.Lock() self.model = YOLO("best.pt") - self.detection_file = "detections.txt" self.last_inference_time = time.time() self.fps = 0 - - open(self.detection_file, "w").close() - - self.inference_thread = threading.Thread(target=self.inference_loop, daemon=True) # use ros executers and executer groups + + self.inference_thread = threading.Thread(target=self.inference_loop, daemon=True) self.inference_thread.start() def image_callback(self, msg): @@ -76,23 +78,27 @@ def inference_loop(self): self.process_results(frame, results, width, height) def process_results(self, frame, results, img_width, img_height): - cv2.putText(frame, f"FPS: {self.fps:.1f}", (10, 30), + cv2.putText(frame, f"FPS: {self.fps:.1f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - with open(self.detection_file, "a") as file: - for result in results: - for box in result.boxes: - x1, y1, x2, y2 = map(int, box.xyxy[0]) - conf = float(box.conf[0]) - cls = int(box.cls[0]) - label = f"{self.model.names[cls]}: {conf:.2f}" + detection_found = False + for result in results: + for box in result.boxes: + conf = float(box.conf[0]) + if conf < self.conf_threshold: + continue + x1, y1, x2, y2 = map(int, box.xyxy[0]) + cls = int(box.cls[0]) + label = f"{self.model.names[cls]}: {conf:.2f}" - cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2) - cv2.putText(frame, label, (x1, y1 - 10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) + cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2) + cv2.putText(frame, label, (x1, y1 - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) + detection_found = True - bb = BoundingBox(img_width, img_height, x1, y1, x2, y2) - file.write(bb.to_msg_format()) + msg = Bool() + msg.data = detection_found + self.detection_pub.publish(msg) cv2.imshow("YOLO Output", frame) cv2.waitKey(1) From 39d5526e851b8fecbe3c2e25990d9d1f8d6bd107 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sat, 4 Apr 2026 13:32:00 -0400 Subject: [PATCH 08/21] more cleanup and testing --- .claude/settings.local.json | 11 - .../navigation/global_planner/CMakeLists.txt | 5 +- .../global_planner/global_planner_algo.hpp | 40 ++ .../mission_executive/CMakeLists.txt | 24 - .../mission_executive_algo.hpp | 67 ++ .../test/test_mission_executive.cpp | 272 -------- .../test/test_mock_pipeline.cpp | 341 ---------- .../nav_bringup/launch/emmn.launch.py | 41 +- .../vector_field_planner/CMakeLists.txt | 13 +- .../vector_field_planner_algo.hpp | 25 +- .../test/test_pure_pursuit_sim.cpp | 591 ------------------ 11 files changed, 134 insertions(+), 1296 deletions(-) delete mode 100644 .claude/settings.local.json delete mode 100644 src/subsystems/navigation/mission_executive/test/test_mission_executive.cpp delete mode 100644 src/subsystems/navigation/mission_executive/test/test_mock_pipeline.cpp delete mode 100644 src/subsystems/navigation/vector_field_planner/test/test_pure_pursuit_sim.cpp diff --git a/.claude/settings.local.json b/.claude/settings.local.json deleted file mode 100644 index 4fe02daa..00000000 --- a/.claude/settings.local.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "permissions": { - "allow": [ - "Bash(find:*)", - "Bash(git rm:*)", - "Bash(git checkout:*)", - "Bash(git -C /Users/mdurrani/Development/umdloop/ros2-devenv/main/athena-code diff main...HEAD)", - "Bash(xargs sed:*)" - ] - } -} diff --git a/src/subsystems/navigation/global_planner/CMakeLists.txt b/src/subsystems/navigation/global_planner/CMakeLists.txt index bfcb7534..63ec1fe8 100644 --- a/src/subsystems/navigation/global_planner/CMakeLists.txt +++ b/src/subsystems/navigation/global_planner/CMakeLists.txt @@ -41,9 +41,6 @@ install(DIRECTORY include/ DESTINATION include ) -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - # Add tests here when they exist -endif() + ament_package() diff --git a/src/subsystems/navigation/global_planner/include/global_planner/global_planner_algo.hpp b/src/subsystems/navigation/global_planner/include/global_planner/global_planner_algo.hpp index c94c7a20..a810c1f3 100644 --- a/src/subsystems/navigation/global_planner/include/global_planner/global_planner_algo.hpp +++ b/src/subsystems/navigation/global_planner/include/global_planner/global_planner_algo.hpp @@ -45,14 +45,54 @@ class GlobalPlannerAlgo { public: GlobalPlannerAlgo() = default; + /* + * Sets the global planner parameters (e.g., costmap usage, resolution). + * Param: params - The configuration parameters. + */ void setParams(const PlannerParams& params) { params_ = params; } + + /* + * Returns the current configuration parameters. + */ const PlannerParams& getParams() const { return params_; } + /* + * Sets the costmap used for A* planning. + * Param: costmap - The 2D grid costmap representing obstacles and terrain slopes. + */ void setCostmap(const Costmap& costmap) { costmap_ = costmap; } + + /* + * Clears the internal costmap, forcing straight-line fallback. + */ void clearCostmap() { costmap_ = std::nullopt; } + + /* + * Checks if a costmap is currently loaded. + * Returns: True if a costmap is present. + */ bool hasCostmap() const { return costmap_.has_value(); } + /* + * Generates a simple straight-line path from start to goal. + * Ignores obstacles, used as a baseline or fallback. + * Param: sx - Start X position. + * Param: sy - Start Y position. + * Param: gx - Goal X position. + * Param: gy - Goal Y position. + * Returns: A sequence of poses forming a straight line. + */ std::vector planStraightLine(double sx, double sy, double gx, double gy) const; + + /* + * Generates an optimal path using A* over the loaded costmap. + * Evaluates cost based on distance and cell weights (slopes). + * Param: sx - Start X position. + * Param: sy - Start Y position. + * Param: gx - Goal X position. + * Param: gy - Goal Y position. + * Returns: A sequence of poses if a path is found, or std::nullopt if unreachable. + */ std::optional> planAstar(double sx, double sy, double gx, double gy) const; private: diff --git a/src/subsystems/navigation/mission_executive/CMakeLists.txt b/src/subsystems/navigation/mission_executive/CMakeLists.txt index caf34217..1d8ff98e 100644 --- a/src/subsystems/navigation/mission_executive/CMakeLists.txt +++ b/src/subsystems/navigation/mission_executive/CMakeLists.txt @@ -53,30 +53,6 @@ install(DIRECTORY include/ DESTINATION include ) -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_mission_executive - test/test_mission_executive.cpp - ) - target_include_directories(test_mission_executive PUBLIC - $ - ) - target_compile_features(test_mission_executive PUBLIC cxx_std_17) - target_link_libraries(test_mission_executive mission_executive_algo) - - ament_add_gtest(test_mock_pipeline - test/test_mock_pipeline.cpp - ../global_planner/src/global_planner_algo.cpp - ../vector_field_planner/src/vector_field_planner_algo.cpp - ) - target_include_directories(test_mock_pipeline PUBLIC - $ - $ - $ - ) - target_compile_features(test_mock_pipeline PUBLIC cxx_std_17) - target_link_libraries(test_mock_pipeline mission_executive_algo) -endif() ament_package() diff --git a/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp b/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp index 290f264d..1da82464 100644 --- a/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp +++ b/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp @@ -92,24 +92,91 @@ class MissionExecutiveAlgo { public: MissionExecutiveAlgo() = default; + /* + * Sets the configuration parameters for the mission executive. + * Param: p - The new mission parameters to apply. + */ void setParams(const MissionParams& p) { params_ = p; } // Inputs + + /* + * Updates the robot's current estimated pose. + * Param: pose - The latest 2D pose (x, y, yaw). + */ void updateRobotPose(const Pose2D& pose) { robot_pose_ = pose; } + + /* + * Updates the planned global path for cross-track error calculations. + * Param: path - A sequence of 2D poses representing the path. + */ void updateGlobalPath(const std::vector& path) { global_path_ = path; } + + /* + * Updates the current IMU angular velocity, used to detect if the robot has stopped. + * Param: angular_vel_mag - Magnitude of angular velocity. + * Param: current_time_s - Current time in seconds. + */ void updateImu(double angular_vel_mag, double current_time_s); + + /* + * Records the latest planner event from the navigation stack. + * Param: event - The event code (e.g., plan failed). + */ void updatePlannerEvent(uint8_t event) { last_planner_event_ = event; } + /* + * Handles a failure in the global planner by aborting the current navigation. + */ void onPlanFailed(); + + /* + * Handles a successful visual detection, usually ending a spiral search. + */ void onDetection(); // Commands + + /* + * Adds or updates a target in the internal target registry. + * Param: entry - The target information to store. + * Returns: CommandResult indicating success or failure. + */ CommandResult setTarget(const TargetEntry& entry); + + /* + * Initiates navigation to a specific target or an inline provided target. + * Param: target_id - The ID of a registered target (can be empty). + * Param: inline_target - A target provided directly (used if target_id is empty). + * Param: is_return - True if this is a return trip to a previously visited target. + * Returns: StartNavResult detailing if the command was accepted and any goals to publish. + */ StartNavResult startNav(const std::string& target_id, const std::optional& inline_target, bool is_return); + + /* + * Aborts the current navigation or autonomous action. + * Returns: CommandResult indicating if the abort was successful. + */ CommandResult abort(); + + /* + * Toggles the teleoperation state, preventing autonomous navigation when enabled. + * Param: enable - True to enable teleop, false to return to idle. + * Returns: CommandResult indicating success. + */ CommandResult setTeleop(bool enable); + + /* + * Cancels the active navigation and resets the state to IDLE. + * Returns: CommandResult indicating success. + */ CommandResult cancelNav(); + /* + * Main update step for the state machine, evaluated periodically. + * Param: current_time_s - Current system time in seconds. + * Returns: TickResult with actions the node should perform (e.g., publishing goals). + */ TickResult tick(double current_time_s); // Getters diff --git a/src/subsystems/navigation/mission_executive/test/test_mission_executive.cpp b/src/subsystems/navigation/mission_executive/test/test_mission_executive.cpp deleted file mode 100644 index 18126282..00000000 --- a/src/subsystems/navigation/mission_executive/test/test_mission_executive.cpp +++ /dev/null @@ -1,272 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 "mission_executive/mission_executive_algo.hpp" - -using namespace mission_executive; - -class MissionExecutiveAlgoTest : public ::testing::Test { -protected: - void SetUp() override { - MissionParams p; - p.stop_angular_vel_threshold = 0.05; - p.arrival_hold_time = 1.0; - p.replan_distance_m = 2.0; - p.spiral_timeout_s = 60.0; - p.spiral_radius_m = 10.0; - p.spiral_spacing_m = 2.0; - p.spiral_angular_step = 0.5; - p.spiral_waypoint_tolerance_m = 1.0; - algo_.setParams(p); - } - - MissionExecutiveAlgo algo_; -}; - -TEST_F(MissionExecutiveAlgoTest, InitialStateIsIdle) { - EXPECT_EQ(algo_.getState(), State::IDLE); - EXPECT_FALSE(algo_.isNavEnabled()); - EXPECT_EQ(algo_.getNavMode(), "stopped"); - EXPECT_FALSE(algo_.hasQueuedGoal()); -} - -TEST_F(MissionExecutiveAlgoTest, RegisterTarget) { - TargetEntry entry{"t1", 10.0, 5.0, 0, 0, 3.0, false}; - auto res = algo_.setTarget(entry); - EXPECT_TRUE(res.success); -} - -TEST_F(MissionExecutiveAlgoTest, StartNavWithoutTargetFails) { - auto res = algo_.startNav("", std::nullopt, false); - EXPECT_FALSE(res.accepted); - EXPECT_EQ(algo_.getState(), State::IDLE); -} - -TEST_F(MissionExecutiveAlgoTest, StartNavWithUnknownTargetFails) { - auto res = algo_.startNav("unknown", std::nullopt, false); - EXPECT_FALSE(res.accepted); - EXPECT_EQ(algo_.getState(), State::IDLE); -} - -TEST_F(MissionExecutiveAlgoTest, StartNavValidTargetTransitionsToNavigating) { - TargetEntry entry{"gnss1", 10.0, 10.0, 0, 0, 2.0, false}; - algo_.setTarget(entry); - - auto res = algo_.startNav("gnss1", std::nullopt, false); - EXPECT_TRUE(res.accepted); - EXPECT_TRUE(res.publish_goal); - EXPECT_EQ(res.goal_to_publish.x, 10.0); - EXPECT_EQ(res.goal_to_publish.y, 10.0); - EXPECT_EQ(algo_.getState(), State::NAVIGATING); - EXPECT_TRUE(algo_.isNavEnabled()); - EXPECT_EQ(algo_.getNavMode(), "autonomous"); -} - -TEST_F(MissionExecutiveAlgoTest, CrossTrackErrorReplan) { - TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; - algo_.setTarget(entry); - algo_.startNav("gnss1", std::nullopt, false); - - // Set a path from (0,0) to (10,0) - std::vector path = {{0.0, 0.0, 0.0}, {10.0, 0.0, 0.0}}; - algo_.updateGlobalPath(path); - - // Robot is at (5, 3), which is 3m away from the path y=0. Replan threshold is 2m. - algo_.updateRobotPose({5.0, 3.0, 0.0}); - - auto res = algo_.tick(0.0); - EXPECT_TRUE(res.publish_goal); - EXPECT_EQ(res.goal_to_publish.x, 10.0); -} - -TEST_F(MissionExecutiveAlgoTest, ArrivalStateTransition) { - TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; - algo_.setTarget(entry); - algo_.startNav("gnss1", std::nullopt, false); - - // Robot moves within tolerance (1.5m < 2.0m) - algo_.updateRobotPose({8.5, 0.0, 0.0}); - - algo_.tick(0.0); - EXPECT_EQ(algo_.getState(), State::ARRIVING); -} - -TEST_F(MissionExecutiveAlgoTest, ArrivalHoldTimeAndStop) { - TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; - algo_.setTarget(entry); - algo_.startNav("gnss1", std::nullopt, false); - - algo_.updateRobotPose({9.0, 0.0, 0.0}); - algo_.tick(0.0); // Transitions to ARRIVING - EXPECT_EQ(algo_.getState(), State::ARRIVING); - - // First IMU reading below threshold starts the timer - algo_.updateImu(0.01, 10.0); - algo_.tick(10.0); - EXPECT_EQ(algo_.getState(), State::ARRIVING); - - // Tick before hold time expires - algo_.updateImu(0.01, 10.5); - algo_.tick(10.5); - EXPECT_EQ(algo_.getState(), State::ARRIVING); - - // Tick after hold time expires - algo_.updateImu(0.01, 11.1); - auto tick_res = algo_.tick(11.1); - EXPECT_EQ(algo_.getState(), State::STOPPED_AT_TARGET); - EXPECT_TRUE(tick_res.action_finished); - EXPECT_TRUE(tick_res.action_success); -} - -TEST_F(MissionExecutiveAlgoTest, ArrivalWithSpiralCoverage) { - // Target type 1 is ARUCO_POST, which triggers spiral coverage after stopping - TargetEntry entry{"post1", 10.0, 0.0, 1, 0, 2.0, false}; - algo_.setTarget(entry); - algo_.startNav("post1", std::nullopt, false); - - algo_.updateRobotPose({9.0, 0.0, 0.0}); - algo_.tick(0.0); // ARRIVING - - algo_.updateImu(0.01, 1.0); - algo_.tick(1.0); - algo_.updateImu(0.01, 2.1); - auto tick_res = algo_.tick(2.1); - - // Since it's type 1, it transitions to SPIRAL_COVERAGE - EXPECT_EQ(algo_.getState(), State::SPIRAL_COVERAGE); - EXPECT_FALSE(tick_res.action_finished); // Action is not finished yet - EXPECT_TRUE(tick_res.publish_goal); // Should publish first spiral waypoint -} - -TEST_F(MissionExecutiveAlgoTest, SpiralCoverageCompletesOnDetection) { - TargetEntry entry{"post1", 10.0, 0.0, 1, 0, 2.0, false}; - algo_.setTarget(entry); - algo_.startNav("post1", std::nullopt, false); - algo_.updateRobotPose({9.0, 0.0, 0.0}); - algo_.tick(0.0); // ARRIVING - algo_.updateImu(0.01, 1.0); - algo_.updateImu(0.01, 2.1); - algo_.tick(2.1); // SPIRAL_COVERAGE - - EXPECT_EQ(algo_.getState(), State::SPIRAL_COVERAGE); - - // Detection occurs! - algo_.onDetection(); - - auto tick_res = algo_.tick(3.0); - EXPECT_EQ(algo_.getState(), State::SPIRAL_DONE); - EXPECT_TRUE(tick_res.action_finished); - EXPECT_TRUE(tick_res.action_success); -} - -TEST_F(MissionExecutiveAlgoTest, SpiralCoverageTimeout) { - TargetEntry entry{"post1", 10.0, 0.0, 1, 0, 2.0, false}; - algo_.setTarget(entry); - algo_.startNav("post1", std::nullopt, false); - algo_.updateRobotPose({9.0, 0.0, 0.0}); - algo_.tick(0.0); // ARRIVING - algo_.updateImu(0.01, 1.0); - algo_.updateImu(0.01, 2.1); - algo_.tick(2.1); // SPIRAL_COVERAGE - - // Jump time forward past the timeout - auto tick_res = algo_.tick(2.1 + 61.0); - - EXPECT_EQ(algo_.getState(), State::SPIRAL_DONE); - EXPECT_TRUE(tick_res.action_finished); - EXPECT_TRUE(tick_res.action_success); // Timeout is considered completion -} - -TEST_F(MissionExecutiveAlgoTest, ReturnFailsIfUnvisited) { - TargetEntry entry{"post1", 10.0, 0.0, 1, 0, 2.0, false}; - algo_.setTarget(entry); - - // Attempt to return to an unvisited target - auto res = algo_.startNav("post1", std::nullopt, true); - EXPECT_FALSE(res.accepted); -} - -TEST_F(MissionExecutiveAlgoTest, ReturnSucceedsIfVisited) { - TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, true}; - algo_.setTarget(entry); - - auto res = algo_.startNav("gnss1", std::nullopt, true); - EXPECT_TRUE(res.accepted); - EXPECT_EQ(algo_.getState(), State::RETURNING); -} - -TEST_F(MissionExecutiveAlgoTest, AbortTransitionsToAborting) { - TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; - algo_.setTarget(entry); - algo_.startNav("gnss1", std::nullopt, false); - EXPECT_EQ(algo_.getState(), State::NAVIGATING); - - auto res = algo_.abort(); - EXPECT_TRUE(res.success); - EXPECT_EQ(algo_.getState(), State::ABORTING); - - // Ticking should complete the abort - auto tick_res = algo_.tick(0.0); - EXPECT_TRUE(tick_res.action_finished); - EXPECT_FALSE(tick_res.action_success); - EXPECT_EQ(algo_.getState(), State::IDLE); -} - -TEST_F(MissionExecutiveAlgoTest, GoalQueuedDuringAbort) { - TargetEntry entry1{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; - algo_.setTarget(entry1); - algo_.startNav("gnss1", std::nullopt, false); - - algo_.abort(); - EXPECT_EQ(algo_.getState(), State::ABORTING); - - TargetEntry entry2{"gnss2", 20.0, 0.0, 0, 0, 2.0, false}; - algo_.setTarget(entry2); - - // Send new goal while aborting - auto res = algo_.startNav("gnss2", std::nullopt, false); - EXPECT_TRUE(res.accepted); - EXPECT_TRUE(algo_.hasQueuedGoal()); - - // Tick finishes abort, promotes queued goal - auto tick_res = algo_.tick(0.0); - EXPECT_TRUE(tick_res.action_finished); - EXPECT_TRUE(tick_res.start_queued_goal); - EXPECT_EQ(algo_.getState(), State::NAVIGATING); - EXPECT_EQ(algo_.getActiveTarget()->id, "gnss2"); -} - -TEST_F(MissionExecutiveAlgoTest, PlanFailedTriggersAbort) { - TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; - algo_.setTarget(entry); - algo_.startNav("gnss1", std::nullopt, false); - - algo_.onPlanFailed(); - EXPECT_EQ(algo_.getState(), State::ABORTING); -} - -TEST_F(MissionExecutiveAlgoTest, TeleopPreventsNav) { - algo_.setTeleop(true); - EXPECT_EQ(algo_.getState(), State::TELEOP); - - TargetEntry entry{"gnss1", 10.0, 0.0, 0, 0, 2.0, false}; - algo_.setTarget(entry); - - auto res = algo_.startNav("gnss1", std::nullopt, false); - EXPECT_FALSE(res.accepted); - - algo_.setTeleop(false); - EXPECT_EQ(algo_.getState(), State::IDLE); -} diff --git a/src/subsystems/navigation/mission_executive/test/test_mock_pipeline.cpp b/src/subsystems/navigation/mission_executive/test/test_mock_pipeline.cpp deleted file mode 100644 index 8c73436c..00000000 --- a/src/subsystems/navigation/mission_executive/test/test_mock_pipeline.cpp +++ /dev/null @@ -1,341 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 -#include - -#include "mission_executive/mission_executive_algo.hpp" -#include "global_planner/global_planner_algo.hpp" -#include "vector_field_planner/vector_field_planner_algo.hpp" - -// ============================================================================ -// Replica of FrontAckermannController logic (from test_pure_pursuit_sim.cpp) -// ============================================================================ - -struct AckermannParams { - double wheelbase = 0.8382; - double track_width = 0.6604; - double wheel_radius = 0.254; - double max_speed = 1.27; - double max_steer_angle = 0.785; -}; - -struct WheelState { - double fl_steer = 0.0, fr_steer = 0.0; - double fl_vel = 0.0, fr_vel = 0.0, rl_vel = 0.0, rr_vel = 0.0; - double linear_vel = 0.0; - double angular_vel = 0.0; -}; - -static WheelState controllerStep( - double linear_vel_cmd, double angular_z_cmd, - const AckermannParams & p) -{ - WheelState ws; - linear_vel_cmd = std::clamp(linear_vel_cmd, -p.max_speed, p.max_speed); - - double steer_cmd = 0.0; - if (std::abs(linear_vel_cmd) > 1e-4) { - steer_cmd = std::atan(angular_z_cmd * p.wheelbase / linear_vel_cmd); - } - steer_cmd = std::clamp(steer_cmd, -p.max_steer_angle, p.max_steer_angle); - - ws.fl_vel = ws.fr_vel = ws.rl_vel = ws.rr_vel = linear_vel_cmd; - - if (std::abs(steer_cmd) > 1e-4) { - const double turn_radius = p.wheelbase / std::tan(steer_cmd); - double angular_vel = std::abs(linear_vel_cmd) / std::abs(turn_radius); - if (linear_vel_cmd < 0.0) angular_vel = -angular_vel; - - const double inner_angle = std::atan(p.wheelbase / (std::abs(turn_radius) - p.track_width / 2.0)); - const double outer_angle = std::atan(p.wheelbase / (std::abs(turn_radius) + p.track_width / 2.0)); - - const double inner_rear = angular_vel * (std::abs(turn_radius) - p.track_width / 2.0); - const double outer_rear = angular_vel * (std::abs(turn_radius) + p.track_width / 2.0); - const double inner_front = angular_vel * std::sqrt(p.wheelbase * p.wheelbase + std::pow(std::abs(turn_radius) - p.track_width / 2.0, 2)); - const double outer_front = angular_vel * std::sqrt(p.wheelbase * p.wheelbase + std::pow(std::abs(turn_radius) + p.track_width / 2.0, 2)); - - if (steer_cmd > 0.0) { - ws.fl_steer = inner_angle; ws.fr_steer = outer_angle; - ws.fl_vel = inner_front; ws.fr_vel = outer_front; - ws.rl_vel = inner_rear; ws.rr_vel = outer_rear; - } else { - ws.fl_steer = -outer_angle; ws.fr_steer = -inner_angle; - ws.fl_vel = outer_front; ws.fr_vel = inner_front; - ws.rl_vel = outer_rear; ws.rr_vel = inner_rear; - } - } - - ws.linear_vel = (ws.rl_vel + ws.rr_vel) / 2.0; - const double avg_steer = (ws.fl_steer + ws.fr_steer) / 2.0; - if (std::abs(p.wheelbase) > 1e-6) { - ws.angular_vel = ws.linear_vel * std::tan(avg_steer) / p.wheelbase; - } - - return ws; -} - -// ============================================================================ -// Pipeline Mock -// ============================================================================ - -struct MockPipeline { - mission_executive::MissionExecutiveAlgo mission; - global_planner::GlobalPlannerAlgo global; - vector_field_planner::VectorFieldPlannerAlgo local; - AckermannParams ackermann; - - double x = 0.0, y = 0.0, yaw = 0.0; - double time_s = 0.0; - double dt = 0.05; // 20 Hz - - std::vector current_global_path; - - MockPipeline() { - // Setup parameters - mission_executive::MissionParams mp; - mp.stop_angular_vel_threshold = 0.05; - mp.arrival_hold_time = 1.0; - mp.replan_distance_m = 3.0; - mp.spiral_radius_m = 10.0; - mission.setParams(mp); - - global_planner::PlannerParams gp; - gp.path_resolution_m = 1.0; - gp.use_costmap = false; // Just use straight line for mock - global.setParams(gp); - - vector_field_planner::PlannerParams vp; - vp.max_speed_mps = 0.8; - vp.lookahead_dist_m = 3.0; - vp.goal_tolerance_m = 1.5; - vp.obstacle_avoidance_enabled = false; - local.setParams(vp); - } - - void setPose(double px, double py, double pyaw) { - x = px; y = py; yaw = pyaw; - } - - void planGlobal(double gx, double gy) { - current_global_path = global.planStraightLine(x, y, gx, gy); - - // Convert to mission and local types - std::vector mp; - std::vector lp; - for (auto& p : current_global_path) { - mp.push_back({p.x, p.y, p.yaw}); - lp.push_back({p.x, p.y}); - } - - mission.updateGlobalPath(mp); - local.setPath(lp); - } - - bool step(int max_steps = 10000) { - bool goal_reached = false; - - for (int i = 0; i < max_steps; ++i) { - mission.updateRobotPose({x, y, yaw}); - - auto tick_res = mission.tick(time_s); - - if (tick_res.publish_goal) { - planGlobal(tick_res.goal_to_publish.x, tick_res.goal_to_publish.y); - } - - if (tick_res.action_finished) { - goal_reached = tick_res.action_success; - if (!tick_res.start_queued_goal) { - break; // Action is complete - } - } - - if (mission.getState() == mission_executive::State::IDLE) { - break; - } - - // If Navigating, get command from Local Planner - double linear_cmd = 0.0; - double angular_cmd = 0.0; - - if (mission.isNavEnabled() && mission.getState() != mission_executive::State::ARRIVING) { - auto local_res = local.compute(x, y, yaw); - if (!local_res.goal_reached) { - linear_cmd = local_res.linear_vel; - angular_cmd = local_res.angular_vel; - } - } else if (mission.getState() == mission_executive::State::ARRIVING) { - // Slow down or stop if arriving - linear_cmd = 0.0; - angular_cmd = 0.0; - } - - // Step Ackermann - auto ws = controllerStep(linear_cmd, angular_cmd, ackermann); - - // Update Pose - const double heading_mid = yaw + ws.angular_vel * dt / 2.0; - x += ws.linear_vel * std::cos(heading_mid) * dt; - y += ws.linear_vel * std::sin(heading_mid) * dt; - yaw += ws.angular_vel * dt; - - while (yaw > M_PI) yaw -= 2.0 * M_PI; - while (yaw < -M_PI) yaw += 2.0 * M_PI; - - // Update Mission IMU - mission.updateImu(std::abs(ws.angular_vel), time_s); - - time_s += dt; - } - - return goal_reached; - } -}; - -TEST(MockPipelineTest, FullAutonomousGNSSNavigation) { - MockPipeline sim; - sim.setPose(0.0, 0.0, 0.0); - - // 1.f.iii: 2 GNSS-only locations. Stopping within 3m is considered successful. - // The local planner uses 1.5m goal tolerance by default, which is safely < 3m. - - mission_executive::TargetEntry gnss1{"gnss1", 20.0, 10.0, 0, 0, 2.5, false}; - sim.mission.setTarget(gnss1); - - auto res = sim.mission.startNav("gnss1", std::nullopt, false); - ASSERT_TRUE(res.accepted); - if (res.publish_goal) sim.planGlobal(res.goal_to_publish.x, res.goal_to_publish.y); - - bool success = sim.step(); - - EXPECT_TRUE(success) << "Failed to reach GNSS target 1"; - EXPECT_EQ(sim.mission.getState(), mission_executive::State::STOPPED_AT_TARGET); - - double dist = std::hypot(sim.x - 20.0, sim.y - 10.0); - EXPECT_LE(dist, 3.0) << "Final distance > 3m limit for GNSS point"; -} - -TEST(MockPipelineTest, ArucoPostWithSpiralSearch) { - MockPipeline sim; - sim.setPose(0.0, 0.0, 0.0); - - // 1.f.iv: Posts with AR markers. - // Target type 1 is ARUCO_POST - mission_executive::TargetEntry post1{"post1", 10.0, 0.0, 1, 0, 2.0, false}; - sim.mission.setTarget(post1); - - auto res = sim.mission.startNav("post1", std::nullopt, false); - ASSERT_TRUE(res.accepted); - if (res.publish_goal) sim.planGlobal(res.goal_to_publish.x, res.goal_to_publish.y); - - // We need to inject a detection while it's spiraling. - // Run until it enters spiral coverage - for (int i=0; i<5000; i++) { - sim.mission.updateRobotPose({sim.x, sim.y, sim.yaw}); - auto tick_res = sim.mission.tick(sim.time_s); - if (tick_res.publish_goal) sim.planGlobal(tick_res.goal_to_publish.x, tick_res.goal_to_publish.y); - - if (sim.mission.getState() == mission_executive::State::SPIRAL_COVERAGE) { - break; - } - - double linear_cmd = 0.0, angular_cmd = 0.0; - if (sim.mission.isNavEnabled() && sim.mission.getState() != mission_executive::State::ARRIVING) { - auto local_res = sim.local.compute(sim.x, sim.y, sim.yaw); - if (!local_res.goal_reached) { - linear_cmd = local_res.linear_vel; - angular_cmd = local_res.angular_vel; - } - } - - auto ws = controllerStep(linear_cmd, angular_cmd, sim.ackermann); - const double heading_mid = sim.yaw + ws.angular_vel * sim.dt / 2.0; - sim.x += ws.linear_vel * std::cos(heading_mid) * sim.dt; - sim.y += ws.linear_vel * std::sin(heading_mid) * sim.dt; - sim.yaw += ws.angular_vel * sim.dt; - sim.mission.updateImu(std::abs(ws.angular_vel), sim.time_s); - sim.time_s += sim.dt; - } - - EXPECT_EQ(sim.mission.getState(), mission_executive::State::SPIRAL_COVERAGE) << "Did not transition to spiral coverage"; - - // Simulate CV detection - sim.mission.onDetection(); - - // Finish step - bool success = sim.step(); - EXPECT_TRUE(success); - EXPECT_EQ(sim.mission.getState(), mission_executive::State::SPIRAL_DONE); -} - -TEST(MockPipelineTest, AbortAndReturnToPreviousGNSS) { - MockPipeline sim; - sim.setPose(0.0, 0.0, 0.0); - - // 1.f.viii: Operators may abort and return to any *previous* GNSS coordinate. - mission_executive::TargetEntry start_gate{"start", 0.0, 0.0, 0, 0, 2.0, true}; - sim.mission.setTarget(start_gate); - - mission_executive::TargetEntry gnss1{"gnss1", 30.0, 0.0, 0, 0, 2.0, false}; - sim.mission.setTarget(gnss1); - - // Start moving to gnss1 - auto res = sim.mission.startNav("gnss1", std::nullopt, false); - ASSERT_TRUE(res.accepted); - if (res.publish_goal) sim.planGlobal(res.goal_to_publish.x, res.goal_to_publish.y); - - // Step 200 times (10 seconds), rover is moving - for(int i=0; i<200; i++) { - sim.mission.updateRobotPose({sim.x, sim.y, sim.yaw}); - auto tick_res = sim.mission.tick(sim.time_s); - if (tick_res.publish_goal) sim.planGlobal(tick_res.goal_to_publish.x, tick_res.goal_to_publish.y); - - auto local_res = sim.local.compute(sim.x, sim.y, sim.yaw); - auto ws = controllerStep(local_res.linear_vel, local_res.angular_vel, sim.ackermann); - const double heading_mid = sim.yaw + ws.angular_vel * sim.dt / 2.0; - sim.x += ws.linear_vel * std::cos(heading_mid) * sim.dt; - sim.y += ws.linear_vel * std::sin(heading_mid) * sim.dt; - sim.yaw += ws.angular_vel * sim.dt; - sim.mission.updateImu(std::abs(ws.angular_vel), sim.time_s); - sim.time_s += sim.dt; - } - - // Rover is now somewhere around X=8.0 - EXPECT_GT(sim.x, 2.0); - EXPECT_LT(sim.x, 15.0); - EXPECT_EQ(sim.mission.getState(), mission_executive::State::NAVIGATING); - - // Trigger ABORT - sim.mission.abort(); - EXPECT_EQ(sim.mission.getState(), mission_executive::State::ABORTING); - - // While aborting, queue a RETURN to start_gate - auto ret_res = sim.mission.startNav("start", std::nullopt, true); - EXPECT_TRUE(ret_res.accepted); - - // Step to completion - bool success = sim.step(); - - EXPECT_TRUE(success) << "Failed to return to start gate"; - EXPECT_EQ(sim.mission.getState(), mission_executive::State::STOPPED_AT_RETURN); - - double dist = std::hypot(sim.x - 0.0, sim.y - 0.0); - EXPECT_LE(dist, 3.0) << "Did not successfully return within tolerance"; -} diff --git a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py index 989ad533..ea3d4974 100644 --- a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py +++ b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py @@ -47,7 +47,6 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # ── Arguments ──────────────────────────────────────────────────────────── sim_arg = DeclareLaunchArgument( 'sim', default_value='false', @@ -57,7 +56,6 @@ def generate_launch_description(): sim = LaunchConfiguration('sim') - # ── Config ─────────────────────────────────────────────────────────────── nav_bringup_dir = get_package_share_directory('nav_bringup') from launch.substitutions import PythonExpression @@ -69,7 +67,6 @@ def generate_launch_description(): dem_costmap_dir = get_package_share_directory('dem_costmap') dem_file = os.path.join(dem_costmap_dir, 'maps', 'north_site800m.tif') - # ── Robot State Publisher ──────────────────────────────────────────────── robot_description_content = Command([ PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', @@ -89,7 +86,6 @@ def generate_launch_description(): }], ) - # ── Sensors Bringup ────────────────────────────────────────────────────── sensors_share = get_package_share_directory('athena_sensors') sensors_launch_file = os.path.join(sensors_share, 'launch', 'sensors.launch.py') @@ -101,28 +97,6 @@ def generate_launch_description(): }.items(), ) - # ── Aruco Detection ────────────────────────────────────────────────────── - aruco_bt_share = get_package_share_directory('aruco_bt') - aruco_launch_file = os.path.join(aruco_bt_share, 'launch', 'aruco.launch.py') - - aruco_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(aruco_launch_file), - launch_arguments={'use_sim_time': sim, 'marker_size': '0.20'}.items() - ) - - # ── YOLO Detection ─────────────────────────────────────────────────────── - # yolo_ros expects /zed/zed_node/left_gray/image_rect_gray and publishes to /yolo_detection - yolo_inference_node = Node( - package='yolo_ros', - executable='inference', - name='yolo_inference', - output='screen', - parameters=[{'use_sim_time': sim}], - # Provide compatible remapping if ZED color is needed instead: - # remappings=[('/zed/zed_node/left_gray/image_rect_gray', '/zed/zed_node/left/image_rect_color')], - ) - - # ── GPS hardware / sim bridge ───────────────────────────────────────────── gps_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( @@ -134,7 +108,6 @@ def generate_launch_description(): launch_arguments={'sim': sim}.items(), ) - # ── gps_pose_publisher ──────────────────────────────────────────────────── gps_pose_publisher_node = Node( package='gps_pose_publisher', executable='gps_pose_publisher_node', @@ -143,8 +116,6 @@ def generate_launch_description(): parameters=[nav_params_file], ) - # ── dem_costmap_converter (DEM GeoTIFF → OccupancyGrid /map) ───────────── - # dem_file_path is passed inline so the YAML can leave it blank by default. dem_costmap_converter_node = Node( package='dem_costmap', executable='map_node', @@ -156,7 +127,6 @@ def generate_launch_description(): ], ) - # ── global_planner (/goal_pose → /global_path) ──────────────────────────── global_planner_node = Node( package='global_planner', executable='global_planner_node', @@ -165,7 +135,6 @@ def generate_launch_description(): parameters=[nav_params_file], ) - # ── vector_field_planner (/global_path → /cmd_vel) ──────────────────────── vector_field_planner_node = Node( package='vector_field_planner', executable='vector_field_planner_node', @@ -178,7 +147,6 @@ def generate_launch_description(): ], ) - # ── mission_executive (state machine) ──────────────────────────────────── mission_executive_node = Node( package='mission_executive', executable='mission_executive_node', @@ -187,11 +155,7 @@ def generate_launch_description(): parameters=[nav_params_file], ) - # ── reframe_pointcloud (relabels ZED frame_id → base_link, sim only) ──────── - # The ZED driver in simulation publishes the cloud under - # athena/base_footprint/zed_depth_sensor. pointcloud_to_laserscan needs a - # consistent, simple frame name so it can produce a valid scan without any - # TF lookup. This relay just rewrites header.frame_id in-place. + reframe_pointcloud_node = Node( package='nav_bringup', executable='reframe_pointcloud.py', @@ -201,7 +165,6 @@ def generate_launch_description(): condition=IfCondition(sim) ) - # ── pointcloud_to_laserscan (ZED PointCloud2 → LaserScan for obstacle avoidance) ── pointcloud_to_laserscan_node = Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', @@ -218,8 +181,6 @@ def generate_launch_description(): sim_arg, robot_state_publisher_node, sensors_launch, - aruco_launch, - yolo_inference_node, gps_launch, gps_pose_publisher_node, dem_costmap_converter_node, diff --git a/src/subsystems/navigation/vector_field_planner/CMakeLists.txt b/src/subsystems/navigation/vector_field_planner/CMakeLists.txt index ffdecc1a..ac1f44a7 100644 --- a/src/subsystems/navigation/vector_field_planner/CMakeLists.txt +++ b/src/subsystems/navigation/vector_field_planner/CMakeLists.txt @@ -47,17 +47,6 @@ install(DIRECTORY include/ DESTINATION include ) -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - - ament_add_gtest(test_pure_pursuit_sim - test/test_pure_pursuit_sim.cpp - ) - target_include_directories(test_pure_pursuit_sim PUBLIC - $ - ) - target_compile_features(test_pure_pursuit_sim PUBLIC cxx_std_17) - target_link_libraries(test_pure_pursuit_sim vector_field_planner_algo) -endif() + ament_package() diff --git a/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp b/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp index 039a1456..a82af9f4 100644 --- a/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp +++ b/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp @@ -69,23 +69,46 @@ class VectorFieldPlannerAlgo { public: VectorFieldPlannerAlgo() = default; + /* + * Sets the parameters for the vector field planner, such as speeds and lookahead distance. + * Param: params - The new planner configuration. + */ void setParams(const PlannerParams& params) { params_ = params; } + /* + * Returns the current planner parameters. + */ const PlannerParams& getParams() const { return params_; } + /* + * Sets the path that the planner should follow. + * Param: path - Sequence of 2D poses constituting the path. + */ void setPath(const std::vector& path) { path_ = path; } - // obstacle_map should only contain valid (non-stale) points in map frame + /* + * Updates the local obstacle map used for repulsive avoidance. + * Expects valid (non-stale) points in the map frame. + * Param: obstacles - A list of obstacle points. + */ void updateObstacles(const std::vector& obstacles) { obstacles_ = obstacles; } + /* + * Computes the velocity and steering commands to follow the path and avoid obstacles. + * Evaluates purely based on current state against the path and obstacles. + * Param: rx - Current robot X position. + * Param: ry - Current robot Y position. + * Param: yaw - Current robot heading (yaw). + * Returns: PlannerResult containing commanded linear/angular velocities and diagnostic info. + */ PlannerResult compute(double rx, double ry, double yaw); // Expose for testing and debugging diff --git a/src/subsystems/navigation/vector_field_planner/test/test_pure_pursuit_sim.cpp b/src/subsystems/navigation/vector_field_planner/test/test_pure_pursuit_sim.cpp deleted file mode 100644 index a1da6440..00000000 --- a/src/subsystems/navigation/vector_field_planner/test/test_pure_pursuit_sim.cpp +++ /dev/null @@ -1,591 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 -#include -#include -#include -#include - -#include "vector_field_planner/vector_field_planner_algo.hpp" - -// ============================================================================ -// Wrapper for Planner logic (using extracted algorithm) -// ============================================================================ - -using SimPath = std::vector; - -/// Calls the extracted planner algorithm to get the velocity commands. -static std::pair plannerStep( - const SimPath & path, double rx, double ry, double yaw, - const vector_field_planner::PlannerParams & p, bool & goal_reached) -{ - vector_field_planner::VectorFieldPlannerAlgo algo; - algo.setParams(p); - algo.setPath(path); - - auto res = algo.compute(rx, ry, yaw); - goal_reached = res.goal_reached; - return {res.linear_vel, res.angular_vel}; -} - -// ============================================================================ -// Replica of FrontAckermannController logic -// ============================================================================ - -struct AckermannParams { - double wheelbase = 0.8382; // from front_ackermann_controller.yaml - double track_width = 0.6604; - double wheel_radius = 0.254; - double max_speed = 1.27; - double max_steer_angle = 0.785; -}; - -struct WheelState { - double fl_steer = 0.0, fr_steer = 0.0; - double fl_vel = 0.0, fr_vel = 0.0, rl_vel = 0.0, rr_vel = 0.0; - // Derived robot-level velocities (open-loop odom) - double linear_vel = 0.0; - double angular_vel = 0.0; -}; - -static WheelState controllerStep( - double linear_vel_cmd, double angular_z_cmd, - const AckermannParams & p) -{ - WheelState ws; - - linear_vel_cmd = std::clamp(linear_vel_cmd, -p.max_speed, p.max_speed); - - double steer_cmd = 0.0; - if (std::abs(linear_vel_cmd) > 1e-4) { - // Controller interprets angular_z as omega, converts to steer angle - steer_cmd = std::atan(angular_z_cmd * p.wheelbase / linear_vel_cmd); - } - steer_cmd = std::clamp(steer_cmd, -p.max_steer_angle, p.max_steer_angle); - - ws.fl_vel = ws.fr_vel = ws.rl_vel = ws.rr_vel = linear_vel_cmd; - - if (std::abs(steer_cmd) > 1e-4) { - const double turn_radius = p.wheelbase / std::tan(steer_cmd); - double angular_vel = std::abs(linear_vel_cmd) / std::abs(turn_radius); - if (linear_vel_cmd < 0.0) angular_vel = -angular_vel; - - const double inner_angle = std::atan(p.wheelbase / (std::abs(turn_radius) - p.track_width / 2.0)); - const double outer_angle = std::atan(p.wheelbase / (std::abs(turn_radius) + p.track_width / 2.0)); - - const double inner_rear = angular_vel * (std::abs(turn_radius) - p.track_width / 2.0); - const double outer_rear = angular_vel * (std::abs(turn_radius) + p.track_width / 2.0); - const double inner_front = angular_vel * std::sqrt( - p.wheelbase * p.wheelbase + - std::pow(std::abs(turn_radius) - p.track_width / 2.0, 2)); - const double outer_front = angular_vel * std::sqrt( - p.wheelbase * p.wheelbase + - std::pow(std::abs(turn_radius) + p.track_width / 2.0, 2)); - - if (steer_cmd > 0.0) { // LEFT TURN: left wheel is inner - ws.fl_steer = inner_angle; ws.fr_steer = outer_angle; - ws.fl_vel = inner_front; ws.fr_vel = outer_front; - ws.rl_vel = inner_rear; ws.rr_vel = outer_rear; - } else { // RIGHT TURN: right wheel is inner - ws.fl_steer = -outer_angle; ws.fr_steer = -inner_angle; - ws.fl_vel = outer_front; ws.fr_vel = inner_front; - ws.rl_vel = outer_rear; ws.rr_vel = inner_rear; - } - } - - // Open-loop odometry (mirrors the open_loop branch in the controller) - ws.linear_vel = (ws.rl_vel + ws.rr_vel) / 2.0; - const double avg_steer = (ws.fl_steer + ws.fr_steer) / 2.0; - if (std::abs(p.wheelbase) > 1e-6) { - ws.angular_vel = ws.linear_vel * std::tan(avg_steer) / p.wheelbase; - } - - return ws; -} - -// ============================================================================ -// Path generator -// ============================================================================ - -/// Straight-line path with poses spaced resolution_m apart. -static SimPath makeStraightPath( - double x0, double y0, double x1, double y1, double resolution_m = 1.0) -{ - SimPath path; - const double dx = x1 - x0, dy = y1 - y0; - const double dist = std::hypot(dx, dy); - const int n = static_cast(std::ceil(dist / resolution_m)) + 1; - for (int i = 0; i < n; ++i) { - const double t = std::min(1.0, (i * resolution_m) / dist); - path.push_back({x0 + t * dx, y0 + t * dy}); - } - path.back() = {x1, y1}; // ensure exact endpoint - return path; -} - -// ============================================================================ -// Helper: run full closed-loop simulation and capture a trajectory -// ============================================================================ - -struct SimResult { - bool goal_reached = false; - int steps = 0; - double final_x = 0.0; - double final_y = 0.0; - double final_yaw = 0.0; - double final_dist_to_goal = 0.0; - std::vector> trajectory; // (x, y, yaw) -}; - -static SimResult runSimulation( - const SimPath & path, double start_x, double start_y, double start_yaw, - const vector_field_planner::PlannerParams & planner, const AckermannParams & ackermann, - double dt = 0.05, int max_steps = 2000) -{ - SimResult result; - double x = start_x, y = start_y, yaw = start_yaw; - - for (int step = 0; step < max_steps; ++step) { - result.trajectory.emplace_back(x, y, yaw); - - bool goal_reached = false; - const auto [lin_x, ang_z] = plannerStep(path, x, y, yaw, planner, goal_reached); - - if (goal_reached) { - result.goal_reached = true; - result.steps = step; - break; - } - - // Controller converts (lin_x, ang_z) → wheel commands - const WheelState ws = controllerStep(lin_x, ang_z, ackermann); - - // Integrate pose with midpoint method (mirrors controller odom) - const double heading_mid = yaw + ws.angular_vel * dt / 2.0; - x += ws.linear_vel * std::cos(heading_mid) * dt; - y += ws.linear_vel * std::sin(heading_mid) * dt; - yaw += ws.angular_vel * dt; - while (yaw > M_PI) yaw -= 2.0 * M_PI; - while (yaw < -M_PI) yaw += 2.0 * M_PI; - - result.steps = step + 1; - } - - result.final_x = x; - result.final_y = y; - result.final_yaw = yaw; - result.final_dist_to_goal = std::hypot( - path.back().x - x, path.back().y - y); - return result; -} - -// ============================================================================ -// Unit tests — planner logic -// ============================================================================ - -TEST(FindClosestIndex, StartOfPath) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - vector_field_planner::VectorFieldPlannerAlgo algo; - algo.setPath(path); - EXPECT_EQ(algo.findClosestIndex(0.0, 0.0), 0u); -} - -TEST(FindClosestIndex, AdvancesAsRobotMoves) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - vector_field_planner::VectorFieldPlannerAlgo algo; - algo.setPath(path); - - // Path direction: (10,-10)/sqrt(200) = (0.707,-0.707) - // A robot at (3.5, -3.5) should map to the closest waypoint index ~5 - const size_t idx = algo.findClosestIndex(3.5, -3.5); - EXPECT_GE(idx, 4u); - EXPECT_LE(idx, 6u); -} - -TEST(FindClosestIndex, NearGoal) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - vector_field_planner::VectorFieldPlannerAlgo algo; - algo.setPath(path); - - const size_t last = path.size() - 1; - EXPECT_EQ(algo.findClosestIndex(10.0, -10.0), last); -} - -// --------------------------------------------------------------------------- - -TEST(FindLookahead, ReturnsPointAtLeastLookaheadAway) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - vector_field_planner::VectorFieldPlannerAlgo algo; - algo.setPath(path); - - vector_field_planner::PlannerParams params; - params.lookahead_dist_m = 3.0; - algo.setParams(params); - - const auto [lx, ly] = algo.findLookahead(0.0, 0.0, 0); - EXPECT_GE(std::hypot(lx, ly), params.lookahead_dist_m) - << "Lookahead point must be at least " << params.lookahead_dist_m << " m from robot"; -} - -TEST(FindLookahead, DirectionIsSoutheast) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - vector_field_planner::VectorFieldPlannerAlgo algo; - algo.setPath(path); - - vector_field_planner::PlannerParams params; - params.lookahead_dist_m = 3.0; - algo.setParams(params); - - const auto [lx, ly] = algo.findLookahead(0.0, 0.0, 0); - EXPECT_GT(lx, 0.0) << "Lookahead x should be positive (east)"; - EXPECT_LT(ly, 0.0) << "Lookahead y should be negative (south)"; -} - -TEST(FindLookahead, FallsBackToGoalWhenPathShorterThanLookahead) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - vector_field_planner::VectorFieldPlannerAlgo algo; - algo.setPath(path); - - vector_field_planner::PlannerParams params; - params.lookahead_dist_m = 3.0; - algo.setParams(params); - - const size_t near_end = path.size() - 2; - const double rx = path[near_end].x; - const double ry = path[near_end].y; - - const auto [lx, ly] = algo.findLookahead(rx, ry, near_end); - EXPECT_NEAR(lx, 10.0, 1e-6) << "Should fall back to goal x"; - EXPECT_NEAR(ly, -10.0, 1e-6) << "Should fall back to goal y"; -} - -TEST(FindLookahead, DoesNotSearchBeforeClosestIndex) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 20.0, 0.0, 1.0); // east-only path - vector_field_planner::VectorFieldPlannerAlgo algo; - algo.setPath(path); - - vector_field_planner::PlannerParams params; - params.lookahead_dist_m = 3.0; - algo.setParams(params); - - // Robot is at x=18, y=0 — very close to end - const size_t closest = algo.findClosestIndex(18.0, 0.0); - const auto [lx, ly] = algo.findLookahead(18.0, 0.0, closest); - EXPECT_GE(lx, 18.0) << "Lookahead must be ahead of the robot, not behind"; -} - -TEST(FindLookahead, SkipBehindRobotWhenSparse) -{ - vector_field_planner::VectorFieldPlannerAlgo algo; - SimPath path; - path.push_back({0.0, 0.0}); - path.push_back({10.0, -10.0}); - algo.setPath(path); - - vector_field_planner::PlannerParams p; - p.lookahead_dist_m = 3.0; - algo.setParams(p); - - const double rx = 4.0 * 0.7071, ry = 4.0 * -0.7071; - - const size_t closest = algo.findClosestIndex(rx, ry); - EXPECT_EQ(closest, 0u); - - const auto [lx, ly] = algo.findLookahead(rx, ry, closest); - - // With the bug fix, it should return (10, -10) instead of the start point - EXPECT_NEAR(lx, 10.0, 1e-9); - EXPECT_NEAR(ly, -10.0, 1e-9); -} - -// --------------------------------------------------------------------------- - -TEST(HeadingError, DiagonalGoalFromEastFacing) -{ - vector_field_planner::VectorFieldPlannerAlgo algo; - vector_field_planner::PlannerParams p; - p.k_p_steering = 1.0; - p.lookahead_dist_m = 0.5; // very small so it picks the goal - p.goal_tolerance_m = 0.1; // so it doesn't immediately return goal_reached - algo.setParams(p); - - SimPath path = {{1.0, -1.0}}; - algo.setPath(path); - - // Robot at origin facing east (yaw=0); lookahead at (1,-1) → bearing = -45° - auto res = algo.compute(0.0, 0.0, 0.0); - - // heading error is returned in res - EXPECT_NEAR(res.heading_err, -M_PI / 4.0, 1e-9); -} - -TEST(HeadingError, NormalisedThroughPi) -{ - vector_field_planner::VectorFieldPlannerAlgo algo; - vector_field_planner::PlannerParams p; - p.k_p_steering = 1.0; - p.goal_tolerance_m = 0.1; // so it doesn't immediately return goal_reached - algo.setParams(p); - - SimPath path = {{1.0, 0.0}}; - algo.setPath(path); - - // Facing west (-π), lookahead directly east → raw error ≈ +2π, must wrap to 0 - auto res = algo.compute(0.0, 0.0, M_PI); - - EXPECT_NEAR(std::abs(res.heading_err), M_PI, 1e-9); // ±π, both are correct -} - -// ============================================================================ -// Unit tests — controller steer conversion -// ============================================================================ - -TEST(AckermannController, AngularZInterpretationMismatch) -{ - const AckermannParams p; - const double linear_v = 0.8; // planner max_speed_mps from nav_params.yaml - - // At max steering command from the planner (angular_z = 0.5 rad) - const double angular_z_max = 0.5; // planner clamps here - // Controller will compute: - const double steer_from_controller = std::atan(angular_z_max * p.wheelbase / linear_v); - - // If angular_z were truly a steer angle, the controller should just pass it through. - // Instead, atan(0.5 * 0.8382 / 0.8) ≈ 0.484 rad — not 0.5. - // The discrepancy grows at higher speeds (lower ratio L/v shrinks the effective steer). - EXPECT_NE(steer_from_controller, angular_z_max) - << "Controller does not pass angular_z through as a steer angle"; - - // Document the actual value so regressions are caught - EXPECT_NEAR(steer_from_controller, std::atan(0.5 * 0.8382 / 0.8), 1e-9); -} - -TEST(AckermannController, HighSpeedReducesEffectiveSteering) -{ - // At high speed the atan(omega*L/v) formula yields a smaller steer angle, - // so the robot under-steers relative to what the planner intended. - const AckermannParams p; - const double angular_z = 0.5; - - const double steer_slow = std::atan(angular_z * p.wheelbase / 0.3); // slow - const double steer_fast = std::atan(angular_z * p.wheelbase / 1.2); // fast - - EXPECT_GT(steer_slow, steer_fast) - << "Effective steering should decrease as speed increases for the same angular_z"; -} - -TEST(AckermannController, SteerSignConvention) -{ - // Negative angular_z → right turn → negative wheel angles - const AckermannParams p; - const WheelState ws = controllerStep(0.8, -0.5, p); - EXPECT_LT(ws.fl_steer, 0.0) << "Front-left steer should be negative for right turn"; - EXPECT_LT(ws.fr_steer, 0.0) << "Front-right steer should be negative for right turn"; - EXPECT_LT(ws.angular_vel, 0.0) << "Robot angular velocity should be negative for right turn"; -} - -TEST(AckermannController, ZeroAngularZGoeseStraight) -{ - const AckermannParams p; - const WheelState ws = controllerStep(0.8, 0.0, p); - EXPECT_NEAR(ws.fl_steer, 0.0, 1e-9); - EXPECT_NEAR(ws.fr_steer, 0.0, 1e-9); - EXPECT_NEAR(ws.angular_vel, 0.0, 1e-9); - EXPECT_NEAR(ws.linear_vel, 0.8, 1e-9); -} - -// ============================================================================ -// Closed-loop simulation: (0,0) → (10,-10) -// ============================================================================ - -static vector_field_planner::PlannerParams defaultPlannerParams() -{ - vector_field_planner::PlannerParams p; - p.lookahead_dist_m = 3.0; - p.k_p_steering = 1.5; - p.max_steering_angle_rad = 0.5; - p.max_speed_mps = 0.8; - p.goal_tolerance_m = 1.5; - return p; -} - -static AckermannParams defaultAckermannParams() -{ - return AckermannParams{}; -} - -TEST(ClosedLoopSim, GoalIsReached) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const SimResult result = runSimulation( - path, 0.0, 0.0, 0.0, - defaultPlannerParams(), defaultAckermannParams()); - - EXPECT_TRUE(result.goal_reached) - << "Goal (10,-10) was never reached after " << result.steps << " steps.\n" - << "Final position: (" << result.final_x << ", " << result.final_y << ")\n" - << "Distance to goal: " << result.final_dist_to_goal << " m\n" - << "Goal tolerance: " << defaultPlannerParams().goal_tolerance_m << " m"; -} - -TEST(ClosedLoopSim, GoalReachedWithinReasonableTime) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const SimResult result = runSimulation( - path, 0.0, 0.0, 0.0, - defaultPlannerParams(), defaultAckermannParams()); - - const int reasonable_steps = 1100; // ~55 s at 20 Hz - EXPECT_LE(result.steps, reasonable_steps) - << "Simulation used " << result.steps << " steps (>" << reasonable_steps - << "). Robot may be oscillating or not progressing."; -} - -TEST(ClosedLoopSim, RobotSteerRightInitially) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const auto p = defaultPlannerParams(); - - bool goal_reached = false; - const auto [lin_x, ang_z] = plannerStep(path, 0.0, 0.0, 0.0, p, goal_reached); - - EXPECT_FALSE(goal_reached); - EXPECT_GT(lin_x, 0.0) << "Speed should be positive"; - EXPECT_LT(ang_z, 0.0) << "angular.z should be negative (right turn toward (10,-10))"; - - const WheelState ws = controllerStep(lin_x, ang_z, defaultAckermannParams()); - EXPECT_LT(ws.angular_vel, 0.0) - << "Controller must produce negative angular velocity for initial right turn"; -} - -TEST(ClosedLoopSim, LookaheadPointIsAlwaysForward) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const auto p = defaultPlannerParams(); - const auto a = defaultAckermannParams(); - const SimResult result = runSimulation(path, 0.0, 0.0, 0.0, p, a); - - vector_field_planner::VectorFieldPlannerAlgo algo; - algo.setPath(path); - algo.setParams(p); - - size_t prev_closest = 0; - for (const auto & [x, y, yaw] : result.trajectory) { - const size_t closest = algo.findClosestIndex(x, y); - const auto [lx, ly] = algo.findLookahead(x, y, closest); - - const size_t lookahead_path_idx = algo.findClosestIndex(lx, ly); - EXPECT_GE(lookahead_path_idx, closest) - << "Lookahead point is behind the robot's current closest path index at (" - << x << ", " << y << ")"; - - prev_closest = closest; - } - (void)prev_closest; -} - -TEST(ClosedLoopSim, DistanceToGoalMonotonicallyDecreasesOnAverage) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const SimResult result = runSimulation( - path, 0.0, 0.0, 0.0, - defaultPlannerParams(), defaultAckermannParams()); - - if (result.trajectory.size() < 3) { - GTEST_SKIP() << "Trajectory too short to test monotonicity"; - } - - const auto distToGoal = [&](size_t idx) { - const auto & [x, y, yaw] = result.trajectory[idx]; - return std::hypot(10.0 - x, -10.0 - y); - }; - - const size_t quarter = result.trajectory.size() / 4; - const size_t mid = result.trajectory.size() / 2; - const size_t thr = 3 * result.trajectory.size() / 4; - - EXPECT_LT(distToGoal(quarter), distToGoal(0)) - << "Robot should be closer to goal at 25% of trajectory than at start"; - EXPECT_LT(distToGoal(mid), distToGoal(quarter)) - << "Robot should be closer to goal at 50% than at 25%"; - EXPECT_LT(distToGoal(thr), distToGoal(mid)) - << "Robot should be closer to goal at 75% than at 50%"; -} - -// ============================================================================ -// Edge cases -// ============================================================================ - -TEST(EdgeCase, AlreadyAtGoal) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const auto p = defaultPlannerParams(); - - bool goal_reached = false; - const auto [lin_x, ang_z] = plannerStep(path, 10.0, -10.0, 0.0, p, goal_reached); - EXPECT_TRUE(goal_reached); - EXPECT_NEAR(lin_x, 0.0, 1e-9); - EXPECT_NEAR(ang_z, 0.0, 1e-9); -} - -TEST(EdgeCase, SinglePosePath) -{ - SimPath path; - path.push_back({5.0, -5.0}); - - const auto p = defaultPlannerParams(); - bool goal_reached = false; - - const auto [lin_x, ang_z] = plannerStep(path, 0.0, 0.0, 0.0, p, goal_reached); - EXPECT_FALSE(goal_reached); - EXPECT_GT(lin_x, 0.0); -} - -TEST(EdgeCase, PathWithOnlyTwoPosesReachesGoal) -{ - SimPath path; - path.push_back({0.0, 0.0}); - path.push_back({10.0, -10.0}); - - const SimResult result = runSimulation( - path, 0.0, 0.0, 0.0, - defaultPlannerParams(), defaultAckermannParams()); - - EXPECT_TRUE(result.goal_reached) - << "BUG FIXED: The robot successfully reaches the goal even with a 2-pose sparse path."; -} - -TEST(EdgeCase, RobotStartsFacingAwayFromGoal) -{ - const SimPath path = makeStraightPath(0.0, 0.0, 10.0, -10.0, 1.0); - const SimResult result = runSimulation( - path, 0.0, 0.0, M_PI, // facing west - defaultPlannerParams(), defaultAckermannParams()); - - EXPECT_TRUE(result.goal_reached) - << "Goal not reached when starting backward. Dist=" << result.final_dist_to_goal; -} From ba26f316dc9f6c9a45a00689595efb2f2bfd8d40 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sat, 11 Apr 2026 03:23:06 -0400 Subject: [PATCH 09/21] working --- src/msgs/action/NavigateToTarget.action | 1 + .../mission_executive/CMakeLists.txt | 19 +- .../mission_executive_algo.hpp | 228 ------ .../mission_executive/scripts/mission_cli.sh | 7 +- .../src/mission_executive_algo.cpp | 366 --------- .../src/mission_executive_node.cpp | 766 +++++++++++++----- .../nav_bringup/config/nav_params_real.yaml | 4 + .../nav_bringup/config/nav_params_sim.yaml | 4 + .../nav_bringup/launch/emmn.launch.py | 16 +- .../navigation/nav_bringup/package.xml | 10 +- .../vector_field_planner/CMakeLists.txt | 11 +- .../vector_field_planner_algo.hpp | 42 +- .../src/vector_field_planner_algo.cpp | 188 +++-- .../src/vector_field_planner_node.cpp | 17 +- .../test/test_vector_field_planner_algo.cpp | 264 ++++++ 15 files changed, 1006 insertions(+), 937 deletions(-) delete mode 100644 src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp delete mode 100644 src/subsystems/navigation/mission_executive/src/mission_executive_algo.cpp create mode 100644 src/subsystems/navigation/vector_field_planner/test/test_vector_field_planner_algo.cpp diff --git a/src/msgs/action/NavigateToTarget.action b/src/msgs/action/NavigateToTarget.action index f52ecbc1..5b9e7204 100644 --- a/src/msgs/action/NavigateToTarget.action +++ b/src/msgs/action/NavigateToTarget.action @@ -4,6 +4,7 @@ uint8 GNSS_ONLY=0 uint8 ARUCO_POST=1 uint8 OBJECT=2 uint8 LOCAL=3 + string target_id float64 lat float64 lon diff --git a/src/subsystems/navigation/mission_executive/CMakeLists.txt b/src/subsystems/navigation/mission_executive/CMakeLists.txt index 1d8ff98e..913ef8a3 100644 --- a/src/subsystems/navigation/mission_executive/CMakeLists.txt +++ b/src/subsystems/navigation/mission_executive/CMakeLists.txt @@ -18,20 +18,9 @@ find_package(tf2_ros REQUIRED) find_package(vision_msgs REQUIRED) find_package(msgs REQUIRED) -add_library(mission_executive_algo src/mission_executive_algo.cpp) -target_include_directories(mission_executive_algo PUBLIC - $ - $ -) -target_compile_features(mission_executive_algo PUBLIC cxx_std_17) - add_executable(mission_executive_node src/mission_executive_node.cpp) -target_include_directories(mission_executive_node PUBLIC - $ -) target_compile_features(mission_executive_node PUBLIC cxx_std_17) target_link_libraries(mission_executive_node - mission_executive_algo rclcpp::rclcpp rclcpp_action::rclcpp_action ${geometry_msgs_TARGETS} @@ -45,14 +34,8 @@ target_link_libraries(mission_executive_node ${msgs_TARGETS} ) -install(TARGETS mission_executive_algo mission_executive_node +install(TARGETS mission_executive_node DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/ - DESTINATION include -) - - - ament_package() diff --git a/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp b/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp deleted file mode 100644 index 1da82464..00000000 --- a/src/subsystems/navigation/mission_executive/include/mission_executive/mission_executive_algo.hpp +++ /dev/null @@ -1,228 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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. - -#pragma once - -#include -#include -#include -#include -#include - -namespace mission_executive { - -enum class State { - IDLE, - NAVIGATING, - ARRIVING, - STOPPED_AT_TARGET, - SPIRAL_COVERAGE, - SPIRAL_DONE, - ABORTING, - RETURNING, - STOPPED_AT_RETURN, - TELEOP -}; - -std::string stateToStr(State s); - -struct Pose2D { - double x{0.0}; - double y{0.0}; - double yaw{0.0}; -}; - -struct TargetEntry { - std::string id; - double x_m{0.0}; - double y_m{0.0}; - uint8_t target_type{0}; - uint8_t goal_source{0}; - double tolerance_m{3.0}; - bool visited{false}; -}; - -struct MissionParams { - double stop_angular_vel_threshold = 0.05; - double arrival_hold_time = 1.0; - double replan_distance_m = 3.0; - double spiral_timeout_s = 120.0; - double spiral_radius_m = 15.0; - double spiral_spacing_m = 2.0; - double spiral_angular_step = 0.5; - double spiral_waypoint_tolerance_m = 2.0; -}; - -struct CommandResult { - bool success; - std::string message; -}; - -struct StartNavResult { - bool accepted = false; - std::string message; - bool preempted_old = false; - bool publish_goal = false; - Pose2D goal_to_publish; -}; - -struct TickResult { - bool publish_goal = false; - Pose2D goal_to_publish; - - bool action_finished = false; - bool action_success = false; - std::string action_message; - - bool start_queued_goal = false; -}; - -class MissionExecutiveAlgo { -public: - MissionExecutiveAlgo() = default; - - /* - * Sets the configuration parameters for the mission executive. - * Param: p - The new mission parameters to apply. - */ - void setParams(const MissionParams& p) { params_ = p; } - - // Inputs - - /* - * Updates the robot's current estimated pose. - * Param: pose - The latest 2D pose (x, y, yaw). - */ - void updateRobotPose(const Pose2D& pose) { robot_pose_ = pose; } - - /* - * Updates the planned global path for cross-track error calculations. - * Param: path - A sequence of 2D poses representing the path. - */ - void updateGlobalPath(const std::vector& path) { global_path_ = path; } - - /* - * Updates the current IMU angular velocity, used to detect if the robot has stopped. - * Param: angular_vel_mag - Magnitude of angular velocity. - * Param: current_time_s - Current time in seconds. - */ - void updateImu(double angular_vel_mag, double current_time_s); - - /* - * Records the latest planner event from the navigation stack. - * Param: event - The event code (e.g., plan failed). - */ - void updatePlannerEvent(uint8_t event) { last_planner_event_ = event; } - - /* - * Handles a failure in the global planner by aborting the current navigation. - */ - void onPlanFailed(); - - /* - * Handles a successful visual detection, usually ending a spiral search. - */ - void onDetection(); - - // Commands - - /* - * Adds or updates a target in the internal target registry. - * Param: entry - The target information to store. - * Returns: CommandResult indicating success or failure. - */ - CommandResult setTarget(const TargetEntry& entry); - - /* - * Initiates navigation to a specific target or an inline provided target. - * Param: target_id - The ID of a registered target (can be empty). - * Param: inline_target - A target provided directly (used if target_id is empty). - * Param: is_return - True if this is a return trip to a previously visited target. - * Returns: StartNavResult detailing if the command was accepted and any goals to publish. - */ - StartNavResult startNav(const std::string& target_id, const std::optional& inline_target, bool is_return); - - /* - * Aborts the current navigation or autonomous action. - * Returns: CommandResult indicating if the abort was successful. - */ - CommandResult abort(); - - /* - * Toggles the teleoperation state, preventing autonomous navigation when enabled. - * Param: enable - True to enable teleop, false to return to idle. - * Returns: CommandResult indicating success. - */ - CommandResult setTeleop(bool enable); - - /* - * Cancels the active navigation and resets the state to IDLE. - * Returns: CommandResult indicating success. - */ - CommandResult cancelNav(); - - /* - * Main update step for the state machine, evaluated periodically. - * Param: current_time_s - Current system time in seconds. - * Returns: TickResult with actions the node should perform (e.g., publishing goals). - */ - TickResult tick(double current_time_s); - - // Getters - State getState() const { return state_; } - bool isNavEnabled() const { - return state_ == State::NAVIGATING || state_ == State::ARRIVING || - state_ == State::RETURNING || state_ == State::SPIRAL_COVERAGE; - } - std::string getNavMode() const; - std::optional getActiveTarget() const { return active_target_; } - double getDistToGoal() const; - double getCrossTrackError() const; - double getHeadingError() const; - double getImuAngularVel() const { return imu_angular_vel_; } - bool isReturn() const { return is_return_; } - uint8_t getLastPlannerEvent() const { return last_planner_event_; } - bool hasQueuedGoal() const { return queued_active_; } - -private: - void transition(State new_state); - void startSpiralCoverage(double current_time_s); - std::vector generateSpiralWaypoints(const Pose2D& start); - - State state_{State::IDLE}; - MissionParams params_; - - std::unordered_map target_registry_; - std::optional active_target_; - bool is_return_{false}; - - bool queued_active_{false}; - TargetEntry queued_entry_; - bool queued_is_return_{false}; - - std::optional robot_pose_; - std::optional> global_path_; - - double imu_angular_vel_{0.0}; - double low_speed_start_s_{0.0}; - bool low_speed_tracking_{false}; - uint8_t last_planner_event_{0}; - - std::vector spiral_waypoints_; - size_t spiral_waypoint_idx_{0}; - double spiral_start_time_s_{0.0}; - bool detection_received_{false}; -}; - -} // namespace mission_executive diff --git a/src/subsystems/navigation/mission_executive/scripts/mission_cli.sh b/src/subsystems/navigation/mission_executive/scripts/mission_cli.sh index 307eaa84..7ff852a7 100755 --- a/src/subsystems/navigation/mission_executive/scripts/mission_cli.sh +++ b/src/subsystems/navigation/mission_executive/scripts/mission_cli.sh @@ -258,9 +258,7 @@ _send_nav_goal() { echo " is_return : $is_return" echo - local yaml="{target_id: '$target_id', lat: $lat, lon: $lon, x_m: $x, y_m: $y, \ -goal_type: $goal_type, target_type: $target_type, \ -tolerance_m: $tolerance, is_return: $is_return}" + local yaml="{target_id: \"$target_id\", lat: $lat, lon: $lon, x_m: $x, y_m: $y, goal_type: $goal_type, target_type: $target_type, tolerance_m: $tolerance, is_return: $is_return}" _run_nav_with_status "$yaml" "$target_type" } @@ -309,8 +307,7 @@ cmd_nav_by_id() { local is_return="${2:-false}" title "navigate_to_target (id=$id is_return=$is_return)" - local yaml="{target_id: '$id', lat: 0.0, lon: 0.0, x_m: 0.0, y_m: 0.0, \ -goal_type: 0, target_type: 0, tolerance_m: 3.0, is_return: $is_return}" + local yaml="{target_id: \"$id\", lat: 0.0, lon: 0.0, x_m: 0.0, y_m: 0.0, goal_type: 0, target_type: 0, tolerance_m: 3.0, is_return: $is_return}" # target_type unknown here — pass 0 (spiral check uses what's in the registry) _run_nav_with_status "$yaml" 0 diff --git a/src/subsystems/navigation/mission_executive/src/mission_executive_algo.cpp b/src/subsystems/navigation/mission_executive/src/mission_executive_algo.cpp deleted file mode 100644 index 8622e3b1..00000000 --- a/src/subsystems/navigation/mission_executive/src/mission_executive_algo.cpp +++ /dev/null @@ -1,366 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 "mission_executive/mission_executive_algo.hpp" -#include -#include -#include - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -#ifndef M_PI_2 -#define M_PI_2 1.57079632679489661923 -#endif - -namespace mission_executive { - -std::string stateToStr(State s) { - switch (s) { - case State::IDLE: return "IDLE"; - case State::NAVIGATING: return "NAVIGATING"; - case State::ARRIVING: return "ARRIVING"; - case State::STOPPED_AT_TARGET: return "STOPPED_AT_TARGET"; - case State::SPIRAL_COVERAGE: return "SPIRAL_COVERAGE"; - case State::SPIRAL_DONE: return "SPIRAL_DONE"; - case State::ABORTING: return "ABORTING"; - case State::RETURNING: return "RETURNING"; - case State::STOPPED_AT_RETURN: return "STOPPED_AT_RETURN"; - case State::TELEOP: return "TELEOP"; - default: return "UNKNOWN"; - } -} - -void MissionExecutiveAlgo::transition(State new_state) { - state_ = new_state; - if (state_ == State::TELEOP || !isNavEnabled()) { - low_speed_tracking_ = false; - } -} - -std::string MissionExecutiveAlgo::getNavMode() const { - switch (state_) { - case State::TELEOP: return "teleop"; - case State::NAVIGATING: - case State::ARRIVING: - case State::RETURNING: - case State::SPIRAL_COVERAGE: return "autonomous"; - default: return "stopped"; - } -} - -double MissionExecutiveAlgo::getHeadingError() const { - if (!robot_pose_.has_value() || !active_target_.has_value()) return 0.0; - const double dx = active_target_->x_m - robot_pose_->x; - const double dy = active_target_->y_m - robot_pose_->y; - const double bearing = std::atan2(dy, dx); - double err = bearing - robot_pose_->yaw; - while (err > M_PI) err -= 2.0 * M_PI; - while (err < -M_PI) err += 2.0 * M_PI; - return err; -} - -double MissionExecutiveAlgo::getDistToGoal() const { - if (!robot_pose_.has_value() || !active_target_.has_value()) return -1.0; - return std::hypot(robot_pose_->x - active_target_->x_m, robot_pose_->y - active_target_->y_m); -} - -double MissionExecutiveAlgo::getCrossTrackError() const { - if (!robot_pose_.has_value() || !global_path_.has_value()) return -1.0; - const auto & poses = global_path_.value(); - if (poses.size() < 2) return -1.0; - - const double rx = robot_pose_->x; - const double ry = robot_pose_->y; - double min_dist = std::numeric_limits::max(); - - for (size_t i = 0; i < poses.size() - 1; ++i) { - const double ax = poses[i].x, ay = poses[i].y; - const double bx = poses[i+1].x, by = poses[i+1].y; - const double dx = bx - ax, dy = by - ay; - const double len2 = dx*dx + dy*dy; - double dist; - if (len2 < 1e-10) { - dist = std::hypot(rx - ax, ry - ay); - } else { - const double t = std::clamp(((rx-ax)*dx + (ry-ay)*dy) / len2, 0.0, 1.0); - dist = std::hypot(rx - (ax + t*dx), ry - (ay + t*dy)); - } - min_dist = std::min(min_dist, dist); - } - return min_dist; -} - -void MissionExecutiveAlgo::updateImu(double angular_vel_mag, double current_time_s) { - imu_angular_vel_ = angular_vel_mag; - if (state_ == State::ARRIVING) { - if (imu_angular_vel_ < params_.stop_angular_vel_threshold) { - if (!low_speed_tracking_) { - low_speed_start_s_ = current_time_s; - low_speed_tracking_ = true; - } else { - if (current_time_s - low_speed_start_s_ >= params_.arrival_hold_time) { - low_speed_tracking_ = false; - if (is_return_) { - transition(State::STOPPED_AT_RETURN); - } else { - if (active_target_.has_value()) { - auto it = target_registry_.find(active_target_->id); - if (it != target_registry_.end()) { - it->second.visited = true; - } - } - transition(State::STOPPED_AT_TARGET); - } - } - } - } else { - low_speed_tracking_ = false; - } - } -} - -void MissionExecutiveAlgo::onPlanFailed() { - if (state_ == State::NAVIGATING || state_ == State::RETURNING) { - transition(State::ABORTING); - } -} - -void MissionExecutiveAlgo::onDetection() { - if (state_ == State::SPIRAL_COVERAGE) { - detection_received_ = true; - transition(State::SPIRAL_DONE); - } -} - -CommandResult MissionExecutiveAlgo::setTarget(const TargetEntry& entry) { - target_registry_[entry.id] = entry; - return {true, "Target '" + entry.id + "' registered"}; -} - -StartNavResult MissionExecutiveAlgo::startNav(const std::string& target_id, const std::optional& inline_target, bool is_return) { - StartNavResult res; - - TargetEntry entry; - if (!target_id.empty()) { - auto it = target_registry_.find(target_id); - if (it == target_registry_.end()) { - res.accepted = false; - res.message = "Unknown target_id: " + target_id; - return res; - } - if (is_return && !it->second.visited) { - res.accepted = false; - res.message = "Target not yet visited — cannot RETURN"; - return res; - } - entry = it->second; - } else if (inline_target.has_value()) { - entry = inline_target.value(); - } else { - res.accepted = false; - res.message = "No target provided"; - return res; - } - - if (state_ == State::TELEOP) { - res.accepted = false; - res.message = "Cannot navigate — currently in TELEOP mode"; - return res; - } - - if (state_ == State::ABORTING) { - if (queued_active_) { - res.preempted_old = true; - } - queued_active_ = true; - queued_entry_ = entry; - queued_is_return_ = is_return; - res.accepted = true; - res.message = "Goal queued — currently ABORTING"; - return res; - } - - bool is_active_nav = (state_ != State::IDLE && state_ != State::STOPPED_AT_TARGET && state_ != State::STOPPED_AT_RETURN && state_ != State::SPIRAL_DONE); - if (is_active_nav) { - res.preempted_old = true; - } - - active_target_ = entry; - is_return_ = is_return; - transition(is_return ? State::RETURNING : State::NAVIGATING); - - res.accepted = true; - res.publish_goal = true; - res.goal_to_publish = {entry.x_m, entry.y_m, 0.0}; - return res; -} - -CommandResult MissionExecutiveAlgo::abort() { - if (state_ == State::IDLE || state_ == State::TELEOP || - state_ == State::ABORTING || - state_ == State::STOPPED_AT_TARGET || state_ == State::STOPPED_AT_RETURN || - state_ == State::SPIRAL_DONE) { - return {false, "Nothing to abort in state " + stateToStr(state_)}; - } - transition(State::ABORTING); - return {true, "Aborting"}; -} - -CommandResult MissionExecutiveAlgo::setTeleop(bool enable) { - if (enable) { - transition(State::TELEOP); - return {true, "Teleop ON"}; - } else { - if (state_ == State::TELEOP) { - transition(State::IDLE); - return {true, "Teleop OFF"}; - } else { - return {false, "Not in TELEOP state"}; - } - } -} - -CommandResult MissionExecutiveAlgo::cancelNav() { - transition(State::IDLE); - return {true, "Cancelled"}; -} - -std::vector MissionExecutiveAlgo::generateSpiralWaypoints(const Pose2D& start) { - std::vector waypoints; - const double a = params_.spiral_spacing_m / (2.0 * M_PI); - if (a <= 0.0) return waypoints; - - const double max_angle = params_.spiral_radius_m / a; - const double min_start_r = 1.5; - const double yaw0 = start.yaw; - - double theta = 0.0; - while (theta <= max_angle) { - const double r = min_start_r + a * theta; - if (r > params_.spiral_radius_m) break; - - const double x = start.x + r * std::cos(yaw0 + theta + M_PI_2); - const double y = start.y + r * std::sin(yaw0 + theta + M_PI_2); - - waypoints.push_back({x, y, 0.0}); - theta += params_.spiral_angular_step; - } - return waypoints; -} - -void MissionExecutiveAlgo::startSpiralCoverage(double current_time_s) { - if (!robot_pose_.has_value()) { - transition(State::SPIRAL_DONE); - return; - } - - spiral_waypoints_ = generateSpiralWaypoints(robot_pose_.value()); - spiral_waypoint_idx_ = 0; - spiral_start_time_s_ = current_time_s; - detection_received_ = false; - - if (spiral_waypoints_.empty()) { - transition(State::SPIRAL_DONE); - } -} - -TickResult MissionExecutiveAlgo::tick(double current_time_s) { - TickResult res; - - // 1. checkArrival - if ((state_ == State::NAVIGATING || state_ == State::RETURNING) && active_target_.has_value()) { - double d = getDistToGoal(); - if (d >= 0.0 && d < active_target_->tolerance_m) { - transition(State::ARRIVING); - } - } - - // 2. checkCrossTrackError - if ((state_ == State::NAVIGATING || state_ == State::RETURNING) && active_target_.has_value()) { - double xte = getCrossTrackError(); - if (xte >= 0.0 && xte > params_.replan_distance_m) { - res.publish_goal = true; - res.goal_to_publish = {active_target_->x_m, active_target_->y_m, 0.0}; - } - } - - // 3. checkSpiralProgress - if (state_ == State::SPIRAL_COVERAGE) { - if (current_time_s - spiral_start_time_s_ >= params_.spiral_timeout_s) { - transition(State::SPIRAL_DONE); - } else if (robot_pose_.has_value() && !spiral_waypoints_.empty()) { - const auto & wp = spiral_waypoints_[spiral_waypoint_idx_]; - const double dist = std::hypot(robot_pose_->x - wp.x, robot_pose_->y - wp.y); - if (dist < params_.spiral_waypoint_tolerance_m) { - ++spiral_waypoint_idx_; - if (spiral_waypoint_idx_ >= spiral_waypoints_.size()) { - transition(State::SPIRAL_DONE); - } else { - res.publish_goal = true; - res.goal_to_publish = spiral_waypoints_[spiral_waypoint_idx_]; - } - } - } - } - - // 4. Action result resolution - if (state_ == State::STOPPED_AT_TARGET) { - // ARUCO_POST = 1, OBJECT = 2 - const bool needs_spiral = active_target_.has_value() && - (active_target_->target_type == 1 || active_target_->target_type == 2); - - if (needs_spiral) { - transition(State::SPIRAL_COVERAGE); - startSpiralCoverage(current_time_s); - if (state_ == State::SPIRAL_COVERAGE && !spiral_waypoints_.empty()) { - res.publish_goal = true; - res.goal_to_publish = spiral_waypoints_[0]; - } - } else { - res.action_finished = true; - res.action_success = true; - res.action_message = "Arrived at target"; - } - } else if (state_ == State::SPIRAL_DONE) { - res.action_finished = true; - res.action_success = true; - res.action_message = detection_received_ ? "Detection found during spiral coverage" : "Spiral coverage complete (timeout or all waypoints visited)"; - } else if (state_ == State::STOPPED_AT_RETURN) { - res.action_finished = true; - res.action_success = true; - res.action_message = "Arrived at target"; - } else if (state_ == State::ABORTING) { - res.action_finished = true; - res.action_success = false; - res.action_message = "Aborted"; - - if (queued_active_) { - active_target_ = queued_entry_; - is_return_ = queued_is_return_; - queued_active_ = false; - transition(is_return_ ? State::RETURNING : State::NAVIGATING); - res.start_queued_goal = true; - res.publish_goal = true; - res.goal_to_publish = {active_target_->x_m, active_target_->y_m, 0.0}; - } else { - transition(State::IDLE); - } - } - - return res; -} - -} // namespace mission_executive diff --git a/src/subsystems/navigation/mission_executive/src/mission_executive_node.cpp b/src/subsystems/navigation/mission_executive/src/mission_executive_node.cpp index 1a5c8444..9617c187 100644 --- a/src/subsystems/navigation/mission_executive/src/mission_executive_node.cpp +++ b/src/subsystems/navigation/mission_executive/src/mission_executive_node.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -43,10 +44,97 @@ #include "msgs/srv/lat_lon_to_enu.hpp" #include "msgs/srv/set_target.hpp" -#include "mission_executive/mission_executive_algo.hpp" - using namespace std::chrono_literals; +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 +#endif + +// ─── Types ─────────────────────────────────────────────────────────────────── + +enum class State { + IDLE, + NAVIGATING, + ARRIVING, + STOPPED_AT_TARGET, + SPIRAL_COVERAGE, + SPIRAL_DONE, + ABORTING, + RETURNING, + STOPPED_AT_RETURN, + TELEOP +}; + +static std::string stateToStr(State s) { + switch (s) { + case State::IDLE: return "IDLE"; + case State::NAVIGATING: return "NAVIGATING"; + case State::ARRIVING: return "ARRIVING"; + case State::STOPPED_AT_TARGET: return "STOPPED_AT_TARGET"; + case State::SPIRAL_COVERAGE: return "SPIRAL_COVERAGE"; + case State::SPIRAL_DONE: return "SPIRAL_DONE"; + case State::ABORTING: return "ABORTING"; + case State::RETURNING: return "RETURNING"; + case State::STOPPED_AT_RETURN: return "STOPPED_AT_RETURN"; + case State::TELEOP: return "TELEOP"; + default: return "UNKNOWN"; + } +} + +struct Pose2D { + double x{0.0}; + double y{0.0}; + double yaw{0.0}; +}; + +struct TargetEntry { + std::string id; + double x_m{0.0}; + double y_m{0.0}; + uint8_t target_type{0}; + uint8_t goal_source{0}; + double tolerance_m{3.0}; + bool visited{false}; +}; + +struct MissionParams { + double stop_angular_vel_threshold{0.05}; + double arrival_hold_time{1.0}; + double replan_distance_m{3.0}; + double spiral_timeout_s{120.0}; + double spiral_radius_m{15.0}; + double spiral_spacing_m{2.0}; + double spiral_angular_step{0.5}; + double spiral_waypoint_tolerance_m{2.0}; +}; + +struct CommandResult { + bool success; + std::string message; +}; + +struct StartNavResult { + bool accepted{false}; + std::string message; + bool preempted_old{false}; + bool publish_goal{false}; + Pose2D goal_to_publish{}; +}; + +struct TickResult { + bool publish_goal{false}; + Pose2D goal_to_publish; + bool action_finished{false}; + bool action_success{false}; + std::string action_message; + bool start_queued_goal{false}; +}; + +// ─── Node ──────────────────────────────────────────────────────────────────── + class MissionExecutive : public rclcpp::Node { public: @@ -56,61 +144,47 @@ class MissionExecutive : public rclcpp::Node explicit MissionExecutive(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("mission_executive", options) { - // Behaviour parameters - declare_parameter("stop_angular_vel_threshold", 0.05); // rad/s — IMU gyro magnitude + declare_parameter("stop_angular_vel_threshold", 0.05); declare_parameter("arrival_hold_time", 1.0); declare_parameter("replan_distance_m", 3.0); declare_parameter("latlon_to_enu_service", std::string("/gps_pose_publisher/latlon_to_enu")); - - // Spiral coverage parameters declare_parameter("spiral_timeout_s", 120.0); declare_parameter("spiral_radius_m", 15.0); declare_parameter("spiral_spacing_m", 2.0); declare_parameter("spiral_angular_step", 0.5); declare_parameter("spiral_waypoint_tolerance_m", 2.0); - - // Detection topics declare_parameter("aruco_detection_topic", std::string("/aruco_loc")); declare_parameter("yolo_detection_topic", std::string("/yolo_detection")); - - // Input topics declare_parameter("imu_topic", std::string("/imu")); declare_parameter("planner_event_topic", std::string("/planner_event")); declare_parameter("global_path_topic", std::string("/global_path")); - - // Output topics declare_parameter("goal_pose_topic", std::string("/goal_pose")); declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); declare_parameter("nav_mode_topic", std::string("/nav_mode")); declare_parameter("active_target_topic", std::string("/active_target")); declare_parameter("nav_status_topic", std::string("/nav_status")); - mission_executive::MissionParams p; - p.stop_angular_vel_threshold = get_parameter("stop_angular_vel_threshold").as_double(); - p.arrival_hold_time = get_parameter("arrival_hold_time").as_double(); - p.replan_distance_m = get_parameter("replan_distance_m").as_double(); - p.spiral_timeout_s = get_parameter("spiral_timeout_s").as_double(); - p.spiral_radius_m = get_parameter("spiral_radius_m").as_double(); - p.spiral_spacing_m = get_parameter("spiral_spacing_m").as_double(); - p.spiral_angular_step = get_parameter("spiral_angular_step").as_double(); - p.spiral_waypoint_tolerance_m= get_parameter("spiral_waypoint_tolerance_m").as_double(); - - algo_.setParams(p); - - const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); + params_.stop_angular_vel_threshold = get_parameter("stop_angular_vel_threshold").as_double(); + params_.arrival_hold_time = get_parameter("arrival_hold_time").as_double(); + params_.replan_distance_m = get_parameter("replan_distance_m").as_double(); + params_.spiral_timeout_s = get_parameter("spiral_timeout_s").as_double(); + params_.spiral_radius_m = get_parameter("spiral_radius_m").as_double(); + params_.spiral_spacing_m = get_parameter("spiral_spacing_m").as_double(); + params_.spiral_angular_step = get_parameter("spiral_angular_step").as_double(); + params_.spiral_waypoint_tolerance_m = get_parameter("spiral_waypoint_tolerance_m").as_double(); + const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); const auto aruco_detection_topic = get_parameter("aruco_detection_topic").as_string(); const auto yolo_detection_topic = get_parameter("yolo_detection_topic").as_string(); - - const auto imu_topic = get_parameter("imu_topic").as_string(); - const auto planner_event_topic = get_parameter("planner_event_topic").as_string(); - const auto global_path_topic = get_parameter("global_path_topic").as_string(); - const auto goal_pose_topic = get_parameter("goal_pose_topic").as_string(); - const auto nav_enabled_topic = get_parameter("nav_enabled_topic").as_string(); - const auto nav_mode_topic = get_parameter("nav_mode_topic").as_string(); - const auto active_target_topic = get_parameter("active_target_topic").as_string(); - const auto nav_status_topic = get_parameter("nav_status_topic").as_string(); + const auto imu_topic = get_parameter("imu_topic").as_string(); + const auto planner_event_topic = get_parameter("planner_event_topic").as_string(); + const auto global_path_topic = get_parameter("global_path_topic").as_string(); + const auto goal_pose_topic = get_parameter("goal_pose_topic").as_string(); + const auto nav_enabled_topic = get_parameter("nav_enabled_topic").as_string(); + const auto nav_mode_topic = get_parameter("nav_mode_topic").as_string(); + const auto active_target_topic = get_parameter("active_target_topic").as_string(); + const auto nav_status_topic = get_parameter("nav_status_topic").as_string(); reentrant_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); @@ -133,15 +207,13 @@ class MissionExecutive : public rclcpp::Node action_server_ = rclcpp_action::create_server( this, "~/navigate_to_target", - [this](const rclcpp_action::GoalUUID &, std::shared_ptr goal) { - return handleGoal(goal); - }, - [this](std::shared_ptr gh) { - return handleCancel(gh); + [this](const rclcpp_action::GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, - [this](std::shared_ptr gh) { - handleAccepted(gh); + [this](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; }, + [this](std::shared_ptr gh) { handleAccepted(gh); }, rcl_action_server_get_default_options(), reentrant_group_); @@ -152,14 +224,10 @@ class MissionExecutive : public rclcpp::Node std_srvs::srv::Trigger::Response::SharedPtr res) { std::lock_guard lk(mutex_); - auto cmd_res = algo_.abort(); - res->success = cmd_res.success; - res->message = cmd_res.message; - if (cmd_res.success) { - publishNavEnabled(algo_.isNavEnabled()); - publishNavMode(algo_.getNavMode()); - publishStatus(); - } + auto cmd = abort(); + res->success = cmd.success; + res->message = cmd.message; + if (cmd.success) { publishNavEnabled(); publishNavMode(); publishStatus(); } }); set_target_srv_ = create_service( @@ -167,9 +235,7 @@ class MissionExecutive : public rclcpp::Node [this]( const msgs::srv::SetTarget::Request::SharedPtr req, msgs::srv::SetTarget::Response::SharedPtr res) - { - onSetTarget(req, res); - }, + { onSetTarget(req, res); }, rmw_qos_profile_services_default, reentrant_group_); @@ -180,14 +246,10 @@ class MissionExecutive : public rclcpp::Node std_srvs::srv::SetBool::Response::SharedPtr res) { std::lock_guard lk(mutex_); - auto cmd_res = algo_.setTeleop(req->data); - res->success = cmd_res.success; - res->message = cmd_res.message; - if (cmd_res.success) { - publishNavEnabled(algo_.isNavEnabled()); - publishNavMode(algo_.getNavMode()); - publishStatus(); - } + auto cmd = setTeleop(req->data); + res->success = cmd.success; + res->message = cmd.message; + if (cmd.success) { publishNavEnabled(); publishNavMode(); publishStatus(); } }); imu_sub_ = create_subscription( @@ -195,54 +257,47 @@ class MissionExecutive : public rclcpp::Node [this](const sensor_msgs::msg::Imu::SharedPtr msg) { std::lock_guard lk(mutex_); const auto & w = msg->angular_velocity; - double angular_vel = std::sqrt(w.x * w.x + w.y * w.y + w.z * w.z); - double current_time = now().seconds(); - algo_.updateImu(angular_vel, current_time); - - // This could trigger a transition to STOPPED_AT_TARGET, which then needs action server response - // but we handle action results in tick() + onImu(std::sqrt(w.x*w.x + w.y*w.y + w.z*w.z), now().seconds()); }); planner_event_sub_ = create_subscription( planner_event_topic, 10, [this](const msgs::msg::PlannerEvent::SharedPtr msg) { std::lock_guard lk(mutex_); - algo_.updatePlannerEvent(msg->event); + last_planner_event_ = msg->event; if (msg->event == msgs::msg::PlannerEvent::PLAN_FAILED) { - algo_.onPlanFailed(); - publishNavEnabled(algo_.isNavEnabled()); - publishNavMode(algo_.getNavMode()); + onPlanFailed(); + publishNavEnabled(); + publishNavMode(); publishStatus(); } }); - auto transient_qos = rclcpp::QoS(1).transient_local(); global_path_sub_ = create_subscription( - global_path_topic, transient_qos, + global_path_topic, rclcpp::QoS(1).transient_local(), [this](const nav_msgs::msg::Path::SharedPtr msg) { std::lock_guard lk(mutex_); - std::vector path; + std::vector path; path.reserve(msg->poses.size()); for (const auto& p : msg->poses) { - path.push_back({p.pose.position.x, p.pose.position.y, quaternionToYaw(p.pose.orientation)}); + path.push_back({p.pose.position.x, p.pose.position.y, + quaternionToYaw(p.pose.orientation)}); } - algo_.updateGlobalPath(path); + global_path_ = std::move(path); }); aruco_sub_ = create_subscription( aruco_detection_topic, 10, [this](const vision_msgs::msg::Detection2D::SharedPtr) { std::lock_guard lk(mutex_); - algo_.onDetection(); + onDetection(); }); yolo_sub_ = create_subscription( yolo_detection_topic, 10, [this](const std_msgs::msg::Bool::SharedPtr msg) { std::lock_guard lk(mutex_); - if (msg->data) { - algo_.onDetection(); - } + if (msg->data) onDetection(); }); status_timer_ = create_wall_timer( @@ -250,129 +305,405 @@ class MissionExecutive : public rclcpp::Node [this]() { std::lock_guard lk(mutex_); refreshPoseFromTF(); - - auto res = algo_.tick(now().seconds()); - - if (res.publish_goal) { - geometry_msgs::msg::PoseStamped p; - p.header.frame_id = "map"; - p.header.stamp = now(); - p.pose.position.x = res.goal_to_publish.x; - p.pose.position.y = res.goal_to_publish.y; - p.pose.orientation.w = 1.0; - goal_pub_->publish(p); - } + auto res = tick(now().seconds()); + if (res.publish_goal) publishGoal(res.goal_to_publish); checkActionResult(res); - - publishNavEnabled(algo_.isNavEnabled()); - publishNavMode(algo_.getNavMode()); + publishNavEnabled(); + publishNavMode(); publishStatus(); }); - publishNavEnabled(false); - publishNavMode("stopped"); - + publishNavEnabled(); + publishNavMode(); RCLCPP_INFO(get_logger(), "MissionExecutive ready — state: IDLE"); } private: - void publishNavEnabled(bool enabled) { + // ── State machine ───────────────────────────────────────────────────────── + + bool isNavEnabled() const { + return state_ == State::NAVIGATING || state_ == State::ARRIVING || + state_ == State::RETURNING || state_ == State::SPIRAL_COVERAGE; + } + + std::string getNavMode() const { + switch (state_) { + case State::TELEOP: return "teleop"; + case State::NAVIGATING: + case State::ARRIVING: + case State::RETURNING: + case State::SPIRAL_COVERAGE: return "autonomous"; + default: return "stopped"; + } + } + + void transition(State new_state) { + state_ = new_state; + if (!isNavEnabled()) low_speed_tracking_ = false; + } + + double getDistToGoal() const { + if (!robot_pose_.has_value() || !active_target_.has_value()) return -1.0; + return std::hypot(robot_pose_->x - active_target_->x_m, + robot_pose_->y - active_target_->y_m); + } + + double getHeadingError() const { + if (!robot_pose_.has_value() || !active_target_.has_value()) return 0.0; + const double dx = active_target_->x_m - robot_pose_->x; + const double dy = active_target_->y_m - robot_pose_->y; + double err = std::atan2(dy, dx) - robot_pose_->yaw; + while (err > M_PI) err -= 2.0 * M_PI; + while (err < -M_PI) err += 2.0 * M_PI; + return err; + } + + double getCrossTrackError() const { + if (!robot_pose_.has_value() || !global_path_.has_value()) return -1.0; + const auto & poses = global_path_.value(); + if (poses.size() < 2) return -1.0; + const double rx = robot_pose_->x, ry = robot_pose_->y; + double min_dist = std::numeric_limits::max(); + for (size_t i = 0; i < poses.size() - 1; ++i) { + const double ax = poses[i].x, ay = poses[i].y; + const double bx = poses[i+1].x, by = poses[i+1].y; + const double dx = bx - ax, dy = by - ay; + const double len2 = dx*dx + dy*dy; + double dist; + if (len2 < 1e-10) { + dist = std::hypot(rx - ax, ry - ay); + } else { + const double t = std::clamp(((rx-ax)*dx + (ry-ay)*dy) / len2, 0.0, 1.0); + dist = std::hypot(rx - (ax + t*dx), ry - (ay + t*dy)); + } + min_dist = std::min(min_dist, dist); + } + return min_dist; + } + + void onImu(double angular_vel_mag, double current_time_s) { + imu_angular_vel_ = angular_vel_mag; + if (state_ != State::ARRIVING) return; + if (imu_angular_vel_ < params_.stop_angular_vel_threshold) { + if (!low_speed_tracking_) { + low_speed_start_s_ = current_time_s; + low_speed_tracking_ = true; + } else if (current_time_s - low_speed_start_s_ >= params_.arrival_hold_time) { + low_speed_tracking_ = false; + if (is_return_) { + transition(State::STOPPED_AT_RETURN); + } else { + if (active_target_.has_value()) { + auto it = target_registry_.find(active_target_->id); + if (it != target_registry_.end()) it->second.visited = true; + } + transition(State::STOPPED_AT_TARGET); + } + } + } else { + low_speed_tracking_ = false; + } + } + + void onPlanFailed() { + if (state_ == State::NAVIGATING || state_ == State::RETURNING) + transition(State::ABORTING); + } + + void onDetection() { + if (state_ == State::SPIRAL_COVERAGE) { + detection_received_ = true; + transition(State::SPIRAL_DONE); + } + } + + CommandResult setTarget(const TargetEntry& entry) { + target_registry_[entry.id] = entry; + return {true, "Target '" + entry.id + "' registered"}; + } + + StartNavResult startNav(const std::string& target_id, + const std::optional& inline_target, + bool is_return) { + StartNavResult res; + TargetEntry entry; + if (!target_id.empty()) { + auto it = target_registry_.find(target_id); + if (it == target_registry_.end()) { + return {false, "Unknown target_id: " + target_id}; + } + if (is_return && !it->second.visited) { + return {false, "Target not yet visited — cannot RETURN"}; + } + entry = it->second; + } else if (inline_target.has_value()) { + entry = inline_target.value(); + } else { + return {false, "No target provided"}; + } + + if (state_ == State::TELEOP) + return {false, "Cannot navigate — currently in TELEOP mode"}; + + if (state_ == State::ABORTING) { + if (queued_active_) res.preempted_old = true; + queued_active_ = true; + queued_entry_ = entry; + queued_is_return_ = is_return; + return {true, "Goal queued — currently ABORTING"}; + } + + const bool is_active_nav = (state_ != State::IDLE && + state_ != State::STOPPED_AT_TARGET && + state_ != State::STOPPED_AT_RETURN && + state_ != State::SPIRAL_DONE); + if (is_active_nav) res.preempted_old = true; + + active_target_ = entry; + is_return_ = is_return; + transition(is_return ? State::RETURNING : State::NAVIGATING); + + res.accepted = true; + res.publish_goal = true; + res.goal_to_publish = {entry.x_m, entry.y_m, 0.0}; + return res; + } + + CommandResult abort() { + if (state_ == State::IDLE || state_ == State::TELEOP || + state_ == State::ABORTING || + state_ == State::STOPPED_AT_TARGET || state_ == State::STOPPED_AT_RETURN || + state_ == State::SPIRAL_DONE) { + return {false, "Nothing to abort in state " + stateToStr(state_)}; + } + transition(State::ABORTING); + return {true, "Aborting"}; + } + + CommandResult setTeleop(bool enable) { + if (enable) { + transition(State::TELEOP); + return {true, "Teleop ON"}; + } + if (state_ != State::TELEOP) return {false, "Not in TELEOP state"}; + transition(State::IDLE); + return {true, "Teleop OFF"}; + } + + void cancelNav() { transition(State::IDLE); } + + std::vector generateSpiralWaypoints(const Pose2D& start) { + std::vector waypoints; + const double a = params_.spiral_spacing_m / (2.0 * M_PI); + if (a <= 0.0) return waypoints; + const double max_angle = params_.spiral_radius_m / a; + const double min_start_r = 1.5; + const double yaw0 = start.yaw; + for (double theta = 0.0; theta <= max_angle; theta += params_.spiral_angular_step) { + const double r = min_start_r + a * theta; + if (r > params_.spiral_radius_m) break; + waypoints.push_back({ + start.x + r * std::cos(yaw0 + theta + M_PI_2), + start.y + r * std::sin(yaw0 + theta + M_PI_2), + 0.0 + }); + } + return waypoints; + } + + void startSpiralCoverage(double current_time_s) { + if (!robot_pose_.has_value()) { transition(State::SPIRAL_DONE); return; } + spiral_waypoints_ = generateSpiralWaypoints(robot_pose_.value()); + spiral_waypoint_idx_ = 0; + spiral_start_time_s_ = current_time_s; + detection_received_ = false; + if (spiral_waypoints_.empty()) transition(State::SPIRAL_DONE); + } + + TickResult tick(double current_time_s) { + TickResult res; + + // 1. Check arrival + if ((state_ == State::NAVIGATING || state_ == State::RETURNING) && + active_target_.has_value()) { + const double d = getDistToGoal(); + if (d >= 0.0 && d < active_target_->tolerance_m) transition(State::ARRIVING); + } + + // 2. Check cross-track error → replan + if ((state_ == State::NAVIGATING || state_ == State::RETURNING) && + active_target_.has_value()) { + const double xte = getCrossTrackError(); + if (xte >= 0.0 && xte > params_.replan_distance_m) { + res.publish_goal = true; + res.goal_to_publish = {active_target_->x_m, active_target_->y_m, 0.0}; + } + } + + // 3. Check spiral progress + if (state_ == State::SPIRAL_COVERAGE) { + if (current_time_s - spiral_start_time_s_ >= params_.spiral_timeout_s) { + transition(State::SPIRAL_DONE); + } else if (robot_pose_.has_value() && !spiral_waypoints_.empty()) { + const auto & wp = spiral_waypoints_[spiral_waypoint_idx_]; + if (std::hypot(robot_pose_->x - wp.x, robot_pose_->y - wp.y) + < params_.spiral_waypoint_tolerance_m) { + ++spiral_waypoint_idx_; + if (spiral_waypoint_idx_ >= spiral_waypoints_.size()) { + transition(State::SPIRAL_DONE); + } else { + res.publish_goal = true; + res.goal_to_publish = spiral_waypoints_[spiral_waypoint_idx_]; + } + } + } + } + + // 4. Action result resolution + if (state_ == State::STOPPED_AT_TARGET) { + const bool needs_spiral = active_target_.has_value() && + (active_target_->target_type == 1 || active_target_->target_type == 2); + if (needs_spiral) { + transition(State::SPIRAL_COVERAGE); + startSpiralCoverage(current_time_s); + if (state_ == State::SPIRAL_COVERAGE && !spiral_waypoints_.empty()) { + res.publish_goal = true; + res.goal_to_publish = spiral_waypoints_[0]; + } + } else { + res.action_finished = true; + res.action_success = true; + res.action_message = "Arrived at target"; + } + } else if (state_ == State::SPIRAL_DONE) { + res.action_finished = true; + res.action_success = true; + res.action_message = detection_received_ + ? "Detection found during spiral coverage" + : "Spiral coverage complete (timeout or all waypoints visited)"; + } else if (state_ == State::STOPPED_AT_RETURN) { + res.action_finished = true; + res.action_success = true; + res.action_message = "Arrived at target"; + } else if (state_ == State::ABORTING) { + res.action_finished = true; + res.action_success = false; + res.action_message = "Aborted"; + if (queued_active_) { + active_target_ = queued_entry_; + is_return_ = queued_is_return_; + queued_active_ = false; + transition(is_return_ ? State::RETURNING : State::NAVIGATING); + res.start_queued_goal = true; + res.publish_goal = true; + res.goal_to_publish = {active_target_->x_m, active_target_->y_m, 0.0}; + } else { + transition(State::IDLE); + } + } + + return res; + } + + // ── ROS helpers ─────────────────────────────────────────────────────────── + + static double quaternionToYaw(const geometry_msgs::msg::Quaternion & q) { + return std::atan2(2.0 * (q.w * q.z + q.x * q.y), + 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); + } + + void refreshPoseFromTF() { + try { + const auto tf = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero); + robot_pose_ = Pose2D{ + tf.transform.translation.x, + tf.transform.translation.y, + quaternionToYaw(tf.transform.rotation) + }; + } catch (const tf2::TransformException &) {} + } + + void publishGoal(const Pose2D& goal) { + geometry_msgs::msg::PoseStamped p; + p.header.frame_id = "map"; + p.header.stamp = now(); + p.pose.position.x = goal.x; + p.pose.position.y = goal.y; + p.pose.orientation.w = 1.0; + goal_pub_->publish(p); + } + + void publishNavEnabled() { std_msgs::msg::Bool msg; - msg.data = enabled; + msg.data = isNavEnabled(); nav_enabled_pub_->publish(msg); } - void publishNavMode(const std::string& mode) { + void publishNavMode() { std_msgs::msg::String msg; - msg.data = mode; + msg.data = getNavMode(); nav_mode_pub_->publish(msg); } void publishActiveTarget() { - auto target = algo_.getActiveTarget(); - if (!target.has_value()) return; + if (!active_target_.has_value()) return; msgs::msg::ActiveTarget at; - at.target_id = target->id; - at.target_type = target->target_type; - at.tolerance_m = target->tolerance_m; - - at.goal_enu.header.frame_id = "map"; - at.goal_enu.header.stamp = now(); - at.goal_enu.pose.position.x = target->x_m; - at.goal_enu.pose.position.y = target->y_m; + at.target_id = active_target_->id; + at.target_type = active_target_->target_type; + at.tolerance_m = active_target_->tolerance_m; + at.goal_enu.header.frame_id = "map"; + at.goal_enu.header.stamp = now(); + at.goal_enu.pose.position.x = active_target_->x_m; + at.goal_enu.pose.position.y = active_target_->y_m; at.goal_enu.pose.orientation.w = 1.0; - - at.goal_source = target->goal_source; - at.status = mission_executive::stateToStr(algo_.getState()); + at.goal_source = active_target_->goal_source; + at.status = stateToStr(state_); active_target_pub_->publish(at); } void publishStatus() { msgs::msg::NavStatus s; - s.state = mission_executive::stateToStr(algo_.getState()); - auto target = algo_.getActiveTarget(); - if (target.has_value()) { - s.active_target_id = target->id; - s.active_target_type = target->target_type; - s.goal_source = target->goal_source; + s.state = stateToStr(state_); + if (active_target_.has_value()) { + s.active_target_id = active_target_->id; + s.active_target_type = active_target_->target_type; + s.goal_source = active_target_->goal_source; } - s.distance_to_goal_m = algo_.getDistToGoal(); - s.cross_track_error_m = algo_.getCrossTrackError(); - s.heading_error_rad = algo_.getHeadingError(); - s.robot_speed_mps = algo_.getImuAngularVel(); - s.is_return = algo_.isReturn(); - s.last_planner_event = algo_.getLastPlannerEvent(); + s.distance_to_goal_m = getDistToGoal(); + s.cross_track_error_m = getCrossTrackError(); + s.heading_error_rad = getHeadingError(); + s.robot_speed_mps = imu_angular_vel_; + s.is_return = is_return_; + s.last_planner_event = last_planner_event_; nav_status_pub_->publish(s); } - static double quaternionToYaw(const geometry_msgs::msg::Quaternion & q) { - const double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); - const double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); - return std::atan2(siny_cosp, cosy_cosp); - } - - void refreshPoseFromTF() { - try { - const auto tf = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero); - mission_executive::Pose2D pose; - pose.x = tf.transform.translation.x; - pose.y = tf.transform.translation.y; - pose.yaw = quaternionToYaw(tf.transform.rotation); - algo_.updateRobotPose(pose); - } catch (const tf2::TransformException &) { - } - } - - void checkActionResult(const mission_executive::TickResult& res) { + void checkActionResult(const TickResult& res) { if (!active_goal_handle_) return; if (active_goal_handle_->is_canceling()) { - auto result = std::make_shared(); + auto result = std::make_shared(); result->success = false; result->message = "Cancelled"; active_goal_handle_->canceled(result); active_goal_handle_ = nullptr; - algo_.cancelNav(); + cancelNav(); return; } if (res.action_finished) { - auto result = std::make_shared(); + auto result = std::make_shared(); result->success = res.action_success; result->message = res.action_message; - - if (res.action_success) { - active_goal_handle_->succeed(result); - } else { - active_goal_handle_->abort(result); - } + if (res.action_success) active_goal_handle_->succeed(result); + else active_goal_handle_->abort(result); active_goal_handle_ = nullptr; } else { - auto fb = std::make_shared(); - fb->distance_to_goal_m = algo_.getDistToGoal(); - fb->cross_track_error_m = algo_.getCrossTrackError(); - fb->state = mission_executive::stateToStr(algo_.getState()); + auto fb = std::make_shared(); + fb->distance_to_goal_m = getDistToGoal(); + fb->cross_track_error_m = getCrossTrackError(); + fb->state = stateToStr(state_); active_goal_handle_->publish_feedback(fb); } @@ -383,24 +714,12 @@ class MissionExecutive : public rclcpp::Node } } - rclcpp_action::GoalResponse handleGoal( - std::shared_ptr) - { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - rclcpp_action::CancelResponse handleCancel(std::shared_ptr) - { - return rclcpp_action::CancelResponse::ACCEPT; - } - void handleAccepted(std::shared_ptr goal_handle) { const auto goal = goal_handle->get_goal(); - std::optional inline_target; - + std::optional inline_target; if (goal->target_id.empty()) { - mission_executive::TargetEntry entry; + TargetEntry entry; entry.id = ""; entry.target_type = goal->target_type; entry.tolerance_m = goal->tolerance_m; @@ -418,7 +737,6 @@ class MissionExecutive : public rclcpp::Node req->lat = goal->lat; req->lon = goal->lon; auto future = latlon_client_->async_send_request(req); - if (future.wait_for(5s) != std::future_status::ready) { auto res = std::make_shared(); res->success = false; @@ -426,7 +744,7 @@ class MissionExecutive : public rclcpp::Node goal_handle->abort(res); return; } - auto resp = future.get(); + const auto resp = future.get(); entry.x_m = resp->x; entry.y_m = resp->y; } else { @@ -438,53 +756,43 @@ class MissionExecutive : public rclcpp::Node { std::lock_guard lk(mutex_); + auto cmd = startNav(goal->target_id, inline_target, goal->is_return); - auto cmd_res = algo_.startNav(goal->target_id, inline_target, goal->is_return); - - if (!cmd_res.accepted) { + if (!cmd.accepted) { auto res = std::make_shared(); res->success = false; - res->message = cmd_res.message; + res->message = cmd.message; goal_handle->abort(res); return; } - if (algo_.hasQueuedGoal() && algo_.getState() == mission_executive::State::ABORTING) { + if (queued_active_ && state_ == State::ABORTING) { if (queued_goal_handle_ && queued_goal_handle_->is_active()) { - auto old_res = std::make_shared(); - old_res->success = false; - old_res->message = "Preempted by newer queued goal"; - queued_goal_handle_->abort(old_res); + auto old = std::make_shared(); + old->success = false; + old->message = "Preempted by newer queued goal"; + queued_goal_handle_->abort(old); } queued_goal_handle_ = goal_handle; return; } - if (cmd_res.preempted_old) { - if (active_goal_handle_ && active_goal_handle_->is_active()) { - auto old_res = std::make_shared(); - old_res->success = false; - old_res->message = "Preempted by new goal"; - active_goal_handle_->abort(old_res); - active_goal_handle_ = nullptr; - } + if (cmd.preempted_old && active_goal_handle_ && active_goal_handle_->is_active()) { + auto old = std::make_shared(); + old->success = false; + old->message = "Preempted by new goal"; + active_goal_handle_->abort(old); + active_goal_handle_ = nullptr; } active_goal_handle_ = goal_handle; - if (cmd_res.publish_goal) { - geometry_msgs::msg::PoseStamped p; - p.header.frame_id = "map"; - p.header.stamp = now(); - p.pose.position.x = cmd_res.goal_to_publish.x; - p.pose.position.y = cmd_res.goal_to_publish.y; - p.pose.orientation.w = 1.0; - goal_pub_->publish(p); + if (cmd.publish_goal) { + publishGoal(cmd.goal_to_publish); publishActiveTarget(); } - - publishNavEnabled(algo_.isNavEnabled()); - publishNavMode(algo_.getNavMode()); + publishNavEnabled(); + publishNavMode(); publishStatus(); } } @@ -493,7 +801,7 @@ class MissionExecutive : public rclcpp::Node const msgs::srv::SetTarget::Request::SharedPtr req, msgs::srv::SetTarget::Response::SharedPtr res) { - mission_executive::TargetEntry entry; + TargetEntry entry; entry.id = req->target_id; entry.target_type = req->target_type; entry.tolerance_m = req->tolerance_m; @@ -514,7 +822,7 @@ class MissionExecutive : public rclcpp::Node res->message = "latlon_to_enu service timeout"; return; } - auto resp = future.get(); + const auto resp = future.get(); entry.x_m = resp->x; entry.y_m = resp->y; } else { @@ -522,22 +830,46 @@ class MissionExecutive : public rclcpp::Node entry.y_m = req->y_m; } - { - std::lock_guard lk(mutex_); - auto cmd_res = algo_.setTarget(entry); - res->success = cmd_res.success; - res->message = cmd_res.message; - } + std::lock_guard lk(mutex_); + auto cmd = setTarget(entry); + res->success = cmd.success; + res->message = cmd.message; } + // ── State ───────────────────────────────────────────────────────────────── + std::mutex mutex_; - mission_executive::MissionExecutiveAlgo algo_; - - std::shared_ptr queued_goal_handle_; - std::shared_ptr active_goal_handle_; + MissionParams params_; + State state_{State::IDLE}; + + std::unordered_map target_registry_; + std::optional active_target_; + bool is_return_{false}; + + bool queued_active_{false}; + TargetEntry queued_entry_; + bool queued_is_return_{false}; + + std::optional robot_pose_; + std::optional> global_path_; + + double imu_angular_vel_{0.0}; + double low_speed_start_s_{0.0}; + bool low_speed_tracking_{false}; + uint8_t last_planner_event_{0}; + + std::vector spiral_waypoints_; + size_t spiral_waypoint_idx_{0}; + double spiral_start_time_s_{0.0}; + bool detection_received_{false}; + + // ── ROS handles ─────────────────────────────────────────────────────────── rclcpp::CallbackGroup::SharedPtr reentrant_group_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + rclcpp::Publisher::SharedPtr goal_pub_; rclcpp::Publisher::SharedPtr nav_enabled_pub_; rclcpp::Publisher::SharedPtr nav_mode_pub_; @@ -550,9 +882,6 @@ class MissionExecutive : public rclcpp::Node rclcpp::Service::SharedPtr set_target_srv_; rclcpp::Service::SharedPtr teleop_srv_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr planner_event_sub_; rclcpp::Subscription::SharedPtr global_path_sub_; @@ -561,6 +890,9 @@ class MissionExecutive : public rclcpp::Node rclcpp::Client::SharedPtr latlon_client_; + std::shared_ptr active_goal_handle_; + std::shared_ptr queued_goal_handle_; + rclcpp::TimerBase::SharedPtr status_timer_; }; diff --git a/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml index 5c3b5be0..976c33b8 100644 --- a/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml @@ -87,6 +87,10 @@ vector_field_planner: repulsion_cutoff_m: 5.0 # obstacles beyond this range are ignored (meters) obstacle_memory_time_s: 3.0 # how long (s) to remember obstacle hits after they leave the scan FoV obstacle_max_points: 2000 # cap on buffered map-frame obstacle points + # ── Approach / velocity-scaled lookahead ──────────────────────────────── + # Lookahead scales from lookahead_dist_m (at rest) to 2x (at max_speed_mps). + # Speed ramps down to min_approach_linear_velocity inside lookahead_dist_m of goal. + min_approach_linear_velocity: 0.2 # floor speed when nearing goal (m/s) — lower than sim for safety # ──────────────────────────────────────────────────────────────────────── goal_tolerance_m: 1.5 # distance to path end that counts as "arrived" (compliant with <3m rules) publish_debug_markers: true diff --git a/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml b/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml index 64ec74c2..21091350 100644 --- a/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml @@ -87,6 +87,10 @@ vector_field_planner: repulsion_cutoff_m: 5.0 # obstacles beyond this range are ignored (meters) obstacle_memory_time_s: 3.0 # how long (s) to remember obstacle hits after they leave the scan FoV obstacle_max_points: 2000 # cap on buffered map-frame obstacle points + # ── Approach / velocity-scaled lookahead ──────────────────────────────── + # Lookahead scales from lookahead_dist_m (at rest) to 2x (at max_speed_mps). + # Speed ramps down to min_approach_linear_velocity inside lookahead_dist_m of goal. + min_approach_linear_velocity: 0.3 # floor speed when nearing goal (m/s) # ──────────────────────────────────────────────────────────────────────── goal_tolerance_m: 1.5 # distance to path end that counts as "arrived" publish_debug_markers: true diff --git a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py index ea3d4974..27c930da 100644 --- a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py +++ b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py @@ -64,8 +64,8 @@ def generate_launch_description(): os.path.join(nav_bringup_dir, 'config', 'nav_params_real.yaml'), "'" ]) - dem_costmap_dir = get_package_share_directory('dem_costmap') - dem_file = os.path.join(dem_costmap_dir, 'maps', 'north_site800m.tif') + athena_map_dir = get_package_share_directory('athena_map') + dem_file = os.path.join(athena_map_dir, 'maps', 'north_site800m.tif') robot_description_content = Command([ PathJoinSubstitution([FindExecutable(name='xacro')]), @@ -117,7 +117,7 @@ def generate_launch_description(): ) dem_costmap_converter_node = Node( - package='dem_costmap', + package='athena_map', executable='map_node', name='dem_costmap_converter', output='screen', @@ -156,15 +156,6 @@ def generate_launch_description(): ) - reframe_pointcloud_node = Node( - package='nav_bringup', - executable='reframe_pointcloud.py', - name='reframe_pointcloud', - output='screen', - parameters=[nav_params_file], - condition=IfCondition(sim) - ) - pointcloud_to_laserscan_node = Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', @@ -185,7 +176,6 @@ def generate_launch_description(): gps_pose_publisher_node, dem_costmap_converter_node, global_planner_node, - reframe_pointcloud_node, pointcloud_to_laserscan_node, vector_field_planner_node, mission_executive_node, diff --git a/src/subsystems/navigation/nav_bringup/package.xml b/src/subsystems/navigation/nav_bringup/package.xml index bf012da3..add1499d 100644 --- a/src/subsystems/navigation/nav_bringup/package.xml +++ b/src/subsystems/navigation/nav_bringup/package.xml @@ -1,7 +1,7 @@ - minimal_nav_bringup + nav_bringup 0.1.0 Top-level bringup package for the navigation stack Maintainer @@ -12,10 +12,14 @@ robot_state_publisher xacro description - athena_planner - localizer athena_sensors athena_gps + gps_pose_publisher + athena_map + global_planner + vector_field_planner + mission_executive + pointcloud_to_laserscan ament_cmake diff --git a/src/subsystems/navigation/vector_field_planner/CMakeLists.txt b/src/subsystems/navigation/vector_field_planner/CMakeLists.txt index ac1f44a7..eb40c6eb 100644 --- a/src/subsystems/navigation/vector_field_planner/CMakeLists.txt +++ b/src/subsystems/navigation/vector_field_planner/CMakeLists.txt @@ -47,6 +47,15 @@ install(DIRECTORY include/ DESTINATION include ) - +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_vector_field_planner_algo + test/test_vector_field_planner_algo.cpp + ) + target_link_libraries(test_vector_field_planner_algo + vector_field_planner_algo + ) +endif() ament_package() diff --git a/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp b/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp index a82af9f4..c5740da7 100644 --- a/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp +++ b/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp @@ -16,7 +16,6 @@ #include #include -#include namespace vector_field_planner { @@ -42,6 +41,12 @@ struct PlannerParams { bool obstacle_avoidance_enabled = false; double repulsion_gain = 0.5; double repulsion_cutoff_m = 3.0; + + // Approach velocity scaling: ramp speed down to min_approach_linear_velocity + // as the robot closes within lookahead_dist_m of the goal. The velocity-scaled + // lookahead and approach window are both derived from lookahead_dist_m and + // max_speed_mps so no additional parameters are needed. + double min_approach_linear_velocity = 0.3; }; struct PlannerResult { @@ -63,6 +68,11 @@ struct PlannerResult { double lateral_right = 0.0; int active_points = 0; double closest_r = -1.0; + + // Debug info for new features + double effective_lookahead_dist = 0.0; // actual lookahead used this tick + bool lookahead_interpolated = false; // true when carrot was interpolated between waypoints + double approach_velocity_scale = 1.0; // fraction applied by approach scaling (1.0 = full speed) }; class VectorFieldPlannerAlgo { @@ -103,22 +113,38 @@ class VectorFieldPlannerAlgo { /* * Computes the velocity and steering commands to follow the path and avoid obstacles. - * Evaluates purely based on current state against the path and obstacles. - * Param: rx - Current robot X position. - * Param: ry - Current robot Y position. - * Param: yaw - Current robot heading (yaw). + * Param: rx - Current robot X position (map frame). + * Param: ry - Current robot Y position (map frame). + * Param: yaw - Current robot heading (yaw, radians). + * Param: current_speed - Current linear speed (m/s), used for velocity-scaled lookahead. * Returns: PlannerResult containing commanded linear/angular velocities and diagnostic info. */ - PlannerResult compute(double rx, double ry, double yaw); + PlannerResult compute(double rx, double ry, double yaw, double current_speed); - // Expose for testing and debugging + // Exposed for testing and debugging size_t findClosestIndex(double rx, double ry) const; - std::pair findLookahead(double rx, double ry, size_t closest_idx) const; + + /* + * Finds the lookahead carrot point at exactly lookahead_dist from the robot by + * linearly interpolating between waypoints. Falls back to the last path point + * when no segment intersection is found (i.e. near goal). + * Returns {x, y, interpolated} where interpolated=true when the point lies between waypoints. + */ + struct LookaheadResult { + double x; + double y; + bool interpolated; + }; + LookaheadResult findLookahead(double rx, double ry, size_t closest_idx, + double lookahead_dist) const; private: PlannerParams params_; std::vector path_; std::vector obstacles_; + + double effectiveLookaheadDist(double current_speed) const; + double approachVelocityScale(double dist_to_goal) const; }; } // namespace vector_field_planner diff --git a/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp index 83825f92..b9282ad6 100644 --- a/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp +++ b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp @@ -20,7 +20,96 @@ namespace vector_field_planner { -PlannerResult VectorFieldPlannerAlgo::compute(double rx, double ry, double yaw) { +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +double VectorFieldPlannerAlgo::effectiveLookaheadDist(double current_speed) const { + // Scale linearly from lookahead_dist_m at rest to 2x at max_speed_mps. + // Derived entirely from existing params — no extra tuning knobs needed. + const double t = std::clamp(std::fabs(current_speed) / params_.max_speed_mps, 0.0, 1.0); + return params_.lookahead_dist_m * (1.0 + t); +} + +double VectorFieldPlannerAlgo::approachVelocityScale(double dist_to_goal) const { + // Ramp begins when goal enters the lookahead window (lookahead_dist_m). + return std::clamp(dist_to_goal / params_.lookahead_dist_m, 0.0, 1.0); +} + +// --------------------------------------------------------------------------- +// Interpolated lookahead carrot +// --------------------------------------------------------------------------- + +VectorFieldPlannerAlgo::LookaheadResult VectorFieldPlannerAlgo::findLookahead( + double rx, double ry, size_t closest_idx, double lookahead_dist) const +{ + if (path_.empty()) { + return {rx, ry, false}; + } + + // Walk segments from closest_idx forward. For each segment [path_[i], path_[i+1]] + // solve the quadratic |path_[i] + t*(path_[i+1]-path_[i]) - robot|^2 = ld^2. + // Take the larger-t root (further along path). If t in [0,1], interpolate and return. + const double ld2 = lookahead_dist * lookahead_dist; + + for (size_t i = closest_idx; i + 1 < path_.size(); ++i) { + const double dx = path_[i + 1].x - path_[i].x; + const double dy = path_[i + 1].y - path_[i].y; + const double fx = path_[i].x - rx; + const double fy = path_[i].y - ry; + + const double a = dx * dx + dy * dy; + if (a < 1e-12) continue; // degenerate segment + + const double b = 2.0 * (fx * dx + fy * dy); + const double c = fx * fx + fy * fy - ld2; + + const double disc = b * b - 4.0 * a * c; + if (disc < 0.0) continue; // circle misses this segment + + // Prefer the forward intersection (larger t) + const double t = (-b + std::sqrt(disc)) / (2.0 * a); + if (t >= 0.0 && t <= 1.0) { + return { + path_[i].x + t * dx, + path_[i].y + t * dy, + true + }; + } + } + + // No segment intersection — robot is within lookahead_dist of the last point, + // or path spacing is coarser than lookahead. Return last point. + return {path_.back().x, path_.back().y, false}; +} + +// --------------------------------------------------------------------------- +// Closest index +// --------------------------------------------------------------------------- + +size_t VectorFieldPlannerAlgo::findClosestIndex(double rx, double ry) const { + size_t best = 0; + double best_dist2 = std::numeric_limits::infinity(); + + for (size_t i = 0; i < path_.size(); ++i) { + const double dx = path_[i].x - rx; + const double dy = path_[i].y - ry; + const double d2 = dx * dx + dy * dy; + if (d2 < best_dist2) { + best_dist2 = d2; + best = i; + } + } + + return best; +} + +// --------------------------------------------------------------------------- +// Main compute +// --------------------------------------------------------------------------- + +PlannerResult VectorFieldPlannerAlgo::compute(double rx, double ry, double yaw, double current_speed) +{ PlannerResult res; res.closest_r = std::numeric_limits::infinity(); @@ -38,34 +127,41 @@ PlannerResult VectorFieldPlannerAlgo::compute(double rx, double ry, double yaw) } res.closest_idx = findClosestIndex(rx, ry); - const auto [lx, ly] = findLookahead(rx, ry, res.closest_idx); - res.lookahead_x = lx; - res.lookahead_y = ly; - const double fwd_dot = (lx - rx) * std::cos(yaw) + (ly - ry) * std::sin(yaw); + // Feature 2: velocity-scaled lookahead + res.effective_lookahead_dist = effectiveLookaheadDist(current_speed); + + // Feature 3: interpolated carrot + const auto lk = findLookahead(rx, ry, res.closest_idx, res.effective_lookahead_dist); + res.lookahead_x = lk.x; + res.lookahead_y = lk.y; + res.lookahead_interpolated = lk.interpolated; + + const double fwd_dot = (lk.x - rx) * std::cos(yaw) + (lk.y - ry) * std::sin(yaw); res.lookahead_behind = (fwd_dot < 0.0); - res.heading_err = std::atan2(ly - ry, lx - rx) - yaw; + res.heading_err = std::atan2(lk.y - ry, lk.x - rx) - yaw; while (res.heading_err > M_PI) res.heading_err -= 2.0 * M_PI; while (res.heading_err < -M_PI) res.heading_err += 2.0 * M_PI; + // Obstacle repulsion if (params_.obstacle_avoidance_enabled && params_.repulsion_gain > 0.0) { for (const auto& p : obstacles_) { const double dx = rx - p.x; const double dy = ry - p.y; const double dist = std::hypot(dx, dy); - + if (dist >= params_.repulsion_cutoff_m || dist < 1e-6) continue; - + res.closest_r = std::min(res.closest_r, dist); - + const double weight = (params_.repulsion_cutoff_m - dist) / params_.repulsion_cutoff_m; // Project unit repulsion vector onto robot's left lateral axis const double lateral = (dx / dist) * (-std::sin(yaw)) + (dy / dist) * std::cos(yaw); res.lateral_sum += lateral * weight; - + ++res.active_points; - + if (lateral > 0.0) { res.lateral_left += lateral * weight; } else { @@ -80,7 +176,12 @@ PlannerResult VectorFieldPlannerAlgo::compute(double rx, double ry, double yaw) -params_.max_steering_angle_rad, params_.max_steering_angle_rad); res.clamped = std::abs(res.steering_unclamped) > params_.max_steering_angle_rad; - res.linear_vel = params_.max_speed_mps; + + // Feature 1: approach velocity scaling + res.approach_velocity_scale = approachVelocityScale(dist_to_goal); + const double approach_vel = params_.min_approach_linear_velocity + + res.approach_velocity_scale * (params_.max_speed_mps - params_.min_approach_linear_velocity); + res.linear_vel = std::min(params_.max_speed_mps, approach_vel); if (std::isinf(res.closest_r)) { res.closest_r = -1.0; @@ -89,65 +190,4 @@ PlannerResult VectorFieldPlannerAlgo::compute(double rx, double ry, double yaw) return res; } -size_t VectorFieldPlannerAlgo::findClosestIndex(double rx, double ry) const { - size_t best = 0; - double best_dist2 = std::numeric_limits::infinity(); - - for (size_t i = 0; i < path_.size(); ++i) { - const double dx = path_[i].x - rx; - const double dy = path_[i].y - ry; - const double d2 = dx * dx + dy * dy; - if (d2 < best_dist2) { - best_dist2 = d2; - best = i; - } - } - - return best; -} - -std::pair VectorFieldPlannerAlgo::findLookahead( - double rx, double ry, size_t closest_idx) const { - if (path_.empty()) { - return {rx, ry}; - } - - for (size_t i = closest_idx; i < path_.size(); ++i) { - const double dx = path_[i].x - rx; - const double dy = path_[i].y - ry; - if (std::hypot(dx, dy) >= params_.lookahead_dist_m) { - // Fix for "lookahead behind robot" bug: - // When the path spacing is larger than lookahead_dist_m, the closest point - // may be behind the robot (e.g. pose 0 that it already passed). - // We check if this point `i` is behind the robot relative to the path segment `[i, i+1]`. - // If it is, and we have a next point, we skip it and look ahead. - if (i + 1 < path_.size()) { - const double path_dx = path_[i+1].x - path_[i].x; - const double path_dy = path_[i+1].y - path_[i].y; - - // Vector from robot to point `i` - const double to_pt_dx = path_[i].x - rx; - const double to_pt_dy = path_[i].y - ry; - - // Dot product of (robot -> point i) and (path direction at point i) - const double dot = to_pt_dx * path_dx + to_pt_dy * path_dy; - - // If dot < 0, the point is behind the robot along the path direction. - // We only do this check for `i == closest_idx` to prevent skipping points - // that naturally turn backwards, but we definitely want to skip the closest point - // if we just passed it. - if (dot < 0.0 && i == closest_idx) { - continue; // Skip this point, check the next one - } - } - - return {path_[i].x, path_[i].y}; - } - } - - // If no point is far enough ahead (e.g. nearing goal), return the goal. - const auto& last = path_.back(); - return {last.x, last.y}; -} - -} // namespace vector_field_planner \ No newline at end of file +} // namespace vector_field_planner diff --git a/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp index f2c2e675..56a70ff8 100644 --- a/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp +++ b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp @@ -65,6 +65,7 @@ class VectorFieldPlanner : public rclcpp::Node declare_parameter("scan_max_age_s", 0.5); declare_parameter("goal_tolerance_m", 1.5); declare_parameter("publish_debug_markers", true); + declare_parameter("min_approach_linear_velocity", 0.3); map_frame_ = get_parameter("map_frame").as_string(); base_frame_ = get_parameter("base_frame").as_string(); @@ -80,7 +81,8 @@ class VectorFieldPlanner : public rclcpp::Node p.repulsion_cutoff_m = get_parameter("repulsion_cutoff_m").as_double(); p.obstacle_avoidance_enabled = get_parameter("obstacle_avoidance_enabled").as_bool(); p.goal_tolerance_m = get_parameter("goal_tolerance_m").as_double(); - + p.min_approach_linear_velocity = get_parameter("min_approach_linear_velocity").as_double(); + algo_.setParams(p); obstacle_memory_time_s_ = get_parameter("obstacle_memory_time_s").as_double(); @@ -149,6 +151,7 @@ class VectorFieldPlanner : public rclcpp::Node tick_count_ = 0; consecutive_clamped_ = 0; stuck_ticks_ = 0; + last_cmd_linear_vel_ = 0.0; obstacle_map_.clear(); publishStop(); } else if (!was_enabled && nav_enabled_) { @@ -332,7 +335,7 @@ class VectorFieldPlanner : public rclcpp::Node } algo_.updateObstacles(algo_obstacles); - const auto res = algo_.compute(rx, ry, yaw); + const auto res = algo_.compute(rx, ry, yaw, last_cmd_linear_vel_); if (res.goal_reached) { const auto & goal_pos = path_->poses.back().pose.position; @@ -405,11 +408,15 @@ class VectorFieldPlanner : public rclcpp::Node RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 250, "[NAV t=%u pos=(%.2f,%.2f) yaw=%.1fd goal=%.2fm idx=%zu/%zu " - "lk=(%.2f,%.2f) lk_dist=%.2fm err=%.1fd rep=%.3f steer=%.3f%s%s%s]", + "lk=(%.2f,%.2f) lk_dist=%.2fm(eff=%.2fm%s) err=%.1fd " + "approach_scale=%.2f lin=%.3f steer=%.3f%s%s%s]", tick_count_, rx, ry, yaw * 180.0 / M_PI, dist_to_goal, res.closest_idx, path_->poses.size() - 1, res.lookahead_x, res.lookahead_y, lookahead_dist, - res.heading_err * 180.0 / M_PI, res.repulsion_steering, res.angular_vel, + res.effective_lookahead_dist, + res.lookahead_interpolated ? "/interp" : "/snap", + res.heading_err * 180.0 / M_PI, + res.approach_velocity_scale, res.linear_vel, res.angular_vel, res.clamped ? " CLAMPED" : "", res.lookahead_behind ? " LK_BEHIND" : "", (p.obstacle_avoidance_enabled && p.repulsion_gain > 0.0) ? " AVOID_ON" : ""); @@ -420,6 +427,7 @@ class VectorFieldPlanner : public rclcpp::Node cmd.twist.linear.x = res.linear_vel; cmd.twist.angular.z = res.angular_vel; cmd_pub_->publish(cmd); + last_cmd_linear_vel_ = res.linear_vel; if (publish_debug_markers_) { publishDebugMarkers(rx, ry, res.lookahead_x, res.lookahead_y); @@ -502,6 +510,7 @@ class VectorFieldPlanner : public rclcpp::Node unsigned int consecutive_clamped_{0}; unsigned int stuck_ticks_{0}; unsigned int scan_msg_count_{0}; + double last_cmd_linear_vel_{0.0}; std::vector obstacle_map_; vector_field_planner::VectorFieldPlannerAlgo algo_; }; diff --git a/src/subsystems/navigation/vector_field_planner/test/test_vector_field_planner_algo.cpp b/src/subsystems/navigation/vector_field_planner/test/test_vector_field_planner_algo.cpp new file mode 100644 index 00000000..2646e9ee --- /dev/null +++ b/src/subsystems/navigation/vector_field_planner/test/test_vector_field_planner_algo.cpp @@ -0,0 +1,264 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 "vector_field_planner/vector_field_planner_algo.hpp" + +using namespace vector_field_planner; + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +static VectorFieldPlannerAlgo makeAlgo(PlannerParams p = {}) +{ + VectorFieldPlannerAlgo algo; + algo.setParams(p); + return algo; +} + +// Straight path along +X from 0 to length_m with step_m spacing +static std::vector straightPath(double length_m, double step_m = 1.0) +{ + std::vector path; + for (double x = 0.0; x <= length_m + 1e-9; x += step_m) { + path.push_back({x, 0.0}); + } + return path; +} + +// --------------------------------------------------------------------------- +// Feature 1: Approach velocity scaling +// Scaling window == lookahead_dist_m (no separate param) +// --------------------------------------------------------------------------- + +TEST(ApproachVelocityScaling, FullSpeedFarFromGoal) +{ + PlannerParams p; + p.max_speed_mps = 2.0; + p.lookahead_dist_m = 3.0; + p.min_approach_linear_velocity = 0.5; + p.goal_tolerance_m = 0.5; + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); // goal at x=20, robot at origin + + // dist_to_goal=20 >> lookahead_dist_m=3 → scale clamped to 1.0 + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_DOUBLE_EQ(res.approach_velocity_scale, 1.0); + EXPECT_NEAR(res.linear_vel, p.max_speed_mps, 1e-9); +} + +TEST(ApproachVelocityScaling, ReducedSpeedInsideWindow) +{ + PlannerParams p; + p.max_speed_mps = 2.0; + p.lookahead_dist_m = 4.0; // scaling window == 4 m + p.min_approach_linear_velocity = 0.5; + p.goal_tolerance_m = 0.2; + + auto algo = makeAlgo(p); + // goal at x=2, robot at x=0 → dist_to_goal=2, lookahead_dist_m=4 → scale=0.5 + algo.setPath({{0.0, 0.0}, {2.0, 0.0}, {2.0, 0.0}}); + + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_NEAR(res.approach_velocity_scale, 0.5, 1e-6); + // vel = min + scale*(max-min) = 0.5 + 0.5*1.5 = 1.25 + EXPECT_NEAR(res.linear_vel, 1.25, 1e-6); +} + +TEST(ApproachVelocityScaling, NeverBelowMinSpeed) +{ + PlannerParams p; + p.max_speed_mps = 2.0; + p.lookahead_dist_m = 10.0; // large window → robot well inside it + p.min_approach_linear_velocity = 0.5; + p.goal_tolerance_m = 0.05; + + auto algo = makeAlgo(p); + // goal at x=0.3 — very close, scale → 0.03, floor must hold + algo.setPath({{0.0, 0.0}, {0.2, 0.0}, {0.3, 0.0}}); + + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_GE(res.linear_vel, p.min_approach_linear_velocity - 1e-9); +} + +// --------------------------------------------------------------------------- +// Feature 2: Velocity-scaled lookahead +// Always on: lookahead = lookahead_dist_m * (1 + speed/max_speed_mps) +// --------------------------------------------------------------------------- + +TEST(VelocityScaledLookahead, ZeroSpeedUsesBaseDist) +{ + PlannerParams p; + p.lookahead_dist_m = 3.0; + p.max_speed_mps = 1.5; + p.goal_tolerance_m = 0.5; + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + // speed=0 → scale=0 → lookahead = 3.0 * 1.0 = 3.0 + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_NEAR(res.effective_lookahead_dist, 3.0, 1e-9); +} + +TEST(VelocityScaledLookahead, MaxSpeedDoublesDist) +{ + PlannerParams p; + p.lookahead_dist_m = 3.0; + p.max_speed_mps = 1.5; + p.goal_tolerance_m = 0.5; + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + // speed=max → scale=1 → lookahead = 3.0 * 2.0 = 6.0 + auto res = algo.compute(0.0, 0.0, 0.0, p.max_speed_mps); + EXPECT_NEAR(res.effective_lookahead_dist, 6.0, 1e-9); +} + +TEST(VelocityScaledLookahead, HalfSpeedIsIntermediate) +{ + PlannerParams p; + p.lookahead_dist_m = 4.0; + p.max_speed_mps = 2.0; + p.goal_tolerance_m = 0.5; + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + // speed=1.0 (half of max) → lookahead = 4.0 * 1.5 = 6.0 + auto res = algo.compute(0.0, 0.0, 0.0, 1.0); + EXPECT_NEAR(res.effective_lookahead_dist, 6.0, 1e-9); +} + +TEST(VelocityScaledLookahead, ClampedAboveMaxSpeed) +{ + PlannerParams p; + p.lookahead_dist_m = 3.0; + p.max_speed_mps = 1.5; + p.goal_tolerance_m = 0.5; + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + // speed beyond max → clamped to 2x + auto res = algo.compute(0.0, 0.0, 0.0, 10.0); + EXPECT_NEAR(res.effective_lookahead_dist, 6.0, 1e-9); +} + +// --------------------------------------------------------------------------- +// Feature 3: Interpolated carrot +// --------------------------------------------------------------------------- + +TEST(InterpolatedCarrot, CoarsePathInterpolated) +{ + PlannerParams p; + p.lookahead_dist_m = 2.5; + p.max_speed_mps = 1.5; + p.goal_tolerance_m = 0.5; + + auto algo = makeAlgo(p); + // Waypoints 5 m apart — carrot must land between them + algo.setPath({{0.0, 0.0}, {5.0, 0.0}, {10.0, 0.0}}); + + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_TRUE(res.lookahead_interpolated); + // Carrot is at exactly effective_lookahead_dist from robot + const double actual_dist = std::hypot(res.lookahead_x, res.lookahead_y); + EXPECT_NEAR(actual_dist, res.effective_lookahead_dist, 1e-6); +} + +TEST(InterpolatedCarrot, NearGoalFallsBackToLastPoint) +{ + PlannerParams p; + p.lookahead_dist_m = 5.0; + p.max_speed_mps = 1.5; + p.goal_tolerance_m = 0.2; + + auto algo = makeAlgo(p); + algo.setPath({{0.0, 0.0}, {1.0, 0.0}, {2.0, 0.0}}); + + // Robot at x=1, only 1 m to goal, lookahead > remaining path → snap to last + auto res = algo.compute(1.0, 0.0, 0.0, 0.0); + EXPECT_FALSE(res.lookahead_interpolated); + EXPECT_NEAR(res.lookahead_x, 2.0, 1e-6); + EXPECT_NEAR(res.lookahead_y, 0.0, 1e-6); +} + +TEST(InterpolatedCarrot, OffAxisPathInterpolation) +{ + PlannerParams p; + p.lookahead_dist_m = 2.0; + p.max_speed_mps = 1.5; + p.goal_tolerance_m = 0.5; + + auto algo = makeAlgo(p); + const double s = 1.0 / std::sqrt(2.0); + algo.setPath({{0.0, 0.0}, {5.0 * s, 5.0 * s}, {10.0 * s, 10.0 * s}}); + + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_TRUE(res.lookahead_interpolated); + const double actual_dist = std::hypot(res.lookahead_x, res.lookahead_y); + EXPECT_NEAR(actual_dist, res.effective_lookahead_dist, 1e-6); +} + +// --------------------------------------------------------------------------- +// Regression: existing behaviour preserved +// --------------------------------------------------------------------------- + +TEST(Regression, GoalReachedWithinTolerance) +{ + PlannerParams p; + p.goal_tolerance_m = 1.0; + + auto algo = makeAlgo(p); + algo.setPath({{0.0, 0.0}, {1.0, 0.0}}); + + auto res = algo.compute(1.0, 0.0, 0.0, 0.0); + EXPECT_TRUE(res.goal_reached); + EXPECT_DOUBLE_EQ(res.linear_vel, 0.0); + EXPECT_DOUBLE_EQ(res.angular_vel, 0.0); +} + +TEST(Regression, EmptyPathReturnsZero) +{ + auto algo = makeAlgo(); + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_FALSE(res.goal_reached); + EXPECT_DOUBLE_EQ(res.linear_vel, 0.0); + EXPECT_DOUBLE_EQ(res.angular_vel, 0.0); +} + +TEST(Regression, StraightAheadZeroSteering) +{ + PlannerParams p; + p.lookahead_dist_m = 3.0; + p.k_p_steering = 1.5; + p.goal_tolerance_m = 0.5; + p.max_speed_mps = 1.5; + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); // goal at x=20, well outside approach window + + // Robot facing +X along path — heading error and steering should be ~0 + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_NEAR(res.heading_err, 0.0, 1e-6); + EXPECT_NEAR(res.angular_vel, 0.0, 1e-6); + EXPECT_NEAR(res.linear_vel, p.max_speed_mps, 1e-6); +} From f3e1664cae61d22a1e7570702bc3ccad32b8f840 Mon Sep 17 00:00:00 2001 From: UMDLoop Jetson Orin NX Date: Sat, 11 Apr 2026 13:31:12 -0400 Subject: [PATCH 10/21] rover updates --- .../drive_controllers/src/rear_ackermann_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp b/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp index fb21c936..35406982 100644 --- a/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp +++ b/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp @@ -212,13 +212,14 @@ controller_interface::return_type RearAckermannController::update( const double rad_s_to_rpm = 60.0 / (2.0 * M_PI); RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 500, - "Wheel speeds [RPM] - FL: %.2f, FR: %.2f, RL: %.2f, RR: %.2f | Steer [rad] - RL: %.3f, RR: %.3f", + "Wheel speeds [RPM] - FL: %.2f, FR: %.2f, RL: %.2f, RR: %.2f | Steer [rad] - RL: %.3f, RR: %.3f \n%.2f", fl_wheel_ang_vel * rad_s_to_rpm, fr_wheel_ang_vel * rad_s_to_rpm, rl_wheel_ang_vel * rad_s_to_rpm, rr_wheel_ang_vel * rad_s_to_rpm, rear_left_angle_clamped, - rear_right_angle_clamped); + rear_right_angle_clamped, + turn_radius); return controller_interface::return_type::OK; } From 242f7c19a73a5c73b87ff945e5d625223df5cbf3 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sat, 11 Apr 2026 14:48:09 -0400 Subject: [PATCH 11/21] added mag heading --- .../navigation/mag_heading/mag_heading/mag_heading_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/subsystems/navigation/mag_heading/mag_heading/mag_heading_node.py b/src/subsystems/navigation/mag_heading/mag_heading/mag_heading_node.py index 05daa8b4..3efc692a 100644 --- a/src/subsystems/navigation/mag_heading/mag_heading/mag_heading_node.py +++ b/src/subsystems/navigation/mag_heading/mag_heading/mag_heading_node.py @@ -19,6 +19,7 @@ def _wrap_pi(a: float) -> float: return (a + math.pi) % (2.0 * math.pi) - math.pi + class MagHeadingNode(Node): def __init__(self): super().__init__('mag_heading_node') From 13e3b2f9739ca775c8875be5c68a3e9bbca9fa19 Mon Sep 17 00:00:00 2001 From: UMDLoop Jetson Orin NX Date: Sat, 11 Apr 2026 15:52:01 -0400 Subject: [PATCH 12/21] added parameters --- .../navigation/mag_heading/config/mag_heading_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/subsystems/navigation/mag_heading/config/mag_heading_params.yaml b/src/subsystems/navigation/mag_heading/config/mag_heading_params.yaml index 62a4f79e..cc82b926 100644 --- a/src/subsystems/navigation/mag_heading/config/mag_heading_params.yaml +++ b/src/subsystems/navigation/mag_heading/config/mag_heading_params.yaml @@ -10,7 +10,7 @@ mag_heading_node: # hard_iron: [x, y, z] bias in Tesla subtracted from raw reading - hard_iron: [-0.00000004, -0.00000004, -0.00000075] + hard_iron: [-0.00000004, -0.00000004, 0.00000075] # soft_iron: row-major 3x3 correction matrix (9 values) # [ r00 r01 r02 r10 r11 r12 r20 r21 r22 ] From 836f5900d8cf7b6133cf2d20cebe1b2ba439163e Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sat, 11 Apr 2026 15:43:14 -0400 Subject: [PATCH 13/21] declination --- .../navigation/mag_heading/mag_heading/mag_heading_node.py | 1 - src/subsystems/navigation/mag_heading/package.xml | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/subsystems/navigation/mag_heading/mag_heading/mag_heading_node.py b/src/subsystems/navigation/mag_heading/mag_heading/mag_heading_node.py index 3efc692a..05daa8b4 100644 --- a/src/subsystems/navigation/mag_heading/mag_heading/mag_heading_node.py +++ b/src/subsystems/navigation/mag_heading/mag_heading/mag_heading_node.py @@ -19,7 +19,6 @@ def _wrap_pi(a: float) -> float: return (a + math.pi) % (2.0 * math.pi) - math.pi - class MagHeadingNode(Node): def __init__(self): super().__init__('mag_heading_node') diff --git a/src/subsystems/navigation/mag_heading/package.xml b/src/subsystems/navigation/mag_heading/package.xml index 8dae251f..c9696c7c 100644 --- a/src/subsystems/navigation/mag_heading/package.xml +++ b/src/subsystems/navigation/mag_heading/package.xml @@ -11,6 +11,7 @@ msgs sensor_msgs std_msgs + python3-pygeomag ament_copyright ament_flake8 From f04b3e4a6165e31334e0ba522ca0f7bc50132f5b Mon Sep 17 00:00:00 2001 From: UMDLoop Jetson Orin NX Date: Sat, 11 Apr 2026 16:02:44 -0400 Subject: [PATCH 14/21] mag_heading --- .../navigation/mag_heading/config/mag_heading_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/subsystems/navigation/mag_heading/config/mag_heading_params.yaml b/src/subsystems/navigation/mag_heading/config/mag_heading_params.yaml index cc82b926..62a4f79e 100644 --- a/src/subsystems/navigation/mag_heading/config/mag_heading_params.yaml +++ b/src/subsystems/navigation/mag_heading/config/mag_heading_params.yaml @@ -10,7 +10,7 @@ mag_heading_node: # hard_iron: [x, y, z] bias in Tesla subtracted from raw reading - hard_iron: [-0.00000004, -0.00000004, 0.00000075] + hard_iron: [-0.00000004, -0.00000004, -0.00000075] # soft_iron: row-major 3x3 correction matrix (9 values) # [ r00 r01 r02 r10 r11 r12 r20 r21 r22 ] From 1f728433347fbd978a7c938ac4b19393f1cc0761 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Thu, 26 Mar 2026 10:59:36 -0400 Subject: [PATCH 15/21] building --- .claude/settings.local.json | 11 + src/msgs/CMakeLists.txt | 4 + src/simulation/scripts/heading_publisher.py | 23 + src/subsystems/minimal_navigation/PLAN.md | 508 ++++ .../ackermann_mppi/CMakeLists.txt | 121 + .../include/ackermann_mppi/critic_data.hpp | 85 + .../ackermann_mppi/critic_function.hpp | 120 + .../critics/constraint_critic.hpp | 46 + .../ackermann_mppi/critics/goal_critic.hpp | 39 + .../critics/obstacles_critic.hpp | 61 + .../critics/path_align_critic.hpp | 43 + .../critics/path_follow_critic.hpp | 40 + .../ackermann_mppi/models/constraints.hpp | 47 + .../models/control_sequence.hpp | 50 + .../models/optimizer_settings.hpp | 47 + .../include/ackermann_mppi/models/path.hpp | 46 + .../include/ackermann_mppi/models/state.hpp | 55 + .../ackermann_mppi/models/trajectories.hpp | 46 + .../include/ackermann_mppi/motion_models.hpp | 133 + .../include/ackermann_mppi/optimizer.hpp | 238 ++ .../ackermann_mppi/tools/noise_generator.hpp | 97 + .../include/ackermann_mppi/tools/utils.hpp | 387 +++ .../ackermann_mppi/package.xml | 36 + .../src/ackermann_mppi_node.cpp | 301 ++ .../src/critics/constraint_critic.cpp | 65 + .../src/critics/goal_critic.cpp | 55 + .../src/critics/obstacles_critic.cpp | 214 ++ .../src/critics/path_align_critic.cpp | 141 + .../src/critics/path_follow_critic.cpp | 69 + .../ackermann_mppi/src/noise_generator.cpp | 103 + .../ackermann_mppi/src/optimizer.cpp | 486 ++++ .../athena_smac_planner/CMakeLists.txt | 114 + .../include/athena_smac_planner/a_star.hpp | 323 +++ .../analytic_expansion.hpp | 200 ++ .../athena_smac_planner/collision_checker.hpp | 139 + .../include/athena_smac_planner/constants.hpp | 105 + .../costmap_downsampler.hpp | 99 + .../distance_heuristic.hpp | 77 + .../athena_smac_planner/goal_manager.hpp | 289 ++ .../athena_smac_planner/node_basic.hpp | 69 + .../athena_smac_planner/node_hybrid.hpp | 386 +++ .../obstacle_heuristic.hpp | 96 + .../smac_planner_hybrid.hpp | 111 + .../include/athena_smac_planner/smoother.hpp | 208 ++ .../thirdparty/robin_hood.h | 2539 +++++++++++++++++ .../include/athena_smac_planner/types.hpp | 257 ++ .../include/athena_smac_planner/utils.hpp | 231 ++ .../athena_smac_planner/package.xml | 31 + .../athena_smac_planner/src/a_star.cpp | 526 ++++ .../src/analytic_expansion.cpp | 449 +++ .../src/collision_checker.cpp | 196 ++ .../src/costmap_downsampler.cpp | 127 + .../src/distance_heuristic.cpp | 157 + .../src/global_planner_node.cpp | 159 ++ .../athena_smac_planner/src/node_basic.cpp | 54 + .../athena_smac_planner/src/node_hybrid.cpp | 540 ++++ .../src/obstacle_heuristic.cpp | 230 ++ .../src/smac_planner_hybrid.cpp | 478 ++++ .../athena_smac_planner/src/smoother.cpp | 436 +++ .../dem_costmap/CMakeLists.txt | 65 + .../minimal_navigation/dem_costmap/LICENSE | 202 ++ .../dem_costmap/config/dem_costmap.yaml | 8 + .../dem_costmap/launch/dem_costmap.launch.py | 30 + .../dem_costmap/maps/north_site800m.tif | 3 + .../dem_costmap/package.xml | 22 + .../dem_costmap/src/map_node.cpp | 236 ++ .../gps_pose_publisher/CMakeLists.txt | 32 + .../gps_pose_publisher/package.xml | 24 + .../src/gps_pose_publisher_node.cpp | 192 ++ .../minimal_nav_bringup/CMakeLists.txt | 10 + .../config/nav_params.yaml | 92 + .../minimal_nav_bringup/launch/nav.launch.py | 147 + .../minimal_nav_bringup/package.xml | 23 + .../mission_executive/CMakeLists.txt | 33 + .../mission_executive/package.xml | 23 + .../src/mission_executive_node.cpp | 784 +++++ src/subsystems/minimal_navigation/todo.md | 69 + 77 files changed, 14338 insertions(+) create mode 100644 .claude/settings.local.json create mode 100644 src/subsystems/minimal_navigation/PLAN.md create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/package.xml create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp create mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/package.xml create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp create mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp create mode 100644 src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/dem_costmap/LICENSE create mode 100644 src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml create mode 100644 src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py create mode 100644 src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif create mode 100644 src/subsystems/minimal_navigation/dem_costmap/package.xml create mode 100644 src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp create mode 100644 src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/gps_pose_publisher/package.xml create mode 100644 src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp create mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml create mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py create mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml create mode 100644 src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt create mode 100644 src/subsystems/minimal_navigation/mission_executive/package.xml create mode 100644 src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp create mode 100644 src/subsystems/minimal_navigation/todo.md diff --git a/.claude/settings.local.json b/.claude/settings.local.json new file mode 100644 index 00000000..4fe02daa --- /dev/null +++ b/.claude/settings.local.json @@ -0,0 +1,11 @@ +{ + "permissions": { + "allow": [ + "Bash(find:*)", + "Bash(git rm:*)", + "Bash(git checkout:*)", + "Bash(git -C /Users/mdurrani/Development/umdloop/ros2-devenv/main/athena-code diff main...HEAD)", + "Bash(xargs sed:*)" + ] + } +} diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 8d6c9814..6d117f96 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -23,6 +23,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ActiveTarget.msg" "msg/NavStatus.msg" "msg/Heading.msg" + "msg/PlannerEvent.msg" + "msg/ActiveTarget.msg" + "msg/NavStatus.msg" + "msg/Heading.msg" "srv/SetController.srv" "srv/LatLonToENU.srv" "srv/SetTarget.srv" diff --git a/src/simulation/scripts/heading_publisher.py b/src/simulation/scripts/heading_publisher.py index 292c366a..60d813a1 100755 --- a/src/simulation/scripts/heading_publisher.py +++ b/src/simulation/scripts/heading_publisher.py @@ -4,7 +4,11 @@ import rclpy from rclpy.node import Node +<<<<<<< HEAD from rclpy.qos import QoSPresetProfiles +======= +from rclpy.qos import SensorDataQoS +>>>>>>> f33dc56 (building) from sensor_msgs.msg import MagneticField from msgs.msg import Heading @@ -22,25 +26,37 @@ def __init__(self): self.heading_acc_ = self.get_parameter('heading_acc').get_parameter_value().double_value self.pub_ = self.create_publisher(Heading, heading_topic, 10) +<<<<<<< HEAD # Always cache the first heading to start at 0 degrees self.initial_heading_enu = None +======= +>>>>>>> f33dc56 (building) self.sub_ = self.create_subscription( MagneticField, mag_topic, self.on_mag, +<<<<<<< HEAD QoSPresetProfiles.SENSOR_DATA.value, ) self.get_logger().info( f'Heading publisher: {mag_topic} -> {heading_topic} (Force Initial Offset: Enabled)' +======= + SensorDataQoS(), + ) + + self.get_logger().info( + f'Heading publisher: {mag_topic} -> {heading_topic}' +>>>>>>> f33dc56 (building) ) def on_mag(self, msg: MagneticField): bx = msg.magnetic_field.x by = msg.magnetic_field.y +<<<<<<< HEAD # In Gazebo's magnetometer the field is reported in the robot body frame # with the simulated Earth field pointing North (+Y world). For a robot # with x=forward, y=left this gives: @@ -62,6 +78,13 @@ def on_mag(self, msg: MagneticField): # Compass bearing: degrees clockwise from North # North is 90 deg ENU. compass_bearing = (90.0 - heading_enu + 360.0) % 360.0 +======= + # compass bearing: degrees clockwise from north + compass_bearing = (math.degrees(math.atan2(-by, bx)) + 360.0) % 360.0 + + # ENU heading: degrees CCW from east + heading_enu = (90.0 - compass_bearing + 360.0) % 360.0 +>>>>>>> f33dc56 (building) out = Heading() out.header = msg.header diff --git a/src/subsystems/minimal_navigation/PLAN.md b/src/subsystems/minimal_navigation/PLAN.md new file mode 100644 index 00000000..09d8b107 --- /dev/null +++ b/src/subsystems/minimal_navigation/PLAN.md @@ -0,0 +1,508 @@ +# Athena Navigation Stack — Implementation Spec + +A GPS-only, Nav2-free navigation system in a single ROS 2 package. No IMU-GNSS fusion — localization is GPS + compass heading only. + + +--- + +## 1. Architecture Overview + +Four nodes form the runtime pipeline: + +``` +gps_pose_publisher → mission_executive → global_planner → mppi_runner + │ │ │ │ + map→base_link TF state machine Hybrid-A* MPPI control + /robot_pose /goal_pose /global_path /cmd_vel +``` + +**TF tree:** `map → base_link` (no `odom` frame — see §1.2). + +### 1.1 Existing Code + +| Source | Role | Required Changes | +|--------|------|------------------| +| `ackermann_mppi/` | Local planner / path follower | Rename `/plan` → `/global_path`; add `/nav_enabled` guard; add costmap debug publisher | +| `smac/` | Global planner (Hybrid-A\*) | Rename package to `athena_smac_planner`; integrate `athena_map` costmap input | +| `athena_map/` | Publishes `nav_msgs/OccupancyGrid` on `/map` | Ensure `frame_id: "map"`, `transient_local` QoS, and resolution matches `global_costmap` (0.5 m). We control this node — adapt as needed for clean integration. | +| `athena_planner/` | BT nodes (ArUco pose extraction, etc.) | Reuse in Phase 2 | +| `aruco_detection/` | ArUco refinement (~80% complete) | Wire in Phase 2c | + +**Do not use `localizer/`** — IMU-GNSS fusion is explicitly excluded from the localization path. + +### 1.2 TF Convention — Intentional `odom` Omission + +REP-105 expects `map → odom → base_link`. This stack uses `map → base_link` directly because there is no fused odometry source suitable for the `odom` frame. `gps_pose_publisher` broadcasts this transform. + +The ZED SDK provides `/odom` (`nav_msgs/Odometry`) with visual-inertial velocity estimates. This is used **only** for measured velocity in stop detection (§3.2.4) — it does not participate in localization or TF. + +### 1.3 Package Rename: `smac/` → `athena_smac_planner` + +The vendored `smac/` package declared `nav2_smac_planner` in `package.xml`, colliding with the system-installed `ros-$ROS_DISTRO-nav2-smac-planner`. Fix: rename to `athena_smac_planner` in both `package.xml` and `CMakeLists.txt`. + +--- + +## 2. Operator Interface + +All operator commands use ROS 2 services and actions (the legacy `OperatorCmd.msg` topic is retired). + +| Command | Interface | Type | Rationale | +|---------|-----------|------|-----------| +| `GO_TO` | Action | `NavigateToTarget` | Long-running; preemptable; provides distance feedback | +| `RETURN` | Action | `NavigateToTarget` | Same semantics as `GO_TO` (with `is_return=true`) | +| `ABORT` | Service | `std_srvs/Trigger` | Instant; needs confirmation | +| `SET_TARGET` | Service | `SetTarget` | Pre-register a target by ID for later navigation | +| `TELEOP_ON` / `TELEOP_OFF` | Service | `std_srvs/SetBool` | Needs ack; returns success + optional message | + +**Intended workflow:** Use `SetTarget` to pre-load named targets (with coordinates, type, and tolerance) into `mission_executive`'s target registry. Then use `NavigateToTarget` with just a `target_id` (+ `is_return` flag) to begin navigation. `NavigateToTarget` also accepts inline coordinates for ad-hoc goals that don't need pre-registration — when `target_id` is empty, the coordinates in the goal message are used directly. + +All custom types live in the `navigation_msgs` package (see §6). + +--- + +## 3. Node Specifications + +### 3.1 `gps_pose_publisher` + +Converts raw GPS + heading into a `map`-frame pose and broadcasts `map → base_link`. + +**Subscriptions:** + +| Topic | Type | Notes | +|-------|------|-------| +| `/gps/fix` | `sensor_msgs/NavSatFix` | | +| `/gps/heading` (configurable) | `std_msgs/Float64` | Input always in degrees. Internally converted to ENU radians (0 = East, CCW+). | + +**Publications:** + +| Topic | Type | Notes | +|-------|------|-------| +| `/robot_pose` | `geometry_msgs/PoseStamped` | Frame: `map` | +| `/gps_status` | `std_msgs/String` | Fix quality + heading source each update | +| TF: `map → base_link` | | | + +**Services:** + +| Service | Type | Purpose | +|---------|------|---------| +| `~/latlon_to_enu` | `LatLonToENU` | Centralizes WGS84 → ENU projection | + +**Core logic:** + +- WGS84 → ENU via `GeographicLib::LocalCartesian`: + ```cpp + GeographicLib::LocalCartesian proj(lat0, lon0, alt0); + proj.Forward(lat, lon, alt, e, n, u); + ``` +- Origin set from first fix, or from `/start_gate_ref` if `use_start_gate_ref: true`. +- Heading conversion: input degrees → ENU radians, then `tf2::Quaternion::setRPY(0, 0, heading_rad)`. + +--- + +### 3.2 `mission_executive` + +Operator-driven state machine managing target sequencing, abort/return, mode switching, and arrival detection. + +#### 3.2.1 States + +`IDLE`, `NAVIGATING`, `ARRIVING`, `STOPPED_AT_TARGET`, `ABORTING`, `RETURNING`, `STOPPED_AT_RETURN`, `TELEOP`. + +`TELEOP_ON` from any state → `TELEOP`. `TELEOP_OFF` → `IDLE` (never auto-resumes navigation). + +#### 3.2.2 Transition Table + +| State | GO_TO | ABORT | RETURN | `PLAN_FAILED` | TELEOP_ON | TELEOP_OFF | +|-------|-------|-------|--------|---------------|-----------|------------| +| `IDLE` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | +| `NAVIGATING` | → `NAVIGATING` (preempt) | → `ABORTING` | ignore | → `ABORTING` | → `TELEOP` | ignore | +| `ARRIVING` | → `NAVIGATING` (cancel) | → `ABORTING` | ignore | ignore | → `TELEOP` | ignore | +| `STOPPED_AT_TARGET` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | +| `ABORTING` | queued | ignore | queued | ignore | → `TELEOP` | ignore | +| `RETURNING` | → `NAVIGATING` (preempt) | → `ABORTING` | ignore | → `ABORTING` | → `TELEOP` | ignore | +| `STOPPED_AT_RETURN` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | +| `TELEOP` | ignore | ignore | ignore | ignore | ignore | → `IDLE` | + +\* `RETURN` requires the target to have been previously visited. Otherwise the action server returns failure immediately. + +#### 3.2.3 `PLAN_FAILED` Handling + +Subscribes to `/planner_event` (`PlannerEvent.msg`). On `event == PLAN_FAILED`: + +- In `NAVIGATING` or `RETURNING`: transition to `ABORTING`, publish `/nav_enabled = false`. +- In all other states: ignore (the executive is authoritative on state). + +#### 3.2.4 Stopped Detection + +Arrival confirmation uses **measured velocity from `/odom`** (published by the ZED SDK — not commanded `/cmd_vel`). The robot is considered stopped when `|twist.linear| < velocity_zero_threshold` is held for `arrival_hold_time` seconds. + +#### 3.2.5 Replanning + +The global planner does **not** self-trigger replans. `mission_executive` computes the robot's cross-track error (perpendicular distance from `/robot_pose` to the nearest segment of the current `/global_path`). When this exceeds `replan_distance_m`, the executive re-publishes `/goal_pose` to trigger a fresh plan. This keeps replanning fully observable via the `/goal_pose` topic stream. + +The executive caches the latest `/global_path` (subscribed with `transient_local` QoS) for this computation. + +#### 3.2.6 Executor Configuration + +`mission_executive` calls `gps_pose_publisher`'s `latlon_to_enu` service during `GO_TO` goal conversion. To avoid deadlock from blocking on `future.get()` inside a callback, **use a `MultiThreadedExecutor` with a reentrant callback group** for the service client. + +#### 3.2.7 Interfaces + +**Subscriptions:** + +| Topic | Type | Purpose | +|-------|------|---------| +| `/robot_pose` | `PoseStamped` | Current position | +| `/odom` | `nav_msgs/Odometry` | Measured velocity for stop detection (ZED SDK) | +| `/planner_event` | `PlannerEvent` | `PLAN_FAILED` triggers abort | +| `/global_path` | `nav_msgs/Path` | Cached for cross-track error (QoS: `transient_local`) | + +**Action servers:** + +| Name | Type | Handles | +|------|------|---------| +| `~/navigate_to_target` | `NavigateToTarget` | `GO_TO`, `RETURN` | + +**Services:** + +| Name | Type | Purpose | +|------|------|---------| +| `~/abort` | `std_srvs/Trigger` | Transition to `ABORTING` | +| `~/set_target` | `SetTarget` | Register/update a named target in the registry | + +**Publications:** + +| Topic | Type | Notes | +|-------|------|-------| +| `/goal_pose` | `PoseStamped` | Always ENU / `map` frame; consumed by global planner | +| `/nav_enabled` | `std_msgs/Bool` | `false` during teleop/stop; MPPI respects this | +| `/nav_mode` | `std_msgs/String` | `"autonomous"` / `"teleop"` / `"stopped"` | +| `/active_target` | `ActiveTarget` | Target metadata + status | +| `/nav_status` | `NavStatus` | 2 Hz + on every state change | + +#### 3.2.8 Target Types + +| Type | Tolerance | Notes | +|------|-----------|-------| +| `GNSS_ONLY` | 3.0 m | GPS-sourced goal | +| `ARUCO_POST` | 2.0 m | GNSS approach → ArUco refines (Phase 2c) | +| `OBJECT` | stop on detection | First two: GNSS < 3 m; third: < 10 m | +| `LOCAL` | user-defined | Meter-sourced goal; no GPS required | + +--- + +### 3.3 `global_planner` (Hybrid-A\* via `athena_smac_planner`) + +Wraps the vendored SMAC planner with `athena_map` costmap input. Plans **only** on receipt of a new `/goal_pose` — no self-triggered replanning. + +**Startup behavior:** On receiving a `/goal_pose` before a valid `/robot_pose` has been received, the planner logs a `WARN` and waits (does not publish `PLAN_FAILED`). Planning proceeds once both pose and map are available. This handles the GPS cold-start window gracefully. + +**Subscriptions:** + +| Topic | Type | Notes | +|-------|------|-------| +| `/goal_pose` | `PoseStamped` | Triggers planning | +| `/robot_pose` | `PoseStamped` | Planner start pose — must be received before planning | +| `/map` | `nav_msgs/OccupancyGrid` | Feeds `StaticLayer` (QoS: `transient_local`) | + +**Publications:** + +| Topic | Type | QoS | Notes | +|-------|------|-----|-------| +| `/global_path` | `nav_msgs/Path` | `transient_local` | Consumed by MPPI + mission_executive | +| `/planner_event` | `PlannerEvent` | | Enum-based event (see §6) | + +--- + +### 3.4 `mppi_runner` (existing `ackermann_mppi`) + +Minimal changes to the existing package. + +**Changes required:** + +1. Rename input topic `/plan` → `/global_path` +2. Subscribe to `/nav_enabled` — skip `evalControl()` when false +3. Add costmap debug publisher via `nav2_costmap_2d::CostmapPublisher` + +**Local costmap** is owned by `mppi_runner` as a child `Costmap2DROS` node, configured under the `local_costmap` namespace nested within `mppi_runner` parameters (see §7). + +**Subscriptions:** `/robot_pose`, `/odom`, `/global_path`, `/goal_pose`, `/nav_enabled` + +**Publications:** `/cmd_vel` (`TwistStamped`) + +**Control loop:** Timer at `controller_frequency` Hz (default 20). Calls `evalControl()` only when pose + path + goal are all available **and** `/nav_enabled` is true. + +**Initial critics:** +`ConstraintCritic`, `GoalCritic`, `GoalAngleCritic`, `PathFollowCritic`, `PathAlignCritic`, `PreferForwardCritic` + +--- + +## 4. Debugging & Observability + +### 4.1 `/nav_status` (published by `mission_executive`) + +Published at 2 Hz and on every state change. See §6 for full message definition. + +### 4.2 Logging Conventions + +| Event | Level | Format | +|-------|-------|--------| +| State transition | `INFO` | `[mission_executive] → : ` | +| Replan trigger | `INFO` | `[mission_executive] replan triggered: xtrack=X.Xm` | +| `evalControl()` skip | `DEBUG` | (fires at 20 Hz — never INFO) | +| GPS origin set | `INFO` | `[gps_pose_publisher] origin set: lat=X lon=Y alt=Z` | +| `PLAN_FAILED` received | `WARN` | `[mission_executive] PLAN_FAILED — transitioning to ABORTING` | +| Planner waiting for fix | `WARN` | `[global_planner] goal received but no robot_pose yet — waiting` | +| Service/action rejection | `WARN` | Reason logged; error returned to caller | + +### 4.3 Topic QoS + +| Topic | QoS | +|-------|-----| +| `/global_path` | `transient_local` (rviz2/Foxglove sees current path on connect) | +| `/map` | `transient_local` (from `athena_map`) | +| `/nav_status` | `keep_last(1)`, reliable | + +--- + +## 5. Extension Phases + +| Phase | Feature | Effort | +|-------|---------|--------| +| **2a** | Depth camera obstacle avoidance | Config only — add `ObstacleLayer` + `InflationLayer` to MPPI local costmap plugins, enable `CostCritic` + `ObstaclesCritic` | +| **2b** | GeoTIFF terrain obstacles | Config only — set `geotiff_path` on `global_planner` | +| **2c** | ArUco post refinement | Wire `correction_node` output to override `/goal_pose` when `ARUCO_POST` target is within ~10 m. Reuse `GetArucoPose` BT node. | +| **2d** | Object detection | `yolo_ros/` detects objects within tolerance → publish to C2 overlay → `mission_executive` transitions to `STOPPED_AT_TARGET` | +| **2e** | Spiral/lawnmower search | `SpiralCoverageAction` BT node (Archimedean spiral, $r = 15$ m, spacing $= 2$ m). Sub-mode when target reached via GNSS but not visually acquired. | + +--- + +## 6. Custom Message / Service / Action Definitions + +All defined in the `navigation_msgs` package. + +### Services + +``` +# LatLonToENU.srv +float64 lat +float64 lon +--- +float64 x +float64 y +float64 z +``` + +``` +# SetTarget.srv — pre-register a named target +string target_id +float64 lat # GPS goal (WGS84) — ignored if goal_type == METER +float64 lon +float64 x_m # Meter goal (ENU) — ignored if goal_type == GPS +float64 y_m +uint8 goal_type # GPS=0, METER=1 +uint8 GPS=0 +uint8 METER=1 +uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 +uint8 GNSS_ONLY=0 +uint8 ARUCO_POST=1 +uint8 OBJECT=2 +uint8 LOCAL=3 +float64 tolerance_m +--- +bool success +string message +``` + +### Actions + +``` +# NavigateToTarget.action +# --- Goal --- +# If target_id is non-empty, coordinates are looked up from the registry +# (previously loaded via SetTarget). If target_id is empty, inline +# coordinates are used as an ad-hoc goal. +string target_id # empty string = use inline coordinates +float64 lat # ad-hoc GPS goal (ignored if target_id set) +float64 lon +float64 x_m # ad-hoc meter goal (ignored if target_id set) +float64 y_m +uint8 goal_type # GPS=0, METER=1 +uint8 GPS=0 +uint8 METER=1 +uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 +uint8 GNSS_ONLY=0 +uint8 ARUCO_POST=1 +uint8 OBJECT=2 +uint8 LOCAL=3 +float64 tolerance_m # only used for ad-hoc goals; registry targets use stored tolerance +bool is_return +--- +# --- Result --- +bool success +string message +--- +# --- Feedback --- +float64 distance_to_goal_m +float64 cross_track_error_m +string state +``` + +### Messages + +``` +# PlannerEvent.msg +uint8 NEW_GOAL=0 +uint8 PLANNING=1 +uint8 PLAN_SUCCEEDED=2 +uint8 PLAN_FAILED=3 +uint8 event +``` + +``` +# ActiveTarget.msg +string target_id +uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 +float64 tolerance_m +geometry_msgs/PoseStamped goal_enu +uint8 goal_source # GPS=0, METER=1 +string status # NAVIGATING, SEARCHING, ARRIVED, ABORTED +``` + +``` +# NavStatus.msg +string state # IDLE, NAVIGATING, ARRIVING, STOPPED_AT_TARGET, + # ABORTING, RETURNING, STOPPED_AT_RETURN, TELEOP +string active_target_id +uint8 active_target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 +uint8 goal_source # GPS=0, METER=1 +float64 distance_to_goal_m # -1.0 if no active goal +float64 cross_track_error_m # -1.0 if no active path +float64 heading_error_rad +float64 robot_speed_mps # from /odom twist (ZED SDK) +bool is_return +uint8 last_planner_event # last PlannerEvent.event value +``` + +--- + +## 7. Parameter Reference (`nav_params.yaml`) + +```yaml +# ── Global ────────────────────────────────────────────── +use_sim_time: false # propagated to all nodes via launch + +# ── Robot geometry (referenced by costmaps + planner) ─── +robot_radius: &robot_radius 0.45 # meters — inscribed radius + +# ── gps_pose_publisher ───────────────────────────────── +gps_pose_publisher: + ros__parameters: + use_sim_time: false + heading_topic: "/gps/heading" + origin_lat: NaN # auto-set from first fix if NaN + origin_lon: NaN + origin_alt: 0.0 + use_start_gate_ref: true # if true, /start_gate_ref sets origin + +# ── mission_executive ────────────────────────────────── +mission_executive: + ros__parameters: + use_sim_time: false + default_targets: [] + velocity_zero_threshold: 0.05 # m/s — applied to /odom twist + arrival_hold_time: 1.0 # seconds of confirmed zero velocity + replan_distance_m: 3.0 # cross-track error threshold to trigger replan + latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" + +# ── global_planner (athena_smac_planner) ─────────────── +global_planner: + ros__parameters: + use_sim_time: false + geotiff_path: "" + grid_size_m: 500.0 + grid_resolution_m: 0.5 + obstacle_threshold: 50 + min_turning_r: 1.2 # must match mppi_runner + allow_reverse: false + +global_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 1.0 + publish_frequency: 0.5 + transform_tolerance: 1.0 # generous — GPS-only localization has jitter + rolling_window: false + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_topic: /map + subscribe_to_updates: true + map_subscribe_transient_local: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 1.0 # >= robot_radius + +# ── mppi_runner (ackermann_mppi) ─────────────────────── +mppi_runner: + ros__parameters: + use_sim_time: false + controller_frequency: 20.0 + motion_model: "Ackermann" + min_turning_r: 1.2 + vx_max: 3.0 + vx_min: 0.0 + wz_max: 1.0 + time_steps: 56 + batch_size: 1000 + model_dt: 0.05 + critics: + - ConstraintCritic + - GoalCritic + - GoalAngleCritic + - PathFollowCritic + - PathAlignCritic + - PreferForwardCritic + # Local costmap — child Costmap2DROS node owned by mppi_runner + local_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 1.0 + rolling_window: true + width: 20 + height: 20 + resolution: 0.1 + plugins: [] # Phase 2a: add ObstacleLayer + InflationLayer +``` + +--- + +## 8. Implementation Order + +1. **`gps_pose_publisher`** — full implementation; validates TF + ENU projection before anything else runs. +2. **`ackermann_mppi` modifications** — topic rename, `/nav_enabled` guard, costmap debug publisher. +3. **`global_planner` / `athena_smac_planner`** — package rename, `athena_map` integration, costmap wiring. +4. **`mission_executive`** — state machine, action/service servers, all operator commands. +5. **`nav.launch.py`** + `CMakeLists.txt` + `package.xml` — single-package build and launch. + +In the final launch file, you need to launch main/athena-code/src/subsystems/navigation/athena_gps/launch/gps_launch.py, and we will use the /odom/ground_truth topic for odom. you also need to map the output using a twist stamper like + + twist_stamper_node = Node( + package='twist_stamper', + executable='twist_stamper', + name='cmd_vel_stamper', + parameters=[{'use_sim_time': sim}], + remappings=[ + ('cmd_vel_in', '/cmd_vel_nav'), + ('cmd_vel_out', '/rear_ackermann_controller/reference'), + ], + ) + diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt b/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt new file mode 100644 index 00000000..8b73412f --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt @@ -0,0 +1,121 @@ +cmake_minimum_required(VERSION 3.8) +project(ackermann_mppi) + +find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(builtin_interfaces REQUIRED) + +include_directories( + include + ${EIGEN3_INCLUDE_DIR} +) + +include(CheckCXXCompilerFlag) +check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA) +if(COMPILER_SUPPORTS_FMA) + add_compile_options(-mfma) +endif() + +# --- Core optimizer library (no ROS node, no pluginlib) --- +add_library(ackermann_mppi_core SHARED + src/noise_generator.cpp + src/optimizer.cpp + src/critics/constraint_critic.cpp + src/critics/goal_critic.cpp + src/critics/obstacles_critic.cpp + src/critics/path_align_critic.cpp + src/critics/path_follow_critic.cpp +) + +# Enable C++17 (required for inline constexpr, structured bindings, etc.) +target_compile_features(ackermann_mppi_core PUBLIC cxx_std_17) + +# Apply -O3 only for Release and RelWithDebInfo builds so Debug stays debuggable. +# Do not force -O3 unconditionally — it disrespects the developer's chosen build type. +target_compile_options(ackermann_mppi_core PUBLIC + $<$:-O3> + $<$:-O3> +) + +target_include_directories(ackermann_mppi_core + PUBLIC + "$" + "$") + +target_link_libraries(ackermann_mppi_core PUBLIC + angles::angles + ${geometry_msgs_TARGETS} + nav2_costmap_2d::layers + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + ${std_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} + ${builtin_interfaces_TARGETS} +) + +# --- ROS2 node executable --- +add_executable(ackermann_mppi_node + src/ackermann_mppi_node.cpp +) + +target_compile_options(ackermann_mppi_node PRIVATE + $<$:-O3> + $<$:-O3> +) + +target_include_directories(ackermann_mppi_node PRIVATE + "$") + +target_link_libraries(ackermann_mppi_node + ackermann_mppi_core + rclcpp::rclcpp + tf2_ros::tf2_ros + tf2_geometry_msgs::tf2_geometry_msgs + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + ${geometry_msgs_TARGETS} +) + +# --- Install --- +install(TARGETS ackermann_mppi_core ackermann_mppi_node + EXPORT ackermann_mppi + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +ament_export_targets(ackermann_mppi HAS_LIBRARY_TARGET) +ament_export_dependencies( + angles + geometry_msgs + nav2_costmap_2d + nav_msgs + rclcpp + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + Eigen3 +) +ament_export_include_directories(include/${PROJECT_NAME}) + +ament_package() diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp new file mode 100644 index 00000000..14e89c51 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp @@ -0,0 +1,85 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITIC_DATA_HPP_ +#define ACKERMANN_MPPI__CRITIC_DATA_HPP_ + +#include + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/models/trajectories.hpp" +#include "ackermann_mppi/models/path.hpp" +#include "ackermann_mppi/motion_models.hpp" + +namespace mppi +{ + +/** + * @struct mppi::CriticData + * @brief Data passed to critics for scoring. GoalChecker replaced with a simple + * distance tolerance float — the optimizer sets this from its parameters. + * + * Has an explicit constructor to prevent silent wrong-field binding when using + * aggregate initialization in C++17 (designated initializers require C++20). + */ +struct CriticData +{ + const models::State & state; + const models::Trajectories & trajectories; + const models::Path & path; + const geometry_msgs::msg::Pose & goal; + + Eigen::ArrayXf & costs; + float & model_dt; + + bool fail_flag{false}; + + // Replaces nav2_core::GoalChecker — set by Optimizer::prepare() from settings + float goal_dist_tolerance{0.25f}; + + std::shared_ptr motion_model; + std::optional> path_pts_valid; + std::optional furthest_reached_path_point; + std::vector trajectories_in_collision; + + // Explicit constructor so call sites use named parameters, preventing silent + // mis-binding if fields are reordered. + CriticData( + const models::State & state_in, + const models::Trajectories & trajectories_in, + const models::Path & path_in, + const geometry_msgs::msg::Pose & goal_in, + Eigen::ArrayXf & costs_in, + float & model_dt_in, + float goal_dist_tolerance_in = 0.25f, + std::shared_ptr motion_model_in = nullptr) + : state(state_in), + trajectories(trajectories_in), + path(path_in), + goal(goal_in), + costs(costs_in), + model_dt(model_dt_in), + fail_flag(false), + goal_dist_tolerance(goal_dist_tolerance_in), + motion_model(std::move(motion_model_in)) {} +}; + +} // namespace mppi + +#endif // ACKERMANN_MPPI__CRITIC_DATA_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp new file mode 100644 index 00000000..90cff7f6 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp @@ -0,0 +1,120 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ +#define ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +#include "ackermann_mppi/critic_data.hpp" + +namespace mppi::critics +{ + +struct CollisionCost +{ + float cost{0}; + bool using_footprint{false}; +}; + +/** + * @class mppi::critics::CriticFunction + * @brief Abstract base for MPPI trajectory scoring critics. + * + * Compared to the original nav2 version: + * - Uses rclcpp::Node::SharedPtr instead of LifecycleNode::WeakPtr + * - No ParametersHandler dependency — uses a simple getParam() helper inline + * - Instantiated directly by the Optimizer (no pluginlib) + */ +class CriticFunction +{ +public: + CriticFunction() = default; + virtual ~CriticFunction() = default; + + /** + * @brief Configure critic on startup. + * @param node ROS2 node (used for parameter access and logging) + * @param parent_name Name of the parent optimizer (for reading shared params) + * @param name Name of this critic (used as param namespace) + * @param costmap_ros Costmap for collision/cost queries + */ + void on_configure( + rclcpp::Node::SharedPtr node, + const std::string & parent_name, + const std::string & name, + std::shared_ptr costmap_ros) + { + node_ = node; + logger_ = node_->get_logger(); + name_ = name; + parent_name_ = parent_name; + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + + // Declare and read the common 'enabled' parameter + declare_param(name_ + ".enabled", true); + node_->get_parameter(name_ + ".enabled", enabled_); + + initialize(); + } + + virtual void score(CriticData & data) = 0; + virtual void initialize() = 0; + + std::string getName() { return name_; } + +protected: + /** + * @brief Declare a parameter with a default value if not already declared. + */ + template + void declare_param(const std::string & full_name, T default_value) + { + if (!node_->has_parameter(full_name)) { + node_->declare_parameter(full_name, default_value); + } + } + + /** + * @brief Get a parameter accessor scoped to a given namespace. + * + * Usage (same pattern as original ParametersHandler::getParamGetter): + * auto getParam = getParamGetter(name_); + * getParam(weight_, "cost_weight", 5.0f); + */ + auto getParamGetter(const std::string & ns) + { + return [this, ns](auto & setting, const std::string & name, auto default_value) { + std::string full_name = ns.empty() ? name : ns + "." + name; + declare_param(full_name, default_value); + node_->get_parameter(full_name, setting); + }; + } + + bool enabled_{true}; + std::string name_, parent_name_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_{nullptr}; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp new file mode 100644 index 00000000..ba4901a3 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ + +#include "ackermann_mppi/critic_function.hpp" + +namespace mppi::critics +{ + +/** + * @class ConstraintCritic (Ackermann-only) + * @brief Penalizes trajectories that violate velocity and turning radius constraints. + * + * For Ackermann: penalizes vx outside [vx_min, vx_max] and steering ratios + * (|vx|/|wz|) that would require a tighter turn than min_turning_r. + */ +class ConstraintCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + // min_turning_r is vehicle geometry — fixed. Velocity limits are read from + // data.motion_model->getConstraints() in score() so speed-limit changes take effect. + float min_turning_r_{0.2f}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp new file mode 100644 index 00000000..f8d69ed1 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp @@ -0,0 +1,39 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ + +#include "ackermann_mppi/critic_function.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +namespace mppi::critics +{ + +class GoalCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + float threshold_to_consider_{0}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp new file mode 100644 index 00000000..25a9ca14 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "ackermann_mppi/critic_function.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +namespace mppi::critics +{ + +class ObstaclesCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + inline bool inCollision(float cost) const; + inline CollisionCost costAtPose(float x, float y, float theta); + inline float distanceToObstacle(const CollisionCost & cost); + float findCircumscribedCost(std::shared_ptr costmap); + +protected: + nav2_costmap_2d::FootprintCollisionChecker + collision_checker_{nullptr}; + + bool consider_footprint_{true}; + float collision_cost_{0}; + float inflation_scale_factor_{0}, inflation_radius_{0}; + float possible_collision_cost_; + float collision_margin_distance_; + float near_goal_distance_; + float circumscribed_cost_{0}, circumscribed_radius_{0}; + + unsigned int power_{0}; + float repulsion_weight_, critical_weight_{0}; + std::string inflation_layer_name_; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp new file mode 100644 index 00000000..db17564e --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023 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. + +#ifndef ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ + +#include "ackermann_mppi/critic_function.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +namespace mppi::critics +{ + +class PathAlignCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + size_t offset_from_furthest_{0}; + int trajectory_point_step_{0}; + float threshold_to_consider_{0}; + float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; + unsigned int power_{0}; + float weight_{0}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp new file mode 100644 index 00000000..ef5485b7 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp @@ -0,0 +1,40 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ +#define ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ + +#include "ackermann_mppi/critic_function.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +namespace mppi::critics +{ + +class PathFollowCritic : public CriticFunction +{ +public: + void initialize() override; + void score(CriticData & data) override; + +protected: + float threshold_to_consider_{0}; + size_t offset_from_furthest_{0}; + unsigned int power_{0}; + float weight_{0}; +}; + +} // namespace mppi::critics + +#endif // ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp new file mode 100644 index 00000000..8f54c8d6 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ +#define ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ + +namespace mppi::models +{ + +/** + * @struct mppi::models::ControlConstraints + * @brief Constraints on control (Ackermann: no vy/ay terms) + */ +struct ControlConstraints +{ + float vx_max; + float vx_min; + float wz; + float ax_max; + float ax_min; + float az_max; +}; + +/** + * @struct mppi::models::SamplingStd + * @brief Noise standard deviations for MPPI sampling (Ackermann: vx and wz only) + */ +struct SamplingStd +{ + float vx; + float wz; +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp new file mode 100644 index 00000000..b4781787 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ +#define ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ + +#include + +namespace mppi::models +{ + +/** + * @struct mppi::models::Control + * @brief A single control step (Ackermann: vx and wz only) + */ +struct Control +{ + float vx, wz; +}; + +/** + * @struct mppi::models::ControlSequence + * @brief A control sequence over time (Ackermann: vx and wz only) + */ +struct ControlSequence +{ + Eigen::ArrayXf vx; + Eigen::ArrayXf wz; + + void reset(unsigned int time_steps) + { + vx.setZero(time_steps); + wz.setZero(time_steps); + } +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp new file mode 100644 index 00000000..af0ad621 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ +#define ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ + +#include +#include "ackermann_mppi/models/constraints.hpp" + +namespace mppi::models +{ + +/** + * @struct mppi::models::OptimizerSettings + * @brief Settings for the optimizer to use + */ +struct OptimizerSettings +{ + models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; + models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; + models::SamplingStd sampling_std{0.0f, 0.0f}; + float model_dt{0.0f}; + float temperature{0.0f}; + float gamma{0.0f}; + unsigned int batch_size{0u}; + unsigned int time_steps{0u}; + unsigned int iteration_count{0u}; + bool shift_control_sequence{false}; + size_t retry_attempt_limit{0}; + bool open_loop{false}; + bool visualize{false}; +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp new file mode 100644 index 00000000..a9171d78 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__PATH_HPP_ +#define ACKERMANN_MPPI__MODELS__PATH_HPP_ + +#include + +namespace mppi::models +{ + +/** + * @struct mppi::models::Path + * @brief Path represented as a tensor + */ +struct Path +{ + Eigen::ArrayXf x; + Eigen::ArrayXf y; + Eigen::ArrayXf yaws; + + /** + * @brief Reset path data + */ + void reset(unsigned int size) + { + x.setZero(size); + y.setZero(size); + yaws.setZero(size); + } +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__PATH_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp new file mode 100644 index 00000000..cb5018f4 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__STATE_HPP_ +#define ACKERMANN_MPPI__MODELS__STATE_HPP_ + +#include + +#include +#include + + +namespace mppi::models +{ + +/** + * @struct mppi::models::State + * @brief State information: velocities, controls, poses, speed + */ +struct State +{ + // Propagated velocities: [batch_size x time_steps] + Eigen::ArrayXXf vx; // longitudinal velocity + Eigen::ArrayXXf wz; // angular velocity (vy is always zero for Ackermann) + + // Noised controls: [batch_size x time_steps] + Eigen::ArrayXXf cvx; + Eigen::ArrayXXf cwz; + + geometry_msgs::msg::PoseStamped pose; + geometry_msgs::msg::Twist speed; + float local_path_length; + + void reset(unsigned int batch_size, unsigned int time_steps) + { + vx.setZero(batch_size, time_steps); + wz.setZero(batch_size, time_steps); + cvx.setZero(batch_size, time_steps); + cwz.setZero(batch_size, time_steps); + } +}; +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__STATE_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp new file mode 100644 index 00000000..8284872a --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ +#define ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ + +#include + +namespace mppi::models +{ + +/** + * @class mppi::models::Trajectories + * @brief Candidate Trajectories + */ +struct Trajectories +{ + Eigen::ArrayXXf x; + Eigen::ArrayXXf y; + Eigen::ArrayXXf yaws; + + /** + * @brief Reset state data + */ + void reset(unsigned int batch_size, unsigned int time_steps) + { + x.setZero(batch_size, time_steps); + y.setZero(batch_size, time_steps); + yaws.setZero(batch_size, time_steps); + } +}; + +} // namespace mppi::models + +#endif // ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp new file mode 100644 index 00000000..87487587 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp @@ -0,0 +1,133 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2025 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. + +#ifndef ACKERMANN_MPPI__MOTION_MODELS_HPP_ +#define ACKERMANN_MPPI__MOTION_MODELS_HPP_ + +#include +#include +#include + +#include "ackermann_mppi/models/control_sequence.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/models/constraints.hpp" + +namespace mppi +{ + +// Forward declaration used in utils.hpp to avoid circular include +namespace utils +{ +float clamp(const float lower_bound, const float upper_bound, const float input); +} + +/** + * @class mppi::MotionModel + * @brief Abstract motion model base class + */ +class MotionModel +{ +public: + MotionModel() = default; + virtual ~MotionModel() = default; + + void initialize(const models::ControlConstraints & control_constraints, float model_dt) + { + control_constraints_ = control_constraints; + model_dt_ = model_dt; + } + + /** + * @brief Propagate velocities forward one step using acceleration constraints. + * Batch operation: state matrices are [batch_size x time_steps]. + */ + virtual void predict(models::State & state) + { + float max_delta_vx = model_dt_ * control_constraints_.ax_max; + float min_delta_vx = model_dt_ * control_constraints_.ax_min; + float max_delta_wz = model_dt_ * control_constraints_.az_max; + + unsigned int n_cols = state.vx.cols(); + for (unsigned int i = 1; i < n_cols; i++) { + auto lower_bound_vx = (state.vx.col(i - 1) > 0).select( + state.vx.col(i - 1) + min_delta_vx, + state.vx.col(i - 1) - max_delta_vx); + auto upper_bound_vx = (state.vx.col(i - 1) > 0).select( + state.vx.col(i - 1) + max_delta_vx, + state.vx.col(i - 1) - min_delta_vx); + + state.cvx.col(i - 1) = state.cvx.col(i - 1) + .cwiseMax(lower_bound_vx) + .cwiseMin(upper_bound_vx); + state.vx.col(i) = state.cvx.col(i - 1); + + state.cwz.col(i - 1) = state.cwz.col(i - 1) + .cwiseMax(state.wz.col(i - 1) - max_delta_wz) + .cwiseMin(state.wz.col(i - 1) + max_delta_wz); + state.wz.col(i) = state.cwz.col(i - 1); + } + } + + virtual bool isHolonomic() = 0; + + /** + * @brief Apply model-specific hard constraints to the optimal control sequence. + * Called after softmax weighting, before returning the command. + */ + virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {} + + /** + * @brief Return the currently active constraints (may differ from base if speed limit is set). + * Critics should read from this instead of caching their own copies. + */ + const models::ControlConstraints & getConstraints() const { return control_constraints_; } + +protected: + float model_dt_{0.0}; + models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; +}; + +/** + * @class mppi::AckermannMotionModel + * @brief Ackermann steering motion model. + * + * Key constraint: angular velocity is bounded by |wz| <= |vx| / min_turning_r + * This prevents commanding steering angles that are physically impossible at a given speed. + */ +class AckermannMotionModel : public MotionModel +{ +public: + explicit AckermannMotionModel(float min_turning_r = 0.2f) + : min_turning_r_(min_turning_r) {} + + bool isHolonomic() override { return false; } + + void applyConstraints(models::ControlSequence & control_sequence) override + { + const auto wz_constrained = control_sequence.vx.abs() / min_turning_r_; + control_sequence.wz = control_sequence.wz + .max(-wz_constrained) + .min(wz_constrained); + } + + float getMinTurningRadius() const { return min_turning_r_; } + +private: + float min_turning_r_{0.2f}; +}; + +} // namespace mppi + +#endif // ACKERMANN_MPPI__MOTION_MODELS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp new file mode 100644 index 00000000..bfcd2e41 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp @@ -0,0 +1,238 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2025 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. + +#ifndef ACKERMANN_MPPI__OPTIMIZER_HPP_ +#define ACKERMANN_MPPI__OPTIMIZER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "tf2_ros/buffer.hpp" + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" + +#include "ackermann_mppi/models/optimizer_settings.hpp" +#include "ackermann_mppi/motion_models.hpp" +#include "ackermann_mppi/critic_data.hpp" +#include "ackermann_mppi/models/state.hpp" +#include "ackermann_mppi/models/trajectories.hpp" +#include "ackermann_mppi/models/path.hpp" +#include "ackermann_mppi/tools/noise_generator.hpp" +#include "ackermann_mppi/tools/utils.hpp" + +// Forward declarations for critic types +namespace mppi::critics +{ +class CriticFunction; +class PathFollowCritic; +class PathAlignCritic; +class GoalCritic; +class ObstaclesCritic; +class ConstraintCritic; +} + +namespace mppi +{ + +/** + * @class mppi::Optimizer + * @brief Ackermann-specific MPPI optimizer. + * + * Differences from the nav2_mppi_controller Optimizer: + * - Initialized with rclcpp::Node::SharedPtr (not LifecycleNode) + * - Critics are created directly (no pluginlib) + * - No OptimalTrajectoryValidator — simple fallback on fail_flag + * - Motion model hardcoded to AckermannMotionModel + * - GoalChecker replaced with a distance threshold parameter + * - Parameters read directly via node->declare_parameter / node->get_parameter + */ +class Optimizer +{ +public: + // Both constructor and destructor defined in optimizer.cpp so critic forward declarations are sufficient here + Optimizer(); + ~Optimizer(); + + /** + * @brief Initialize the optimizer. + * @param node ROS2 node for parameters, logging, and clock + * @param name Parameter namespace prefix (e.g. "mppi") + * @param costmap_ros Costmap for obstacle avoidance critics + * @param tf_buffer TF buffer for robot pose lookups + */ + void initialize( + rclcpp::Node::SharedPtr node, + const std::string & name, + std::shared_ptr costmap_ros, + std::shared_ptr tf_buffer); + + void shutdown(); + + /** + * @brief Run MPPI to compute the next control command. + * @param robot_pose Current robot pose (in costmap frame) + * @param robot_speed Current robot velocity (from odometry) + * @param plan Global path to follow + * @param goal Goal pose (last pose of plan, or explicit goal) + * @return [TwistStamped command, optimal trajectory as [time_steps x 3] (x, y, yaw)] + * @throws std::runtime_error if all trajectories are in collision after retries + */ + std::tuple evalControl( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal); + + // --- Accessors for visualization / debugging --- + + models::Trajectories & getGeneratedTrajectories() { return generated_trajectories_; } + Eigen::ArrayXXf getOptimizedTrajectory(); + const models::ControlSequence & getOptimalControlSequence() { return control_sequence_; } + const Eigen::ArrayXf & getCosts() const { return costs_; } + + /** + * @brief Per-critic cost breakdown from last evalControl call. + * Each entry is (critic_name, cost_delta_array[batch_size]). + */ + const std::vector> & getCriticCosts() const + { + return critic_costs_; + } + + const std::vector & getCollisionFlags() const + { + return critics_data_.trajectories_in_collision; + } + + /** + * @brief Scale max speeds by a ratio (e.g. from a speed zone costmap filter). + * @param speed_limit Absolute speed OR percentage of max speed + * @param percentage True if speed_limit is a percentage [0–100] + */ + void setSpeedLimit(double speed_limit, bool percentage); + + void reset(bool reset_dynamic_speed_limits = true); + + bool isSpeedLimitActive() const; + + const models::OptimizerSettings & getSettings() const { return settings_; } + +protected: + void optimize(); + void evalTrajectoriesScores(); + + void prepare( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal); + + void getParams(); + void loadCritics(); + + void shiftControlSequence(); + void generateNoisedTrajectories(); + void applyControlSequenceConstraints(); + void updateStateVelocities(models::State & state) const; + void updateInitialStateVelocities(models::State & state) const; + void propagateStateVelocitiesFromInitials(models::State & state) const; + + void integrateStateVelocities( + models::Trajectories & trajectories, + const models::State & state) const; + + void integrateStateVelocities( + Eigen::Array & trajectory, + const Eigen::ArrayXXf & sequence) const; + + void updateControlSequence(); + + geometry_msgs::msg::TwistStamped + getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time & stamp); + + bool fallback(bool fail); + + size_t fallback_counter_{0}; // member — not static, so reset() clears it correctly + + template + void declareParam(const std::string & full_name, T default_value) + { + if (!node_->has_parameter(full_name)) { + node_->declare_parameter(full_name, default_value); + } + } + + auto getParamGetter(const std::string & ns) + { + return [this, ns](auto & setting, const std::string & name, auto default_value) { + std::string full_name = ns.empty() ? name : ns + "." + name; + declareParam(full_name, default_value); + node_->get_parameter(full_name, setting); + }; + } + +protected: + rclcpp::Node::SharedPtr node_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_{nullptr}; + std::string name_; + std::string base_frame_; + std::shared_ptr tf_buffer_; + + std::shared_ptr motion_model_; + + // Critics owned directly (no pluginlib) + std::vector> critics_; + std::vector> critic_costs_; + + NoiseGenerator noise_generator_; + + models::OptimizerSettings settings_; + float goal_dist_tolerance_{0.25f}; + + models::State state_; + models::ControlSequence control_sequence_; + std::array control_history_; + models::Trajectories generated_trajectories_; + models::Path path_; + geometry_msgs::msg::Pose goal_; + Eigen::ArrayXf costs_; + + CriticData critics_data_{ + state_, + generated_trajectories_, + path_, + goal_, + costs_, + settings_.model_dt, + goal_dist_tolerance_}; + + rclcpp::Logger logger_{rclcpp::get_logger("AckermannMPPI")}; + geometry_msgs::msg::Twist last_command_vel_; +}; + +} // namespace mppi + +#endif // ACKERMANN_MPPI__OPTIMIZER_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp new file mode 100644 index 00000000..c4b69720 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ +#define ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "ackermann_mppi/models/optimizer_settings.hpp" +#include "ackermann_mppi/models/control_sequence.hpp" +#include "ackermann_mppi/models/state.hpp" + +namespace mppi +{ + +/** + * @class mppi::NoiseGenerator + * @brief Generates Gaussian-perturbed control sequences for MPPI sampling. + * + * When regenerate_noises=true (set via optimizer param), noise generation runs in a + * background thread that pre-generates noise for the *next* iteration while the + * current one scores trajectories. This hides latency on slower hardware. + * + * When regenerate_noises=false (default), noise is generated synchronously each call. + */ +class NoiseGenerator +{ +public: + NoiseGenerator() = default; + + /** + * @brief Initialize noise generator. + * @param settings Optimizer settings (batch_size, time_steps, sampling_std) + * @param regenerate_noises If true, run noise generation in background thread + */ + void initialize( + mppi::models::OptimizerSettings & settings, + bool regenerate_noises = false); + + void shutdown(); + + /** + * @brief Signal the background thread to generate next iteration's noises. + * No-op when regenerate_noises=false. + */ + void generateNextNoises(); + + /** + * @brief Apply noises to the current optimal control sequence to get noised controls. + * Stores in state.cvx / state.cwz for use by the motion model predict(). + */ + void setNoisedControls( + models::State & state, + const models::ControlSequence & control_sequence); + + void reset(mppi::models::OptimizerSettings & settings); + +protected: + void noiseThread(); + void generateNoisedControls(); + + Eigen::ArrayXXf noises_vx_; + Eigen::ArrayXXf noises_wz_; + + std::default_random_engine generator_; + std::normal_distribution ndistribution_vx_; + std::normal_distribution ndistribution_wz_; + + mppi::models::OptimizerSettings settings_; + bool regenerate_noises_{false}; + + std::thread noise_thread_; + std::condition_variable noise_cond_; + std::mutex noise_lock_; + bool active_{false}, ready_{false}; +}; + +} // namespace mppi + +#endif // ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp new file mode 100644 index 00000000..f499ddea --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp @@ -0,0 +1,387 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#ifndef ACKERMANN_MPPI__TOOLS__UTILS_HPP_ +#define ACKERMANN_MPPI__TOOLS__UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "std_msgs/msg/color_rgba.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include "ackermann_mppi/models/optimizer_settings.hpp" +#include "ackermann_mppi/models/control_sequence.hpp" +#include "ackermann_mppi/models/path.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "ackermann_mppi/critic_data.hpp" + +namespace mppi::utils +{ + +// Use constexpr instead of #define to avoid macro pollution and enable type checking. +inline constexpr float M_PIF = 3.141592653589793238462643383279502884e+00F; +inline constexpr float M_PIF_2 = 1.5707963267948966e+00F; + +inline geometry_msgs::msg::Pose createPose(double x, double y, double z) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + pose.orientation.w = 1; + pose.orientation.x = 0; + pose.orientation.y = 0; + pose.orientation.z = 0; + return pose; +} + +inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +inline visualization_msgs::msg::Marker createMarker( + int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns) +{ + using visualization_msgs::msg::Marker; + Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = ns; + marker.id = id; + marker.type = Marker::SPHERE; + marker.action = Marker::ADD; + marker.pose = pose; + marker.scale = scale; + marker.color = color; + return marker; +} + +inline geometry_msgs::msg::TwistStamped toTwistStamped( + float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) +{ + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = frame; + twist.header.stamp = stamp; + twist.twist.linear.x = vx; + twist.twist.angular.z = wz; + return twist; +} + +/** + * @brief Convert path to a tensor of (x, y, yaw) arrays + */ +inline models::Path toTensor(const nav_msgs::msg::Path & path) +{ + auto result = models::Path{}; + result.reset(path.poses.size()); + for (size_t i = 0; i < path.poses.size(); ++i) { + result.x(i) = path.poses[i].pose.position.x; + result.y(i) = path.poses[i].pose.position.y; + result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation); + } + return result; +} + +/** + * @brief Get the last pose in the path tensor + */ +inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) +{ + const unsigned int path_last_idx = path.x.size() - 1; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, path.yaws(path_last_idx)); + geometry_msgs::msg::Pose p; + p.position.x = path.x(path_last_idx); + p.position.y = path.y(path_last_idx); + p.orientation = tf2::toMsg(q); + return p; +} + +template +auto normalize_angles(const T & angles) +{ + return (angles + M_PIF).unaryExpr( + [&](const float x) { + float remainder = std::fmod(x, 2.0f * M_PIF); + return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; + }); +} + +template +auto shortest_angular_distance(const F & from, const T & to) +{ + return normalize_angles(to - from); +} + +/** + * @brief Find the path point index furthest along the path that any trajectory in the batch + * reaches (by closest-point mapping). Used by PathFollow and PathAlign critics. + * + * Vectorized: outer loop iterates over path points (n_path), inner computation is a + * batch-wide Eigen array op over all trajectories simultaneously. This is significantly + * faster than the naive scalar O(n_batch × n_path) nested loop because each iteration + * of the outer loop operates on a [batch_size] SIMD-vectorizable array. + */ +inline size_t findPathFurthestReachedPoint(const CriticData & data) +{ + const int last_col = data.trajectories.x.cols() - 1; + // End-positions of all trajectories: [batch_size] arrays. + const auto traj_x = data.trajectories.x.col(last_col); + const auto traj_y = data.trajectories.y.col(last_col); + + const Eigen::Index n_batch = traj_x.rows(); + const Eigen::Index n_path = static_cast(data.path.x.size()); + + // Per-trajectory: closest path index found so far and its squared distance. + Eigen::ArrayXf min_dist2 = Eigen::ArrayXf::Constant(n_batch, std::numeric_limits::max()); + Eigen::ArrayXi best_idx = Eigen::ArrayXi::Zero(n_batch); + + for (Eigen::Index j = 0; j != n_path; ++j) { + // Vectorized over all batch_size trajectories at once. + const Eigen::ArrayXf dx = traj_x - data.path.x(j); + const Eigen::ArrayXf dy = traj_y - data.path.y(j); + const Eigen::ArrayXf d2 = dx * dx + dy * dy; + + const auto better = (d2 < min_dist2).eval(); + min_dist2 = better.select(d2, min_dist2); + best_idx = better.select(Eigen::ArrayXi::Constant(n_batch, static_cast(j)), best_idx); + + // Early exit: already found a trajectory that reaches the last path point. + if (best_idx.maxCoeff() == static_cast(n_path) - 1) { + break; + } + } + + return static_cast(best_idx.maxCoeff()); +} + +inline void setPathFurthestPointIfNotSet(CriticData & data) +{ + if (!data.furthest_reached_path_point) { + data.furthest_reached_path_point = findPathFurthestReachedPoint(data); + } +} + +/** + * @brief Check which path points are collision-free according to the costmap. + * Stores results in data.path_pts_valid for reuse across critics. + */ +inline void findPathCosts( + CriticData & data, + std::shared_ptr costmap_ros) +{ + auto * costmap = costmap_ros->getCostmap(); + unsigned int map_x, map_y; + const size_t path_segments_count = data.path.x.size() - 1; + data.path_pts_valid = std::vector(path_segments_count, false); + const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); + + for (unsigned int idx = 0; idx < path_segments_count; idx++) { + if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) { + (*data.path_pts_valid)[idx] = false; + continue; + } + switch (costmap->getCost(map_x, map_y)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + (*data.path_pts_valid)[idx] = false; + continue; + case (nav2_costmap_2d::NO_INFORMATION): + (*data.path_pts_valid)[idx] = tracking_unknown ? true : false; + continue; + } + (*data.path_pts_valid)[idx] = true; + } +} + +inline void setPathCostsIfNotSet( + CriticData & data, + std::shared_ptr costmap_ros) +{ + if (!data.path_pts_valid) { + findPathCosts(data, costmap_ros); + } +} + +inline float posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) +{ + float pose_x = pose.position.x; + float pose_y = pose.position.y; + float pose_yaw = tf2::getYaw(pose.orientation); + float yaw = atan2f(point_y - pose_y, point_x - pose_x); + + if (!forward_preference) { + return std::min( + fabs(angles::shortest_angular_distance(yaw, pose_yaw)), + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); + } + return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); +} + +/** + * @brief Apply Savitzky-Golay smoothing filter to the optimal control sequence. + * Uses a 9-point quadratic filter to smooth sharp changes between control steps. + */ +inline void savitskyGolayFilter( + models::ControlSequence & control_sequence, + std::array & control_history, + const models::OptimizerSettings & settings) +{ + // Standard 9-point quadratic Savitzky-Golay smoothing coefficients (window = ±4). + // Denominator 231 is the normalization factor for this window/polynomial order. + // Reference: Savitzky & Golay, Analytical Chemistry, 1964, Table I (m=4, n=2). + Eigen::Array filter = { + -21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, -21.0f}; + filter /= 231.0f; + + const unsigned int num_sequences = control_sequence.vx.size() - 1; + // Skip smoothing for very short horizons: the 9-point window needs at least + // 5 look-ahead points (indices 0..4), so sequences shorter than 20 provide + // too little context for meaningful smoothing. + if (num_sequences < 20) {return;} + + auto applyFilter = [&](const Eigen::Array & data) -> float { + return (data * filter).eval().sum(); + }; + + auto applyFilterOverAxis = + [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence, + const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void + { + float pt_m4 = hist_0, pt_m3 = hist_1, pt_m2 = hist_2, pt_m1 = hist_3; + float pt = initial_sequence(0); + float pt_p1 = initial_sequence(1), pt_p2 = initial_sequence(2); + float pt_p3 = initial_sequence(3), pt_p4 = initial_sequence(4); + + for (unsigned int idx = 0; idx != num_sequences; idx++) { + sequence(idx) = applyFilter( + {pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); + pt_m4 = pt_m3; pt_m3 = pt_m2; pt_m2 = pt_m1; pt_m1 = pt; + pt = pt_p1; pt_p1 = pt_p2; pt_p2 = pt_p3; pt_p3 = pt_p4; + pt_p4 = (idx + 5 < num_sequences) ? + initial_sequence(idx + 5) : initial_sequence(num_sequences); + } + }; + + const models::ControlSequence initial_control_sequence = control_sequence; + applyFilterOverAxis( + control_sequence.vx, initial_control_sequence.vx, + control_history[0].vx, control_history[1].vx, + control_history[2].vx, control_history[3].vx); + applyFilterOverAxis( + control_sequence.wz, initial_control_sequence.wz, + control_history[0].wz, control_history[1].wz, + control_history[2].wz, control_history[3].wz); + + unsigned int offset = settings.shift_control_sequence ? 1 : 0; + control_history[0] = control_history[1]; + control_history[1] = control_history[2]; + control_history[2] = control_history[3]; + control_history[3] = {control_sequence.vx(offset), control_sequence.wz(offset)}; +} + +inline unsigned int findClosestPathPt( + const std::vector & vec, const float dist, const unsigned int init = 0u) +{ + float distim1 = init != 0u ? vec[init] : 0.0f; + float disti = 0.0f; + const unsigned int size = vec.size(); + for (unsigned int i = init + 1; i != size; i++) { + disti = vec[i]; + if (disti > dist) { + if (i > 0 && dist - distim1 < disti - dist) {return i - 1;} + return i; + } + distim1 = disti; + } + return size - 1; +} + +struct Pose2D { float x, y, theta; }; + +/** + * @brief Shift columns of an Eigen Array left (-1) or right (1) by one place. + * Used in trajectory integration and control sequence shifting. + */ +inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) +{ + int size = e.size(); + if (size == 1) {return;} + if (abs(direction) != 1) { + throw std::logic_error("Invalid direction, only 1 and -1 are valid values."); + } + if ((e.cols() == 1 || e.rows() == 1) && size > 1) { + auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; + auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; + while (start_ptr != end_ptr) { + *(start_ptr + direction) = *start_ptr; + start_ptr -= direction; + } + *(start_ptr + direction) = *start_ptr; + } else { + auto start_ptr = direction == 1 ? + e.data() + size - 2 * e.rows() : e.data() + e.rows(); + auto end_ptr = direction == 1 ? + e.data() : e.data() + size - e.rows(); + auto span = e.rows(); + while (start_ptr != end_ptr) { + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + start_ptr -= (direction * span); + } + std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); + } +} + +inline float clamp(const float lower_bound, const float upper_bound, const float input) +{ + return std::min(upper_bound, std::max(input, lower_bound)); +} + +} // namespace mppi::utils + +#endif // ACKERMANN_MPPI__TOOLS__UTILS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/package.xml b/src/subsystems/minimal_navigation/ackermann_mppi/package.xml new file mode 100644 index 00000000..cf682381 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/package.xml @@ -0,0 +1,36 @@ + + + + ackermann_mppi + 0.1.0 + + Standalone MPPI controller for Ackermann steering robots. + A minimal extraction of nav2_mppi_controller with the nav2 plugin + scaffolding removed — no pluginlib, no LifecycleNode dependency, + no GoalChecker, no OptimalTrajectoryValidator. + Runs as a plain rclcpp::Node. + + UMD Loop + Apache-2.0 + + ament_cmake + + angles + builtin_interfaces + geometry_msgs + nav2_costmap_2d + nav_msgs + rclcpp + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + + eigen3_cmake_module + Eigen3 + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp new file mode 100644 index 00000000..730e1081 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp @@ -0,0 +1,301 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/bool.hpp" + +#include "ackermann_mppi/optimizer.hpp" + +/** + * @class AckermannMPPINode + * @brief Standalone ROS2 node that runs MPPI for an Ackermann steering vehicle. + * + * Inputs: + * /global_path (nav_msgs/msg/Path) — global path to follow + * /odom (nav_msgs/msg/Odometry) — robot velocity feedback + * + * Outputs: + * /cmd_vel (geometry_msgs/msg/Twist) — velocity command + * + * The node also owns a Costmap2DROS instance (subscribes to sensor topics + * independently). Configure it via the standard nav2 costmap parameters. + * + * Key parameters (under the node's namespace): + * controller_frequency (double, default 10.0) — control loop rate in Hz + * plan_timeout (double, default 2.0) — seconds before stale plan triggers stop + * mppi.model_dt (float, default 0.05) — must equal 1/controller_frequency + * mppi.batch_size (int, default 1000) + * mppi.time_steps (int, default 56) + * mppi.vx_max (float, default 0.5) + * mppi.vx_min (float, default -0.35) + * mppi.wz_max (float, default 1.9) + * mppi.AckermannConstraints.min_turning_r (float, default 0.2) + * ... (see optimizer.cpp getParams() for full list) + */ +class AckermannMPPINode : public rclcpp::Node +{ +public: + explicit AckermannMPPINode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("ackermann_mppi", options) + { + // Declare parameters in constructor so they're available before configure() + declare_parameter("controller_frequency", 10.0); + declare_parameter("plan_timeout", 2.0); + declare_parameter("odom_topic", std::string("/odom")); + declare_parameter("plan_topic", std::string("/global_path")); + declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); + declare_parameter("cmd_vel_topic", std::string("/cmd_vel")); + declare_parameter("robot_base_frame", std::string("base_link")); + declare_parameter("global_frame", std::string("map")); + } + + /** + * @brief Finish setup after construction (requires shared_from_this()). + * Call this immediately after make_shared(). + */ + void configure() + { + RCLCPP_INFO(get_logger(), "AckermannMPPINode configuring..."); + + // Cache frame names — these never change at runtime. + get_parameter("global_frame", global_frame_); + get_parameter("robot_base_frame", base_frame_); + get_parameter("plan_timeout", plan_timeout_s_); + + // TF buffer and listener + tf_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_); + + // Costmap2DROS manages its own ROS2 lifecycle (sensors, inflation layers, etc). + // It needs to be spun in the same executor as this node. + // Parameters are read from the "local_costmap" namespace (standard nav2 costmap params). + costmap_ros_ = std::make_shared( + "local_costmap", + get_namespace(), // parent namespace + "local_costmap" // local namespace + ); + // Transfer the TF buffer so the costmap shares our TF instance + costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", get_parameter("use_sim_time").as_bool())); + costmap_ros_->configure(); + costmap_ros_->activate(); + + optimizer_.initialize(shared_from_this(), "mppi", costmap_ros_, tf_); + + double controller_frequency; + get_parameter("controller_frequency", controller_frequency); + + // Subscriptions + std::string odom_topic, plan_topic, nav_enabled_topic; + get_parameter("odom_topic", odom_topic); + get_parameter("plan_topic", plan_topic); + get_parameter("nav_enabled_topic", nav_enabled_topic); + + // NOTE: both callbacks write shared state that is read by the control timer. + // The mutexes (plan_mutex_, odom_mutex_) below protect against the data race + // in the MultiThreadedExecutor where these run in parallel with controlLoop(). + odom_sub_ = create_subscription( + odom_topic, rclcpp::SensorDataQoS(), + [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(odom_mutex_); + last_odom_ = *msg; + }); + + plan_sub_ = create_subscription( + plan_topic, rclcpp::QoS(1).transient_local(), + [this](const nav_msgs::msg::Path::SharedPtr msg) { + if (!msg->poses.empty()) { + std::lock_guard lock(plan_mutex_); + current_plan_ = *msg; + last_plan_time_ = now(); + } + }); + + nav_enabled_sub_ = create_subscription( + nav_enabled_topic, 1, + [this](const std_msgs::msg::Bool::SharedPtr msg) { + nav_enabled_.store(msg->data); + }); + + // Publisher + std::string cmd_vel_topic; + get_parameter("cmd_vel_topic", cmd_vel_topic); + cmd_vel_pub_ = create_publisher(cmd_vel_topic, 1); + + // Control loop timer + const auto period = std::chrono::duration(1.0 / controller_frequency); + control_timer_ = create_wall_timer(period, [this]() { controlLoop(); }); + + RCLCPP_INFO( + get_logger(), + "AckermannMPPINode configured. Control loop: %.1f Hz, plan timeout: %.1f s", + controller_frequency, plan_timeout_s_); + } + + std::shared_ptr getCostmapROS() + { + return costmap_ros_; + } + + void deactivate() + { + control_timer_->cancel(); + optimizer_.shutdown(); + costmap_ros_->deactivate(); + costmap_ros_->cleanup(); + } + +private: + void controlLoop() + { + if (!nav_enabled_.load()) { + RCLCPP_DEBUG(get_logger(), "Navigation disabled — sending zero velocity."); + cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); + return; + } + + // --- Snapshot shared state under locks --- + nav_msgs::msg::Path plan; + nav_msgs::msg::Odometry odom; + rclcpp::Time plan_time; + { + std::lock_guard lock(plan_mutex_); + if (current_plan_.poses.empty()) { + return; // No plan received yet + } + plan = current_plan_; + plan_time = last_plan_time_; + } + { + std::lock_guard lock(odom_mutex_); + odom = last_odom_; + } + + // --- Staleness check --- + const double plan_age = (now() - plan_time).seconds(); + if (plan_age > plan_timeout_s_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 2000, + "Plan is stale (%.1f s old, timeout %.1f s) — sending zero velocity.", + plan_age, plan_timeout_s_); + cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); + optimizer_.reset(); + return; + } + + // --- Look up robot pose in the global frame --- + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = global_frame_; + robot_pose.header.stamp = now(); + + try { + auto transform = tf_->lookupTransform( + global_frame_, base_frame_, + tf2::TimePointZero); + robot_pose.pose.position.x = transform.transform.translation.x; + robot_pose.pose.position.y = transform.transform.translation.y; + robot_pose.pose.position.z = transform.transform.translation.z; + robot_pose.pose.orientation = transform.transform.rotation; + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, + "Could not get robot pose: %s", ex.what()); + return; + } + + // Robot speed from odometry + geometry_msgs::msg::Twist robot_speed; + robot_speed.linear.x = odom.twist.twist.linear.x; + robot_speed.linear.y = odom.twist.twist.linear.y; + robot_speed.angular.z = odom.twist.twist.angular.z; + + // Use the last pose of the plan as the goal + const auto & goal = plan.poses.back().pose; + + try { + auto [cmd, optimal_traj] = optimizer_.evalControl( + robot_pose, robot_speed, plan, goal); + cmd_vel_pub_->publish(cmd.twist); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR(get_logger(), "MPPI failed: %s — sending zero velocity.", ex.what()); + cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); + optimizer_.reset(); + } + } + + // TF + std::shared_ptr tf_; + std::shared_ptr tf_listener_; + + // Costmap (sensor integration + inflation) + std::shared_ptr costmap_ros_; + + // MPPI optimizer + mppi::Optimizer optimizer_; + + // ROS interfaces + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr plan_sub_; + rclcpp::Subscription::SharedPtr nav_enabled_sub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::TimerBase::SharedPtr control_timer_; + + std::atomic nav_enabled_{true}; + + // Shared state — protected by corresponding mutexes. + // Written by subscription callbacks, read by control timer; all run in a + // MultiThreadedExecutor so concurrent access is real. + std::mutex plan_mutex_; + std::mutex odom_mutex_; + nav_msgs::msg::Path current_plan_; + nav_msgs::msg::Odometry last_odom_; + rclcpp::Time last_plan_time_{0, 0, RCL_ROS_TIME}; + + // Cached parameters (never change after configure()) + std::string global_frame_{"map"}; + std::string base_frame_{"base_link"}; + double plan_timeout_s_{2.0}; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + node->configure(); + + // Use a multi-threaded executor so the Costmap2DROS lifecycle node + // (which has its own callbacks for sensor data) runs alongside the control loop. + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.add_node(node->getCostmapROS()->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp new file mode 100644 index 00000000..3b985feb --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp @@ -0,0 +1,65 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "ackermann_mppi/critics/constraint_critic.hpp" + +namespace mppi::critics +{ + +void ConstraintCritic::initialize() +{ + auto getParam = getParamGetter(name_); + auto getParentParam = getParamGetter(parent_name_); + + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 4.0f); + + // min_turning_r is vehicle geometry — fixed at init. + // vx_min/vx_max are read live from data.motion_model in score() so speed limits take effect. + getParentParam(min_turning_r_, "AckermannConstraints.min_turning_r", 0.2f); + + RCLCPP_INFO( + logger_, "ConstraintCritic (Ackermann) instantiated with %d power and %f weight.", + power_, weight_); +} + +void ConstraintCritic::score(CriticData & data) +{ + if (!enabled_) {return;} + + // Read velocity limits live so that setSpeedLimit() changes take effect immediately. + const auto & c = data.motion_model->getConstraints(); + const float max_vel = c.vx_max; + // Preserve original sign convention: vx_min is typically negative for reverse. + const float min_vel = (c.vx_min > 0.0f ? 1.0f : -1.0f) * std::abs(c.vx_min); + + auto & vx = data.state.vx; + auto & wz = data.state.wz; + + // Penalize steering ratio violations: |wz| <= |vx| / min_turning_r + constexpr float kEpsilon = 1e-6f; + auto wz_safe = wz.abs().max(kEpsilon); + auto out_of_turning_rad_motion = (min_turning_r_ - (vx.abs() / wz_safe)).max(0.0f); + + if (power_ > 1u) { + data.costs += ((((vx - max_vel).max(0.0f) + (min_vel - vx).max(0.0f) + + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * + weight_).pow(power_).eval(); + } else { + data.costs += ((((vx - max_vel).max(0.0f) + (min_vel - vx).max(0.0f) + + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_).eval(); + } +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp new file mode 100644 index 00000000..27c9baa7 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp @@ -0,0 +1,55 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "ackermann_mppi/critics/goal_critic.hpp" + +namespace mppi::critics +{ + +void GoalCritic::initialize() +{ + auto getParam = getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 5.0f); + getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); + + RCLCPP_INFO( + logger_, "GoalCritic instantiated with %d power and %f weight.", + power_, weight_); +} + +void GoalCritic::score(CriticData & data) +{ + // Only activate when near the goal to provide terminal cost + if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); + const auto goal_x = goal.position.x; + const auto goal_y = goal.position.y; + + const auto delta_x = data.trajectories.x - goal_x; + const auto delta_y = data.trajectories.y - goal_y; + + if (power_ > 1u) { + data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * + weight_).pow(power_); + } else { + data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * + weight_).eval(); + } +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp new file mode 100644 index 00000000..128204e2 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp @@ -0,0 +1,214 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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. + +#include +#include "ackermann_mppi/critics/obstacles_critic.hpp" + +namespace mppi::critics +{ + +void ObstaclesCritic::initialize() +{ + auto getParam = getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(repulsion_weight_, "repulsion_weight", 1.5f); + getParam(critical_weight_, "critical_weight", 20.0f); + getParam(collision_cost_, "collision_cost", 100000.0f); + getParam(collision_margin_distance_, "collision_margin_distance", 0.10f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + + collision_checker_.setCostmap(costmap_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + + if (possible_collision_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation radius is not set sufficiently for " + "non-circular collision checking. Set inflation_radius >= half of the robot's largest " + "cross-section. This will substantially impact run-time performance."); + } + + RCLCPP_INFO( + logger_, + "ObstaclesCritic instantiated with %d power and %f / %f weights. " + "Collision check based on %s cost.", + power_, critical_weight_, repulsion_weight_, + consider_footprint_ ? "footprint" : "circular"); +} + +float ObstaclesCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + return circumscribed_cost_; + } + + std::shared_ptr inflation_layer; + for (auto & layer : *costmap->getLayeredCostmap()->getPlugins()) { + auto candidate = std::dynamic_pointer_cast(layer); + if (candidate && + (inflation_layer_name_.empty() || candidate->getName() == inflation_layer_name_)) + { + inflation_layer = candidate; + break; + } + } + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + const std::string & ln = inflation_layer->getName(); + rclcpp::Parameter p; + if (costmap->get_parameter(ln + ".cost_scaling_factor", p)) { + inflation_scale_factor_ = static_cast(p.as_double()); + } + if (costmap->get_parameter(ln + ".inflation_radius", p)) { + inflation_radius_ = static_cast(p.as_double()); + } + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found. Cannot use costmap potential field for fast collision " + "pre-screening. Only absolute collisions will be detected."); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + return circumscribed_cost_; +} + +float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) +{ + const float scale_factor = inflation_scale_factor_; + const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + // nav2 inflation layer maps INSCRIBED_INFLATED_OBSTACLE (254) - 1 = 253 to the inscribed + // radius. We invert the exponential cost formula to recover metric distance. + constexpr float kNavInscribedCostThreshold = 253.0f; + float dist_to_obj = + (scale_factor * min_radius - logf(cost.cost) + logf(kNavInscribedCostThreshold)) / + scale_factor; + if (!cost.using_footprint) { + dist_to_obj -= min_radius; + } + return dist_to_obj; +} + +void ObstaclesCritic::score(CriticData & data) +{ + if (!enabled_) {return;} + + if (consider_footprint_) { + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + } + + bool near_goal = data.state.local_path_length < near_goal_distance_; + + Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size()); + Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size()); + + const unsigned int traj_len = data.trajectories.x.cols(); + const unsigned int batch_size = data.trajectories.x.rows(); + bool all_trajectories_collide = true; + + auto & collisions = data.trajectories_in_collision; + const bool track_collisions = !collisions.empty(); + + for (unsigned int i = 0; i != batch_size; i++) { + bool trajectory_collide = false; + float traj_cost = 0.0f; + const auto & traj = data.trajectories; + CollisionCost pose_cost; + + for (unsigned int j = 0; j != traj_len; j++) { + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); + if (pose_cost.cost < 1.0f) {continue;} + + if (inCollision(pose_cost.cost)) { + trajectory_collide = true; + break; + } + + if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {continue;} + + const float dist_to_obj = distanceToObstacle(pose_cost); + if (dist_to_obj < collision_margin_distance_) { + traj_cost += (collision_margin_distance_ - dist_to_obj); + } + if (!near_goal) { + repulsive_cost[i] += inflation_radius_ - dist_to_obj; + } + } + + if (!trajectory_collide) {all_trajectories_collide = false;} + raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost; + if (trajectory_collide && track_collisions) {collisions[i] = true;} + } + + auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len; + + if (power_ > 1u) { + data.costs += + ((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized)) + .pow(power_); + } else { + data.costs += + (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized); + } + + data.fail_flag = all_trajectories_collide; +} + +bool ObstaclesCritic::inCollision(float cost) const +{ + bool is_tracking_unknown = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + using namespace nav2_costmap_2d; // NOLINT + switch (static_cast(cost)) { + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint_ ? false : true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + return false; +} + +CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) +{ + CollisionCost collision_cost; + float & cost = collision_cost.cost; + collision_cost.using_footprint = false; + + unsigned int x_i, y_i; + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { + cost = nav2_costmap_2d::NO_INFORMATION; + return collision_cost; + } + cost = collision_checker_.pointCost(x_i, y_i); + + if (consider_footprint_ && + (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) + { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + collision_cost.using_footprint = true; + } + return collision_cost; +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp new file mode 100644 index 00000000..be4813a5 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp @@ -0,0 +1,141 @@ +// Copyright (c) 2023 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. + +#include "ackermann_mppi/critics/path_align_critic.hpp" + +namespace mppi::critics +{ + +void PathAlignCritic::initialize() +{ + auto getParam = getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 10.0f); + getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); + getParam(offset_from_furthest_, "offset_from_furthest", 20); + getParam(trajectory_point_step_, "trajectory_point_step", 4); + getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); + getParam(use_path_orientations_, "use_path_orientations", false); + + RCLCPP_INFO( + logger_, "PathAlignCritic instantiated with %d power and %f weight", + power_, weight_); +} + +void PathAlignCritic::score(CriticData & data) +{ + if (!enabled_ || data.state.local_path_length < threshold_to_consider_) {return;} + + utils::setPathFurthestPointIfNotSet(data); + const size_t path_segments_count = *data.furthest_reached_path_point; + float path_segments_flt = static_cast(path_segments_count); + if (path_segments_count < offset_from_furthest_) {return;} + + utils::setPathCostsIfNotSet(data, costmap_ros_); + std::vector & path_pts_valid = *data.path_pts_valid; + float invalid_ctr = 0.0f; + for (size_t i = 0; i < path_segments_count; i++) { + if (!path_pts_valid[i]) {invalid_ctr += 1.0f;} + if (invalid_ctr / path_segments_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) { + return; + } + } + + const size_t batch_size = data.trajectories.x.rows(); + Eigen::ArrayXf cost(data.costs.rows()); + cost.setZero(); + + std::vector path_integrated_distances(path_segments_count, 0.0f); + std::vector path(path_segments_count); + float dx = 0.0f, dy = 0.0f; + for (unsigned int i = 1; i != path_segments_count; i++) { + auto & pose = path[i - 1]; + pose.x = data.path.x(i - 1); + pose.y = data.path.y(i - 1); + pose.theta = data.path.yaws(i - 1); + dx = data.path.x(i) - pose.x; + dy = data.path.y(i) - pose.y; + path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy); + } + + auto & final_pose = path[path_segments_count - 1]; + final_pose.x = data.path.x(path_segments_count - 1); + final_pose.y = data.path.y(path_segments_count - 1); + final_pose.theta = data.path.yaws(path_segments_count - 1); + + int strided_traj_rows = data.trajectories.x.rows(); + int strided_traj_cols = + static_cast(floor((data.trajectories.x.cols() - 1) / trajectory_point_step_)) + 1; + int outer_stride = strided_traj_rows * trajectory_point_step_; + + const auto T_x = Eigen::Map>( + data.trajectories.x.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_y = Eigen::Map>( + data.trajectories.y.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto T_yaw = Eigen::Map>( + data.trajectories.yaws.data(), + strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); + const auto traj_sampled_size = T_x.cols(); + + float summed_path_dist = 0.0f, dyaw = 0.0f; + unsigned int num_samples = 0u, path_pt = 0u; + float traj_integrated_distance = 0.0f; + float Tx_m1, Ty_m1; + + for (size_t t = 0; t < batch_size; ++t) { + summed_path_dist = 0.0f; + num_samples = 0u; + traj_integrated_distance = 0.0f; + path_pt = 0u; + Tx_m1 = T_x(t, 0); + Ty_m1 = T_y(t, 0); + + for (int p = 1; p < traj_sampled_size; p++) { + const float Tx = T_x(t, p); + const float Ty = T_y(t, p); + dx = Tx - Tx_m1; + dy = Ty - Ty_m1; + Tx_m1 = Tx; + Ty_m1 = Ty; + traj_integrated_distance += sqrtf(dx * dx + dy * dy); + path_pt = utils::findClosestPathPt(path_integrated_distances, traj_integrated_distance, + path_pt); + + if (path_pts_valid[path_pt]) { + const auto & pose = path[path_pt]; + dx = pose.x - Tx; + dy = pose.y - Ty; + num_samples++; + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(pose.theta, T_yaw(t, p)); + summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); + } else { + summed_path_dist += sqrtf(dx * dx + dy * dy); + } + } + } + cost(t) = num_samples > 0u ? + summed_path_dist / static_cast(num_samples) : 0.0f; + } + + if (power_ > 1u) { + data.costs += (cost * weight_).pow(power_).eval(); + } else { + data.costs += (cost * weight_).eval(); + } +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp new file mode 100644 index 00000000..d6e1207e --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp @@ -0,0 +1,69 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "ackermann_mppi/critics/path_follow_critic.hpp" +#include + +namespace mppi::critics +{ + +void PathFollowCritic::initialize() +{ + auto getParam = getParamGetter(name_); + getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); + getParam(offset_from_furthest_, "offset_from_furthest", 6); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 5.0f); +} + +void PathFollowCritic::score(CriticData & data) +{ + if (!enabled_) {return;} + + if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) { + return; + } + + utils::setPathFurthestPointIfNotSet(data); + utils::setPathCostsIfNotSet(data, costmap_ros_); + const size_t path_size = data.path.x.size() - 1; + + auto offsetted_idx = std::min( + *data.furthest_reached_path_point + offset_from_furthest_, path_size); + + // Drive to first valid (non-obstructed) path point at or after the lookahead + bool valid = false; + while (!valid && offsetted_idx < path_size - 1) { + valid = (*data.path_pts_valid)[offsetted_idx]; + if (!valid) {offsetted_idx++;} + } + + const auto path_x = data.path.x(offsetted_idx); + const auto path_y = data.path.y(offsetted_idx); + + const int rightmost_idx = data.trajectories.x.cols() - 1; + const auto last_x = data.trajectories.x.col(rightmost_idx); + const auto last_y = data.trajectories.y.col(rightmost_idx); + + const auto delta_x = last_x - path_x; + const auto delta_y = last_y - path_y; + + if (power_ > 1u) { + data.costs += (((delta_x.square() + delta_y.square()).sqrt()) * weight_).pow(power_); + } else { + data.costs += ((delta_x.square() + delta_y.square()).sqrt()) * weight_; + } +} + +} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp new file mode 100644 index 00000000..2160d7f2 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp @@ -0,0 +1,103 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "ackermann_mppi/tools/noise_generator.hpp" + +namespace mppi +{ + +void NoiseGenerator::initialize( + mppi::models::OptimizerSettings & settings, + bool regenerate_noises) +{ + settings_ = settings; + regenerate_noises_ = regenerate_noises; + active_ = true; + + ndistribution_vx_ = std::normal_distribution(0.0f, settings_.sampling_std.vx); + ndistribution_wz_ = std::normal_distribution(0.0f, settings_.sampling_std.wz); + + if (regenerate_noises_) { + noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + } else { + generateNoisedControls(); + } +} + +void NoiseGenerator::shutdown() +{ + active_ = false; + ready_ = true; + noise_cond_.notify_all(); + if (noise_thread_.joinable()) { + noise_thread_.join(); + } +} + +void NoiseGenerator::generateNextNoises() +{ + { + std::unique_lock guard(noise_lock_); + ready_ = true; + } + noise_cond_.notify_all(); +} + +void NoiseGenerator::setNoisedControls( + models::State & state, + const models::ControlSequence & control_sequence) +{ + std::unique_lock guard(noise_lock_); + state.cvx = noises_vx_.rowwise() + control_sequence.vx.transpose(); + state.cwz = noises_wz_.rowwise() + control_sequence.wz.transpose(); +} + +void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings) +{ + settings_ = settings; + + { + std::unique_lock guard(noise_lock_); + noises_vx_.setZero(settings_.batch_size, settings_.time_steps); + noises_wz_.setZero(settings_.batch_size, settings_.time_steps); + ready_ = true; + } + + if (regenerate_noises_) { + noise_cond_.notify_all(); + } else { + generateNoisedControls(); + } +} + +void NoiseGenerator::noiseThread() +{ + do { + std::unique_lock guard(noise_lock_); + noise_cond_.wait(guard, [this]() {return ready_;}); + ready_ = false; + generateNoisedControls(); + } while (active_); +} + +void NoiseGenerator::generateNoisedControls() +{ + auto & s = settings_; + noises_vx_ = Eigen::ArrayXXf::NullaryExpr( + s.batch_size, s.time_steps, [&]() {return ndistribution_vx_(generator_);}); + noises_wz_ = Eigen::ArrayXXf::NullaryExpr( + s.batch_size, s.time_steps, [&]() {return ndistribution_wz_(generator_);}); +} + +} // namespace mppi diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp new file mode 100644 index 00000000..17d75f66 --- /dev/null +++ b/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp @@ -0,0 +1,486 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2025 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. + +#include "ackermann_mppi/optimizer.hpp" + +#include +#include +#include +#include +#include +#include + +#include "ackermann_mppi/critics/path_follow_critic.hpp" +#include "ackermann_mppi/critics/path_align_critic.hpp" +#include "ackermann_mppi/critics/goal_critic.hpp" +#include "ackermann_mppi/critics/obstacles_critic.hpp" +#include "ackermann_mppi/critics/constraint_critic.hpp" + +namespace mppi +{ + +Optimizer::Optimizer() = default; + +Optimizer::~Optimizer() +{ + shutdown(); +} + +void Optimizer::initialize( + rclcpp::Node::SharedPtr node, + const std::string & name, + std::shared_ptr costmap_ros, + std::shared_ptr tf_buffer) +{ + node_ = node; + name_ = name; + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + tf_buffer_ = tf_buffer; + logger_ = node_->get_logger(); + base_frame_ = costmap_ros_->getBaseFrameID(); + + getParams(); + loadCritics(); + + bool regenerate_noises = false; + declareParam(name_ + ".regenerate_noises", false); + node_->get_parameter(name_ + ".regenerate_noises", regenerate_noises); + noise_generator_.initialize(settings_, regenerate_noises); + + reset(); +} + +void Optimizer::loadCritics() +{ + // Direct instantiation — add or remove critics here to change scoring behavior. + // Order matters: critics run in order and can set fail_flag to short-circuit the rest. + auto add = [&](auto * raw, const std::string & short_name) { + critics_.emplace_back(raw); + const std::string full_name = name_ + "." + short_name; + critics_.back()->on_configure(node_, name_, full_name, costmap_ros_); + RCLCPP_INFO(logger_, "Loaded critic: %s", short_name.c_str()); + }; + + add(new critics::ConstraintCritic(), "ConstraintCritic"); + add(new critics::ObstaclesCritic(), "ObstaclesCritic"); + add(new critics::PathFollowCritic(), "PathFollowCritic"); + add(new critics::PathAlignCritic(), "PathAlignCritic"); + add(new critics::GoalCritic(), "GoalCritic"); +} + +void Optimizer::shutdown() +{ + noise_generator_.shutdown(); +} + +void Optimizer::getParams() +{ + auto & s = settings_; + auto getParam = getParamGetter(name_); + + getParam(s.model_dt, "model_dt", 0.05f); + getParam(s.time_steps, "time_steps", 56); + getParam(s.batch_size, "batch_size", 1000); + getParam(s.iteration_count, "iteration_count", 1); + getParam(s.temperature, "temperature", 0.3f); + getParam(s.gamma, "gamma", 0.015f); + getParam(s.base_constraints.vx_max, "vx_max", 0.5f); + getParam(s.base_constraints.vx_min, "vx_min", -0.35f); + getParam(s.base_constraints.wz, "wz_max", 1.9f); + getParam(s.base_constraints.ax_max, "ax_max", 3.0f); + getParam(s.base_constraints.ax_min, "ax_min", -3.0f); + getParam(s.base_constraints.az_max, "az_max", 3.5f); + getParam(s.sampling_std.vx, "vx_std", 0.2f); + getParam(s.sampling_std.wz, "wz_std", 0.4f); + getParam(s.retry_attempt_limit, "retry_attempt_limit", 1); + getParam(s.open_loop, "open_loop", false); + getParam(s.visualize, "visualize", false); + getParam(goal_dist_tolerance_, "goal_dist_tolerance", 0.25f); + + s.base_constraints.ax_max = fabs(s.base_constraints.ax_max); + if (s.base_constraints.ax_min > 0.0f) { + s.base_constraints.ax_min = -s.base_constraints.ax_min; + RCLCPP_WARN(logger_, "ax_min sign incorrect, setting negative."); + } + + float min_turning_r = 0.2f; + getParam(min_turning_r, "AckermannConstraints.min_turning_r", 0.2f); + motion_model_ = std::make_shared(min_turning_r); + + s.constraints = s.base_constraints; + + // Determine if control period matches model_dt to enable sequence shifting + double controller_frequency = 10.0; + declareParam("controller_frequency", 10.0); + node_->get_parameter("controller_frequency", controller_frequency); + const double controller_period = 1.0 / controller_frequency; + constexpr double eps = 1e-6; + if (std::abs(controller_period - s.model_dt) < eps) { + s.shift_control_sequence = true; + RCLCPP_INFO(logger_, "Control sequence shifting enabled (controller_period == model_dt)."); + } else if (controller_period > s.model_dt + eps) { + RCLCPP_WARN( + logger_, + "controller_frequency (%.2f Hz, period=%.4f s) > model_dt (%.4f s). " + "Set controller_frequency = 1/model_dt for best performance.", + controller_frequency, controller_period, s.model_dt); + } +} + +void Optimizer::reset(bool reset_dynamic_speed_limits) +{ + state_.reset(settings_.batch_size, settings_.time_steps); + control_sequence_.reset(settings_.time_steps); + control_history_.fill({}); + + if (settings_.open_loop) { + last_command_vel_ = geometry_msgs::msg::Twist(); + } + + if (reset_dynamic_speed_limits) { + settings_.constraints = settings_.base_constraints; + } + + costs_.setZero(settings_.batch_size); + generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); + noise_generator_.reset(settings_); + motion_model_->initialize(settings_.constraints, settings_.model_dt); + + // Update critic_data_ references (they hold refs to member variables) + critics_data_.goal_dist_tolerance = goal_dist_tolerance_; + + RCLCPP_INFO(logger_, "Optimizer reset"); +} + +bool Optimizer::isSpeedLimitActive() const +{ + const auto & base = settings_.base_constraints; + const auto & curr = settings_.constraints; + return base.vx_max != curr.vx_max || + base.vx_min != curr.vx_min || + base.wz != curr.wz; +} + +std::tuple Optimizer::evalControl( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal) +{ + prepare(robot_pose, robot_speed, plan, goal); + Eigen::ArrayXXf optimal_trajectory; + + do { + optimize(); + optimal_trajectory = getOptimizedTrajectory(); + } while (fallback(critics_data_.fail_flag)); + + auto control = getControlFromSequenceAsTwist(plan.header.stamp); + last_command_vel_ = control.twist; + + if (settings_.shift_control_sequence) { + shiftControlSequence(); + } + + return std::make_tuple(control, optimal_trajectory); +} + +void Optimizer::optimize() +{ + for (size_t i = 0; i < settings_.iteration_count; ++i) { + generateNoisedTrajectories(); + evalTrajectoriesScores(); + updateControlSequence(); + } +} + +void Optimizer::evalTrajectoriesScores() +{ + critic_costs_.clear(); + + for (auto & critic : critics_) { + if (critics_data_.fail_flag) {break;} + + if (settings_.visualize) { + Eigen::ArrayXf costs_before = critics_data_.costs; + critic->score(critics_data_); + critic_costs_.emplace_back(critic->getName(), critics_data_.costs - costs_before); + } else { + critic->score(critics_data_); + } + } +} + +bool Optimizer::fallback(bool fail) +{ + if (!fail) { + fallback_counter_ = 0; + return false; + } + + reset(false); + + if (++fallback_counter_ > settings_.retry_attempt_limit) { + fallback_counter_ = 0; + throw std::runtime_error("AckermannMPPI: all trajectories in collision, no valid control."); + } + return true; +} + +void Optimizer::prepare( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::Twist & robot_speed, + const nav_msgs::msg::Path & plan, + const geometry_msgs::msg::Pose & goal) +{ + state_.pose = robot_pose; + state_.speed = settings_.open_loop ? last_command_vel_ : robot_speed; + + // Compute approximate path length (sum of Euclidean segment distances) + float path_length = 0.0f; + for (size_t i = 1; i < plan.poses.size(); ++i) { + float dx = plan.poses[i].pose.position.x - plan.poses[i - 1].pose.position.x; + float dy = plan.poses[i].pose.position.y - plan.poses[i - 1].pose.position.y; + path_length += sqrtf(dx * dx + dy * dy); + } + state_.local_path_length = path_length; + + path_ = utils::toTensor(plan); + costs_.setZero(settings_.batch_size); + goal_ = goal; + + critics_data_.fail_flag = false; + critics_data_.motion_model = motion_model_; + critics_data_.furthest_reached_path_point.reset(); + critics_data_.path_pts_valid.reset(); + critics_data_.trajectories_in_collision.clear(); +} + +void Optimizer::shiftControlSequence() +{ + auto size = control_sequence_.vx.size(); + utils::shiftColumnsByOnePlace(control_sequence_.vx, -1); + utils::shiftColumnsByOnePlace(control_sequence_.wz, -1); + control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2); + control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2); +} + +void Optimizer::generateNoisedTrajectories() +{ + noise_generator_.setNoisedControls(state_, control_sequence_); + noise_generator_.generateNextNoises(); + updateStateVelocities(state_); + integrateStateVelocities(generated_trajectories_, state_); +} + +void Optimizer::applyControlSequenceConstraints() +{ + auto & s = settings_; + + float max_delta_vx = s.model_dt * s.constraints.ax_max; + float min_delta_vx = s.model_dt * s.constraints.ax_min; + float max_delta_wz = s.model_dt * s.constraints.az_max; + + float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, + control_sequence_.vx(0)); + float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0)); + control_sequence_.vx(0) = vx_last; + control_sequence_.wz(0) = wz_last; + + for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { + float & vx_curr = control_sequence_.vx(i); + vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr); + if (vx_last > 0) { + vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr); + } else { + vx_curr = utils::clamp(vx_last - max_delta_vx, vx_last - min_delta_vx, vx_curr); + } + vx_last = vx_curr; + + float & wz_curr = control_sequence_.wz(i); + wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr); + wz_curr = utils::clamp(wz_last - max_delta_wz, wz_last + max_delta_wz, wz_curr); + wz_last = wz_curr; + } + + motion_model_->applyConstraints(control_sequence_); +} + +void Optimizer::updateStateVelocities(models::State & state) const +{ + updateInitialStateVelocities(state); + propagateStateVelocitiesFromInitials(state); +} + +void Optimizer::updateInitialStateVelocities(models::State & state) const +{ + state.vx.col(0) = static_cast(state.speed.linear.x); + state.wz.col(0) = static_cast(state.speed.angular.z); + // vy is always zero for Ackermann (non-holonomic) +} + +void Optimizer::propagateStateVelocitiesFromInitials(models::State & state) const +{ + motion_model_->predict(state); +} + +void Optimizer::integrateStateVelocities( + Eigen::Array & trajectory, + const Eigen::ArrayXXf & sequence) const +{ + float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); + + const auto vx = sequence.col(0); + const auto wz = sequence.col(1); + auto traj_x = trajectory.col(0); + auto traj_y = trajectory.col(1); + auto traj_yaws = trajectory.col(2); + + const size_t n_size = traj_yaws.size(); + if (n_size == 0) {return;} + + float last_yaw = initial_yaw; + for (size_t i = 0; i != n_size; i++) { + last_yaw += wz(i) * settings_.model_dt; + traj_yaws(i) = last_yaw; + } + + Eigen::ArrayXf yaw_cos = traj_yaws.cos(); + Eigen::ArrayXf yaw_sin = traj_yaws.sin(); + utils::shiftColumnsByOnePlace(yaw_cos, 1); + utils::shiftColumnsByOnePlace(yaw_sin, 1); + yaw_cos(0) = cosf(initial_yaw); + yaw_sin(0) = sinf(initial_yaw); + + float last_x = state_.pose.pose.position.x; + float last_y = state_.pose.pose.position.y; + for (size_t i = 0; i != n_size; i++) { + last_x += vx(i) * yaw_cos(i) * settings_.model_dt; + last_y += vx(i) * yaw_sin(i) * settings_.model_dt; + traj_x(i) = last_x; + traj_y(i) = last_y; + } +} + +void Optimizer::integrateStateVelocities( + models::Trajectories & trajectories, + const models::State & state) const +{ + auto initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); + const size_t n_cols = trajectories.yaws.cols(); + + Eigen::ArrayXf last_yaws = Eigen::ArrayXf::Constant(trajectories.yaws.rows(), initial_yaw); + for (size_t i = 0; i != n_cols; i++) { + last_yaws += state.wz.col(i) * settings_.model_dt; + trajectories.yaws.col(i) = last_yaws; + } + + Eigen::ArrayXXf yaw_cos = trajectories.yaws.cos(); + Eigen::ArrayXXf yaw_sin = trajectories.yaws.sin(); + utils::shiftColumnsByOnePlace(yaw_cos, 1); + utils::shiftColumnsByOnePlace(yaw_sin, 1); + yaw_cos.col(0) = cosf(initial_yaw); + yaw_sin.col(0) = sinf(initial_yaw); + + // Ackermann: no vy term + auto dx = (state.vx * yaw_cos).eval(); + auto dy = (state.vx * yaw_sin).eval(); + + Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant( + trajectories.x.rows(), state.pose.pose.position.x); + Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant( + trajectories.y.rows(), state.pose.pose.position.y); + + for (size_t i = 0; i != n_cols; i++) { + last_x += dx.col(i) * settings_.model_dt; + last_y += dy.col(i) * settings_.model_dt; + trajectories.x.col(i) = last_x; + trajectories.y.col(i) = last_y; + } +} + +Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() +{ + // Build [time_steps x 2] sequence (vx, wz) and integrate to poses + Eigen::ArrayXXf sequence(settings_.time_steps, 2); + Eigen::Array trajectory(settings_.time_steps, 3); + + sequence.col(0) = control_sequence_.vx; + sequence.col(1) = control_sequence_.wz; + + integrateStateVelocities(trajectory, sequence); + return trajectory; +} + +void Optimizer::updateControlSequence() +{ + auto & s = settings_; + + auto vx_T = control_sequence_.vx.transpose(); + auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; + const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx); + costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); + + if (s.sampling_std.wz > 0.0f) { + auto wz_T = control_sequence_.wz.transpose(); + auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; + const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz); + costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); + } + + auto costs_normalized = costs_ - costs_.minCoeff(); + const float inv_temp = 1.0f / s.temperature; + auto softmaxes = (-inv_temp * costs_normalized).exp().eval(); + softmaxes /= softmaxes.sum(); + + auto softmax_mat = softmaxes.matrix(); + control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat; + control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat; + + utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); + applyControlSequenceConstraints(); +} + +geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( + const builtin_interfaces::msg::Time & stamp) +{ + unsigned int offset = settings_.shift_control_sequence ? 1 : 0; + return utils::toTwistStamped( + control_sequence_.vx(offset), + control_sequence_.wz(offset), + stamp, base_frame_); +} + +void Optimizer::setSpeedLimit(double speed_limit, bool percentage) +{ + auto & s = settings_; + // nav2 costmap speed filter sentinel: 255.0 means "no limit active". + // Matches nav2_costmap_2d::NO_SPEED_LIMIT from filter_values.hpp. + constexpr double NO_SPEED_LIMIT = 255.0; + if (speed_limit == NO_SPEED_LIMIT) { + s.constraints.vx_max = s.base_constraints.vx_max; + s.constraints.vx_min = s.base_constraints.vx_min; + s.constraints.wz = s.base_constraints.wz; + } else { + double ratio = percentage ? speed_limit / 100.0 : speed_limit / s.base_constraints.vx_max; + s.constraints.vx_max = s.base_constraints.vx_max * ratio; + s.constraints.vx_min = s.base_constraints.vx_min * ratio; + s.constraints.wz = s.base_constraints.wz * ratio; + } + motion_model_->initialize(settings_.constraints, settings_.model_dt); +} + +} // namespace mppi diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt b/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt new file mode 100644 index 00000000..80e5cd00 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt @@ -0,0 +1,114 @@ +cmake_minimum_required(VERSION 3.8) +project(athena_smac_planner) + +find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_smoother REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(nlohmann_json REQUIRED) +find_package(ompl REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(msgs REQUIRED) + +nav2_package() + +if(MSVC) + add_compile_definitions(_USE_MATH_DEFINES) +else() + add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) +endif() + +set(library_name athena_smac_planner) + +add_library(${library_name} SHARED + src/a_star.cpp + src/analytic_expansion.cpp + src/collision_checker.cpp + src/costmap_downsampler.cpp + src/node_basic.cpp + src/node_hybrid.cpp + src/obstacle_heuristic.cpp + src/distance_heuristic.cpp + src/smoother.cpp + src/smac_planner_hybrid.cpp +) +target_include_directories(${library_name} + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} + ${nav2_core_INCLUDE_DIRS} + ${nav2_smoother_INCLUDE_DIRS} +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_costmap_2d::layers + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2::tf2 + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name} PRIVATE + angles::angles +) + +install(TARGETS ${library_name} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +add_executable(global_planner_node src/global_planner_node.cpp) +target_include_directories(global_planner_node PRIVATE + "$" + ${OMPL_INCLUDE_DIRS} + ${nav2_core_INCLUDE_DIRS} +) +target_link_libraries(global_planner_node + ${library_name} + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + ${msgs_TARGETS} +) + +install(TARGETS global_planner_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_libraries(${library_name}) +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_costmap_2d + nav_msgs + nlohmann_json + ompl + rclcpp + rclcpp_lifecycle + tf2 + tf2_ros + visualization_msgs +) +ament_export_targets(${PROJECT_NAME}) +ament_package() diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp new file mode 100644 index 00000000..fc587fb8 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp @@ -0,0 +1,323 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__A_STAR_HPP_ +#define ATHENA_SMAC_PLANNER__A_STAR_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_core/exceptions.hpp" + +#include "athena_smac_planner/thirdparty/robin_hood.h" +#include "athena_smac_planner/analytic_expansion.hpp" +#include "athena_smac_planner/node_hybrid.hpp" +#include "athena_smac_planner/node_basic.hpp" +#include "athena_smac_planner/goal_manager.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/constants.hpp" + +namespace athena_smac_planner +{ + +/** + * @class athena_smac_planner::AStarAlgorithm + * @brief An A* implementation for planning in a costmap. Templated based on the Node type. + */ +template +class AStarAlgorithm +{ +public: + typedef NodeT * NodePtr; + typedef robin_hood::unordered_node_map Graph; + typedef std::vector NodeVector; + typedef std::pair> NodeElement; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + typedef typename NodeVector::iterator NeighborIterator; + typedef std::function NodeGetter; + typedef GoalManager GoalManagerT; + using NodeContext = typename NodeT::NodeContext; + + + /** + * @struct athena_smac_planner::NodeComparator + * @brief Node comparison for priority queue sorting + */ + struct NodeComparator + { + bool operator()(const NodeElement & a, const NodeElement & b) const + { + return a.first > b.first; + } + }; + + typedef std::priority_queue, NodeComparator> NodeQueue; + + /** + * @brief A constructor for athena_smac_planner::AStarAlgorithm + */ + explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); + + /** + * @brief A destructor for athena_smac_planner::AStarAlgorithm + */ + ~AStarAlgorithm(); + + /** + * @brief Initialization of the planner with defaults + * @param allow_unknown Allow search in unknown space, good for navigation while mapping + * @param max_iterations Maximum number of iterations to use while expanding search + * @param max_on_approach_iterations Maximum number of iterations before returning a valid + * path once within thresholds to refine path + * comes at more compute time but smoother paths. + * @param terminal_checking_interval Number of iterations to check if the task has been canceled or + * or planning time exceeded + * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns + * false after this timeout + * @param lookup_table_size Size of the lookup table to store heuristic values + * @param dim_3_size Number of quantization bins + */ + void initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations, + const int & terminal_checking_interval, + const double & max_planning_time, + const float & lookup_table_size, + const unsigned int & dim_3_size); + + /** + * @brief Creating path from given costmap, start, and goal + * @param path Reference to a vector of indices of generated path + * @param num_iterations Reference to number of iterations to create plan + * @param tolerance Reference to tolerance in costmap nodes + * @param cancel_checker Function to check if the task has been canceled + * @param expansions_log Optional expansions logged for debug + * @return if plan was successful + */ + bool createPath( + CoordinateVector & path, int & num_iterations, const float & tolerance, + std::function cancel_checker, + std::vector> * expansions_log = nullptr); + + /** + * @brief Update the search info used by the algorithm + * @param search_info Search info to update + */ + void setSearchInfo(const SearchInfo & search_info) {_search_info = search_info;} + + /** + * @brief Sets the collision checker to use + * @param collision_checker Collision checker to use for checking state validity + */ + void setCollisionChecker(GridCollisionChecker * collision_checker); + + /** + * @brief Set the goal for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + * @param goal_heading_mode The goal heading mode to use + * @param coarse_search_resolution The resolution to search for goal heading + */ + void setGoal( + const float & mx, + const float & my, + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT, + const int & coarse_search_resolution = 1); + + /** + * @brief Set the starting pose for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setStart( + const float & mx, + const float & my, + const unsigned int & dim_3); + + /** + * @brief Get maximum number of iterations to plan + * @return Reference to Maximum iterations parameter + */ + int & getMaxIterations(); + + /** + * @brief Get pointer reference to starting node + * @return Node pointer reference to starting node + */ + NodePtr & getStart(); + + /** + * @brief Get maximum number of on-approach iterations after within threshold + * @return Reference to Maximum on-approach iterations parameter + */ + int & getOnApproachMaxIterations(); + + /** + * @brief Get tolerance, in node nodes + * @return Reference to tolerance parameter + */ + float & getToleranceHeuristic(); + + /** + * @brief Get size of graph in X + * @return Size in X + */ + unsigned int & getSizeX(); + + /** + * @brief Get size of graph in Y + * @return Size in Y + */ + unsigned int & getSizeY(); + + /** + * @brief Get number of angle quantization bins (SE2) or Z coordinate (XYZ) + * @return Number of angle bins / Z dimension + */ + unsigned int & getSizeDim3(); + + /** + * @brief Get the resolution of the coarse search + * @return Size of the goals to expand + */ + unsigned int getCoarseSearchResolution(); + + /** + * @brief Get the goals manager class + * @return Goal manager class + */ + GoalManagerT getGoalManager(); + + /** + * @brief Get pointer to shared node context + * @return Node context pointer + */ + NodeContext * getContext(); + +protected: + /** + * @brief Get pointer to next goal in open set + * @return Node pointer reference to next heuristically scored node + */ + inline NodePtr getNextNode(); + + /** + * @brief Add a node to the open set + * @param cost The cost to sort into the open set of the node + * @param node Node pointer reference to add to open set + */ + inline void addNode(const float & cost, NodePtr & node); + + /** + * @brief Adds node to graph + * @param index Node index to add + */ + inline NodePtr addToGraph(const uint64_t & index); + + /** + * @brief Get cost of heuristic of node + * @param node Node pointer to get heuristic for + * @return Heuristic cost for node + */ + inline float getHeuristicCost(const NodePtr & node); + + /** + * @brief Check if inputs to planner are valid + * @return Are valid + */ + inline bool areInputsValid(); + + /** + * @brief Get the closest path within tolerance if available + * @param path Vector of coordinates to fill with path + * @return if a valid path was found within tolerance + */ + inline bool getClosestPathWithinTolerance(CoordinateVector & path); + + /** + * @brief Clear heuristic queue of nodes to search + */ + inline void clearQueue(); + + /** + * @brief Clear graph of nodes searched + */ + inline void clearGraph(); + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param dim3 Z coordinate / theta bin of point + * @return Index + */ + inline uint64_t getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & dim3); + + /** + * @brief Check if node has been visited + * @param current_node Node to check if visited + * @return if node has been visited + */ + inline bool onVisitationCheckNode(const NodePtr & node); + + /** + * @brief Populate a debug log of expansions for Hybrid-A* for visualization + * @param node Node expanded + * @param expansions_log Log to add not expanded to + */ + inline void populateExpansionsLog( + const NodePtr & node, std::vector> * expansions_log); + + bool _traverse_unknown; + bool _is_initialized; + int _max_iterations; + int _max_on_approach_iterations; + int _terminal_checking_interval; + double _max_planning_time; + float _tolerance; + unsigned int _x_size; + unsigned int _y_size; + unsigned int _dim3_size; + unsigned int _coarse_search_resolution; + SearchInfo _search_info; + + NodePtr _start; + GoalManagerT _goal_manager; + Graph _graph; + NodeQueue _queue; + + MotionModel _motion_model; + NodeHeuristicPair _best_heuristic_node; + + GridCollisionChecker * _collision_checker; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr> _expander; + std::shared_ptr _shared_ctx; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__A_STAR_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp new file mode 100644 index 00000000..1c3263cc --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp @@ -0,0 +1,200 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ +#define ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "athena_smac_planner/node_hybrid.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/constants.hpp" + +namespace athena_smac_planner +{ + +template +class AnalyticExpansion +{ +public: + typedef NodeT * NodePtr; + typedef std::vector NodeVector; + typedef typename NodeT::Coordinates Coordinates; + typedef std::function NodeGetter; + typedef typename NodeT::CoordinateVector CoordinateVector; + using NodeContext = typename NodeT::NodeContext; + + /** + * @struct athena_smac_planner::AnalyticExpansion::AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + */ + struct AnalyticExpansionNode + { + AnalyticExpansionNode( + NodePtr & node_in, + Coordinates & initial_coords_in, + Coordinates & proposed_coords_in) + : node(node_in), + initial_coords(initial_coords_in), + proposed_coords(proposed_coords_in) + { + } + + NodePtr node; + Coordinates initial_coords; + Coordinates proposed_coords; + }; + + /** + * @struct AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + * + * This structure holds a collection of analytic expansion nodes and the number of direction + * changes encountered during the expansion. + */ + struct AnalyticExpansionNodes + { + AnalyticExpansionNodes() = default; + + void add( + NodePtr & node, + Coordinates & initial_coords, + Coordinates & proposed_coords) + { + nodes.emplace_back(node, initial_coords, proposed_coords); + } + + void setDirectionChanges(int changes) + { + direction_changes = changes; + } + + std::vector nodes; + int direction_changes{0}; + }; + + /** + * @brief Constructor for analytic expansion object + */ + AnalyticExpansion( + const MotionModel & motion_model, + const SearchInfo & search_info, + const bool & traverse_unknown, + const unsigned int & dim_3_size); + + /** + * @brief Sets the collision checker and costmap to use in expansion validation + * @param collision_checker Collision checker to use + */ + void setCollisionChecker(GridCollisionChecker * collision_checker); + + /** + * @brief Sets the shared context to use + * @param ctx Shared context pointer + */ + void setContext(NodeContext * ctx); + + /** + * @brief Attempt an analytic path completion + * @param node The node to start the analytic path from + * @param coarse_check_goals Coarse list of goals nodes to plan to + * @param fine_check_goals Fine list of goals nodes to plan to + * @param goals_coords vector of goal coordinates to plan to + * @param getter Gets a node at a set of coordinates + * @param iterations Iterations to run over + * @param closest_distance Closest distance to goal + * @return Node pointer reference to goal node with the best score out of the goals node if + * successful, else return nullptr + */ + NodePtr tryAnalyticExpansion( + const NodePtr & current_node, + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, + const NodeGetter & getter, int & iterations, + int & closest_distance); + + /** + * @brief Perform an analytic path expansion to the goal + * @param node The node to start the analytic path from + * @param goal The goal node to plan to + * @param getter The function object that gets valid nodes from the graph + * @param state_space State space to use for computing analytic expansions + * @return A set of analytically expanded nodes to the goal from current node, if possible + */ + AnalyticExpansionNodes getAnalyticPath( + const NodePtr & node, const NodePtr & goal, + const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space); + + /** + * @brief Refined analytic path from the current node to the goal + * @param node The node to start the analytic path from. Node head may + * change as a result of refinement + * @param goal_node The goal node to plan to + * @param getter The function object that gets valid nodes from the graph + * @param analytic_nodes The set of analytic nodes to refine + * @return The score of the refined path + */ + float refineAnalyticPath( + NodePtr & node, + const NodePtr & goal_node, + const NodeGetter & getter, + AnalyticExpansionNodes & analytic_nodes); + + /** + * @brief Takes final analytic expansion and appends to current expanded node + * @param node The node to start the analytic path from + * @param goal The goal node to plan to + * @param expanded_nodes Expanded nodes to append to end of current search path + * @return Node pointer to goal node if successful, else return nullptr + */ + NodePtr setAnalyticPath( + const NodePtr & node, const NodePtr & goal, + const AnalyticExpansionNodes & expanded_nodes); + + /** + * @brief Counts the number of direction changes in a Reeds-Shepp path + * @param path The Reeds-Shepp path to count direction changes in + * @return The number of direction changes in the path + */ + int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path); + + /** + * @brief Takes an expanded nodes to clean up, if necessary, of any state + * information that may be polluting it from a prior search iteration + * @param expanded_nodes Expanded node to clean up from search + */ + void cleanNode(const NodePtr & nodes); + +protected: + MotionModel _motion_model; + SearchInfo _search_info; + bool _traverse_unknown; + unsigned int _dim_3_size; + GridCollisionChecker * _collision_checker; + std::list> _detached_nodes; + NodeContext * _ctx = nullptr; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp new file mode 100644 index 00000000..ebd5023f --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp @@ -0,0 +1,139 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "athena_smac_planner/constants.hpp" +#include "rclcpp/rclcpp.hpp" + +#ifndef ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ +#define ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ + +namespace athena_smac_planner +{ + +/** + * @class athena_smac_planner::GridCollisionChecker + * @brief A costmap grid collision checker + */ +class GridCollisionChecker + : public nav2_costmap_2d::FootprintCollisionChecker +{ +public: + /** + * @brief A constructor for athena_smac_planner::GridCollisionChecker + * for use when regular bin intervals are appropriate + * @param costmap The costmap to collision check against + * @param num_quantizations The number of quantizations to precompute footprint + * @param node Node to extract clock and logger from + * orientations for to speed up collision checking + */ + GridCollisionChecker( + std::shared_ptr costmap, + unsigned int num_quantizations, + rclcpp::Node::SharedPtr node); + + /** + * @brief A constructor for athena_smac_planner::GridCollisionChecker + * for use when irregular bin intervals are appropriate + * @param costmap The costmap to collision check against + * @param angles The vector of possible angle bins to precompute for + * orientations for to speed up collision checking, in radians + */ + // GridCollisionChecker( + // nav2_costmap_2d::Costmap2D * costmap, + // std::vector & angles); + + /** + * @brief Set the footprint to use with collision checker + * @param footprint The footprint to collision check against + * @param radius Whether or not the footprint is a circle and use radius collision checking + */ + void setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_collision_cost); + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param x X coordinate of pose to check against + * @param y Y coordinate of pose to check against + * @param theta Angle bin number of pose to check against (NOT radians) + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const float & x, + const float & y, + const float & theta, + const bool & traverse_unknown); + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param i Index to search collision status of + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const unsigned int & i, + const bool & traverse_unknown); + + /** + * @brief Get cost at footprint pose in costmap + * @return the cost at the pose in costmap + */ + float getCost(); + + /** + * @brief Get the angles of the precomputed footprint orientations + * @return the ordered vector of angles corresponding to footprints + */ + std::vector & getPrecomputedAngles() + { + return angles_; + } + + /** + * @brief Get costmap ros object for inflation layer params + * @return Costmap ros + */ + std::shared_ptr getCostmapROS() {return costmap_ros_;} + + /** + * @brief Check if value outside the range + * @param min Minimum value of the range + * @param max Maximum value of the range + * @param value the value to check if it is within the range + * @return boolean if in range or not + */ + bool outsideRange(const unsigned int & max, const float & value); + +protected: + std::shared_ptr costmap_ros_; + std::vector oriented_footprints_; + nav2_costmap_2d::Footprint unoriented_footprint_; + float center_cost_; + bool footprint_is_radius_{false}; + std::vector angles_; + float possible_collision_cost_{-1}; + rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp new file mode 100644 index 00000000..78e145ef --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ +#define ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ + +#include + +namespace athena_smac_planner +{ +enum class MotionModel +{ + UNKNOWN = 0, + TWOD = 1, + DUBIN = 2, + REEDS_SHEPP = 3, + STATE_LATTICE = 4, +}; + +enum class GoalHeadingMode +{ + UNKNOWN = 0, + DEFAULT = 1, + BIDIRECTIONAL = 2, + ALL_DIRECTION = 3, +}; + +inline std::string toString(const MotionModel & n) +{ + switch (n) { + case MotionModel::TWOD: + return "2D"; + case MotionModel::DUBIN: + return "Dubin"; + case MotionModel::REEDS_SHEPP: + return "Reeds-Shepp"; + case MotionModel::STATE_LATTICE: + return "State Lattice"; + default: + return "Unknown"; + } +} + +inline MotionModel fromString(const std::string & n) +{ + if (n == "2D") { + return MotionModel::TWOD; + } else if (n == "DUBIN") { + return MotionModel::DUBIN; + } else if (n == "REEDS_SHEPP") { + return MotionModel::REEDS_SHEPP; + } else if (n == "STATE_LATTICE") { + return MotionModel::STATE_LATTICE; + } else { + return MotionModel::UNKNOWN; + } +} + +inline std::string toString(const GoalHeadingMode & n) +{ + switch (n) { + case GoalHeadingMode::DEFAULT: + return "DEFAULT"; + case GoalHeadingMode::BIDIRECTIONAL: + return "BIDIRECTIONAL"; + case GoalHeadingMode::ALL_DIRECTION: + return "ALL_DIRECTION"; + default: + return "Unknown"; + } +} + +inline GoalHeadingMode fromStringToGH(const std::string & n) +{ + if (n == "DEFAULT") { + return GoalHeadingMode::DEFAULT; + } else if (n == "BIDIRECTIONAL") { + return GoalHeadingMode::BIDIRECTIONAL; + } else if (n == "ALL_DIRECTION") { + return GoalHeadingMode::ALL_DIRECTION; + } else { + return GoalHeadingMode::UNKNOWN; + } +} + +const float UNKNOWN_COST = 255.0; +const float OCCUPIED_COST = 254.0; +const float INSCRIBED_COST = 253.0; +const float MAX_NON_OBSTACLE_COST = 252.0; +const float FREE_COST = 0; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp new file mode 100644 index 00000000..00d49f3f --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2020, Carlos Luis +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ +#define ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ + +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "athena_smac_planner/constants.hpp" + +namespace athena_smac_planner +{ + +/** + * @class athena_smac_planner::CostmapDownsampler + * @brief A costmap downsampler for more efficient path planning + */ +class CostmapDownsampler +{ +public: + /** + * @brief A constructor for CostmapDownsampler + */ + CostmapDownsampler(); + + /** + * @brief A destructor for CostmapDownsampler + */ + ~CostmapDownsampler(); + + /** + * @brief Configure the downsampled costmap object + * @param costmap The costmap we want to downsample + * @param downsampling_factor Multiplier for the costmap resolution + * @param use_min_cost_neighbor If true, min function is used instead of max for downsampling + */ + void on_configure( + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor, + const bool & use_min_cost_neighbor = false); + + /** + * @brief Cleanup the downsampled costmap + */ + void on_cleanup(); + + /** + * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap + * @param downsampling_factor Multiplier for the costmap resolution + * @return A ptr to the downsampled costmap + */ + nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor); + + /** + * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version + */ + void resizeCostmap(); + +protected: + /** + * @brief Update the sizes X-Y of the costmap and its downsampled version + */ + void updateCostmapSize(); + + /** + * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell + * @param new_mx The X-coordinate of the cell in the new costmap + * @param new_my The Y-coordinate of the cell in the new costmap + */ + void setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my); + + unsigned int _size_x; + unsigned int _size_y; + unsigned int _downsampled_size_x; + unsigned int _downsampled_size_y; + unsigned int _downsampling_factor; + bool _use_min_cost_neighbor; + float _downsampled_resolution; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _downsampled_costmap; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp new file mode 100644 index 00000000..3a74bf2e --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp @@ -0,0 +1,77 @@ +// Copyright (c) 2026, 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ +#define ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ + +#include "athena_smac_planner/constants.hpp" +#include "athena_smac_planner/types.hpp" + +namespace athena_smac_planner +{ +struct HybridMotionTable; +class NodeHybrid; + +/** + * @class athena_smac_planner::DistanceHeuristic + * @brief Distance Heuristic implementation for graph, Hybrid-A* + */ +template +class DistanceHeuristic +{ +public: + /** + * @brief A constructor for athena_smac_planner::DistanceHeuristic + */ + DistanceHeuristic() {} + + /** + * @brief Compute the SE2 distance heuristic + * @param lookup_table_dim Size, in costmap pixels, of the + * each lookup table dimension to populate + * @param motion_model Motion model to use for state space + * @param dim_3_size Number of quantization bins for caching + * @param search_info Info containing minimum radius to use + */ + template + void precomputeDistanceHeuristic( + const float & lookup_table_dim, + const MotionModel & motion_model, + const unsigned int & dim_3_size, + const SearchInfo & search_info, + MotionTableT & motion_table); + + /** + * @brief Compute the Distance heuristic + * @param node_coords Coordinates to get heuristic at + * @param goal_coords Coordinates to compute heuristic to + * @param obstacle_heuristic Value of the obstacle heuristic to compute + * additional motion heuristics if required + * @return heuristic Heuristic value + */ + template + float getDistanceHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const float & obstacle_heuristic, + MotionTableT & motion_table); + +protected: + // Dubin / Reeds-Shepp lookup and size for dereferencing + LookupTable dist_heuristic_lookup_table_; + float size_lookup_; +}; + +} // namespace athena_smac_planner +#endif // ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp new file mode 100644 index 00000000..4c2c1d87 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp @@ -0,0 +1,289 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// Copyright (c) 2024, Stevedan Ogochukwu Omodolor Omodia +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ +#define ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ + +#include +#include +#include +#include + +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/node_hybrid.hpp" +#include "athena_smac_planner/node_basic.hpp" +#include "athena_smac_planner/collision_checker.hpp" + + +namespace athena_smac_planner +{ + +/** +* @class athena_smac_planner::GoalManager +* @brief Responsible for managing multiple variables storing information on the goal +*/ +template +class GoalManager +{ +public: + typedef NodeT * NodePtr; + typedef std::vector NodeVector; + typedef std::unordered_set NodeSet; + typedef std::vector> GoalStateVector; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + using NodeContext = typename NodeT::NodeContext; + + /** + * @brief Constructor: Initializes empty goal state. sets and coordinate lists. + */ + GoalManager() + : _goals_set(NodeSet()), + _goals_state(GoalStateVector()), + _goals_coordinate(CoordinateVector()), + _ref_goal_coord(Coordinates()) + { + } + + /** + * @brief Destructor for the GoalManager + */ + ~GoalManager() = default; + + /** + * @brief Sets the node context for goal nodes + * @param ctx Pointer to the NodeContext + */ + void setContext(NodeContext * ctx) + { + _ctx = ctx; + } + + /** + * @brief Checks if the goals set is empty + * @return true if the goals set is empty + */ + bool goalsIsEmpty() + { + return _goals_state.empty(); + } + + /** + * @brief Adds goal to the goal vector + *@param goal Reference to the NodePtr + */ + void addGoal(NodePtr & goal) + { + _goals_state.push_back({goal, true}); + } + + /** + * @brief Clears all internal goal data, including goals, states, and coordinates. + */ + void clear() + { + _goals_set.clear(); + _goals_state.clear(); + _goals_coordinate.clear(); + } + + /** + * @brief Populates coarse and fine goal lists for analytic expansion. + * @param coarse_check_goals Output list of goals for coarse search expansion. + * @param fine_check_goals Output list of goals for fine search refinement. + * @param coarse_search_resolution Number of fine goals per coarse goal. + */ + void prepareGoalsForAnalyticExpansion( + NodeVector & coarse_check_goals, NodeVector & fine_check_goals, + int coarse_search_resolution) + { + for (unsigned int i = 0; i < _goals_state.size(); i++) { + if (_goals_state[i].is_valid) { + if (i % coarse_search_resolution == 0) { + coarse_check_goals.push_back(_goals_state[i].goal); + } else { + fine_check_goals.push_back(_goals_state[i].goal); + } + } + } + } + + /** + * @brief Checks if zone within the radius of a node is feasible. Returns true if + * there's at least one non-lethal cell within the node radius. + * @param node Input node. + * @param radius Search radius. + * @param collision_checker Collision checker to validate nearby nodes. + * @param traverse_unknown Flag whether traversal through unknown space is allowed. + * @return true + * @return false + */ + bool isZoneValid( + const NodePtr node, const float & radius, GridCollisionChecker * collision_checker, + const bool & traverse_unknown) const + { + if (radius < 1) { + return false; + } + + const auto size_x = collision_checker->getCostmap()->getSizeInCellsX(); + const auto size_y = collision_checker->getCostmap()->getSizeInCellsY(); + + auto getIndexFromPoint = [this](const Coordinates & point) { + const auto mx = static_cast(point.x); + const auto my = static_cast(point.y); + const auto angle = static_cast(point.theta); + return NodeT::getIndex(mx, my, angle, _ctx->motion_table.size_x, + _ctx->motion_table.num_angle_quantization); + }; + + const Coordinates & center_point = node->pose; + const float min_x = std::max(0.0f, std::floor(center_point.x - radius)); + const float min_y = std::max(0.0f, std::floor(center_point.y - radius)); + const float max_x = + std::min(static_cast(size_x - 1), std::ceil(center_point.x + radius)); + const float max_y = + std::min(static_cast(size_y - 1), std::ceil(center_point.y + radius)); + const float radius_sq = radius * radius; + + Coordinates m; + for (m.x = min_x; m.x <= max_x; m.x += 1.0f) { + for (m.y = min_y; m.y <= max_y; m.y += 1.0f) { + const float dx = m.x - center_point.x; + const float dy = m.y - center_point.y; + + if (dx * dx + dy * dy > radius_sq) { + continue; + } + + NodeT current_node(getIndexFromPoint(m), _ctx); + current_node.setPose(m); + + if (current_node.isNodeValid(traverse_unknown, collision_checker)) { + return true; + } + } + } + + return false; + } + + /** + * @brief Filters and marks invalid goals based on collision checking and tolerance thresholds. + * + * Stores only valid (or tolerably infeasible) goals into internal goal sets and coordinates. + * + * @param tolerance Heuristic tolerance allowed for infeasible goals. + * @param collision_checker Collision checker to validate goal positions. + * @param traverse_unknown Flag whether traversal through unknown space is allowed. + */ + void removeInvalidGoals( + const float & tolerance, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown) + { + // Make sure that there was a goal clear before this was run + if (!_goals_set.empty() || !_goals_coordinate.empty()) { + throw std::runtime_error( + "Goal set should be cleared before calling " + "removeinvalidgoals"); + } + for (unsigned int i = 0; i < _goals_state.size(); i++) { + if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) || + isZoneValid(_goals_state[i].goal, tolerance, collision_checker, traverse_unknown)) + { + _goals_state[i].is_valid = true; + _goals_set.insert(_goals_state[i].goal); + _goals_coordinate.push_back(_goals_state[i].goal->pose); + } else { + _goals_state[i].is_valid = false; + } + } + } + + /** + * @brief Check if a given node is part of the goal set. + * @param node Node pointer to check. + * @return if node matches any goal in the goal set. + */ + inline bool isGoal(const NodePtr & node) + { + return _goals_set.find(node) != _goals_set.end(); + } + + /** + * @brief Get pointer reference to goals set vector + * @return unordered_set of node pointers reference to the goals nodes + */ + inline NodeSet & getGoalsSet() + { + return _goals_set; + } + + /** + * @brief Get pointer reference to goals state + * @return vector of node pointers reference to the goals state + */ + inline GoalStateVector & getGoalsState() + { + return _goals_state; + } + + /** + * @brief Get pointer reference to goals coordinates + * @return vector of goals coordinates reference to the goals coordinates + */ + inline CoordinateVector & getGoalsCoordinates() + { + return _goals_coordinate; + } + + /** + * @brief Set the Reference goal coordinate + * @param coord Coordinates to set as Reference goal + */ + inline void setRefGoalCoordinates(const Coordinates & coord) + { + _ref_goal_coord = coord; + } + + /** + * @brief Checks whether the Reference goal coordinate has changed. + * @param coord Coordinates to compare with the current Reference goal coordinate. + * @return true if the Reference goal coordinate has changed, false otherwise. + */ + inline bool hasGoalChanged(const Coordinates & coord) + { + /** + * Note: This function checks if the goal has changed. This has to be done with + * the coordinates not the Node pointer because the Node pointer + * can be reused for different goals, but the coordinates will always + * be unique for each goal. + */ + return _ref_goal_coord != coord; + } + +protected: + NodeSet _goals_set; + GoalStateVector _goals_state; + CoordinateVector _goals_coordinate; + Coordinates _ref_goal_coord; + NodeContext * _ctx = nullptr; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp new file mode 100644 index 00000000..70f464c9 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ +#define ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ + +#include "athena_smac_planner/constants.hpp" +#include "athena_smac_planner/node_hybrid.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/collision_checker.hpp" + +namespace athena_smac_planner +{ + +/** + * @class athena_smac_planner::NodeBasic + * @brief NodeBasic implementation for priority queue insertion + */ +template +class NodeBasic +{ +public: + /** + * @brief A constructor for athena_smac_planner::NodeBasic + * @param index The index of this node for self-reference + */ + explicit NodeBasic(const uint64_t new_index) + : graph_node_ptr(nullptr), + index(new_index) + { + } + + /** + * @brief Take a NodeBasic and populate it with any necessary state + * cached in the queue for NodeT. + * @param node NodeT ptr to populate metadata into NodeBasic + */ + void populateSearchNode(NodeT * & node); + + /** + * @brief Take a NodeBasic and populate it with any necessary state + * cached in the queue for NodeTs. + * @param node Search node (basic) object to initialize internal node + * with state + */ + void processSearchNode(); + + typename NodeT::Coordinates pose; // Used by NodeHybrid + NodeT * graph_node_ptr; + uint64_t index; + unsigned int motion_index; + bool backward; + TurnDirection turn_dir; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp new file mode 100644 index 00000000..b9ed6092 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp @@ -0,0 +1,386 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ +#define ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ + +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#include "athena_smac_planner/constants.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/collision_checker.hpp" +#include "athena_smac_planner/costmap_downsampler.hpp" +#include "athena_smac_planner/obstacle_heuristic.hpp" +#include "athena_smac_planner/distance_heuristic.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + +namespace athena_smac_planner +{ + +// Must forward declare +class NodeHybrid; + +/** + * @struct athena_smac_planner::HybridMotionTable + * @brief A table of motion primitives and related functions + */ +struct HybridMotionTable +{ + /** + * @brief A constructor for athena_smac_planner::HybridMotionTable + */ + HybridMotionTable() {} + + /** + * @brief Initializing using Dubin model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initDubin( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Initializing using Reeds-Shepp model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initReedsShepp( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Get projections of motion models + * @param node Ptr to NodeHybrid + * @return A set of motion poses + */ + MotionPoses getProjections(const NodeHybrid * node); + + /** + * @brief Get the angular bin to use from a raw orientation + * @param theta Angle in radians + * @return bin index of closest angle to request + */ + unsigned int getClosestAngularBin(const double & theta); + + /** + * @brief Get the raw orientation from an angular bin + * @param bin_idx Index of the bin + * @return Raw orientation in radians + */ + float getAngleFromBin(const unsigned int & bin_idx); + + /** + * @brief Get the angle scaled across bins from a raw orientation + * @param theta Angle in radians + * @return angle scaled across bins + */ + double getAngle(const double & theta); + + MotionModel motion_model = MotionModel::UNKNOWN; + MotionPoses projections; + unsigned int size_x; + unsigned int num_angle_quantization; + float num_angle_quantization_float; + float min_turning_radius; + float bin_size; + float change_penalty; + float non_straight_penalty; + float cost_penalty; + float reverse_penalty; + float travel_distance_reward; + bool downsample_obstacle_heuristic; + bool use_quadratic_cost_penalty; + bool allow_primitive_interpolation; + ompl::base::StateSpacePtr state_space; + std::vector> delta_xs; + std::vector> delta_ys; + std::vector trig_values; + std::vector travel_costs; +}; + +/** + * @class athena_smac_planner::NodeHybrid + * @brief NodeHybrid implementation for graph, Hybrid-A* + */ +class NodeHybrid +{ +public: + typedef NodeHybrid * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + using Coordinates = athena_smac_planner::Coordinates; + typedef std::vector CoordinateVector; + + struct NodeContext + { + /** + * @brief A constructor for athena_smac_planner::NodeContext + */ + NodeContext() + { + obstacle_heuristic = std::make_unique(); + distance_heuristic = std::make_unique>(); + } + HybridMotionTable motion_table; + std::unique_ptr obstacle_heuristic; + std::unique_ptr> distance_heuristic; + }; + /** + * @brief A constructor for athena_smac_planner::NodeHybrid + * @param index The index of this node for self-reference + */ + explicit NodeHybrid(const uint64_t index, NodeContext * ctx); + + /** + * @brief A destructor for athena_smac_planner::NodeHybrid + */ + ~NodeHybrid(); + + /** + * @brief operator== for comparisons + * @param NodeHybrid right hand side node reference + * @return If cell indices are equal + */ + bool operator==(const NodeHybrid & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + + /** + * @brief Reset method for new search + */ + void reset(); + + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float & cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Sets the motion primitive index used to achieve node in search + * @param reference to motion primitive idx + */ + inline void setMotionPrimitiveIndex(const unsigned int & idx, const TurnDirection & turn_dir) + { + _motion_primitive_index = idx; + _turn_dir = turn_dir; + } + + /** + * @brief Gets the motion primitive index used to achieve node in search + * @return reference to motion primitive idx + */ + inline unsigned int & getMotionPrimitiveIndex() + { + return _motion_primitive_index; + } + + /** + * @brief Gets the motion primitive turning direction used to achieve node in search + * @return reference to motion primitive turning direction + */ + inline TurnDirection & getTurnDirection() + { + return _turn_dir; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + inline void visited() + { + _was_visited = true; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline uint64_t getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @param traverse_unknown If we can explore unknown nodes on the graph + * @param collision_checker: Collision checker object + * @return whether this node is valid and collision free + */ + bool isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker * collision_checker); + + /** + * @brief Get traversal cost of parent node to child node + * @param child Node pointer to child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @param width Width of costmap + * @param angle_quantization Number of theta bins + * @return Index + */ + static inline uint64_t getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle, + const unsigned int & width, const unsigned int & angle_quantization) + { + return static_cast(angle) + static_cast(x) * + static_cast(angle_quantization) + + static_cast(y) * static_cast(width) * + static_cast(angle_quantization); + } + + /** + * @brief Get coordinates at index + * @param index Index of point + * @param width Width of costmap + * @param angle_quantization Theta size of costmap + * @return Coordinates + */ + static inline Coordinates getCoords( + const uint64_t & index, + const unsigned int & width, const unsigned int & angle_quantization) + { + return Coordinates( + (index / angle_quantization) % width, // x + index / (angle_quantization * width), // y + index % angle_quantization); // theta + } + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + float getHeuristicCost( + const Coordinates & node_coords, + const CoordinateVector & goals_coords); + + /** + * @brief Initialize motion models + * @param motion_model Motion model enum to use + * @param size_x Size of X of graph + * @param size_y Size of y of graph + * @param angle_quantization Size of theta bins of graph + * @param search_info Search info to use + */ + static void initMotionModel( + NodeContext * ctx, + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & angle_quantization, + SearchInfo & search_info); + + /** + * @brief Retrieve all valid neighbors of a node. + * @param validity_checker Functor for state validity checking + * @param collision_checker Collision checker to use + * @param traverse_unknown If unknown costs are valid to traverse + * @param neighbors Vector of neighbors to be filled + */ + void getNeighbors( + std::function & validity_checker, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + /** + * @brief Set the starting pose for planning, as a node index + * @param path Reference to a vector of indices of generated path + * @return whether the path was able to be backtraced + */ + bool backtracePath(CoordinateVector & path); + + NodeHybrid * parent; + Coordinates pose; + +private: + float _cell_cost; + float _accumulated_cost; + uint64_t _index; + bool _was_visited; + unsigned int _motion_primitive_index; + TurnDirection _turn_dir; + bool _is_node_valid{false}; + NodeContext * _ctx = nullptr; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp new file mode 100644 index 00000000..2a439d02 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp @@ -0,0 +1,96 @@ +// Copyright (c) 2026, 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ +#define ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ + +#include +#include +#include +#include "athena_smac_planner/constants.hpp" +#include "athena_smac_planner/types.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace athena_smac_planner +{ + +typedef std::pair ObstacleHeuristicElement; +struct ObstacleHeuristicComparator +{ + bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const + { + return a.first > b.first; + } +}; + +typedef std::vector ObstacleHeuristicQueue; + +/** + * @class athena_smac_planner::ObstacleHeuristic + * @brief Obstacle Heuristic implementation for graph, Hybrid-A* + */ +class ObstacleHeuristic +{ +public: + /** + * @brief A constructor for athena_smac_planner::ObstacleHeuristic + */ + ObstacleHeuristic() {} + + /** + * @brief A destructor for athena_smac_planner::ObstacleHeuristic + */ + ~ObstacleHeuristic() {} + + /** + * @brief Compute the wavefront heuristic + * @param costmap Costmap to use + * @param goal_coords Coordinates to start heuristic expansion at + */ + void resetObstacleHeuristic( + std::shared_ptr costmap_ros_i, + const unsigned int & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y, + const bool downsample_obstacle_heuristic); + + /** + * @brief Compute the Obstacle heuristic + * @param node_coords Coordinates to get heuristic at + * @param goal_coords Coordinates to compute heuristic to + * @return heuristic Heuristic value + */ + float getObstacleHeuristic( + const Coordinates & node_coords, + const float & cost_penalty, + const bool use_quadratic_cost_penalty, + const bool downsample_obstacle_heuristic); + + inline float distanceHeuristic2D( + const uint64_t idx, const unsigned int size_x, + const unsigned int target_x, const unsigned int target_y) + { + int dx = static_cast(idx % size_x) - static_cast(target_x); + int dy = static_cast(idx / size_x) - static_cast(target_y); + return std::sqrt(dx * dx + dy * dy); + } + +protected: + LookupTable obstacle_heuristic_lookup_table_; + ObstacleHeuristicQueue obstacle_heuristic_queue_; + std::shared_ptr costmap_ros; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp new file mode 100644 index 00000000..c5a2cd84 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ +#define ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ + +#include +#include +#include +#include + +#include "athena_smac_planner/a_star.hpp" +#include "athena_smac_planner/smoother.hpp" +#include "athena_smac_planner/utils.hpp" +#include "athena_smac_planner/costmap_downsampler.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_array.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.hpp" + +namespace athena_smac_planner +{ + +class SmacPlannerHybrid +{ +public: + /** + * @brief Constructor — initializes the planner from ROS parameters. + * @param node Shared ptr to an rclcpp::Node for parameter reading and publisher creation. + * @param costmap_ros Costmap2DROS providing the collision costmap. + * @param name Parameter namespace prefix (default "SmacPlannerHybrid"). + */ + SmacPlannerHybrid( + rclcpp::Node::SharedPtr node, + std::shared_ptr costmap_ros, + const std::string & name = "SmacPlannerHybrid"); + + /** + * @brief Destructor + */ + ~SmacPlannerHybrid(); + + /** + * @brief Create a path from start to goal. + * @param start Start pose in map frame. + * @param goal Goal pose in map frame. + * @param cancel_checker Callable that returns true when planning should abort. + * @return nav_msgs::msg::Path of the generated path. + */ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker); + +protected: + std::unique_ptr> _a_star; + GridCollisionChecker _collision_checker; + std::unique_ptr _smoother; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")}; + nav2_costmap_2d::Costmap2D * _costmap; + std::shared_ptr _costmap_ros; + std::unique_ptr _costmap_downsampler; + std::string _global_frame, _name; + float _lookup_table_dim; + float _tolerance; + bool _downsample_costmap; + int _downsampling_factor; + double _angle_bin_size; + unsigned int _angle_quantizations; + bool _allow_unknown; + int _max_iterations; + int _max_on_approach_iterations; + int _terminal_checking_interval; + SearchInfo _search_info; + double _max_planning_time; + double _lookup_table_size; + double _minimum_turning_radius_global_coords; + bool _debug_visualizations; + std::string _motion_model_for_search; + MotionModel _motion_model; + GoalHeadingMode _goal_heading_mode; + int _coarse_search_resolution; + rclcpp::Publisher::SharedPtr _raw_plan_publisher; + rclcpp::Publisher::SharedPtr + _planned_footprints_publisher; + rclcpp::Publisher::SharedPtr + _smoothed_footprints_publisher; + rclcpp::Publisher::SharedPtr + _expansions_publisher; + std::mutex _mutex; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp new file mode 100644 index 00000000..06997543 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp @@ -0,0 +1,208 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ +#define ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ + +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "athena_smac_planner/types.hpp" +#include "athena_smac_planner/constants.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "ompl/base/StateSpace.h" + +namespace athena_smac_planner +{ + +/** + * @struct athena_smac_planner::BoundaryPoints + * @brief Set of boundary condition points from expansion + */ +struct BoundaryPoints +{ + /** + * @brief A constructor for BoundaryPoints + */ + BoundaryPoints(double & x_in, double & y_in, double & theta_in) + : x(x_in), y(y_in), theta(theta_in) + {} + + double x; + double y; + double theta; +}; + +/** + * @struct athena_smac_planner::BoundaryExpansion + * @brief Boundary expansion state + */ +struct BoundaryExpansion +{ + double path_end_idx{0.0}; + double expansion_path_length{0.0}; + double original_path_length{0.0}; + std::vector pts; + bool in_collision{false}; +}; + +typedef std::vector BoundaryExpansions; +typedef std::vector::iterator PathIterator; +typedef std::vector::reverse_iterator ReversePathIterator; + +/** + * @class athena_smac_planner::Smoother + * @brief A path smoother implementation + */ +class Smoother +{ +public: + /** + * @brief A constructor for athena_smac_planner::Smoother + */ + explicit Smoother(const SmootherParams & params); + + /** + * @brief A destructor for athena_smac_planner::Smoother + */ + ~Smoother() {} + + /** + * @brief Initialization of the smoother + * @param min_turning_radius Minimum turning radius (m) + * @param motion_model Motion model type + */ + void initialize( + const double & min_turning_radius); + + /** + * @brief Smoother API method + * @param path Reference to path + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smooth( + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time); + +protected: + /** + * @brief Smoother method - does the smoothing on a segment + * @param path Reference to path + * @param reversing_segment Return if this is a reversing segment + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time); + + /** + * @brief Get the field value for a given dimension + * @param msg Current pose to sample + * @param dim Dimension ID of interest + * @return dim value + */ + inline double getFieldByDim( + const geometry_msgs::msg::PoseStamped & msg, + const unsigned int & dim); + + /** + * @brief Set the field value for a given dimension + * @param msg Current pose to sample + * @param dim Dimension ID of interest + * @param value to set the dimension to for the pose + */ + inline void setFieldByDim( + geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, + const double & value); + + /** + * @brief Enforced minimum curvature boundary conditions on plan output + * the robot is traveling in the same direction (e.g. forward vs reverse) + * @param start_pose Start pose of the feasible path to maintain + * @param path Path to modify for curvature constraints on start / end of path + * @param costmap Costmap to check for collisions + * @param reversing_segment Whether this path segment is in reverse + */ + void enforceStartBoundaryConditions( + const geometry_msgs::msg::Pose & start_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment); + + /** + * @brief Enforced minimum curvature boundary conditions on plan output + * the robot is traveling in the same direction (e.g. forward vs reverse) + * @param end_pose End pose of the feasible path to maintain + * @param path Path to modify for curvature constraints on start / end of path + * @param costmap Costmap to check for collisions + * @param reversing_segment Whether this path segment is in reverse + */ + void enforceEndBoundaryConditions( + const geometry_msgs::msg::Pose & end_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment); + + /** + * @brief Given a set of boundary expansion, find the one which is shortest + * such that it is least likely to contain a loop-de-loop when working with + * close-by primitive markers. Instead, select a further away marker which + * generates a shorter ` + * @param boundary_expansions Set of boundary expansions + * @return Idx of the shorest boundary expansion option + */ + unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions & boundary_expansions); + + /** + * @brief Populate a motion model expansion from start->end into expansion + * @param start Start pose of the feasible path to maintain + * @param end End pose of the feasible path to maintain + * @param expansion Expansion object to populate + * @param costmap Costmap to check for collisions + * @param reversing_segment Whether this path segment is in reverse + */ + void findBoundaryExpansion( + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & end, + BoundaryExpansion & expansion, + const nav2_costmap_2d::Costmap2D * costmap); + + /** + * @brief Generates boundary expansions with end idx at least strategic + * distances away, using either Reverse or (Forward) Path Iterators. + * @param start iterator to start search in path for + * @param end iterator to end search for + * @return Boundary expansions with end idxs populated + */ + template + BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end); + + double min_turning_rad_, tolerance_, data_w_, smooth_w_; + int max_its_, refinement_ctr_, refinement_num_; + bool is_holonomic_, do_refinement_; + MotionModel motion_model_; + ompl::base::StateSpacePtr state_space_; +}; + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h new file mode 100644 index 00000000..c6b0fd60 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h @@ -0,0 +1,2539 @@ +// Copyright (c) 2018-2021 Martin Ankerl +// ______ _____ ______ _________ +// ______________ ___ /_ ___(_)_______ ___ /_ ______ ______ ______ / +// __ ___/_ __ \__ __ \__ / __ __ \ __ __ \_ __ \_ __ \_ __ / +// _ / / /_/ /_ /_/ /_ / _ / / / _ / / // /_/ // /_/ // /_/ / +// /_/ \____/ /_.___/ /_/ /_/ /_/ ________/_/ /_/ \____/ \____/ \__,_/ +// _/_____/ +// +// Fast & memory efficient hashtable based on robin hood hashing for C++11/14/17/20 +// https://github.com/martinus/robin-hood-hashing +// +// Licensed under the MIT License . +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2021 Martin Ankerl +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ +#define NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ + +// see https://semver.org/ +#define ROBIN_HOOD_VERSION_MAJOR 3 // for incompatible API changes +#define ROBIN_HOOD_VERSION_MINOR 11 // for adding functionality in a backwards-compatible manner +#define ROBIN_HOOD_VERSION_PATCH 5 // for backwards-compatible bug fixes + +#include +#include +#include +#include +#include +#include // only to support hash of smart pointers +#include +#include +#include +#include +#include +#if __cplusplus >= 201703L +# include +#endif +/* *INDENT-OFF* */ + +// #define ROBIN_HOOD_LOG_ENABLED +#ifdef ROBIN_HOOD_LOG_ENABLED +# include +# define ROBIN_HOOD_LOG(...) \ + std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; +#else +# define ROBIN_HOOD_LOG(x) +#endif + +// #define ROBIN_HOOD_TRACE_ENABLED +#ifdef ROBIN_HOOD_TRACE_ENABLED +# include +# define ROBIN_HOOD_TRACE(...) \ + std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; +#else +# define ROBIN_HOOD_TRACE(x) +#endif + +// #define ROBIN_HOOD_COUNT_ENABLED +#ifdef ROBIN_HOOD_COUNT_ENABLED +# include +# define ROBIN_HOOD_COUNT(x) ++counts().x; +namespace robin_hood { +struct Counts { + uint64_t shiftUp{}; + uint64_t shiftDown{}; +}; +inline std::ostream& operator<<(std::ostream& os, Counts const& c) { + return os << c.shiftUp << " shiftUp" << std::endl << c.shiftDown << " shiftDown" << std::endl; +} + +static Counts& counts() { + static Counts counts{}; + return counts; +} +} // namespace robin_hood +#else +# define ROBIN_HOOD_COUNT(x) +#endif + +// all non-argument macros should use this facility. See +// https://www.fluentcpp.com/2019/05/28/better-macros-better-flags/ +#define ROBIN_HOOD(x) ROBIN_HOOD_PRIVATE_DEFINITION_##x() + +// mark unused members with this macro +#define ROBIN_HOOD_UNUSED(identifier) + +// bitness +#if SIZE_MAX == UINT32_MAX +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 32 +#elif SIZE_MAX == UINT64_MAX +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 +#else +# error Unsupported bitness +#endif + +// endianness +#ifdef _MSC_VER +# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() 1 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() 0 +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() \ + (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) +# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) +#endif + +// inline +#ifdef _MSC_VER +# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __declspec(noinline) +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __attribute__((noinline)) +#endif + +// exceptions +#if !defined(__cpp_exceptions) && !defined(__EXCEPTIONS) && !defined(_CPPUNWIND) +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 0 +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 1 +#endif + +// count leading/trailing bits +#if !defined(ROBIN_HOOD_DISABLE_INTRINSICS) +# ifdef _MSC_VER +# if ROBIN_HOOD(BITNESS) == 32 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward64 +# endif +# include +# pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD)) +# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ + [](size_t mask) noexcept -> int { \ + unsigned long index; \ // NOLINT + return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast(index) \ + : ROBIN_HOOD(BITNESS); \ + }(x) +# else +# if ROBIN_HOOD(BITNESS) == 32 +# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzl +# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzl +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzll +# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzll +# endif +# define ROBIN_HOOD_COUNT_LEADING_ZEROES(x) ((x) ? ROBIN_HOOD(CLZ)(x) : ROBIN_HOOD(BITNESS)) +# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) ((x) ? ROBIN_HOOD(CTZ)(x) : ROBIN_HOOD(BITNESS)) +# endif +#endif + +// fallthrough +#ifndef __has_cpp_attribute // For backwards compatibility +# define __has_cpp_attribute(x) 0 +#endif +#if __has_cpp_attribute(clang::fallthrough) +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[clang::fallthrough]] +#elif __has_cpp_attribute(gnu::fallthrough) +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[gnu::fallthrough]] +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() +#endif + +// likely/unlikely +#ifdef _MSC_VER +# define ROBIN_HOOD_LIKELY(condition) condition +# define ROBIN_HOOD_UNLIKELY(condition) condition +#else +# define ROBIN_HOOD_LIKELY(condition) __builtin_expect(condition, 1) +# define ROBIN_HOOD_UNLIKELY(condition) __builtin_expect(condition, 0) +#endif + +// detect if native wchar_t type is available in MSVC +#ifdef _MSC_VER +# ifdef _NATIVE_WCHAR_T_DEFINED +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 0 +# endif +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 +#endif + +// detect if MSVC supports the pair(std::piecewise_construct_t,...) constructor being constexpr +#ifdef _MSC_VER +# if _MSC_VER <= 1900 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 1 +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 +# endif +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 +#endif + +// workaround missing "is_trivially_copyable" in g++ < 5.0 +// See https://stackoverflow.com/a/31798726/48181 +#if defined(__GNUC__) && __GNUC__ < 5 +# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) __has_trivial_copy(__VA_ARGS__) +#else +# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) std::is_trivially_copyable<__VA_ARGS__>::value +#endif + +// helpers for C++ versions, see https://gcc.gnu.org/onlinedocs/cpp/Standard-Predefined-Macros.html +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX() __cplusplus +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX98() 199711L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX11() 201103L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX14() 201402L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX17() 201703L + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) +# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() [[nodiscard]] +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() +#endif + +namespace robin_hood { + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) +# define ROBIN_HOOD_STD std +#else + +// c++11 compatibility layer +namespace ROBIN_HOOD_STD { +template +struct alignment_of + : std::integral_constant::type)> {}; + +template +class integer_sequence { +public: + using value_type = T; + static_assert(std::is_integral::value, "not integral type"); + static constexpr std::size_t size() noexcept { + return sizeof...(Ints); + } +}; +template +using index_sequence = integer_sequence; + +namespace detail_ { +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0 && Begin < End, "unexpected argument (Begin<0 || Begin<=End)"); + + template + struct IntSeqCombiner; + + template + struct IntSeqCombiner, integer_sequence> { + using TResult = integer_sequence; + }; + + using TResult = + typename IntSeqCombiner::TResult, + typename IntSeqImpl::TResult>::TResult; +}; + +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0, "unexpected argument (Begin<0)"); + using TResult = integer_sequence; +}; + +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0, "unexpected argument (Begin<0)"); + using TResult = integer_sequence; +}; +} // namespace detail_ + +template +using make_integer_sequence = typename detail_::IntSeqImpl::TResult; + +template +using make_index_sequence = make_integer_sequence; + +template +using index_sequence_for = make_index_sequence; + +} // namespace ROBIN_HOOD_STD + +#endif + +namespace detail { + +// make sure we static_cast to the correct type for hash_int +#if ROBIN_HOOD(BITNESS) == 64 +using SizeT = uint64_t; +#else +using SizeT = uint32_t; +#endif + +template +T rotr(T x, unsigned k) { + return (x >> k) | (x << (8U * sizeof(T) - k)); +} + +// This cast gets rid of warnings like "cast from 'uint8_t*' {aka 'unsigned char*'} to +// 'uint64_t*' {aka 'long unsigned int*'} increases required alignment of target type". Use with +// care! +template +inline T reinterpret_cast_no_cast_align_warning(void* ptr) noexcept { + return reinterpret_cast(ptr); +} + +template +inline T reinterpret_cast_no_cast_align_warning(void const* ptr) noexcept { + return reinterpret_cast(ptr); +} + +// make sure this is not inlined as it is slow and dramatically enlarges code, thus making other +// inlinings more difficult. Throws are also generally the slow path. +template +[[noreturn]] ROBIN_HOOD(NOINLINE) +#if ROBIN_HOOD(HAS_EXCEPTIONS) + void doThrow(Args&&... args) { + throw E(std::forward(args)...); +} +#else + void doThrow(Args&&... ROBIN_HOOD_UNUSED(args) /*unused*/) { + abort(); +} +#endif + +template +T* assertNotNull(T* t, Args&&... args) { + if (ROBIN_HOOD_UNLIKELY(nullptr == t)) { + doThrow(std::forward(args)...); + } + return t; +} + +template +inline T unaligned_load(void const* ptr) noexcept { + // using memcpy so we don't get into unaligned load problems. + // compiler should optimize this very well anyways. + T t; + std::memcpy(&t, ptr, sizeof(T)); + return t; +} + +// Allocates bulks of memory for objects of type T. This deallocates the memory in the destructor, +// and keeps a linked list of the allocated memory around. Overhead per allocation is the size of a +// pointer. +template +class BulkPoolAllocator { +public: + BulkPoolAllocator() noexcept = default; + + // does not copy anything, just creates a new allocator. + BulkPoolAllocator(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept + : mHead(nullptr) + , mListForFree(nullptr) {} + + BulkPoolAllocator(BulkPoolAllocator&& o) noexcept + : mHead(o.mHead) + , mListForFree(o.mListForFree) { + o.mListForFree = nullptr; + o.mHead = nullptr; + } + + BulkPoolAllocator& operator=(BulkPoolAllocator&& o) noexcept { + reset(); + mHead = o.mHead; + mListForFree = o.mListForFree; + o.mListForFree = nullptr; + o.mHead = nullptr; + return *this; + } + + BulkPoolAllocator& + operator=(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept { + // does not do anything + return *this; + } + + ~BulkPoolAllocator() noexcept { + reset(); + } + + // Deallocates all allocated memory. + void reset() noexcept { + while (mListForFree) { + T* tmp = *mListForFree; + ROBIN_HOOD_LOG("std::free") + std::free(mListForFree); + mListForFree = reinterpret_cast_no_cast_align_warning(tmp); + } + mHead = nullptr; + } + + // allocates, but does NOT initialize. Use in-place new constructor, e.g. + // T* obj = pool.allocate(); + // ::new (static_cast(obj)) T(); + T* allocate() { + T* tmp = mHead; + if (!tmp) { + tmp = performAllocation(); + } + + mHead = *reinterpret_cast_no_cast_align_warning(tmp); + return tmp; + } + + // does not actually deallocate but puts it in store. + // make sure you have already called the destructor! e.g. with + // obj->~T(); + // pool.deallocate(obj); + void deallocate(T* obj) noexcept { + *reinterpret_cast_no_cast_align_warning(obj) = mHead; + mHead = obj; + } + + // Adds an already allocated block of memory to the allocator. This allocator is from now on + // responsible for freeing the data (with free()). If the provided data is not large enough to + // make use of, it is immediately freed. Otherwise it is reused and freed in the destructor. + void addOrFree(void* ptr, const size_t numBytes) noexcept { + // calculate number of available elements in ptr + if (numBytes < ALIGNMENT + ALIGNED_SIZE) { + // not enough data for at least one element. Free and return. + ROBIN_HOOD_LOG("std::free") + std::free(ptr); + } else { + ROBIN_HOOD_LOG("add to buffer") + add(ptr, numBytes); + } + } + + void swap(BulkPoolAllocator& other) noexcept { + using std::swap; + swap(mHead, other.mHead); + swap(mListForFree, other.mListForFree); + } + +private: + // iterates the list of allocated memory to calculate how many to alloc next. + // Recalculating this each time saves us a size_t member. + // This ignores the fact that memory blocks might have been added manually with addOrFree. In + // practice, this should not matter much. + ROBIN_HOOD(NODISCARD) size_t calcNumElementsToAlloc() const noexcept { + auto tmp = mListForFree; + size_t numAllocs = MinNumAllocs; + + while (numAllocs * 2 <= MaxNumAllocs && tmp) { + auto x = reinterpret_cast(tmp); // NOLINT + tmp = *x; + numAllocs *= 2; + } + + return numAllocs; + } + + // WARNING: Underflow if numBytes < ALIGNMENT! This is guarded in addOrFree(). + void add(void* ptr, const size_t numBytes) noexcept { + const size_t numElements = (numBytes - ALIGNMENT) / ALIGNED_SIZE; + auto data = reinterpret_cast(ptr); + + // link free list + auto x = reinterpret_cast(data); + *x = mListForFree; + mListForFree = data; + + // create linked list for newly allocated data + auto* const headT = + reinterpret_cast_no_cast_align_warning(reinterpret_cast(ptr) + ALIGNMENT); + + auto* const head = reinterpret_cast(headT); + + // Visual Studio compiler automatically unrolls this loop, which is pretty cool + for (size_t i = 0; i < numElements; ++i) { + *reinterpret_cast_no_cast_align_warning(head + i * ALIGNED_SIZE) = + head + (i + 1) * ALIGNED_SIZE; + } + + // last one points to 0 + *reinterpret_cast_no_cast_align_warning(head + (numElements - 1) * ALIGNED_SIZE) = + mHead; + mHead = headT; + } + + // Called when no memory is available (mHead == 0). + // Don't inline this slow path. + ROBIN_HOOD(NOINLINE) T* performAllocation() { + size_t const numElementsToAlloc = calcNumElementsToAlloc(); + + // alloc new memory: [prev |T, T, ... T] + size_t const bytes = ALIGNMENT + ALIGNED_SIZE * numElementsToAlloc; + ROBIN_HOOD_LOG("std::malloc " << bytes << " = " << ALIGNMENT << " + " << ALIGNED_SIZE + << " * " << numElementsToAlloc) + add(assertNotNull(std::malloc(bytes)), bytes); + return mHead; + } + + // enforce byte alignment of the T's +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) + static constexpr size_t ALIGNMENT = + (std::max)(std::alignment_of::value, std::alignment_of::value); +#else + static const size_t ALIGNMENT = + (ROBIN_HOOD_STD::alignment_of::value > ROBIN_HOOD_STD::alignment_of::value) + ? ROBIN_HOOD_STD::alignment_of::value + : +ROBIN_HOOD_STD::alignment_of::value; // the + is for walkarround +#endif + + static constexpr size_t ALIGNED_SIZE = ((sizeof(T) - 1) / ALIGNMENT + 1) * ALIGNMENT; + + static_assert(MinNumAllocs >= 1, "MinNumAllocs"); + static_assert(MaxNumAllocs >= MinNumAllocs, "MaxNumAllocs"); + static_assert(ALIGNED_SIZE >= sizeof(T*), "ALIGNED_SIZE"); + static_assert(0 == (ALIGNED_SIZE % sizeof(T*)), "ALIGNED_SIZE mod"); + static_assert(ALIGNMENT >= sizeof(T*), "ALIGNMENT"); + + T* mHead{nullptr}; + T** mListForFree{nullptr}; +}; + +template +struct NodeAllocator; + +// dummy allocator that does nothing +template +struct NodeAllocator { + // we are not using the data, so just free it. + void addOrFree(void* ptr, size_t ROBIN_HOOD_UNUSED(numBytes)/*unused*/) noexcept { + ROBIN_HOOD_LOG("std::free") + std::free(ptr); + } +}; + +template +struct NodeAllocator : public BulkPoolAllocator {}; + +// c++14 doesn't have is_nothrow_swappable, and clang++ 6.0.1 doesn't like it either, so I'm making +// my own here. +namespace swappable { +#if ROBIN_HOOD(CXX) < ROBIN_HOOD(CXX17) +using std::swap; +template +struct nothrow { + static const bool value = noexcept(swap(std::declval(), std::declval())); +}; +#else +template +struct nothrow { + static const bool value = std::is_nothrow_swappable::value; +}; +#endif +} // namespace swappable + +} // namespace detail + +struct is_transparent_tag {}; + +// A custom pair implementation is used in the map because std::pair is not is_trivially_copyable, +// which means it would not be allowed to be used in std::memcpy. This struct is copiable, which is +// also tested. +template +struct pair { + using first_type = T1; + using second_type = T2; + + template ::value && + std::is_default_constructible::value>::type> + constexpr pair() noexcept(noexcept(U1()) && noexcept(U2())) + : first() + , second() {} + + // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. + explicit constexpr pair(std::pair const& o) noexcept( + noexcept(T1(std::declval())) && noexcept(T2(std::declval()))) + : first(o.first) + , second(o.second) {} + + // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. + explicit constexpr pair(std::pair&& o) noexcept(noexcept( + T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) + : first(std::move(o.first)) + , second(std::move(o.second)) {} + + constexpr pair(T1&& a, T2&& b) noexcept(noexcept( + T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) + : first(std::move(a)) + , second(std::move(b)) {} + + template + constexpr pair(U1&& a, U2&& b) noexcept(noexcept(T1(std::forward( + std::declval()))) && noexcept(T2(std::forward(std::declval())))) + : first(std::forward(a)) + , second(std::forward(b)) {} + + template + // MSVC 2015 produces error "C2476: ‘constexpr’ constructor does not initialize all members" + // if this constructor is constexpr +#if !ROBIN_HOOD(BROKEN_CONSTEXPR) + constexpr +#endif + pair(std::piecewise_construct_t /*unused*/, std::tuple a, + std::tuple + b) noexcept(noexcept(pair(std::declval&>(), + std::declval&>(), + ROBIN_HOOD_STD::index_sequence_for(), + ROBIN_HOOD_STD::index_sequence_for()))) + : pair(a, b, ROBIN_HOOD_STD::index_sequence_for(), + ROBIN_HOOD_STD::index_sequence_for()) { + } + + // constructor called from the std::piecewise_construct_t ctor + template + pair( + std::tuple& a, std::tuple& b, + ROBIN_HOOD_STD::index_sequence /*unused*/, + ROBIN_HOOD_STD::index_sequence /*unused*/) noexcept( + noexcept(T1(std::forward(std::get( + std::declval&>()))...)) && noexcept(T2(std:: + forward(std::get( + std::declval&>()))...))) + : first(std::forward(std::get(a))...) + , second(std::forward(std::get(b))...) { + // make visual studio compiler happy about warning about unused a & b. + // Visual studio's pair implementation disables warning 4100. + (void)a; + (void)b; + } + + void swap(pair& o) noexcept((detail::swappable::nothrow::value) && + (detail::swappable::nothrow::value)) { + using std::swap; + swap(first, o.first); + swap(second, o.second); + } + + T1 first; // NOLINT + T2 second; // NOLINT +}; + +template +inline void swap(pair& a, pair& b) noexcept( + noexcept(std::declval&>().swap(std::declval&>()))) { + a.swap(b); +} + +template +inline constexpr bool operator==(pair const& x, pair const& y) { + return (x.first == y.first) && (x.second == y.second); +} +template +inline constexpr bool operator!=(pair const& x, pair const& y) { + return !(x == y); +} +template +inline constexpr bool operator<(pair const& x, pair const& y) noexcept(noexcept( + std::declval() < std::declval()) && noexcept(std::declval() < + std::declval())) { + return x.first < y.first || (!(y.first < x.first) && x.second < y.second); +} +template +inline constexpr bool operator>(pair const& x, pair const& y) { + return y < x; +} +template +inline constexpr bool operator<=(pair const& x, pair const& y) { + return !(x > y); +} +template +inline constexpr bool operator>=(pair const& x, pair const& y) { + return !(x < y); +} + +inline size_t hash_bytes(void const* ptr, size_t len) noexcept { + static constexpr uint64_t m = UINT64_C(0xc6a4a7935bd1e995); + static constexpr uint64_t seed = UINT64_C(0xe17a1465); + static constexpr unsigned int r = 47; + + auto const* const data64 = static_cast(ptr); + uint64_t h = seed ^ (len * m); + + size_t const n_blocks = len / 8; + for (size_t i = 0; i < n_blocks; ++i) { + auto k = detail::unaligned_load(data64 + i); + + k *= m; + k ^= k >> r; + k *= m; + + h ^= k; + h *= m; + } + + auto const* const data8 = reinterpret_cast(data64 + n_blocks); + switch (len & 7U) { + case 7: + h ^= static_cast(data8[6]) << 48U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 6: + h ^= static_cast(data8[5]) << 40U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 5: + h ^= static_cast(data8[4]) << 32U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 4: + h ^= static_cast(data8[3]) << 24U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 3: + h ^= static_cast(data8[2]) << 16U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 2: + h ^= static_cast(data8[1]) << 8U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 1: + h ^= static_cast(data8[0]); + h *= m; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + default: + break; + } + + h ^= h >> r; + + // not doing the final step here, because this will be done by keyToIdx anyways + // h *= m; + // h ^= h >> r; + return static_cast(h); +} + +inline size_t hash_int(uint64_t x) noexcept { + // tried lots of different hashes, let's stick with murmurhash3. It's simple, fast, well tested, + // and doesn't need any special 128bit operations. + x ^= x >> 33U; + x *= UINT64_C(0xff51afd7ed558ccd); + x ^= x >> 33U; + + // not doing the final step here, because this will be done by keyToIdx anyways + // x *= UINT64_C(0xc4ceb9fe1a85ec53); + // x ^= x >> 33U; + return static_cast(x); +} + +// A thin wrapper around std::hash, performing an additional simple mixing step of the result. +template +struct hash : public std::hash { + size_t operator()(T const& obj) const + noexcept(noexcept(std::declval>().operator()(std::declval()))) { + // call base hash + auto result = std::hash::operator()(obj); + // return mixed of that, to be save against identity has + return hash_int(static_cast(result)); + } +}; + +template +struct hash> { + size_t operator()(std::basic_string const& str) const noexcept { + return hash_bytes(str.data(), sizeof(CharT) * str.size()); + } +}; + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) +template +struct hash> { + size_t operator()(std::basic_string_view const& sv) const noexcept { + return hash_bytes(sv.data(), sizeof(CharT) * sv.size()); + } +}; +#endif + +template +struct hash { + size_t operator()(T* ptr) const noexcept { + return hash_int(reinterpret_cast(ptr)); + } +}; + +template +struct hash> { + size_t operator()(std::unique_ptr const& ptr) const noexcept { + return hash_int(reinterpret_cast(ptr.get())); + } +}; + +template +struct hash> { + size_t operator()(std::shared_ptr const& ptr) const noexcept { + return hash_int(reinterpret_cast(ptr.get())); + } +}; + +template +struct hash::value>::type> { + size_t operator()(Enum e) const noexcept { + using Underlying = typename std::underlying_type::type; + return hash{}(static_cast(e)); + } +}; + +#define ROBIN_HOOD_HASH_INT(T) \ + template <> \ + struct hash { \ + size_t operator()(T const& obj) const noexcept { \ + return hash_int(static_cast(obj)); \ + } \ + } + +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wuseless-cast" +#endif +// see https://en.cppreference.com/w/cpp/utility/hash +ROBIN_HOOD_HASH_INT(bool); +ROBIN_HOOD_HASH_INT(char); +ROBIN_HOOD_HASH_INT(signed char); +ROBIN_HOOD_HASH_INT(unsigned char); +ROBIN_HOOD_HASH_INT(char16_t); +ROBIN_HOOD_HASH_INT(char32_t); +#if ROBIN_HOOD(HAS_NATIVE_WCHART) +ROBIN_HOOD_HASH_INT(wchar_t); +#endif +ROBIN_HOOD_HASH_INT(short); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned short); // NOLINT +ROBIN_HOOD_HASH_INT(int); +ROBIN_HOOD_HASH_INT(unsigned int); +ROBIN_HOOD_HASH_INT(long); // NOLINT +ROBIN_HOOD_HASH_INT(long long); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned long); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned long long); // NOLINT +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic pop +#endif +namespace detail { + +template +struct void_type { + using type = void; +}; + +template +struct has_is_transparent : public std::false_type {}; + +template +struct has_is_transparent::type> + : public std::true_type {}; + +// using wrapper classes for hash and key_equal prevents the diamond problem when the same type +// is used. see https://stackoverflow.com/a/28771920/48181 +template +struct WrapHash : public T { + WrapHash() = default; + explicit WrapHash(T const& o) noexcept(noexcept(T(std::declval()))) + : T(o) {} +}; + +template +struct WrapKeyEqual : public T { + WrapKeyEqual() = default; + explicit WrapKeyEqual(T const& o) noexcept(noexcept(T(std::declval()))) + : T(o) {} +}; + +// A highly optimized hashmap implementation, using the Robin Hood algorithm. +// +// In most cases, this map should be usable as a drop-in replacement for std::unordered_map, but +// be about 2x faster in most cases and require much less allocations. +// +// This implementation uses the following memory layout: +// +// [Node, Node, ... Node | info, info, ... infoSentinel ] +// +// * Node: either a DataNode that directly has the std::pair as member, +// or a DataNode with a pointer to std::pair. Which DataNode representation to use +// depends on how fast the swap() operation is. Heuristically, this is automatically chosen +// based on sizeof(). there are always 2^n Nodes. +// +// * info: Each Node in the map has a corresponding info byte, so there are 2^n info bytes. +// Each byte is initialized to 0, meaning the corresponding Node is empty. Set to 1 means the +// corresponding node contains data. Set to 2 means the corresponding Node is filled, but it +// actually belongs to the previous position and was pushed out because that place is already +// taken. +// +// * infoSentinel: Sentinel byte set to 1, so that iterator's ++ can stop at end() without the +// need for a idx variable. +// +// According to STL, order of templates has effect on throughput. That's why I've moved the +// boolean to the front. +// https://www.reddit.com/r/cpp/comments/ahp6iu/compile_time_binary_size_reductions_and_cs_future/eeguck4/ +template +class Table + : public WrapHash, + public WrapKeyEqual, + detail::NodeAllocator< + typename std::conditional< + std::is_void::value, Key, + robin_hood::pair::type, T>>::type, + 4, 16384, IsFlat> { +public: + static constexpr bool is_flat = IsFlat; + static constexpr bool is_map = !std::is_void::value; + static constexpr bool is_set = !is_map; + static constexpr bool is_transparent = + has_is_transparent::value && has_is_transparent::value; + + using key_type = Key; + using mapped_type = T; + using value_type = typename std::conditional< + is_set, Key, + robin_hood::pair::type, T>>::type; + using size_type = size_t; + using hasher = Hash; + using key_equal = KeyEqual; + using Self = Table; + +private: + static_assert(MaxLoadFactor100 > 10 && MaxLoadFactor100 < 100, + "MaxLoadFactor100 needs to be >10 && < 100"); + + using WHash = WrapHash; + using WKeyEqual = WrapKeyEqual; + + // configuration defaults + + // make sure we have 8 elements, needed to quickly rehash mInfo + static constexpr size_t InitialNumElements = sizeof(uint64_t); + static constexpr uint32_t InitialInfoNumBits = 5; + static constexpr uint8_t InitialInfoInc = 1U << InitialInfoNumBits; + static constexpr size_t InfoMask = InitialInfoInc - 1U; + static constexpr uint8_t InitialInfoHashShift = 0; + using DataPool = detail::NodeAllocator; + + // type needs to be wider than uint8_t. + using InfoType = uint32_t; + + // DataNode //////////////////////////////////////////////////////// + + // Primary template for the data node. We have special implementations for small and big + // objects. For large objects it is assumed that swap() is fairly slow, so we allocate these + // on the heap so swap merely swaps a pointer. + template + class DataNode {}; + + // Small: just allocate on the stack. + template + class DataNode final { + public: + template + explicit DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, Args&&... args) noexcept( + noexcept(value_type(std::forward(args)...))) + : mData(std::forward(args)...) {} + + DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept( + std::is_nothrow_move_constructible::value) + : mData(std::move(n.mData)) {} + + // doesn't do anything + void destroy(M& ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {} + void destroyDoNotDeallocate() noexcept {} + + value_type const* operator->() const noexcept { + return &mData; + } + value_type* operator->() noexcept { + return &mData; + } + + const value_type& operator*() const noexcept { + return mData; + } + + value_type& operator*() noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData.first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type + getFirst() const noexcept { + return mData.first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() const noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() noexcept { + return mData.second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() const noexcept { + return mData.second; + } + + void swap(DataNode& o) noexcept( + noexcept(std::declval().swap(std::declval()))) { + mData.swap(o.mData); + } + + private: + value_type mData; + }; + + // big object: allocate on heap. + template + class DataNode { + public: + template + explicit DataNode(M& map, Args&&... args) + : mData(map.allocate()) { + ::new (static_cast(mData)) value_type(std::forward(args)...); + } + + DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept + : mData(std::move(n.mData)) {} + + void destroy(M& map) noexcept { + // don't deallocate, just put it into list of datapool. + mData->~value_type(); + map.deallocate(mData); + } + + void destroyDoNotDeallocate() noexcept { + mData->~value_type(); + } + + value_type const* operator->() const noexcept { + return mData; + } + + value_type* operator->() noexcept { + return mData; + } + + const value_type& operator*() const { + return *mData; + } + + value_type& operator*() { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData->first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type + getFirst() const noexcept { + return mData->first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() const noexcept { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() noexcept { + return mData->second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() const noexcept { + return mData->second; + } + + void swap(DataNode& o) noexcept { + using std::swap; + swap(mData, o.mData); + } + + private: + value_type* mData; + }; + + using Node = DataNode; + + // helpers for insertKeyPrepareEmptySpot: extract first entry (only const required) + ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(Node const& n) const noexcept { + return n.getFirst(); + } + + // in case we have void mapped_type, we are not using a pair, thus we just route k through. + // No need to disable this because it's just not used if not applicable. + ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(key_type const& k) const noexcept { + return k; + } + + // in case we have non-void mapped_type, we have a standard robin_hood::pair + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, key_type const&>::type + getFirstConst(value_type const& vt) const noexcept { + return vt.first; + } + + // Cloner ////////////////////////////////////////////////////////// + + template + struct Cloner; + + // fast path: Just copy data, without allocating anything. + template + struct Cloner { + void operator()(M const& source, M& target) const { + auto const* const src = reinterpret_cast(source.mKeyVals); + auto* tgt = reinterpret_cast(target.mKeyVals); + auto const numElementsWithBuffer = target.calcNumElementsWithBuffer(target.mMask + 1); + std::copy(src, src + target.calcNumBytesTotal(numElementsWithBuffer), tgt); + } + }; + + template + struct Cloner { + void operator()(M const& s, M& t) const { + auto const numElementsWithBuffer = t.calcNumElementsWithBuffer(t.mMask + 1); + std::copy(s.mInfo, s.mInfo + t.calcNumBytesInfo(numElementsWithBuffer), t.mInfo); + + for (size_t i = 0; i < numElementsWithBuffer; ++i) { + if (t.mInfo[i]) { + ::new (static_cast(t.mKeyVals + i)) Node(t, *s.mKeyVals[i]); + } + } + } + }; + + // Destroyer /////////////////////////////////////////////////////// + + template + struct Destroyer {}; + + template + struct Destroyer { + void nodes(M& m) const noexcept { + m.mNumElements = 0; + } + + void nodesDoNotDeallocate(M& m) const noexcept { + m.mNumElements = 0; + } + }; + + template + struct Destroyer { + void nodes(M& m) const noexcept { + m.mNumElements = 0; + // clear also resets mInfo to 0, that's sometimes not necessary. + auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); + + for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { + if (0 != m.mInfo[idx]) { + Node& n = m.mKeyVals[idx]; + n.destroy(m); + n.~Node(); + } + } + } + + void nodesDoNotDeallocate(M& m) const noexcept { + m.mNumElements = 0; + // clear also resets mInfo to 0, that's sometimes not necessary. + auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); + for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { + if (0 != m.mInfo[idx]) { + Node& n = m.mKeyVals[idx]; + n.destroyDoNotDeallocate(); + n.~Node(); + } + } + } + }; + + // Iter //////////////////////////////////////////////////////////// + + struct fast_forward_tag {}; + + // generic iterator for both const_iterator and iterator. + template + class Iter { + private: + using NodePtr = typename std::conditional::type; + + public: + using difference_type = std::ptrdiff_t; + using value_type = typename Self::value_type; + using reference = typename std::conditional::type; + using pointer = typename std::conditional::type; + using iterator_category = std::forward_iterator_tag; + + // default constructed iterator can be compared to itself, but WON'T return true when + // compared to end(). + Iter() = default; + + // Rule of zero: nothing specified. The conversion constructor is only enabled for + // iterator to const_iterator, so it doesn't accidentally work as a copy ctor. + + // Conversion constructor from iterator to const_iterator. + template ::type> + Iter(Iter const& other) noexcept + : mKeyVals(other.mKeyVals) + , mInfo(other.mInfo) {} + + Iter(NodePtr valPtr, uint8_t const* infoPtr) noexcept + : mKeyVals(valPtr) + , mInfo(infoPtr) {} + + Iter(NodePtr valPtr, uint8_t const* infoPtr, + fast_forward_tag ROBIN_HOOD_UNUSED(tag) /*unused*/) noexcept + : mKeyVals(valPtr) + , mInfo(infoPtr) { + fastForward(); + } + + template ::type> + Iter& operator=(Iter const& other) noexcept { + mKeyVals = other.mKeyVals; + mInfo = other.mInfo; + return *this; + } + + // prefix increment. Undefined behavior if we are at end()! + Iter& operator++() noexcept { + mInfo++; + mKeyVals++; + fastForward(); + return *this; + } + + Iter operator++(int) noexcept { + Iter tmp = *this; + ++(*this); + return tmp; + } + + reference operator*() const { + return **mKeyVals; + } + + pointer operator->() const { + return &**mKeyVals; + } + + template + bool operator==(Iter const& o) const noexcept { + return mKeyVals == o.mKeyVals; + } + + template + bool operator!=(Iter const& o) const noexcept { + return mKeyVals != o.mKeyVals; + } + + private: + // fast forward to the next non-free info byte + // I've tried a few variants that don't depend on intrinsics, but unfortunately they are + // quite a bit slower than this one. So I've reverted that change again. See map_benchmark. + void fastForward() noexcept { + size_t n = 0; + while (0U == (n = detail::unaligned_load(mInfo))) { + mInfo += sizeof(size_t); + mKeyVals += sizeof(size_t); + } +#if defined(ROBIN_HOOD_DISABLE_INTRINSICS) + // we know for certain that within the next 8 bytes we'll find a non-zero one. + if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { + mInfo += 4; + mKeyVals += 4; + } + if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { + mInfo += 2; + mKeyVals += 2; + } + if (ROBIN_HOOD_UNLIKELY(0U == *mInfo)) { + mInfo += 1; + mKeyVals += 1; + } +#else +# if ROBIN_HOOD(LITTLE_ENDIAN) + auto inc = ROBIN_HOOD_COUNT_TRAILING_ZEROES(n) / 8; +# else + auto inc = ROBIN_HOOD_COUNT_LEADING_ZEROES(n) / 8; +# endif + mInfo += inc; + mKeyVals += inc; +#endif + } + + friend class Table; + NodePtr mKeyVals{nullptr}; + uint8_t const* mInfo{nullptr}; + }; + + //////////////////////////////////////////////////////////////////// + + // highly performance relevant code. + // Lower bits are used for indexing into the array (2^n size) + // The upper 1-5 bits need to be a reasonable good hash, to save comparisons. + template + void keyToIdx(HashKey&& key, size_t* idx, InfoType* info) const { + // In addition to whatever hash is used, add another mul & shift so we get better hashing. + // This serves as a bad hash prevention, if the given data is + // badly mixed. + auto h = static_cast(WHash::operator()(key)); + + h *= mHashMultiplier; + h ^= h >> 33U; + + // the lower InitialInfoNumBits are reserved for info. + *info = mInfoInc + static_cast((h & InfoMask) >> mInfoHashShift); + *idx = (static_cast(h) >> InitialInfoNumBits) & mMask; + } + + // forwards the index by one, wrapping around at the end + void next(InfoType* info, size_t* idx) const noexcept { + *idx = *idx + 1; + *info += mInfoInc; + } + + void nextWhileLess(InfoType* info, size_t* idx) const noexcept { + // unrolling this by hand did not bring any speedups. + while (*info < mInfo[*idx]) { + next(info, idx); + } + } + + // Shift everything up by one element. Tries to move stuff around. + void + shiftUp(size_t startIdx, + size_t const insertion_idx) noexcept(std::is_nothrow_move_assignable::value) { + auto idx = startIdx; + ::new (static_cast(mKeyVals + idx)) Node(std::move(mKeyVals[idx - 1])); + while (--idx != insertion_idx) { + mKeyVals[idx] = std::move(mKeyVals[idx - 1]); + } + + idx = startIdx; + while (idx != insertion_idx) { + ROBIN_HOOD_COUNT(shiftUp) + mInfo[idx] = static_cast(mInfo[idx - 1] + mInfoInc); + if (ROBIN_HOOD_UNLIKELY(mInfo[idx] + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + --idx; + } + } + + void shiftDown(size_t idx) noexcept(std::is_nothrow_move_assignable::value) { + // until we find one that is either empty or has zero offset. + // TODO(martinus) we don't need to move everything, just the last one for the same + // bucket. + mKeyVals[idx].destroy(*this); + + // until we find one that is either empty or has zero offset. + while (mInfo[idx + 1] >= 2 * mInfoInc) { + ROBIN_HOOD_COUNT(shiftDown) + mInfo[idx] = static_cast(mInfo[idx + 1] - mInfoInc); + mKeyVals[idx] = std::move(mKeyVals[idx + 1]); + ++idx; + } + + mInfo[idx] = 0; + // don't destroy, we've moved it + // mKeyVals[idx].destroy(*this); + mKeyVals[idx].~Node(); + } + + // copy of find(), except that it returns iterator instead of const_iterator. + template + ROBIN_HOOD(NODISCARD) + size_t findIdx(Other const& key) const { + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + + do { + // unrolling this twice gives a bit of a speedup. More unrolling did not help. + if (info == mInfo[idx] && + ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { + return idx; + } + next(&info, &idx); + if (info == mInfo[idx] && + ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { + return idx; + } + next(&info, &idx); + } while (info <= mInfo[idx]); + + // nothing found! + return mMask == 0 ? 0 + : static_cast(std::distance( + mKeyVals, reinterpret_cast_no_cast_align_warning(mInfo))); + } + + void cloneData(const Table& o) { + Cloner()(o, *this); + } + + // inserts a keyval that is guaranteed to be new, e.g. when the hashmap is resized. + // @return True on success, false if something went wrong + void insert_move(Node&& keyval) { + // we don't retry, fail if overflowing + // don't need to check max num elements + if (0 == mMaxNumElementsAllowed && !try_increase_info()) { + throwOverflowError(); + } + + size_t idx{}; + InfoType info{}; + keyToIdx(keyval.getFirst(), &idx, &info); + + // skip forward. Use <= because we are certain that the element is not there. + while (info <= mInfo[idx]) { + idx = idx + 1; + info += mInfoInc; + } + + // key not found, so we are now exactly where we want to insert it. + auto const insertion_idx = idx; + auto const insertion_info = static_cast(info); + if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + + // find an empty spot + while (0 != mInfo[idx]) { + next(&info, &idx); + } + + auto& l = mKeyVals[insertion_idx]; + if (idx == insertion_idx) { + ::new (static_cast(&l)) Node(std::move(keyval)); + } else { + shiftUp(idx, insertion_idx); + l = std::move(keyval); + } + + // put at empty spot + mInfo[insertion_idx] = insertion_info; + + ++mNumElements; + } + +public: + using iterator = Iter; + using const_iterator = Iter; + + Table() noexcept(noexcept(Hash()) && noexcept(KeyEqual())) + : WHash() + , WKeyEqual() { + ROBIN_HOOD_TRACE(this) + } + + // Creates an empty hash map. Nothing is allocated yet, this happens at the first insert. + // This tremendously speeds up ctor & dtor of a map that never receives an element. The + // penalty is paid at the first insert, and not before. Lookup of this empty map works + // because everybody points to DummyInfoByte::b. parameter bucket_count is dictated by the + // standard, but we can ignore it. + explicit Table( + size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/, const Hash& h = Hash{}, + const KeyEqual& equal = KeyEqual{}) noexcept(noexcept(Hash(h)) && noexcept(KeyEqual(equal))) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + } + + template + Table(Iter first, Iter last, size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, + const Hash& h = Hash{}, const KeyEqual& equal = KeyEqual{}) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + insert(first, last); + } + + Table(std::initializer_list initlist, + size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, const Hash& h = Hash{}, + const KeyEqual& equal = KeyEqual{}) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + insert(initlist.begin(), initlist.end()); + } + + Table(Table&& o) noexcept + : WHash(std::move(static_cast(o))) + , WKeyEqual(std::move(static_cast(o))) + , DataPool(std::move(static_cast(o))) { + ROBIN_HOOD_TRACE(this) + if (o.mMask) { + mHashMultiplier = std::move(o.mHashMultiplier); + mKeyVals = std::move(o.mKeyVals); + mInfo = std::move(o.mInfo); + mNumElements = std::move(o.mNumElements); + mMask = std::move(o.mMask); + mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); + mInfoInc = std::move(o.mInfoInc); + mInfoHashShift = std::move(o.mInfoHashShift); + // set other's mask to 0 so its destructor won't do anything + o.init(); + } + } + + Table& operator=(Table&& o) noexcept { + ROBIN_HOOD_TRACE(this) + if (&o != this) { + if (o.mMask) { + // only move stuff if the other map actually has some data + destroy(); + mHashMultiplier = std::move(o.mHashMultiplier); + mKeyVals = std::move(o.mKeyVals); + mInfo = std::move(o.mInfo); + mNumElements = std::move(o.mNumElements); + mMask = std::move(o.mMask); + mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); + mInfoInc = std::move(o.mInfoInc); + mInfoHashShift = std::move(o.mInfoHashShift); + WHash::operator=(std::move(static_cast(o))); + WKeyEqual::operator=(std::move(static_cast(o))); + DataPool::operator=(std::move(static_cast(o))); + + o.init(); + + } else { + // nothing in the other map => just clear us. + clear(); + } + } + return *this; + } + + Table(const Table& o) + : WHash(static_cast(o)) + , WKeyEqual(static_cast(o)) + , DataPool(static_cast(o)) { + ROBIN_HOOD_TRACE(this) + if (!o.empty()) { + // not empty: create an exact copy. it is also possible to just iterate through all + // elements and insert them, but copying is probably faster. + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + + ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mHashMultiplier = o.mHashMultiplier; + mKeyVals = static_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + // no need for calloc because clonData does memcpy + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + mNumElements = o.mNumElements; + mMask = o.mMask; + mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; + mInfoInc = o.mInfoInc; + mInfoHashShift = o.mInfoHashShift; + cloneData(o); + } + } + + // Creates a copy of the given map. Copy constructor of each entry is used. + // Not sure why clang-tidy thinks this doesn't handle self assignment, it does + Table& operator=(Table const& o) { + ROBIN_HOOD_TRACE(this) + if (&o == this) { + // prevent assigning of itself + return *this; + } + + // we keep using the old allocator and not assign the new one, because we want to keep + // the memory available. when it is the same size. + if (o.empty()) { + if (0 == mMask) { + // nothing to do, we are empty too + return *this; + } + + // not empty: destroy what we have there + // clear also resets mInfo to 0, that's sometimes not necessary. + destroy(); + init(); + WHash::operator=(static_cast(o)); + WKeyEqual::operator=(static_cast(o)); + DataPool::operator=(static_cast(o)); + + return *this; + } + + // clean up old stuff + Destroyer::value>{}.nodes(*this); + + if (mMask != o.mMask) { + // no luck: we don't have the same array size allocated, so we need to realloc. + if (0 != mMask) { + // only deallocate if we actually have data! + ROBIN_HOOD_LOG("std::free") + std::free(mKeyVals); + } + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mKeyVals = static_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + + // no need for calloc here because cloneData performs a memcpy. + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + // sentinel is set in cloneData + } + WHash::operator=(static_cast(o)); + WKeyEqual::operator=(static_cast(o)); + DataPool::operator=(static_cast(o)); + mHashMultiplier = o.mHashMultiplier; + mNumElements = o.mNumElements; + mMask = o.mMask; + mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; + mInfoInc = o.mInfoInc; + mInfoHashShift = o.mInfoHashShift; + cloneData(o); + + return *this; + } + + // Swaps everything between the two maps. + void swap(Table& o) { + ROBIN_HOOD_TRACE(this) + using std::swap; + swap(o, *this); + } + + // Clears all data, without resizing. + void clear() { + ROBIN_HOOD_TRACE(this) + if (empty()) { + // don't do anything! also important because we don't want to write to + // DummyInfoByte::b, even though we would just write 0 to it. + return; + } + + Destroyer::value>{}.nodes(*this); + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + // clear everything, then set the sentinel again + uint8_t const z = 0; + std::fill(mInfo, mInfo + calcNumBytesInfo(numElementsWithBuffer), z); + mInfo[numElementsWithBuffer] = 1; + + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + // Destroys the map and all it's contents. + ~Table() { + ROBIN_HOOD_TRACE(this) + destroy(); + } + + // Checks if both tables contain the same entries. Order is irrelevant. + bool operator==(const Table& other) const { + ROBIN_HOOD_TRACE(this) + if (other.size() != size()) { + return false; + } + for (auto const& otherEntry : other) { + if (!has(otherEntry)) { + return false; + } + } + + return true; + } + + bool operator!=(const Table& other) const { + ROBIN_HOOD_TRACE(this) + return !operator==(other); + } + + template + typename std::enable_if::value, Q&>::type operator[](const key_type& key) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) + Node(*this, std::piecewise_construct, std::forward_as_tuple(key), + std::forward_as_tuple()); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(key), std::forward_as_tuple()); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + } + + return mKeyVals[idxAndState.first].getSecond(); + } + + template + typename std::enable_if::value, Q&>::type operator[](key_type&& key) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) + Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), + std::forward_as_tuple()); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = + Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), + std::forward_as_tuple()); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + } + + return mKeyVals[idxAndState.first].getSecond(); + } + + template + void insert(Iter first, Iter last) { + for (; first != last; ++first) { + // value_type ctor needed because this might be called with std::pair's + insert(value_type(*first)); + } + } + + void insert(std::initializer_list ilist) { + for (auto&& vt : ilist) { + insert(std::move(vt)); + } + } + + template + std::pair emplace(Args&&... args) { + ROBIN_HOOD_TRACE(this) + Node n{*this, std::forward(args)...}; + auto idxAndState = insertKeyPrepareEmptySpot(getFirstConst(n)); + switch (idxAndState.second) { + case InsertionState::key_found: + n.destroy(*this); + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node(*this, std::move(n)); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = std::move(n); + break; + + case InsertionState::overflow_error: + n.destroy(*this); + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + template + iterator emplace_hint(const_iterator position, Args&&... args) { + (void)position; + return emplace(std::forward(args)...).first; + } + + template + std::pair try_emplace(const key_type& key, Args&&... args) { + return try_emplace_impl(key, std::forward(args)...); + } + + template + std::pair try_emplace(key_type&& key, Args&&... args) { + return try_emplace_impl(std::move(key), std::forward(args)...); + } + + template + iterator try_emplace(const_iterator hint, const key_type& key, Args&&... args) { + (void)hint; + return try_emplace_impl(key, std::forward(args)...).first; + } + + template + iterator try_emplace(const_iterator hint, key_type&& key, Args&&... args) { + (void)hint; + return try_emplace_impl(std::move(key), std::forward(args)...).first; + } + + template + std::pair insert_or_assign(const key_type& key, Mapped&& obj) { + return insertOrAssignImpl(key, std::forward(obj)); + } + + template + std::pair insert_or_assign(key_type&& key, Mapped&& obj) { + return insertOrAssignImpl(std::move(key), std::forward(obj)); + } + + template + iterator insert_or_assign(const_iterator hint, const key_type& key, Mapped&& obj) { + (void)hint; + return insertOrAssignImpl(key, std::forward(obj)).first; + } + + template + iterator insert_or_assign(const_iterator hint, key_type&& key, Mapped&& obj) { + (void)hint; + return insertOrAssignImpl(std::move(key), std::forward(obj)).first; + } + + std::pair insert(const value_type& keyval) { + ROBIN_HOOD_TRACE(this) + return emplace(keyval); + } + + iterator insert(const_iterator hint, const value_type& keyval) { + (void)hint; + return emplace(keyval).first; + } + + std::pair insert(value_type&& keyval) { + return emplace(std::move(keyval)); + } + + iterator insert(const_iterator hint, value_type&& keyval) { + (void)hint; + return emplace(std::move(keyval)).first; + } + + // Returns 1 if key is found, 0 otherwise. + size_t count(const key_type& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { + return 1; + } + return 0; + } + + template + typename std::enable_if::type count(const OtherKey& key) const { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { + return 1; + } + return 0; + } + + bool contains(const key_type& key) const { // NOLINT + return 1U == count(key); + } + + template + typename std::enable_if::type contains(const OtherKey& key) const { + return 1U == count(key); + } + + // Returns a reference to the value found for key. + // Throws std::out_of_range if element cannot be found + template + typename std::enable_if::value, Q&>::type at(key_type const& key) { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { + doThrow("key not found"); + } + return kv->getSecond(); + } + + // Returns a reference to the value found for key. + // Throws std::out_of_range if element cannot be found + template + typename std::enable_if::value, Q const&>::type at(key_type const& key) const { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { + doThrow("key not found"); + } + return kv->getSecond(); + } + + const_iterator find(const key_type& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + template + const_iterator find(const OtherKey& key, is_transparent_tag /*unused*/) const { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + template + typename std::enable_if::type // NOLINT + find(const OtherKey& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + iterator find(const key_type& key) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + template + iterator find(const OtherKey& key, is_transparent_tag/*unused*/) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + template + typename std::enable_if::type find(const OtherKey& key) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + iterator begin() { + ROBIN_HOOD_TRACE(this) + if (empty()) { + return end(); + } + return iterator(mKeyVals, mInfo, fast_forward_tag{}); + } + const_iterator begin() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return cbegin(); + } + const_iterator cbegin() const { // NOLINT + ROBIN_HOOD_TRACE(this) + if (empty()) { + return cend(); + } + return const_iterator(mKeyVals, mInfo, fast_forward_tag{}); + } + + iterator end() { + ROBIN_HOOD_TRACE(this) + // no need to supply valid info pointer: end() must not be dereferenced, and only node + // pointer is compared. + return iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; + } + const_iterator end() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return cend(); + } + const_iterator cend() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return const_iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; + } + + iterator erase(const_iterator pos) { + ROBIN_HOOD_TRACE(this) + // its safe to perform const cast here + return erase(iterator{const_cast(pos.mKeyVals), const_cast(pos.mInfo)}); + } + + // Erases element at pos, returns iterator to the next element. + iterator erase(iterator pos) { + ROBIN_HOOD_TRACE(this) + // we assume that pos always points to a valid entry, and not end(). + auto const idx = static_cast(pos.mKeyVals - mKeyVals); + + shiftDown(idx); + --mNumElements; + + if (*pos.mInfo) { + // we've backward shifted, return this again + return pos; + } + + // no backward shift, return next element + return ++pos; + } + + size_t erase(const key_type& key) { + ROBIN_HOOD_TRACE(this) + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + + // check while info matches with the source idx + do { + if (info == mInfo[idx] && WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { + shiftDown(idx); + --mNumElements; + return 1; + } + next(&info, &idx); + } while (info <= mInfo[idx]); + + // nothing found to delete + return 0; + } + + // reserves space for the specified number of elements. Makes sure the old data fits. + // exactly the same as reserve(c). + void rehash(size_t c) { + // forces a reserve + reserve(c, true); + } + + // reserves space for the specified number of elements. Makes sure the old data fits. + // Exactly the same as rehash(c). Use rehash(0) to shrink to fit. + void reserve(size_t c) { + // reserve, but don't force rehash + reserve(c, false); + } + + // If possible reallocates the map to a smaller one. This frees the underlying table. + // Does not do anything if load_factor is too large for decreasing the table's size. + void compact() { + ROBIN_HOOD_TRACE(this) + auto newSize = InitialNumElements; + while (calcMaxNumElementsAllowed(newSize) < mNumElements && newSize != 0) { + newSize *= 2; + } + if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { + throwOverflowError(); + } + + ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") + + // only actually do anything when the new size is bigger than the old one. This prevents to + // continuously allocate for each reserve() call. + if (newSize < mMask + 1) { + rehashPowerOfTwo(newSize, true); + } + } + + size_type size() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return mNumElements; + } + + size_type max_size() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return static_cast(-1); + } + + ROBIN_HOOD(NODISCARD) bool empty() const noexcept { + ROBIN_HOOD_TRACE(this) + return 0 == mNumElements; + } + + float max_load_factor() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return MaxLoadFactor100 / 100.0F; + } + + // Average number of elements per bucket. Since we allow only 1 per bucket + float load_factor() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return static_cast(size()) / static_cast(mMask + 1); + } + + ROBIN_HOOD(NODISCARD) size_t mask() const noexcept { + ROBIN_HOOD_TRACE(this) + return mMask; + } + + ROBIN_HOOD(NODISCARD) size_t calcMaxNumElementsAllowed(size_t maxElements) const noexcept { + if (ROBIN_HOOD_LIKELY(maxElements <= (std::numeric_limits::max)() / 100)) { + return maxElements * MaxLoadFactor100 / 100; + } + + // we might be a bit imprecise, but since maxElements is quite large that doesn't matter + return (maxElements / 100) * MaxLoadFactor100; + } + + ROBIN_HOOD(NODISCARD) size_t calcNumBytesInfo(size_t numElements) const noexcept { + // we add a uint64_t, which houses the sentinel (first byte) and padding so we can load + // 64bit types. + return numElements + sizeof(uint64_t); + } + + ROBIN_HOOD(NODISCARD) + size_t calcNumElementsWithBuffer(size_t numElements) const noexcept { + auto maxNumElementsAllowed = calcMaxNumElementsAllowed(numElements); + return numElements + (std::min)(maxNumElementsAllowed, (static_cast(0xFF))); + } + + // calculation only allowed for 2^n values + ROBIN_HOOD(NODISCARD) size_t calcNumBytesTotal(size_t numElements) const { +#if ROBIN_HOOD(BITNESS) == 64 + return numElements * sizeof(Node) + calcNumBytesInfo(numElements); +#else + // make sure we're doing 64bit operations, so we are at least safe against 32bit overflows. + auto const ne = static_cast(numElements); + auto const s = static_cast(sizeof(Node)); + auto const infos = static_cast(calcNumBytesInfo(numElements)); + + auto const total64 = ne * s + infos; + auto const total = static_cast(total64); + + if (ROBIN_HOOD_UNLIKELY(static_cast(total) != total64)) { + throwOverflowError(); + } + return total; +#endif + } + +private: + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, bool>::type has(const value_type& e) const { + ROBIN_HOOD_TRACE(this) + auto it = find(e.first); + return it != end() && it->second == e.second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, bool>::type has(const value_type& e) const { + ROBIN_HOOD_TRACE(this) + return find(e) != end(); + } + + void reserve(size_t c, bool forceRehash) { + ROBIN_HOOD_TRACE(this) + auto const minElementsAllowed = (std::max)(c, mNumElements); + auto newSize = InitialNumElements; + while (calcMaxNumElementsAllowed(newSize) < minElementsAllowed && newSize != 0) { + newSize *= 2; + } + if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { + throwOverflowError(); + } + + ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") + + // only actually do anything when the new size is bigger than the old one. This prevents to + // continuously allocate for each reserve() call. + if (forceRehash || newSize > mMask + 1) { + rehashPowerOfTwo(newSize, false); + } + } + + // reserves space for at least the specified number of elements. + // only works if numBuckets if power of two + // True on success, false otherwise + void rehashPowerOfTwo(size_t numBuckets, bool forceFree) { + ROBIN_HOOD_TRACE(this) + + Node* const oldKeyVals = mKeyVals; + uint8_t const* const oldInfo = mInfo; + + const size_t oldMaxElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + + // resize operation: move stuff + initData(numBuckets); + if (oldMaxElementsWithBuffer > 1) { + for (size_t i = 0; i < oldMaxElementsWithBuffer; ++i) { + if (oldInfo[i] != 0) { + // might throw an exception, which is really bad since we are in the middle of + // moving stuff. + insert_move(std::move(oldKeyVals[i])); + // destroy the node but DON'T destroy the data. + oldKeyVals[i].~Node(); + } + } + + // this check is not necessary as it's guarded by the previous if, but it helps + // silence g++'s overeager "attempt to free a non-heap object 'map' + // [-Werror=free-nonheap-object]" warning. + if (oldKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { + // don't destroy old data: put it into the pool instead + if (forceFree) { + std::free(oldKeyVals); + } else { + DataPool::addOrFree(oldKeyVals, calcNumBytesTotal(oldMaxElementsWithBuffer)); + } + } + } + } + + ROBIN_HOOD(NOINLINE) void throwOverflowError() const { +#if ROBIN_HOOD(HAS_EXCEPTIONS) + throw std::overflow_error("robin_hood::map overflow"); +#else + abort(); +#endif + } + + template + std::pair try_emplace_impl(OtherKey&& key, Args&&... args) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node( + *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(args)...)); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(args)...)); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + template + std::pair insertOrAssignImpl(OtherKey&& key, Mapped&& obj) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + mKeyVals[idxAndState.first].getSecond() = std::forward(obj); + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node( + *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(obj))); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(obj))); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + void initData(size_t max_elements) { + mNumElements = 0; + mMask = max_elements - 1; + mMaxNumElementsAllowed = calcMaxNumElementsAllowed(max_elements); + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(max_elements); + + // malloc & zero mInfo. Faster than calloc everything. + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + ROBIN_HOOD_LOG("std::calloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mKeyVals = reinterpret_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + std::memset(mInfo, 0, numBytesTotal - numElementsWithBuffer * sizeof(Node)); + + // set sentinel + mInfo[numElementsWithBuffer] = 1; + + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + enum class InsertionState { overflow_error, key_found, new_node, overwrite_node }; + + // Finds key, and if not already present prepares a spot where to pot the key & value. + // This potentially shifts nodes out of the way, updates mInfo and number of inserted + // elements, so the only operation left to do is create/assign a new node at that spot. + template + std::pair insertKeyPrepareEmptySpot(OtherKey&& key) { + for (int i = 0; i < 256; ++i) { + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + nextWhileLess(&info, &idx); + + // while we potentially have a match + while (info == mInfo[idx]) { + if (WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { + // key already exists, do NOT insert. + // see http://en.cppreference.com/w/cpp/container/unordered_map/insert + return std::make_pair(idx, InsertionState::key_found); + } + next(&info, &idx); + } + + // unlikely that this evaluates to true + if (ROBIN_HOOD_UNLIKELY(mNumElements >= mMaxNumElementsAllowed)) { + if (!increase_size()) { + return std::make_pair(size_t(0), InsertionState::overflow_error); + } + continue; + } + + // key not found, so we are now exactly where we want to insert it. + auto const insertion_idx = idx; + auto const insertion_info = info; + if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + + // find an empty spot + while (0 != mInfo[idx]) { + next(&info, &idx); + } + + if (idx != insertion_idx) { + shiftUp(idx, insertion_idx); + } + // put at empty spot + mInfo[insertion_idx] = static_cast(insertion_info); + ++mNumElements; + return std::make_pair(insertion_idx, idx == insertion_idx + ? InsertionState::new_node + : InsertionState::overwrite_node); + } + + // enough attempts failed, so finally give up. + return std::make_pair(size_t(0), InsertionState::overflow_error); + } + + bool try_increase_info() { + ROBIN_HOOD_LOG("mInfoInc=" << mInfoInc << ", numElements=" << mNumElements + << ", maxNumElementsAllowed=" + << calcMaxNumElementsAllowed(mMask + 1)) + if (mInfoInc <= 2) { + // need to be > 2 so that shift works (otherwise undefined behavior!) + return false; + } + // we got space left, try to make info smaller + mInfoInc = static_cast(mInfoInc >> 1U); + + // remove one bit of the hash, leaving more space for the distance info. + // This is extremely fast because we can operate on 8 bytes at once. + ++mInfoHashShift; + auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + + for (size_t i = 0; i < numElementsWithBuffer; i += 8) { + auto val = unaligned_load(mInfo + i); + val = (val >> 1U) & UINT64_C(0x7f7f7f7f7f7f7f7f); + std::memcpy(mInfo + i, &val, sizeof(val)); + } + // update sentinel, which might have been cleared out! + mInfo[numElementsWithBuffer] = 1; + + mMaxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); + return true; + } + + // True if resize was possible, false otherwise + bool increase_size() { + // nothing allocated yet? just allocate InitialNumElements + if (0 == mMask) { + initData(InitialNumElements); + return true; + } + + auto const maxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); + if (mNumElements < maxNumElementsAllowed && try_increase_info()) { + return true; + } + + ROBIN_HOOD_LOG("mNumElements=" << mNumElements << ", maxNumElementsAllowed=" + << maxNumElementsAllowed << ", load=" + << (static_cast(mNumElements) * 100.0 / + (static_cast(mMask) + 1))) + + if (mNumElements * 2 < calcMaxNumElementsAllowed(mMask + 1)) { + // we have to resize, even though there would still be plenty of space left! + // Try to rehash instead. Delete freed memory so we don't steadyily increase mem in case + // we have to rehash a few times + nextHashMultiplier(); + rehashPowerOfTwo(mMask + 1, true); + } else { + // we've reached the capacity of the map, so the hash seems to work nice. Keep using it. + rehashPowerOfTwo((mMask + 1) * 2, false); + } + return true; + } + + void nextHashMultiplier() { + // adding an *even* number, so that the multiplier will always stay odd. This is necessary + // so that the hash stays a mixing function (and thus doesn't have any information loss). + mHashMultiplier += UINT64_C(0xc4ceb9fe1a85ec54); + } + + void destroy() { + if (0 == mMask) { + // don't deallocate! + return; + } + + Destroyer::value>{} + .nodesDoNotDeallocate(*this); + + // This protection against not deleting mMask shouldn't be needed as it's sufficiently + // protected with the 0==mMask check, but I have this anyways because g++ 7 otherwise + // reports a compile error: attempt to free a non-heap object 'fm' + // [-Werror=free-nonheap-object] + if (mKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { + ROBIN_HOOD_LOG("std::free") + std::free(mKeyVals); + } + } + + void init() noexcept { + mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); + mInfo = reinterpret_cast(&mMask); + mNumElements = 0; + mMask = 0; + mMaxNumElementsAllowed = 0; + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + // members are sorted so no padding occurs + uint64_t mHashMultiplier = UINT64_C(0xc4ceb9fe1a85ec53); // 8 byte 8 + Node* mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); // 8 byte 16 + uint8_t* mInfo = reinterpret_cast(&mMask); // 8 byte 24 + size_t mNumElements = 0; // 8 byte 32 + size_t mMask = 0; // 8 byte 40 + size_t mMaxNumElementsAllowed = 0; // 8 byte 48 + InfoType mInfoInc = InitialInfoInc; // 4 byte 52 + InfoType mInfoHashShift = InitialInfoHashShift; // 4 byte 56 + // 16 byte 56 if NodeAllocator +}; + +} // namespace detail + +// map + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_flat_map = detail::Table; + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_node_map = detail::Table; + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_map = + detail::Table) <= sizeof(size_t) * 6 && + std::is_nothrow_move_constructible>::value && + std::is_nothrow_move_assignable>::value, + MaxLoadFactor100, Key, T, Hash, KeyEqual>; + +// set + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_flat_set = detail::Table; + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_node_set = detail::Table; + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_set = detail::Table < sizeof(Key) <= sizeof(size_t) * 6 && + std::is_nothrow_move_constructible::value && + std::is_nothrow_move_assignable::value, + MaxLoadFactor100, Key, void, Hash, KeyEqual>; + +} // namespace robin_hood +/* *INDENT-ON* */ + +#endif // NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp new file mode 100644 index 00000000..f8aa72ff --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp @@ -0,0 +1,257 @@ +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__TYPES_HPP_ +#define ATHENA_SMAC_PLANNER__TYPES_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace athena_smac_planner +{ + +typedef std::pair NodeHeuristicPair; +typedef std::vector LookupTable; +typedef std::pair TrigValues; + +/** + * @struct athena_smac_planner::SearchInfo + * @brief Search properties and penalties + */ +struct SearchInfo +{ + float minimum_turning_radius{8.0}; + float non_straight_penalty{1.05}; + float change_penalty{0.0}; + float reverse_penalty{2.0}; + float cost_penalty{2.0}; + float retrospective_penalty{0.015}; + float rotation_penalty{5.0}; + float analytic_expansion_ratio{3.5}; + float analytic_expansion_max_length{60.0}; + float analytic_expansion_max_cost{200.0}; + bool analytic_expansion_max_cost_override{false}; + std::string lattice_filepath; + bool cache_obstacle_heuristic{false}; + bool allow_reverse_expansion{false}; + bool allow_primitive_interpolation{false}; + bool downsample_obstacle_heuristic{true}; + bool use_quadratic_cost_penalty{false}; +}; + +/** + * @struct athena_smac_planner::SmootherParams + * @brief Parameters for the smoother + */ +struct SmootherParams +{ + /** + * @brief A constructor for athena_smac_planner::SmootherParams + */ + SmootherParams() + : holonomic_(false) + { + } + + /** + * @brief Get params from ROS parameter + * @param node Ptr to node + * @param name Name of plugin + */ + void get(rclcpp::Node * node, const std::string & name) + { + std::string prefix = name + ".smoother."; + auto dp = [node, &prefix](const std::string & p, auto def) { + if (!node->has_parameter(prefix + p)) { + node->declare_parameter(prefix + p, def); + } + return node->get_parameter(prefix + p).get_value(); + }; + tolerance_ = dp("tolerance", 1e-10); + max_its_ = dp("max_iterations", 1000); + w_data_ = dp("w_data", 0.2); + w_smooth_ = dp("w_smooth", 0.3); + do_refinement_ = dp("do_refinement", true); + refinement_num_ = dp("refinement_num", 2); + } + + double tolerance_; + int max_its_; + double w_data_; + double w_smooth_; + bool holonomic_; + bool do_refinement_; + int refinement_num_; +}; + +/** + * @struct athena_smac_planner::TurnDirection + * @brief A struct with the motion primitive's direction embedded + */ +enum class TurnDirection +{ + UNKNOWN = 0, + FORWARD = 1, + LEFT = 2, + RIGHT = 3, + REVERSE = 4, + REV_LEFT = 5, + REV_RIGHT = 6 +}; + +/** + * @struct athena_smac_planner::MotionPose + * @brief A struct for poses in motion primitives + */ +struct MotionPose +{ + /** + * @brief A constructor for athena_smac_planner::MotionPose + */ + MotionPose() {} + + /** + * @brief A constructor for athena_smac_planner::MotionPose + * @param x X pose + * @param y Y pose + * @param theta Angle of pose + * @param TurnDirection Direction of the primitive's turn + */ + MotionPose(const float & x, const float & y, const float & theta, const TurnDirection & turn_dir) + : _x(x), _y(y), _theta(theta), _turn_dir(turn_dir) + {} + + MotionPose operator-(const MotionPose & p2) + { + return MotionPose( + this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta, TurnDirection::UNKNOWN); + } + + float _x; + float _y; + float _theta; + TurnDirection _turn_dir; +}; + +typedef std::vector MotionPoses; + +/** + * @struct athena_smac_planner::LatticeMetadata + * @brief A struct of all lattice metadata + */ +struct LatticeMetadata +{ + float min_turning_radius; + float grid_resolution; + unsigned int number_of_headings; + std::vector heading_angles; + unsigned int number_of_trajectories; + std::string motion_model; +}; + +/** + * @struct athena_smac_planner::MotionPrimitive + * @brief A struct of all motion primitive data + */ +struct MotionPrimitive +{ + unsigned int trajectory_id; + float start_angle; + float end_angle; + float turning_radius; + float trajectory_length; + float arc_length; + float straight_length; + bool left_turn; + MotionPoses poses; +}; + +/** + * @struct athena_smac_planner::GoalState + * @brief A struct to store the goal state + */ +template +struct GoalState +{ + NodeT * goal = nullptr; + bool is_valid = true; +}; + +typedef std::vector MotionPrimitives; +typedef std::vector MotionPrimitivePtrs; + +/** + * @class athena_smac_planner::Coordinates + * @brief Implementation of coordinate2d structure + */ +struct Coordinates2D +{ + Coordinates2D() {} + Coordinates2D(const float & x_in, const float & y_in) + : x(x_in), y(y_in) + {} + + inline bool operator==(const Coordinates2D & rhs) const + { + return this->x == rhs.x && this->y == rhs.y; + } + + inline bool operator!=(const Coordinates2D & rhs) const + { + return !(*this == rhs); + } + + float x, y; +}; + +/** + * @class athena_smac_planner::Coordinates + * @brief Implementation of coordinate structure + */ +struct Coordinates +{ + /** + * @brief A constructor for athena_smac_planner::NodeHybrid::Coordinates + */ + Coordinates() {} + + /** + * @brief A constructor for athena_smac_planner::NodeHybrid::Coordinates + * @param x_in X coordinate + * @param y_in Y coordinate + * @param theta_in Theta coordinate + */ + Coordinates(const float & x_in, const float & y_in, const float & theta_in) + : x(x_in), y(y_in), theta(theta_in) + {} + + inline bool operator==(const Coordinates & rhs) const + { + return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; + } + + inline bool operator!=(const Coordinates & rhs) const + { + return !(*this == rhs); + } + + float x, y, theta; +}; +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__TYPES_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp new file mode 100644 index 00000000..494f68d4 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp @@ -0,0 +1,231 @@ +// Copyright (c) 2021, Samsung Research America +// Copyright (c) 2023, 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. Reserved. + +#ifndef ATHENA_SMAC_PLANNER__UTILS_HPP_ +#define ATHENA_SMAC_PLANNER__UTILS_HPP_ + +#include +#include +#include + +#include "nlohmann/json.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "tf2/utils.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "athena_smac_planner/types.hpp" +#include + +namespace athena_smac_planner +{ + +/** +* @brief Create an Eigen Vector2D of world poses from continuous map coords +* @param mx float of map X coordinate +* @param my float of map Y coordinate +* @param costmap Costmap pointer +* @return Eigen::Vector2d eigen vector of the generated path +*/ +inline geometry_msgs::msg::Pose getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) +{ + geometry_msgs::msg::Pose msg; + msg.position.x = + static_cast(costmap->getOriginX()) + mx * costmap->getResolution(); + msg.position.y = + static_cast(costmap->getOriginY()) + my * costmap->getResolution(); + return msg; +} + +/** +* @brief Create quaternion from radians +* @param theta continuous bin coordinates angle +* @return quaternion orientation in map frame +*/ +inline geometry_msgs::msg::Quaternion getWorldOrientation( + const float & theta) +{ + // theta is in radians already + tf2::Quaternion q; + q.setEuler(0.0, 0.0, theta); + return tf2::toMsg(q); +} + +/** +* @brief Find the min cost of the inflation decay function for which the robot MAY be +* in collision in any orientation +* @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) +* @return double circumscribed cost, any higher than this and need to do full footprint collision checking +* since some element of the robot could be in collision +*/ +inline double findCircumscribedCost(std::shared_ptr costmap) +{ + double result = -1.0; + std::vector>::iterator layer; + + // check if the costmap has an inflation layer + std::shared_ptr inflation_layer = nullptr; + for (auto & layer : *costmap->getLayeredCostmap()->getPlugins()) { + auto candidate = std::dynamic_pointer_cast(layer); + if (candidate) { + inflation_layer = candidate; + break; + } + } + if (inflation_layer != nullptr) { + double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + double resolution = costmap->getCostmap()->getResolution(); + result = static_cast(inflation_layer->computeCost(circum_radius / resolution)); + } else { + RCLCPP_WARN( + rclcpp::get_logger("computeCircumscribedCost"), + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!"); + } + + return result; +} + +/** + * @brief convert json to lattice metadata + * @param[in] json json object + * @param[out] lattice meta data + */ +inline void fromJsonToMetaData(const nlohmann::json & json, LatticeMetadata & lattice_metadata) +{ + json.at("turning_radius").get_to(lattice_metadata.min_turning_radius); + json.at("grid_resolution").get_to(lattice_metadata.grid_resolution); + json.at("num_of_headings").get_to(lattice_metadata.number_of_headings); + json.at("heading_angles").get_to(lattice_metadata.heading_angles); + json.at("number_of_trajectories").get_to(lattice_metadata.number_of_trajectories); + json.at("motion_model").get_to(lattice_metadata.motion_model); +} + +/** + * @brief convert json to pose + * @param[in] json json object + * @param[out] pose + */ +inline void fromJsonToPose(const nlohmann::json & json, MotionPose & pose) +{ + pose._x = json[0]; + pose._y = json[1]; + pose._theta = json[2]; +} + +/** + * @brief convert json to motion primitive + * @param[in] json json object + * @param[out] motion primitive + */ +inline void fromJsonToMotionPrimitive( + const nlohmann::json & json, MotionPrimitive & motion_primitive) +{ + json.at("trajectory_id").get_to(motion_primitive.trajectory_id); + json.at("start_angle_index").get_to(motion_primitive.start_angle); + json.at("end_angle_index").get_to(motion_primitive.end_angle); + json.at("trajectory_radius").get_to(motion_primitive.turning_radius); + json.at("trajectory_length").get_to(motion_primitive.trajectory_length); + json.at("arc_length").get_to(motion_primitive.arc_length); + json.at("straight_length").get_to(motion_primitive.straight_length); + json.at("left_turn").get_to(motion_primitive.left_turn); + + for (unsigned int i = 0; i < json["poses"].size(); i++) { + MotionPose pose; + fromJsonToPose(json["poses"][i], pose); + motion_primitive.poses.push_back(pose); + } +} + +/** + * @brief transform footprint into edges + * @param[in] robot position , orientation and footprint + * @param[out] robot footprint edges + */ +inline std::vector transformFootprintToEdges( + const geometry_msgs::msg::Pose & pose, + const std::vector & footprint) +{ + const double & x = pose.position.x; + const double & y = pose.position.y; + const double & yaw = tf2::getYaw(pose.orientation); + const double sin_yaw = sin(yaw); + const double cos_yaw = cos(yaw); + + std::vector out_footprint; + out_footprint.resize(2 * footprint.size()); + for (unsigned int i = 0; i < footprint.size(); i++) { + out_footprint[2 * i].x = x + cos_yaw * footprint[i].x - sin_yaw * footprint[i].y; + out_footprint[2 * i].y = y + sin_yaw * footprint[i].x + cos_yaw * footprint[i].y; + if (i == 0) { + out_footprint.back().x = out_footprint[i].x; + out_footprint.back().y = out_footprint[i].y; + } else { + out_footprint[2 * i - 1].x = out_footprint[2 * i].x; + out_footprint[2 * i - 1].y = out_footprint[2 * i].y; + } + } + return out_footprint; +} + +/** + * @brief initializes marker to visualize shape of linestring + * @param edge edge to mark of footprint + * @param i marker ID + * @param frame_id frame of the marker + * @param timestamp timestamp of the marker + * @return marker populated + */ +inline visualization_msgs::msg::Marker createMarker( + const std::vector edge, + unsigned int i, const std::string & frame_id, const rclcpp::Time & timestamp) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.frame_locked = false; + marker.ns = "planned_footprint"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.lifetime = rclcpp::Duration(0, 0); + + marker.id = i; + for (auto & point : edge) { + marker.points.push_back(point); + } + + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.05; + marker.scale.y = 0.05; + marker.scale.z = 0.05; + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + marker.color.a = 1.3f; + return marker; +} + + +} // namespace athena_smac_planner + +#endif // ATHENA_SMAC_PLANNER__UTILS_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/package.xml b/src/subsystems/minimal_navigation/athena_smac_planner/package.xml new file mode 100644 index 00000000..fbae2a06 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/package.xml @@ -0,0 +1,31 @@ + + + + athena_smac_planner + 1.4.0 + Vendored Hybrid-A* planner (SMAC) for the Athena navigation stack + UMD Loop + Apache-2.0 + + ament_cmake + nav2_common + + angles + eigen + geometry_msgs + msgs + nav2_core + nav2_costmap_2d + nav_msgs + nlohmann-json-dev + ompl + rclcpp + rclcpp_lifecycle + tf2_ros + tf2 + visualization_msgs + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp new file mode 100644 index 00000000..9f2ce25f --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp @@ -0,0 +1,526 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "athena_smac_planner/a_star.hpp" +using namespace std::chrono; // NOLINT + +namespace athena_smac_planner +{ + +template +AStarAlgorithm::AStarAlgorithm( + const MotionModel & motion_model, + const SearchInfo & search_info) +: _traverse_unknown(true), + _is_initialized(false), + _max_iterations(0), + _terminal_checking_interval(5000), + _max_planning_time(0), + _x_size(0), + _y_size(0), + _search_info(search_info), + _start(nullptr), + _goal_manager(GoalManagerT()), + _motion_model(motion_model) +{ + _graph.reserve(100000); +} + +template +AStarAlgorithm::~AStarAlgorithm() +{ +} + +template +void AStarAlgorithm::initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations, + const int & terminal_checking_interval, + const double & max_planning_time, + const float & lookup_table_size, + const unsigned int & dim_3_size) +{ + _traverse_unknown = allow_unknown; + _max_iterations = max_iterations; + _max_on_approach_iterations = max_on_approach_iterations; + _terminal_checking_interval = terminal_checking_interval; + _max_planning_time = max_planning_time; + if (!_is_initialized) { + _shared_ctx = std::make_shared(); + _shared_ctx->distance_heuristic->precomputeDistanceHeuristic(lookup_table_size, _motion_model, + dim_3_size, + _search_info, _shared_ctx->motion_table); + } + _is_initialized = true; + _dim3_size = dim_3_size; + _expander = std::make_unique>( + _motion_model, _search_info, _traverse_unknown, _dim3_size); +} + +template +void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision_checker) +{ + _collision_checker = collision_checker; + _costmap = collision_checker->getCostmap(); + unsigned int x_size = _costmap->getSizeInCellsX(); + unsigned int y_size = _costmap->getSizeInCellsY(); + + clearGraph(); + + if (getSizeX() != x_size || getSizeY() != y_size) { + _x_size = x_size; + _y_size = y_size; + } + + // Always refresh the motion model so dynamic penalty parameters take effect immediately + NodeT::initMotionModel(_shared_ctx.get(), _motion_model, _x_size, _y_size, _dim3_size, + _search_info); + + // Always set context pointers to ensure newly allocated objects get their contexts restored + _goal_manager.setContext(_shared_ctx.get()); + _expander->setContext(_shared_ctx.get()); + _expander->setCollisionChecker(_collision_checker); +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( + const uint64_t & index) +{ + auto iter = _graph.find(index); + if (iter != _graph.end()) { + return &(iter->second); + } + + return &(_graph.emplace(index, NodeT(index, _shared_ctx.get())).first->second); +} + +template +void AStarAlgorithm::setStart( + const float & mx, + const float & my, + const unsigned int & dim_3) +{ + _start = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + _start->setPose(Coordinates(mx, my, dim_3)); +} + +template +void AStarAlgorithm::populateExpansionsLog( + const NodePtr & node, + std::vector> * expansions_log) +{ + typename NodeT::Coordinates coords = node->pose; + expansions_log->emplace_back( + _costmap->getOriginX() + ((coords.x + 0.5) * _costmap->getResolution()), + _costmap->getOriginY() + ((coords.y + 0.5) * _costmap->getResolution()), + _shared_ctx->motion_table.getAngleFromBin(coords.theta)); +} + +template +void AStarAlgorithm::setGoal( + const float & mx, + const float & my, + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode, + const int & coarse_search_resolution) +{ + // Default to minimal resolution unless overridden for ALL_DIRECTION + _coarse_search_resolution = 1; + + _goal_manager.clear(); + Coordinates ref_goal_coord(mx, my, static_cast(dim_3)); + + if (!_search_info.cache_obstacle_heuristic || + _goal_manager.hasGoalChanged(ref_goal_coord)) + { + if (!_start) { + throw std::runtime_error("Start must be set before goal."); + } + + _shared_ctx->obstacle_heuristic->resetObstacleHeuristic( + _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my, + _shared_ctx->motion_table.downsample_obstacle_heuristic); + } + + _goal_manager.setRefGoalCoordinates(ref_goal_coord); + + unsigned int num_bins = _shared_ctx->motion_table.num_angle_quantization; + // set goal based on heading mode + switch (goal_heading_mode) { + case GoalHeadingMode::DEFAULT: { + // add a single goal node with single heading + auto goal = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); + break; + } + + case GoalHeadingMode::BIDIRECTIONAL: { + // Add two goals, one for each direction + // add goal in original direction + auto goal = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); + + // Add goal node in opposite (180°) direction + unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; + auto opposite_goal = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + opposite_heading)); + opposite_goal->setPose( + typename NodeT::Coordinates(mx, my, static_cast(opposite_heading))); + _goal_manager.addGoal(opposite_goal); + break; + } + + case GoalHeadingMode::ALL_DIRECTION: { + // Set the coarse search resolution only for all direction + _coarse_search_resolution = coarse_search_resolution; + + // Add goal nodes for all headings + for (unsigned int i = 0; i < num_bins; ++i) { + auto goal = addToGraph( + getIndex( + static_cast(mx), + static_cast(my), + i)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(i))); + _goal_manager.addGoal(goal); + } + break; + } + case GoalHeadingMode::UNKNOWN: + throw std::runtime_error("Goal heading is UNKNOWN."); + } +} + +template +bool AStarAlgorithm::areInputsValid() +{ + // Check if graph was filled in + if (_graph.empty()) { + throw std::runtime_error("Failed to compute path, no costmap given."); + } + + // Check if points were filled in + if (!_start || _goal_manager.goalsIsEmpty()) { + throw std::runtime_error("Failed to compute path, no valid start or goal given."); + } + + // remove invalid goals + _goal_manager.removeInvalidGoals(getToleranceHeuristic(), _collision_checker, _traverse_unknown); + + // Check if ending point is valid + if (_goal_manager.getGoalsSet().empty()) { + throw nav2_core::PlannerException("Goal was in lethal cost"); + } + + // Note: We do not check the if the start is valid because it is cleared + return true; +} + +template +bool AStarAlgorithm::getClosestPathWithinTolerance(CoordinateVector & path) +{ + if (_best_heuristic_node.first < getToleranceHeuristic()) { + _graph.at(_best_heuristic_node.second).backtracePath(path); + return true; + } + + return false; +} + +template +bool AStarAlgorithm::createPath( + CoordinateVector & path, int & iterations, + const float & tolerance, + std::function cancel_checker, + std::vector> * expansions_log) +{ + steady_clock::time_point start_time = steady_clock::now(); + _tolerance = tolerance; + _best_heuristic_node = {std::numeric_limits::max(), 0}; + clearQueue(); + + if (!areInputsValid()) { + return false; + } + + NodeVector coarse_check_goals, fine_check_goals; + _goal_manager.prepareGoalsForAnalyticExpansion( + coarse_check_goals, fine_check_goals, + _coarse_search_resolution); + + // 0) Add starting point to the open set + addNode(0.0, getStart()); + getStart()->setAccumulatedCost(0.0); + + // Optimization: preallocate all variables + NodePtr current_node = nullptr; + NodePtr neighbor = nullptr; + NodePtr expansion_result = nullptr; + float g_cost = 0.0; + NodeVector neighbors; + int approach_iterations = 0; + NeighborIterator neighbor_iterator; + int analytic_iterations = 0; + int closest_distance = std::numeric_limits::max(); + + // Given an index, return a node ptr reference if its collision-free and valid + const uint64_t max_index = static_cast(getSizeX()) * + static_cast(getSizeY()) * + static_cast(getSizeDim3()); + NodeGetter neighborGetter = + [&, this](const uint64_t & index, NodePtr & neighbor_rtn) -> bool + { + if (index >= max_index) { + return false; + } + + neighbor_rtn = addToGraph(index); + return true; + }; + + while (iterations < getMaxIterations() && !_queue.empty()) { + // Check for planning timeout and cancel only on every Nth iteration + if (iterations % _terminal_checking_interval == 0) { + if (cancel_checker()) { + throw nav2_core::PlannerException("Planner was cancelled"); + } + std::chrono::duration planning_duration = + std::chrono::duration_cast>(steady_clock::now() - start_time); + if (static_cast(planning_duration.count()) >= _max_planning_time) { + // In case of timeout, return the path that is closest, if within tolerance. + return getClosestPathWithinTolerance(path); + } + } + + // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue + current_node = getNextNode(); + + // Save current node coordinates for debug + if (expansions_log) { + populateExpansionsLog(current_node, expansions_log); + } + + // We allow for nodes to be queued multiple times in case + // shorter paths result in it, but we can visit only once + // Also a chance to perform last-checks necessary. + if (onVisitationCheckNode(current_node)) { + continue; + } + + iterations++; + + // 2) Mark Nbest as visited + current_node->visited(); + + // 2.1) Use an analytic expansion (if available) to generate a path + expansion_result = nullptr; + expansion_result = _expander->tryAnalyticExpansion( + current_node, coarse_check_goals, fine_check_goals, + _goal_manager.getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); + if (expansion_result != nullptr) { + current_node = expansion_result; + } + + // 3) Check if we're at the goal, backtrace if required + if (_goal_manager.isGoal(current_node)) { + return current_node->backtracePath(path); + } else if (_best_heuristic_node.first < getToleranceHeuristic()) { + // Optimization: Let us find when in tolerance and refine within reason + approach_iterations++; + if (approach_iterations >= getOnApproachMaxIterations()) { + return _graph.at(_best_heuristic_node.second).backtracePath(path); + } + } + + // 4) Expand neighbors of Nbest not visited + neighbors.clear(); + current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors); + + for (neighbor_iterator = neighbors.begin(); + neighbor_iterator != neighbors.end(); ++neighbor_iterator) + { + neighbor = *neighbor_iterator; + + // 4.1) Compute the cost to go to this node + g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor); + + // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach + if (g_cost < neighbor->getAccumulatedCost()) { + neighbor->setAccumulatedCost(g_cost); + neighbor->parent = current_node; + + // 4.3) Add to queue with heuristic cost + addNode(g_cost + getHeuristicCost(neighbor), neighbor); + } + } + } + + // If we run out of search options, return the path that is closest, if within tolerance. + return getClosestPathWithinTolerance(path); +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() +{ + return _start; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +{ + NodeBasic node = _queue.top().second; + _queue.pop(); + node.processSearchNode(); + return node.graph_node_ptr; +} + +template +void AStarAlgorithm::addNode(const float & cost, NodePtr & node) +{ + NodeBasic queued_node(node->getIndex()); + queued_node.populateSearchNode(node); + _queue.emplace(cost, queued_node); +} + +template +float AStarAlgorithm::getHeuristicCost(const NodePtr & node) +{ + const Coordinates node_coords = + NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); + float heuristic = node->getHeuristicCost(node_coords, _goal_manager.getGoalsCoordinates()); + if (heuristic < _best_heuristic_node.first) { + _best_heuristic_node = {heuristic, node->getIndex()}; + } + + return heuristic; +} + +template +bool AStarAlgorithm::onVisitationCheckNode(const NodePtr & current_node) +{ + return current_node->wasVisited(); +} + +template +void AStarAlgorithm::clearQueue() +{ + NodeQueue q; + std::swap(_queue, q); +} + +template +void AStarAlgorithm::clearGraph() +{ + Graph g; + std::swap(_graph, g); + _graph.reserve(100000); +} + +template +uint64_t AStarAlgorithm::getIndex( + const unsigned int & x, const unsigned int & y, + const unsigned int & dim_3) +{ + return NodeT::getIndex(x, y, dim_3, _shared_ctx->motion_table.size_x, + _shared_ctx->motion_table.num_angle_quantization); +} + +template +int & AStarAlgorithm::getMaxIterations() +{ + return _max_iterations; +} + +template +int & AStarAlgorithm::getOnApproachMaxIterations() +{ + return _max_on_approach_iterations; +} + +template +float & AStarAlgorithm::getToleranceHeuristic() +{ + return _tolerance; +} + +template +unsigned int & AStarAlgorithm::getSizeX() +{ + return _x_size; +} + +template +unsigned int & AStarAlgorithm::getSizeY() +{ + return _y_size; +} + +template +unsigned int & AStarAlgorithm::getSizeDim3() +{ + return _dim3_size; +} + +template +unsigned int AStarAlgorithm::getCoarseSearchResolution() +{ + return _coarse_search_resolution; +} + +template +typename AStarAlgorithm::GoalManagerT AStarAlgorithm::getGoalManager() +{ + return _goal_manager; +} + +template +typename AStarAlgorithm::NodeContext * AStarAlgorithm::getContext() +{ + return _shared_ctx.get(); +} + +// Instantiate algorithm for the supported template types +template class AStarAlgorithm; + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp new file mode 100644 index 00000000..c8c057a1 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp @@ -0,0 +1,449 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#include +#include +#include + +#include "athena_smac_planner/analytic_expansion.hpp" + +namespace athena_smac_planner +{ + +template +AnalyticExpansion::AnalyticExpansion( + const MotionModel & motion_model, + const SearchInfo & search_info, + const bool & traverse_unknown, + const unsigned int & dim_3_size) +: _motion_model(motion_model), + _search_info(search_info), + _traverse_unknown(traverse_unknown), + _dim_3_size(dim_3_size), + _collision_checker(nullptr) +{ +} + +template +void AnalyticExpansion::setCollisionChecker( + GridCollisionChecker * collision_checker) +{ + _collision_checker = collision_checker; +} + +template +void AnalyticExpansion::setContext(NodeContext * ctx) +{ + _ctx = ctx; +} + +template +typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( + const NodePtr & current_node, + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, + const NodeGetter & getter, int & analytic_iterations, + int & closest_distance) +{ + // This must be a valid motion model for analytic expansion to be attempted + if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP || + _motion_model == MotionModel::STATE_LATTICE) + { + // See if we are closer and should be expanding more often + const Coordinates node_coords = + NodeT::getCoords( + current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); + + AnalyticExpansionNodes current_best_analytic_nodes; + NodePtr current_best_goal = nullptr; + NodePtr current_best_node = nullptr; + float current_best_score = std::numeric_limits::max(); + + closest_distance = std::min( + closest_distance, + static_cast(current_node->getHeuristicCost(node_coords, goals_coords))); + // We want to expand at a rate of d/expansion_ratio, + // but check to see if we are so close that we would be expanding every iteration + // If so, limit it to the expansion ratio (rounded up) + int desired_iterations = std::max( + static_cast(closest_distance / _search_info.analytic_expansion_ratio), + static_cast(std::ceil(_search_info.analytic_expansion_ratio))); + + // If we are closer now, we should update the target number of iterations to go + analytic_iterations = + std::min(analytic_iterations, desired_iterations); + + // Always run the expansion on the first run in case there is a + // trivial path to be found + if (analytic_iterations <= 0) { + // Reset the counter and try the analytic path expansion + analytic_iterations = desired_iterations; + bool found_valid_expansion = false; + + // First check the coarse search resolution goals + for (auto & current_goal_node : coarse_check_goals) { + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, current_goal_node, getter, + _ctx->motion_table.state_space); + if (!analytic_nodes.nodes.empty()) { + found_valid_expansion = true; + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); + // Update the best score if we found a better path + if (score < current_best_score) { + current_best_analytic_nodes = analytic_nodes; + current_best_goal = current_goal_node; + current_best_score = score; + current_best_node = node; + } + } + } + + // perform a final search if we found a goal + if (found_valid_expansion) { + for (auto & current_goal_node : fine_check_goals) { + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, current_goal_node, getter, + _ctx->motion_table.state_space); + if (!analytic_nodes.nodes.empty()) { + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); + // Update the best score if we found a better path + if (score < current_best_score) { + current_best_analytic_nodes = analytic_nodes; + current_best_goal = current_goal_node; + current_best_score = score; + current_best_node = node; + } + } + } + } + } + + if (!current_best_analytic_nodes.nodes.empty()) { + return setAnalyticPath( + current_best_node, current_best_goal, + current_best_analytic_nodes); + } + analytic_iterations--; + } + + // No valid motion model - return nullptr + return NodePtr(nullptr); +} + +template +int AnalyticExpansion::countDirectionChanges( + const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path) +{ + const double * lengths = path.length_; + int changes = 0; + int last_dir = 0; + for (int i = 0; i < 5; ++i) { + if (lengths[i] == 0.0) { + continue; + } + + int currentDirection = (lengths[i] > 0.0) ? 1 : -1; + if (last_dir != 0 && currentDirection != last_dir) { + ++changes; + } + last_dir = currentDirection; + } + + return changes; +} + +template +typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( + const NodePtr & node, + const NodePtr & goal, + const NodeGetter & node_getter, + const ompl::base::StateSpacePtr & state_space) +{ + ompl::base::ScopedState<> from(state_space), to(state_space), s(state_space); + from[0] = node->pose.x; + from[1] = node->pose.y; + from[2] = _ctx->motion_table.getAngleFromBin(node->pose.theta); + to[0] = goal->pose.x; + to[1] = goal->pose.y; + to[2] = _ctx->motion_table.getAngleFromBin(goal->pose.theta); + + float d = state_space->distance(from(), to()); + + auto rs_state_space = dynamic_cast(state_space.get()); + int direction_changes = 0; + if (rs_state_space) { + direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get())); + } + + // A move of sqrt(2) is guaranteed to be in a new cell + static const float sqrt_2 = sqrtf(2.0f); + + // If the length is too far, exit. This prevents unsafe shortcutting of paths + // into higher cost areas far out from the goal itself, let search to the work of getting + // close before the analytic expansion brings it home. This should never be smaller than + // 4-5x the minimum turning radius being used, or planning times will begin to spike. + if (d > _search_info.analytic_expansion_max_length || d < sqrt_2) { + return AnalyticExpansionNodes(); + } + + unsigned int num_intervals = static_cast(std::floor(d / sqrt_2)); + + AnalyticExpansionNodes possible_nodes; + // When "from" and "to" are zero or one cell away, + // num_intervals == 0 + possible_nodes.nodes.reserve(num_intervals); // We won't store this node or the goal + std::vector reals; + double theta; + + // Pre-allocate + NodePtr prev(node); + uint64_t index = 0; + NodePtr next(nullptr); + float angle = 0.0; + Coordinates proposed_coordinates; + bool failure = false; + std::vector node_costs; + node_costs.reserve(num_intervals); + + // Check intermediary poses (non-goal, non-start) + for (float i = 1; i <= num_intervals; i++) { + state_space->interpolate(from(), to(), i / num_intervals, s()); + reals = s.reals(); + // Make sure in range [0, 2PI) + theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; + theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; + angle = _ctx->motion_table.getAngle(theta); + + // Turn the pose into a node, and check if it is valid + index = NodeT::getIndex( + static_cast(reals[0]), + static_cast(reals[1]), + static_cast(angle), + _ctx->motion_table.size_x, + _ctx->motion_table.num_angle_quantization); + // Get the node from the graph + if (node_getter(index, next)) { + Coordinates initial_node_coords = next->pose; + proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; + next->setPose(proposed_coordinates); + if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { + // Save the node, and its previous coordinates in case we need to abort + possible_nodes.add(next, initial_node_coords, proposed_coordinates); + node_costs.emplace_back(next->getCost()); + prev = next; + } else { + // Abort + next->setPose(initial_node_coords); + failure = true; + break; + } + } else { + // Abort + failure = true; + break; + } + } + + if (!failure) { + // We found 'a' valid expansion. Now to tell if its a quality option... + const float max_cost = _search_info.analytic_expansion_max_cost; + auto max_cost_it = std::max_element(node_costs.begin(), node_costs.end()); + if (max_cost_it != node_costs.end() && *max_cost_it > max_cost) { + // If any element is above the comfortable cost limit, check edge cases: + // (1) Check if goal is in greater than max_cost space requiring + // entering it, but only entering it on final approach, not in-and-out + // (2) Checks if goal is in normal space, but enters costed space unnecessarily + // mid-way through, skirting obstacle or in non-globally confined space + bool cost_exit_high_cost_region = false; + for (auto iter = node_costs.rbegin(); iter != node_costs.rend(); ++iter) { + const float & curr_cost = *iter; + if (curr_cost <= max_cost) { + cost_exit_high_cost_region = true; + } else if (curr_cost > max_cost && cost_exit_high_cost_region) { + failure = true; + break; + } + } + + // (3) Handle exception: there may be no other option close to goal + // if max cost is set too low (optional) + if (failure) { + if (d < 2.0f * M_PI * _ctx->motion_table.min_turning_radius && + _search_info.analytic_expansion_max_cost_override) + { + failure = false; + } + } + } + } + + // Reset to initial poses to not impact future searches + for (const auto & node_pose : possible_nodes.nodes) { + const auto & n = node_pose.node; + n->setPose(node_pose.initial_coords); + } + + if (failure) { + return AnalyticExpansionNodes(); + } + + possible_nodes.setDirectionChanges(direction_changes); + return possible_nodes; +} + +template +float AnalyticExpansion::refineAnalyticPath( + NodePtr & node, + const NodePtr & goal_node, + const NodeGetter & getter, + AnalyticExpansionNodes & analytic_nodes) +{ + NodePtr test_node = node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && + test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + // print the goals pose + refined_analytic_nodes = + getAnalyticPath( + test_node, goal_node, getter, + _ctx->motion_table.state_space); + if (refined_analytic_nodes.nodes.empty()) { + break; + } + if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) { + // If the direction changes are worse, we don't want to use this path + continue; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; + } + } + + // The analytic expansion can short-cut near obstacles when closer to a goal + // So, we can attempt to refine it more by increasing the possible radius + // higher than the minimum turning radius and use the best solution based on + // a scoring function similar to that used in traversal cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.nodes.size() < 2) { + return std::numeric_limits::max(); + } + + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x, + expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y); + const float & weight = _ctx->motion_table.cost_penalty; + for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; + + float original_score = scoringFn(analytic_nodes); + float best_score = original_score; + float score = std::numeric_limits::max(); + float min_turn_rad = _ctx->motion_table.min_turning_radius; + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (_ctx->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); + } + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + score = scoringFn(refined_analytic_nodes); + + // Normal scoring: prioritize lower cost as long as not more directional changes + if (score <= best_score && + refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes) + { + analytic_nodes = refined_analytic_nodes; + best_score = score; + continue; + } + + // Special case: If we have a better score than original (only) and less directional changes + // the path quality is still better than the original and is less operationally complex + if (score <= original_score && + refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes) + { + analytic_nodes = refined_analytic_nodes; + best_score = score; + } + } + + return best_score; +} + +template +typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( + const NodePtr & node, + const NodePtr & goal_node, + const AnalyticExpansionNodes & expanded_nodes) +{ + _detached_nodes.clear(); + // Legitimate final path - set the parent relationships, states, and poses + NodePtr prev = node; + for (const auto & node_pose : expanded_nodes.nodes) { + auto n = node_pose.node; + cleanNode(n); + if (n->getIndex() != goal_node->getIndex()) { + if (n->wasVisited()) { + _detached_nodes.push_back(std::make_unique(-1, _ctx)); + n = _detached_nodes.back().get(); + } + n->parent = prev; + n->pose = node_pose.proposed_coords; + n->visited(); + prev = n; + } + } + if (goal_node != prev) { + goal_node->parent = prev; + cleanNode(goal_node); + goal_node->visited(); + } + return goal_node; +} + +template +void AnalyticExpansion::cleanNode(const NodePtr & /*expanded_nodes*/) +{ +} + +template class AnalyticExpansion; + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp new file mode 100644 index 00000000..24c8baf7 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp @@ -0,0 +1,196 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#include "athena_smac_planner/collision_checker.hpp" + +namespace athena_smac_planner +{ + +GridCollisionChecker::GridCollisionChecker( + std::shared_ptr costmap_ros, + unsigned int num_quantizations, + rclcpp::Node::SharedPtr node) +: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr) +{ + if (node) { + clock_ = node->get_clock(); + logger_ = node->get_logger(); + } + + if (costmap_ros) { + costmap_ros_ = costmap_ros; + } + + // Convert number of regular bins into angles + float bin_size = 2 * M_PI / static_cast(num_quantizations); + angles_.reserve(num_quantizations); + for (unsigned int i = 0; i != num_quantizations; i++) { + angles_.push_back(bin_size * i); + } +} + +// GridCollisionChecker::GridCollisionChecker( +// nav2_costmap_2d::Costmap2D * costmap, +// std::vector & angles) +// : FootprintCollisionChecker(costmap), +// angles_(angles) +// { +// } + +void GridCollisionChecker::setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_collision_cost) +{ + possible_collision_cost_ = static_cast(possible_collision_cost); + if (possible_collision_cost_ <= 0.0f) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/athena_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + footprint_is_radius_ = radius; + + // Use radius, no caching required + if (radius) { + return; + } + + // No change, no updates required + if (footprint == unoriented_footprint_) { + return; + } + + oriented_footprints_.clear(); + oriented_footprints_.reserve(angles_.size()); + double sin_th, cos_th; + geometry_msgs::msg::Point new_pt; + const unsigned int footprint_size = footprint.size(); + + // Precompute the orientation bins for checking to use + for (unsigned int i = 0; i != angles_.size(); i++) { + sin_th = sin(angles_[i]); + cos_th = cos(angles_[i]); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint_size); + + for (unsigned int j = 0; j < footprint_size; j++) { + new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; + new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; + oriented_footprint.push_back(new_pt); + } + + oriented_footprints_.push_back(oriented_footprint); + } + + unoriented_footprint_ = footprint; +} + +bool GridCollisionChecker::inCollision( + const float & x, + const float & y, + const float & angle_bin, + const bool & traverse_unknown) +{ + // Check to make sure cell is inside the map + if (outsideRange(costmap_->getSizeInCellsX(), x) || + outsideRange(costmap_->getSizeInCellsY(), y)) + { + return true; + } + + // Assumes setFootprint already set + center_cost_ = static_cast(costmap_->getCost( + static_cast(x + 0.5f), static_cast(y + 0.5f))); + + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points, but first see + // if the robot is even potentially in an inscribed collision + if (center_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { + return false; + } + + // If its inscribed, in collision, or unknown in the middle, + // no need to even check the footprint, its invalid + if (center_cost_ == UNKNOWN_COST && !traverse_unknown) { + return true; + } + + if (center_cost_ == INSCRIBED_COST || center_cost_ == OCCUPIED_COST) { + return true; + } + + // if possible inscribed, need to check actual footprint pose. + // Use precomputed oriented footprints are done on initialization, + // offset by translation value to collision check + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + geometry_msgs::msg::Point new_pt; + const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { + new_pt.x = wx + oriented_footprint[i].x; + new_pt.y = wy + oriented_footprint[i].y; + current_footprint.push_back(new_pt); + } + + float footprint_cost = static_cast(footprintCost(current_footprint)); + + if (footprint_cost == UNKNOWN_COST && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost >= OCCUPIED_COST; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + if (center_cost_ == UNKNOWN_COST && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return center_cost_ >= INSCRIBED_COST; + } +} + +bool GridCollisionChecker::inCollision( + const unsigned int & i, + const bool & traverse_unknown) +{ + center_cost_ = costmap_->getCost(i); + if (center_cost_ == UNKNOWN_COST && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return center_cost_ >= INSCRIBED_COST; +} + +float GridCollisionChecker::getCost() +{ + // Assumes inCollision called prior + return static_cast(center_cost_); +} + +bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value) +{ + return value < 0.0f || value > max; +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp new file mode 100644 index 00000000..4cfac234 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp @@ -0,0 +1,127 @@ +// Copyright (c) 2020, Carlos Luis +// Copyright (c) 2020, Samsung Research America +// +// 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. Reserved. + +#include "athena_smac_planner/costmap_downsampler.hpp" + +#include +#include +#include + +namespace athena_smac_planner +{ + +CostmapDownsampler::CostmapDownsampler() +: _costmap(nullptr), + _downsampled_costmap(nullptr) +{ +} + +CostmapDownsampler::~CostmapDownsampler() +{ +} + +void CostmapDownsampler::on_configure( + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor, + const bool & use_min_cost_neighbor) +{ + _costmap = costmap; + _downsampling_factor = downsampling_factor; + _use_min_cost_neighbor = use_min_cost_neighbor; + updateCostmapSize(); + + _downsampled_costmap = std::make_unique( + _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, + _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN_COST); +} + +void CostmapDownsampler::on_cleanup() +{ + _costmap = nullptr; + _downsampled_costmap.reset(); +} + +nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( + const unsigned int & downsampling_factor) +{ + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + // Adjust costmap size if needed + if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || + _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || + _downsampled_costmap->getResolution() != _downsampled_resolution) + { + resizeCostmap(); + } + + // Assign costs + for (unsigned int i = 0; i < _downsampled_size_x; ++i) { + for (unsigned int j = 0; j < _downsampled_size_y; ++j) { + setCostOfCell(i, j); + } + } + + return _downsampled_costmap.get(); +} + +void CostmapDownsampler::updateCostmapSize() +{ + _size_x = _costmap->getSizeInCellsX(); + _size_y = _costmap->getSizeInCellsY(); + _downsampled_size_x = ceil(static_cast(_size_x) / _downsampling_factor); + _downsampled_size_y = ceil(static_cast(_size_y) / _downsampling_factor); + _downsampled_resolution = _downsampling_factor * _costmap->getResolution(); +} + +void CostmapDownsampler::resizeCostmap() +{ + _downsampled_costmap->resizeMap( + _downsampled_size_x, + _downsampled_size_y, + _downsampled_resolution, + _costmap->getOriginX(), + _costmap->getOriginY()); +} + +void CostmapDownsampler::setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my) +{ + unsigned int mx, my; + unsigned char cost = _use_min_cost_neighbor ? 255 : 0; + unsigned int x_offset = new_mx * _downsampling_factor; + unsigned int y_offset = new_my * _downsampling_factor; + + for (unsigned int i = 0; i < _downsampling_factor; ++i) { + mx = x_offset + i; + if (mx >= _size_x) { + continue; + } + for (unsigned int j = 0; j < _downsampling_factor; ++j) { + my = y_offset + j; + if (my >= _size_y) { + continue; + } + cost = _use_min_cost_neighbor ? + std::min(cost, _costmap->getCost(mx, my)) : + std::max(cost, _costmap->getCost(mx, my)); + } + } + + _downsampled_costmap->setCost(new_mx, new_my, cost); +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp new file mode 100644 index 00000000..1b16307e --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2026, 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. Reserved. + +#include "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "ompl/base/spaces/ReedsSheppStateSpace.h" +#include "athena_smac_planner/distance_heuristic.hpp" +#include "athena_smac_planner/node_hybrid.hpp" + +namespace athena_smac_planner +{ + +template<> +template +void DistanceHeuristic::precomputeDistanceHeuristic( + const float & lookup_table_dim, + const MotionModel & motion_model, + const unsigned int & dim_3_size, + const SearchInfo & search_info, + MotionTableT & motion_table) +{ + // Dubin or Reeds-Shepp shortest distances + if (motion_model == MotionModel::DUBIN) { + motion_table.state_space = std::make_shared( + search_info.minimum_turning_radius); + } else if (motion_model == MotionModel::REEDS_SHEPP) { + motion_table.state_space = std::make_shared( + search_info.minimum_turning_radius); + } else { + throw std::runtime_error( + "Node attempted to precompute distance heuristics " + "with invalid motion model!"); + } + + ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + to[0] = 0.0; + to[1] = 0.0; + to[2] = 0.0; + size_lookup_ = lookup_table_dim; + float motion_heuristic = 0.0; + unsigned int index = 0; + int dim_3_size_int = static_cast(dim_3_size); + float angular_bin_size = 2 * M_PI / static_cast(dim_3_size); + + // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal + // to help drive the search towards admissible approaches. Deu to symmetries in the + // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror + // around the X axis any relative node lookup. This reduces memory overhead and increases + // the size of a window a platform can store in memory. + dist_heuristic_lookup_table_.resize(size_lookup_ * ceil(size_lookup_ / 2.0) * dim_3_size_int); + for (float x = ceil(-size_lookup_ / 2.0); x <= floor(size_lookup_ / 2.0); x += 1.0) { + for (float y = 0.0; y <= floor(size_lookup_ / 2.0); y += 1.0) { + for (int heading = 0; heading != dim_3_size_int; heading++) { + from[0] = x; + from[1] = y; + from[2] = heading * angular_bin_size; + motion_heuristic = motion_table.state_space->distance(from(), to()); + dist_heuristic_lookup_table_[index] = motion_heuristic; + index++; + } + } + } +} + +template +template +float DistanceHeuristic::getDistanceHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const float & obstacle_heuristic, + MotionTableT & motion_table) +{ + // rotate and translate node_coords such that goal_coords relative is (0,0,0) + // Due to the rounding involved in exact cell increments for caching, + // this is not an exact replica of a live heuristic, but has bounded error. + // (Usually less than 1 cell length) + + // This angle is negative since we are de-rotating the current node + // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th) + const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta]; + const float cos_th = trig_vals.first; + const float sin_th = -trig_vals.second; + const float dx = node_coords.x - goal_coords.x; + const float dy = node_coords.y - goal_coords.y; + + double dtheta_bin = node_coords.theta - goal_coords.theta; + if (dtheta_bin < 0) { + dtheta_bin += motion_table.num_angle_quantization; + } + if (dtheta_bin > motion_table.num_angle_quantization) { + dtheta_bin -= motion_table.num_angle_quantization; + } + + Coordinates node_coords_relative( + round(dx * cos_th - dy * sin_th), + round(dx * sin_th + dy * cos_th), + round(dtheta_bin)); + + // Check if the relative node coordinate is within the localized window around the goal + // to apply the distance heuristic. Since the lookup table is contains only the positive + // X axis, we mirror the Y and theta values across the X axis to find the heuristic values. + float motion_heuristic = 0.0; + const int floored_size = floor(size_lookup_ / 2.0); + const int ceiling_size = ceil(size_lookup_ / 2.0); + const float mirrored_relative_y = abs(node_coords_relative.y); + if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) { + // Need to mirror angle if Y coordinate was mirrored + int theta_pos; + if (node_coords_relative.y < 0.0) { + theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta; + } else { + theta_pos = node_coords_relative.theta; + } + const int x_pos = node_coords_relative.x + floored_size; + const int y_pos = static_cast(mirrored_relative_y); + const int index = + x_pos * ceiling_size * motion_table.num_angle_quantization + + y_pos * motion_table.num_angle_quantization + + theta_pos; + motion_heuristic = dist_heuristic_lookup_table_[index]; + } else if (obstacle_heuristic <= 0.0) { + ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + to[0] = goal_coords.x; + to[1] = goal_coords.y; + from[0] = node_coords.x; + from[1] = node_coords.y; + if constexpr (std::is_same_v) { + to[2] = goal_coords.theta * motion_table.num_angle_quantization; + from[2] = node_coords.theta * motion_table.num_angle_quantization; + } else { + to[2] = motion_table.getAngleFromBin(goal_coords.theta); + from[2] = motion_table.getAngleFromBin(node_coords.theta); + } + motion_heuristic = motion_table.state_space->distance(from(), to()); + } + + return motion_heuristic; +} + +// Instantiate algorithm for the supported template types +template void DistanceHeuristic::precomputeDistanceHeuristic( + const float &, const MotionModel &, const unsigned int &, const SearchInfo &, + HybridMotionTable &); +template float DistanceHeuristic::getDistanceHeuristic( + const Coordinates &, const Coordinates &, const float &, HybridMotionTable &); +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp new file mode 100644 index 00000000..76c3b95b --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp @@ -0,0 +1,159 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "athena_smac_planner/smac_planner_hybrid.hpp" +#include "msgs/msg/planner_event.hpp" + +class GlobalPlannerNode : public rclcpp::Node +{ +public: + explicit GlobalPlannerNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("global_planner", options) + { + declare_parameter("use_sim_time", false); + } + + void configure() + { + costmap_ros_ = std::make_shared( + "global_costmap", + get_namespace(), + "global_costmap"); + costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", get_parameter("use_sim_time").as_bool())); + costmap_ros_->configure(); + costmap_ros_->activate(); + + planner_ = std::make_shared( + shared_from_this(), costmap_ros_); + + path_pub_ = create_publisher( + "/global_path", rclcpp::QoS(1).transient_local()); + event_pub_ = create_publisher("/planner_event", 10); + + robot_pose_sub_ = create_subscription( + "/robot_pose", 10, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + std::optional pending; + { + std::lock_guard lk(mutex_); + robot_pose_ = *msg; + // If a goal arrived before robot_pose was ready, plan now + if (pending_goal_.has_value()) { + pending = *pending_goal_; + pending_goal_.reset(); + } + } + if (pending.has_value()) { + RCLCPP_INFO(get_logger(), "[global_planner] robot_pose received — planning pending goal"); + plan(*pending); + } + }); + + goal_pose_sub_ = create_subscription( + "/goal_pose", 10, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { onGoal(msg); }); + + RCLCPP_INFO(get_logger(), "GlobalPlannerNode configured."); + } + + std::shared_ptr getCostmapROS() + { + return costmap_ros_; + } + +private: + void publishEvent(uint8_t event_type) + { + msgs::msg::PlannerEvent ev; + ev.event = event_type; + event_pub_->publish(ev); + } + + void onGoal(const geometry_msgs::msg::PoseStamped::SharedPtr goal) + { + publishEvent(msgs::msg::PlannerEvent::NEW_GOAL); + + { + std::lock_guard lk(mutex_); + if (!robot_pose_.has_value()) { + RCLCPP_WARN(get_logger(), + "[global_planner] goal received but no robot_pose yet — waiting"); + pending_goal_ = *goal; // cache; plan() called once robot_pose arrives + return; + } + // Overwrite any stale pending goal — the new goal supersedes it + pending_goal_.reset(); + } + + plan(*goal); + } + + void plan(const geometry_msgs::msg::PoseStamped & goal) + { + geometry_msgs::msg::PoseStamped start; + { + std::lock_guard lk(mutex_); + start = robot_pose_.value(); + } + + publishEvent(msgs::msg::PlannerEvent::PLANNING); + + try { + auto path = planner_->createPlan(start, goal, [] { return false; }); + path_pub_->publish(path); + publishEvent(msgs::msg::PlannerEvent::PLAN_SUCCEEDED); + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Planning failed: %s", ex.what()); + publishEvent(msgs::msg::PlannerEvent::PLAN_FAILED); + } + } + + std::shared_ptr costmap_ros_; + std::shared_ptr planner_; + + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr event_pub_; + rclcpp::Subscription::SharedPtr robot_pose_sub_; + rclcpp::Subscription::SharedPtr goal_pose_sub_; + + std::mutex mutex_; + std::optional robot_pose_; + std::optional pending_goal_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + node->configure(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.add_node(node->getCostmapROS()->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp new file mode 100644 index 00000000..ee1dc183 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp @@ -0,0 +1,54 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#include "athena_smac_planner/node_basic.hpp" + +namespace athena_smac_planner +{ + +template +void NodeBasic::processSearchNode() +{ +} + +template<> +void NodeBasic::processSearchNode() +{ + // We only want to override the node's pose if it has not yet been visited + // to prevent the case that a node has been queued multiple times and + // a new branch is overriding one of lower cost already visited. + if (!this->graph_node_ptr->wasVisited()) { + this->graph_node_ptr->pose = this->pose; + this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index, this->turn_dir); + } +} + +template +void NodeBasic::populateSearchNode(NodeT * & node) +{ + this->graph_node_ptr = node; +} + +template<> +void NodeBasic::populateSearchNode(NodeHybrid * & node) +{ + this->pose = node->pose; + this->graph_node_ptr = node; + this->motion_index = node->getMotionPrimitiveIndex(); + this->turn_dir = node->getTurnDirection(); +} + +template class NodeBasic; + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp new file mode 100644 index 00000000..0c71a622 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp @@ -0,0 +1,540 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// Copyright (c) 2023, 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. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "ompl/base/spaces/ReedsSheppStateSpace.h" + +#include "athena_smac_planner/node_hybrid.hpp" + +using namespace std::chrono; // NOLINT + +namespace athena_smac_planner +{ + +// Each of these tables are the projected motion models through +// time and space applied to the search on the current node in +// continuous map-coordinates (e.g. not meters but partial map cells) +// Currently, these are set to project *at minimum* into a neighboring +// cell. Though this could be later modified to project a certain +// amount of time or particular distance forward. + +// http://planning.cs.uiuc.edu/planning/node821.html +// Model for ackermann style vehicle with minimum radius restriction +void HybridMotionTable::initDubin( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + travel_distance_reward = 1.0f - search_info.retrospective_penalty; + downsample_obstacle_heuristic = search_info.downsample_obstacle_heuristic; + use_quadratic_cost_penalty = search_info.use_quadratic_cost_penalty; + + // if nothing changed, no need to re-compute primitives + if (num_angle_quantization_in == num_angle_quantization && + min_turning_radius == search_info.minimum_turning_radius && + allow_primitive_interpolation == search_info.allow_primitive_interpolation && + motion_model == MotionModel::DUBIN) + { + return; + } + + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + min_turning_radius = search_info.minimum_turning_radius; + allow_primitive_interpolation = search_info.allow_primitive_interpolation; + motion_model = MotionModel::DUBIN; + + // angle must meet 3 requirements: + // 1) be increment of quantized bin size + // 2) chord length must be greater than sqrt(2) to leave current cell + // 3) maximum curvature must be respected, represented by minimum turning angle + // Thusly: + // On circle of radius minimum turning angle, we need select motion primitives + // with chord length > sqrt(2) and be an increment of our bin size + // + // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size + // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) + float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); + // Now make sure angle is an increment of the quantized bin size + // And since its based on the minimum chord, we need to make sure its always larger + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + // Search dimensions are clean multiples of quantization - this prevents + // paths with loops in them + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + // find deflections + // If we make a right triangle out of the chord in circle of radius + // min turning angle, we can see that delta X = R * sin (angle) + const float delta_x = min_turning_radius * sin(angle); + // Using that same right triangle, we can see that the complement + // to delta Y is R * cos (angle). If we subtract R, we get the actual value + const float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); + const float delta_dist = hypotf(delta_x, delta_y); + + projections.clear(); + projections.reserve(3); + projections.emplace_back(delta_dist, 0.0, 0.0, TurnDirection::FORWARD); // Forward + projections.emplace_back(delta_x, delta_y, increments, TurnDirection::LEFT); // Left + projections.emplace_back(delta_x, -delta_y, -increments, TurnDirection::RIGHT); // Right + + if (search_info.allow_primitive_interpolation && increments > 1.0f) { + // Create primitives that are +/- N to fill in search space to use all set angular quantizations + // Allows us to create N many primitives so that each search iteration can expand into any angle + // bin possible with the minimum turning radius constraint, not just the most extreme turns. + projections.reserve(3 + (2 * (increments - 1))); + for (unsigned int i = 1; i < static_cast(increments); i++) { + const float angle_n = static_cast(i) * bin_size; + const float turning_rad_n = delta_dist / (2.0f * sin(angle_n / 2.0f)); + const float delta_x_n = turning_rad_n * sin(angle_n); + const float delta_y_n = turning_rad_n - (turning_rad_n * cos(angle_n)); + projections.emplace_back( + delta_x_n, delta_y_n, static_cast(i), TurnDirection::LEFT); // Left + projections.emplace_back( + delta_x_n, -delta_y_n, -static_cast(i), TurnDirection::RIGHT); // Right + } + } + + // Create the correct OMPL state space + state_space = std::make_shared(min_turning_radius); + + // Precompute projection deltas + delta_xs.resize(projections.size()); + delta_ys.resize(projections.size()); + trig_values.resize(num_angle_quantization); + + for (unsigned int i = 0; i != projections.size(); i++) { + delta_xs[i].resize(num_angle_quantization); + delta_ys[i].resize(num_angle_quantization); + + for (unsigned int j = 0; j != num_angle_quantization; j++) { + double cos_theta = cos(bin_size * j); + double sin_theta = sin(bin_size * j); + if (i == 0) { + // if first iteration, cache the trig values for later + trig_values[j] = {cos_theta, sin_theta}; + } + delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; + delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; + } + } + + // Precompute travel costs for each motion primitive + travel_costs.resize(projections.size()); + for (unsigned int i = 0; i != projections.size(); i++) { + const TurnDirection turn_dir = projections[i]._turn_dir; + if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { + // Turning, so length is the arc length + const float arc_angle = projections[i]._theta * bin_size; + const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); + travel_costs[i] = turning_rad * arc_angle; + } else { + travel_costs[i] = delta_dist; + } + } +} + +// http://planning.cs.uiuc.edu/planning/node822.html +// Same as Dubin model but now reverse is valid +// See notes in Dubin for explanation +void HybridMotionTable::initReedsShepp( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + travel_distance_reward = 1.0f - search_info.retrospective_penalty; + downsample_obstacle_heuristic = search_info.downsample_obstacle_heuristic; + use_quadratic_cost_penalty = search_info.use_quadratic_cost_penalty; + + // if nothing changed, no need to re-compute primitives + if (num_angle_quantization_in == num_angle_quantization && + min_turning_radius == search_info.minimum_turning_radius && + allow_primitive_interpolation == search_info.allow_primitive_interpolation && + motion_model == MotionModel::REEDS_SHEPP) + { + return; + } + + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + min_turning_radius = search_info.minimum_turning_radius; + allow_primitive_interpolation = search_info.allow_primitive_interpolation; + motion_model = MotionModel::REEDS_SHEPP; + + float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + const float delta_x = min_turning_radius * sin(angle); + const float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); + const float delta_dist = hypotf(delta_x, delta_y); + + projections.clear(); + projections.reserve(6); + projections.emplace_back(delta_dist, 0.0, 0.0, TurnDirection::FORWARD); // Forward + projections.emplace_back( + delta_x, delta_y, increments, TurnDirection::LEFT); // Forward + Left + projections.emplace_back( + delta_x, -delta_y, -increments, TurnDirection::RIGHT); // Forward + Right + projections.emplace_back(-delta_dist, 0.0, 0.0, TurnDirection::REVERSE); // Backward + projections.emplace_back( + -delta_x, delta_y, -increments, TurnDirection::REV_LEFT); // Backward + Left + projections.emplace_back( + -delta_x, -delta_y, increments, TurnDirection::REV_RIGHT); // Backward + Right + + if (search_info.allow_primitive_interpolation && increments > 1.0f) { + // Create primitives that are +/- N to fill in search space to use all set angular quantizations + // Allows us to create N many primitives so that each search iteration can expand into any angle + // bin possible with the minimum turning radius constraint, not just the most extreme turns. + projections.reserve(6 + (4 * (increments - 1))); + for (unsigned int i = 1; i < static_cast(increments); i++) { + const float angle_n = static_cast(i) * bin_size; + const float turning_rad_n = delta_dist / (2.0f * sin(angle_n / 2.0f)); + const float delta_x_n = turning_rad_n * sin(angle_n); + const float delta_y_n = turning_rad_n - (turning_rad_n * cos(angle_n)); + projections.emplace_back( + delta_x_n, delta_y_n, static_cast(i), TurnDirection::LEFT); // Forward + Left + projections.emplace_back( + delta_x_n, -delta_y_n, -static_cast(i), TurnDirection::RIGHT); // Forward + Right + projections.emplace_back( + -delta_x_n, delta_y_n, -static_cast(i), + TurnDirection::REV_LEFT); // Backward + Left + projections.emplace_back( + -delta_x_n, -delta_y_n, static_cast(i), + TurnDirection::REV_RIGHT); // Backward + Right + } + } + + // Create the correct OMPL state space + state_space = std::make_shared(min_turning_radius); + + // Precompute projection deltas + delta_xs.resize(projections.size()); + delta_ys.resize(projections.size()); + trig_values.resize(num_angle_quantization); + + for (unsigned int i = 0; i != projections.size(); i++) { + delta_xs[i].resize(num_angle_quantization); + delta_ys[i].resize(num_angle_quantization); + + for (unsigned int j = 0; j != num_angle_quantization; j++) { + double cos_theta = cos(bin_size * j); + double sin_theta = sin(bin_size * j); + if (i == 0) { + // if first iteration, cache the trig values for later + trig_values[j] = {cos_theta, sin_theta}; + } + delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; + delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; + } + } + + // Precompute travel costs for each motion primitive + travel_costs.resize(projections.size()); + for (unsigned int i = 0; i != projections.size(); i++) { + const TurnDirection turn_dir = projections[i]._turn_dir; + if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { + // Turning, so length is the arc length + const float arc_angle = projections[i]._theta * bin_size; + const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); + travel_costs[i] = turning_rad * arc_angle; + } else { + travel_costs[i] = delta_dist; + } + } +} + +MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) +{ + MotionPoses projection_list; + projection_list.reserve(projections.size()); + + for (unsigned int i = 0; i != projections.size(); i++) { + const MotionPose & proj_motion_model = projections[i]; + + // normalize theta, I know its overkill, but I've been burned before... + const float & node_heading = node->pose.theta; + float new_heading = node_heading + proj_motion_model._theta; + + if (new_heading < 0.0) { + new_heading += num_angle_quantization_float; + } + + if (new_heading >= num_angle_quantization_float) { + new_heading -= num_angle_quantization_float; + } + + projection_list.emplace_back( + delta_xs[i][node_heading] + node->pose.x, + delta_ys[i][node_heading] + node->pose.y, + new_heading, proj_motion_model._turn_dir); + } + + return projection_list; +} + +unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) +{ + auto bin = static_cast(round(static_cast(theta) / bin_size)); + return bin < num_angle_quantization ? bin : 0u; +} + +float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) +{ + return bin_idx * bin_size; +} + +double HybridMotionTable::getAngle(const double & theta) +{ + return theta / bin_size; +} + +NodeHybrid::NodeHybrid(const uint64_t index, NodeContext * ctx) +: parent(nullptr), + pose(0.0f, 0.0f, 0.0f), + _cell_cost(std::numeric_limits::quiet_NaN()), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _motion_primitive_index(std::numeric_limits::max()), + _is_node_valid(false), + _ctx(ctx) +{ +} + +NodeHybrid::~NodeHybrid() +{ + parent = nullptr; +} + +void NodeHybrid::reset() +{ + parent = nullptr; + _cell_cost = std::numeric_limits::quiet_NaN(); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _motion_primitive_index = std::numeric_limits::max(); + pose.x = 0.0f; + pose.y = 0.0f; + pose.theta = 0.0f; + _is_node_valid = false; +} + +bool NodeHybrid::isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker * collision_checker) +{ + // Already found, we can return the result + if (!std::isnan(_cell_cost)) { + return _is_node_valid; + } + + _is_node_valid = !collision_checker->inCollision( + this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown); + _cell_cost = collision_checker->getCost(); + return _is_node_valid; +} + +float NodeHybrid::getTraversalCost(const NodePtr & child) +{ + const float normalized_cost = child->getCost() / 252.0f; + if (std::isnan(normalized_cost)) { + throw std::runtime_error( + "Node attempted to get traversal " + "cost without a known SE2 collision cost!"); + } + + const TurnDirection & child_turn_dir = child->getTurnDirection(); + float travel_cost_raw = _ctx->motion_table.travel_costs[child->getMotionPrimitiveIndex()]; + float travel_cost = 0.0; + + if (_ctx->motion_table.use_quadratic_cost_penalty) { + travel_cost_raw *= + (_ctx->motion_table.travel_distance_reward + + (_ctx->motion_table.cost_penalty * normalized_cost * normalized_cost)); + } else { + travel_cost_raw *= + (_ctx->motion_table.travel_distance_reward + _ctx->motion_table.cost_penalty * + normalized_cost); + } + + if (child_turn_dir == TurnDirection::FORWARD || child_turn_dir == TurnDirection::REVERSE || + getMotionPrimitiveIndex() == std::numeric_limits::max()) + { + // New motion is a straight motion, no additional costs to be applied + travel_cost = travel_cost_raw; + } else { + if (getTurnDirection() == child_turn_dir) { + // Turning motion but keeps in same direction: encourages to commit to turning if starting it + travel_cost = travel_cost_raw * _ctx->motion_table.non_straight_penalty; + } else { + // Turning motion and changing direction: penalizes wiggling + travel_cost = travel_cost_raw * + (_ctx->motion_table.non_straight_penalty + _ctx->motion_table.change_penalty); + } + } + + if (child_turn_dir == TurnDirection::REV_RIGHT || + child_turn_dir == TurnDirection::REV_LEFT || + child_turn_dir == TurnDirection::REVERSE) + { + // reverse direction + travel_cost *= _ctx->motion_table.reverse_penalty; + } + + return travel_cost; +} + +float NodeHybrid::getHeuristicCost( + const Coordinates & node_coords, + const CoordinateVector & goals_coords) +{ + // obstacle heuristic does not depend on goal heading + const float obstacle_heuristic = + _ctx->obstacle_heuristic->getObstacleHeuristic(node_coords, _ctx->motion_table.cost_penalty, + _ctx->motion_table.use_quadratic_cost_penalty, + _ctx->motion_table.downsample_obstacle_heuristic); + float distance_heuristic = std::numeric_limits::max(); + for (unsigned int i = 0; i < goals_coords.size(); i++) { + distance_heuristic = std::min( + distance_heuristic, + _ctx->distance_heuristic->getDistanceHeuristic(node_coords, goals_coords[i], + obstacle_heuristic, _ctx->motion_table)); + } + return std::max(obstacle_heuristic, distance_heuristic); +} + +void NodeHybrid::initMotionModel( + NodeContext * ctx, + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & num_angle_quantization, + SearchInfo & search_info) +{ + // find the motion model selected + switch (motion_model) { + case MotionModel::DUBIN: + ctx->motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); + break; + case MotionModel::REEDS_SHEPP: + ctx->motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); + break; + default: + throw std::runtime_error( + "Invalid motion model for Hybrid A*. Please select between" + " Dubin (Ackermann forward only)," + " Reeds-Shepp (Ackermann forward and back)."); + } +} + +void NodeHybrid::getNeighbors( + std::function & NeighborGetter, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) +{ + uint64_t index = 0; + NodePtr neighbor = nullptr; + Coordinates initial_node_coords; + const MotionPoses motion_projections = _ctx->motion_table.getProjections(this); + + for (unsigned int i = 0; i != motion_projections.size(); i++) { + index = NodeHybrid::getIndex( + static_cast(motion_projections[i]._x), + static_cast(motion_projections[i]._y), + static_cast(motion_projections[i]._theta), + _ctx->motion_table.size_x, _ctx->motion_table.num_angle_quantization); + + if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { + // Cache the initial pose in case it was visited but valid + // don't want to disrupt continuous coordinate expansion + initial_node_coords = neighbor->pose; + neighbor->setPose( + Coordinates( + motion_projections[i]._x, + motion_projections[i]._y, + motion_projections[i]._theta)); + if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { + neighbor->setMotionPrimitiveIndex(i, motion_projections[i]._turn_dir); + neighbors.push_back(neighbor); + } else { + neighbor->setPose(initial_node_coords); + } + } + } +} + +bool NodeHybrid::backtracePath(CoordinateVector & path) +{ + if (!this->parent) { + return false; + } + + NodePtr current_node = this; + + while (current_node->parent) { + path.push_back(current_node->pose); + // Convert angle to radians + path.back().theta = _ctx->motion_table.getAngleFromBin(path.back().theta); + current_node = current_node->parent; + } + + // add the start pose + path.push_back(current_node->pose); + // Convert angle to radians + path.back().theta = _ctx->motion_table.getAngleFromBin(path.back().theta); + + return true; +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp new file mode 100644 index 00000000..74715329 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp @@ -0,0 +1,230 @@ +// Copyright (c) 2026, 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. Reserved. + +#include "athena_smac_planner/obstacle_heuristic.hpp" + +namespace athena_smac_planner +{ + +void ObstacleHeuristic::resetObstacleHeuristic( + std::shared_ptr costmap_ros_i, + const unsigned int & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y, + const bool downsample_obstacle_heuristic) +{ + // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up + // the planner considerably to search through 75% less cells with no detectable + // erosion of path quality after even modest smoothing. The error would be no more + // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality + costmap_ros = costmap_ros_i; + auto costmap = costmap_ros->getCostmap(); + + // Clear lookup table + unsigned int size = 0u; + unsigned int size_x = 0u; + if (downsample_obstacle_heuristic) { + size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); + size = size_x * + ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); + } else { + size_x = costmap->getSizeInCellsX(); + size = size_x * costmap->getSizeInCellsY(); + } + + if (obstacle_heuristic_lookup_table_.size() == size) { + // must reset all values + std::fill( + obstacle_heuristic_lookup_table_.begin(), + obstacle_heuristic_lookup_table_.end(), 0.0f); + } else { + unsigned int obstacle_size = obstacle_heuristic_lookup_table_.size(); + obstacle_heuristic_lookup_table_.resize(size, 0.0f); + // must reset values for non-constructed indices + std::fill_n( + obstacle_heuristic_lookup_table_.begin(), obstacle_size, 0.0f); + } + + obstacle_heuristic_queue_.clear(); + obstacle_heuristic_queue_.reserve(size); + + // Set initial goal point to queue from. Divided by 2 due to downsampled costmap. + unsigned int goal_index; + if (downsample_obstacle_heuristic) { + goal_index = floor(goal_y / 2.0f) * size_x + floor(goal_x / 2.0f); + } else { + goal_index = floor(goal_y) * size_x + floor(goal_x); + } + + obstacle_heuristic_queue_.emplace_back( + distanceHeuristic2D(goal_index, size_x, start_x, start_y), goal_index); + + // initialize goal cell with a very small value to differentiate it from 0.0 (~uninitialized) + // the negative value means the cell is in the open set + obstacle_heuristic_lookup_table_[goal_index] = -0.00001f; +} + +float ObstacleHeuristic::getObstacleHeuristic( + const Coordinates & node_coords, + const float & cost_penalty, + const bool use_quadratic_cost_penalty, + const bool downsample_obstacle_heuristic) +{ + // If already expanded, return the cost + auto costmap = costmap_ros->getCostmap(); + unsigned int size_x = 0u; + unsigned int size_y = 0u; + if (downsample_obstacle_heuristic) { + size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); + size_y = ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); + } else { + size_x = costmap->getSizeInCellsX(); + size_y = costmap->getSizeInCellsY(); + } + + // Divided by 2 due to downsampled costmap. + unsigned int start_y, start_x; + if (downsample_obstacle_heuristic) { + start_y = floor(node_coords.y / 2.0f); + start_x = floor(node_coords.x / 2.0f); + } else { + start_y = floor(node_coords.y); + start_x = floor(node_coords.x); + } + + const unsigned int start_index = start_y * size_x + start_x; + const float & requested_node_cost = obstacle_heuristic_lookup_table_[start_index]; + if (requested_node_cost > 0.0f) { + // costs are doubled due to downsampling + return downsample_obstacle_heuristic ? 2.0f * requested_node_cost : requested_node_cost; + } + + // If not, expand until it is included. This dynamic programming ensures that + // we only expand the MINIMUM spanning set of the costmap per planning request. + // Rather than naively expanding the entire (potentially massive) map for a limited + // path, we only expand to the extent required for the furthest expansion in the + // search-planning request that dynamically updates during search as needed. + + // start_x and start_y have changed since last call + // we need to recompute 2D distance heuristic and reprioritize queue + for (auto & n : obstacle_heuristic_queue_) { + n.first = -obstacle_heuristic_lookup_table_[n.second] + + distanceHeuristic2D(n.second, size_x, start_x, start_y); + } + std::make_heap( + obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), + ObstacleHeuristicComparator{}); + + const int size_x_int = static_cast(size_x); + const float sqrt2 = sqrtf(2.0f); + float c_cost, cost, travel_cost, new_cost, existing_cost; + unsigned int mx, my; + unsigned int idx, new_idx = 0; + + const std::vector neighborhood = {1, -1, // left right + size_x_int, -size_x_int, // up down + size_x_int + 1, size_x_int - 1, // upper diagonals + -size_x_int + 1, -size_x_int - 1}; // lower diagonals + + while (!obstacle_heuristic_queue_.empty()) { + idx = obstacle_heuristic_queue_.front().second; + std::pop_heap( + obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), + ObstacleHeuristicComparator{}); + obstacle_heuristic_queue_.pop_back(); + c_cost = obstacle_heuristic_lookup_table_[idx]; + if (c_cost > 0.0f) { + // cell has been processed and closed, no further cost improvements + // are mathematically possible thanks to euclidean distance heuristic consistency + continue; + } + c_cost = -c_cost; + obstacle_heuristic_lookup_table_[idx] = c_cost; // set a positive value to close the cell + + // find neighbors + for (unsigned int i = 0; i != neighborhood.size(); i++) { + new_idx = static_cast(static_cast(idx) + neighborhood[i]); + + // if neighbor path is better and non-lethal, set new cost and add to queue + if (new_idx < size_x * size_y) { + if (downsample_obstacle_heuristic) { + // Get costmap values as if downsampled + unsigned int y_offset = (new_idx / size_x) * 2; + unsigned int x_offset = (new_idx - ((new_idx / size_x) * size_x)) * 2; + cost = costmap->getCost(x_offset, y_offset); + for (unsigned int k = 0; k < 2u; ++k) { + unsigned int mxd = x_offset + k; + if (mxd >= costmap->getSizeInCellsX()) { + continue; + } + for (unsigned int j = 0; j < 2u; ++j) { + unsigned int myd = y_offset + j; + if (myd >= costmap->getSizeInCellsY()) { + continue; + } + if (k == 0 && j == 0) { + continue; + } + cost = std::min(cost, static_cast(costmap->getCost(mxd, myd))); + } + } + } else { + cost = static_cast(costmap->getCost(new_idx)); + } + + if (cost >= INSCRIBED_COST) { + continue; + } + + my = new_idx / size_x; + mx = new_idx - (my * size_x); + + if (mx >= size_x - 3 || mx <= 3) { + continue; + } + if (my >= size_y - 3 || my <= 3) { + continue; + } + + existing_cost = obstacle_heuristic_lookup_table_[new_idx]; + if (existing_cost <= 0.0f) { + if (use_quadratic_cost_penalty) { + travel_cost = + (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 63504.0f)); // 252^2 + } else { + travel_cost = + ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 252.0f)); + } + + new_cost = c_cost + travel_cost; + if (existing_cost == 0.0f || -existing_cost > new_cost) { + // the negative value means the cell is in the open set + obstacle_heuristic_lookup_table_[new_idx] = -new_cost; + obstacle_heuristic_queue_.emplace_back( + new_cost + distanceHeuristic2D(new_idx, size_x, start_x, start_y), new_idx); + std::push_heap( + obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), + ObstacleHeuristicComparator{}); + } + } + } + } + + if (idx == start_index) { + break; + } + } + return downsample_obstacle_heuristic ? 2.0f * requested_node_cost : requested_node_cost; +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp new file mode 100644 index 00000000..bfe1879d --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp @@ -0,0 +1,478 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2023, 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. Reserved. + +#include +#include +#include +#include +#include +#include + +#include "athena_smac_planner/smac_planner_hybrid.hpp" + +// #define BENCHMARK_TESTING + +namespace athena_smac_planner +{ + +using namespace std::chrono; // NOLINT + +SmacPlannerHybrid::SmacPlannerHybrid( + rclcpp::Node::SharedPtr node, + std::shared_ptr costmap_ros, + const std::string & name) +: _a_star(nullptr), + _collision_checker(nullptr, 1, nullptr), + _smoother(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr) +{ + _name = name; + _logger = node->get_logger(); + _clock = node->get_clock(); + _costmap = costmap_ros->getCostmap(); + _costmap_ros = costmap_ros; + _global_frame = costmap_ros->getGlobalFrameID(); + + RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerHybrid", _name.c_str()); + + // Helper: declare param if absent then get its value + auto p = [&node, &name](const std::string & param, auto def) { + if (!node->has_parameter(name + "." + param)) { + node->declare_parameter(name + "." + param, def); + } + return node->get_parameter(name + "." + param).get_value(); + }; + + int angle_quantizations; + double analytic_expansion_max_length_m; + bool smooth_path; + + // General planner params + _downsample_costmap = p("downsample_costmap", false); + _downsampling_factor = p("downsampling_factor", 1); + + angle_quantizations = p("angle_quantization_bins", 72); + _angle_bin_size = 2.0 * M_PI / angle_quantizations; + _angle_quantizations = static_cast(angle_quantizations); + + _tolerance = static_cast(p("tolerance", 0.25)); + _allow_unknown = p("allow_unknown", true); + _max_iterations = p("max_iterations", 1000000); + _max_on_approach_iterations = p("max_on_approach_iterations", 1000); + _terminal_checking_interval = p("terminal_checking_interval", 5000); + smooth_path = p("smooth_path", true); + + _minimum_turning_radius_global_coords = p("minimum_turning_radius", 0.4); + _search_info.allow_primitive_interpolation = p("allow_primitive_interpolation", false); + _search_info.cache_obstacle_heuristic = p("cache_obstacle_heuristic", false); + _search_info.reverse_penalty = p("reverse_penalty", 2.0); + _search_info.change_penalty = p("change_penalty", 0.0); + _search_info.non_straight_penalty = p("non_straight_penalty", 1.2); + _search_info.cost_penalty = p("cost_penalty", 2.0); + _search_info.retrospective_penalty = p("retrospective_penalty", 0.015); + _search_info.analytic_expansion_ratio = p("analytic_expansion_ratio", 3.5); + _search_info.analytic_expansion_max_cost = p("analytic_expansion_max_cost", 200.0); + _search_info.analytic_expansion_max_cost_override = + p("analytic_expansion_max_cost_override", false); + _search_info.use_quadratic_cost_penalty = p("use_quadratic_cost_penalty", false); + _search_info.downsample_obstacle_heuristic = p("downsample_obstacle_heuristic", true); + + analytic_expansion_max_length_m = p("analytic_expansion_max_length", 3.0); + _search_info.analytic_expansion_max_length = + analytic_expansion_max_length_m / _costmap->getResolution(); + + _max_planning_time = p("max_planning_time", 5.0); + _lookup_table_size = p("lookup_table_size", 20.0); + _debug_visualizations = p("debug_visualizations", false); + _motion_model_for_search = p("motion_model_for_search", std::string("DUBIN")); + + std::string goal_heading_type = p("goal_heading_mode", std::string("DEFAULT")); + _goal_heading_mode = fromStringToGH(goal_heading_type); + + _coarse_search_resolution = p("coarse_search_resolution", 1); + + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { + throw std::runtime_error( + "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION."); + } + + _motion_model = fromString(_motion_model_for_search); + + if (_motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.", + _motion_model_for_search.c_str()); + } + + if (_max_on_approach_iterations <= 0) { + RCLCPP_WARN( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + + if (_max_iterations <= 0) { + RCLCPP_WARN( + _logger, "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0, " + "disabling coarse iteration resolution search for goal heading"); + _coarse_search_resolution = 1; + } + + if (_angle_quantizations % _coarse_search_resolution != 0) { + throw std::runtime_error( + "coarse iteration should be an increment of the number of angular bins configured"); + } + + if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) { + RCLCPP_WARN( + _logger, "Min turning radius cannot be less than the search grid cell resolution!"); + _minimum_turning_radius_global_coords = _costmap->getResolution() * _downsampling_factor; + } + + // Convert to grid coordinates + if (!_downsample_costmap) { + _downsampling_factor = 1; + } + _search_info.minimum_turning_radius = + _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); + _lookup_table_dim = + static_cast(_lookup_table_size) / + static_cast(_costmap->getResolution() * _downsampling_factor); + + // Make sure its a whole number + _lookup_table_dim = static_cast(static_cast(_lookup_table_dim)); + + // Make sure its an odd number + if (static_cast(_lookup_table_dim) % 2 == 0) { + RCLCPP_INFO( + _logger, + "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", + _lookup_table_dim); + _lookup_table_dim += 1.0; + } + + // Initialize collision checker + _collision_checker = GridCollisionChecker( + _costmap_ros, _angle_quantizations, node); + _collision_checker.setFootprint( + costmap_ros->getRobotFootprint(), + costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); + + // Initialize A* template + _a_star = std::make_unique>(_motion_model, _search_info); + _a_star->initialize( + _allow_unknown, + _max_iterations, + _max_on_approach_iterations, + _terminal_checking_interval, + _max_planning_time, + _lookup_table_dim, + _angle_quantizations); + + // Initialize path smoother + if (smooth_path) { + SmootherParams smoother_params; + smoother_params.get(node.get(), _name); + _smoother = std::make_unique(smoother_params); + _smoother->initialize(_minimum_turning_radius_global_coords); + } + + // Initialize costmap downsampler + if (_downsample_costmap && _downsampling_factor > 1) { + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure(_costmap, _downsampling_factor); + } + + // Create publishers + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + + if (_debug_visualizations) { + _expansions_publisher = + node->create_publisher("expansions", 1); + _planned_footprints_publisher = + node->create_publisher("planned_footprints", 1); + _smoothed_footprints_publisher = + node->create_publisher("smoothed_footprints", 1); + } + + RCLCPP_INFO( + _logger, "Configured %s of type SmacPlannerHybrid with " + "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f." + "Using motion model: %s.", + _name.c_str(), _max_iterations, _max_on_approach_iterations, + _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + _tolerance, toString(_motion_model).c_str()); +} + +SmacPlannerHybrid::~SmacPlannerHybrid() +{ + RCLCPP_INFO(_logger, "Destroying %s of type SmacPlannerHybrid", _name.c_str()); + _a_star.reset(); + _smoother.reset(); + if (_costmap_downsampler) { + _costmap_downsampler->on_cleanup(); + _costmap_downsampler.reset(); + } +} + +nav_msgs::msg::Path SmacPlannerHybrid::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) +{ + std::lock_guard lock_reinit(_mutex); + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_downsample_costmap && _downsampling_factor > 1) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + _collision_checker.setCostmap(costmap); + } + + // Set collision checker and costmap information + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); + _a_star->setCollisionChecker(&_collision_checker); + + // Set starting point, in A* bin search coordinates + float mx_start, my_start, mx_goal, my_goal; + { + unsigned int umx, umy; + if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, umx, umy)) { + throw std::runtime_error( + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); + } + mx_start = static_cast(umx); + my_start = static_cast(umy); + } + + double start_orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); + while (start_orientation_bin < 0.0) { + start_orientation_bin += static_cast(_angle_quantizations); + } + // This is needed to handle precision issues + if (start_orientation_bin >= static_cast(_angle_quantizations)) { + start_orientation_bin -= static_cast(_angle_quantizations); + } + unsigned int start_orientation_bin_int = + static_cast(start_orientation_bin); + _a_star->setStart(mx_start, my_start, start_orientation_bin_int); + + // Set goal point, in A* bin search coordinates + { + unsigned int umx, umy; + if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, umx, umy)) { + throw std::runtime_error( + "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + + std::to_string(goal.pose.position.y) + ") was outside bounds"); + } + mx_goal = static_cast(umx); + my_goal = static_cast(umy); + } + double goal_orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); + while (goal_orientation_bin < 0.0) { + goal_orientation_bin += static_cast(_angle_quantizations); + } + // This is needed to handle precision issues + if (goal_orientation_bin >= static_cast(_angle_quantizations)) { + goal_orientation_bin -= static_cast(_angle_quantizations); + } + unsigned int goal_orientation_bin_int = + static_cast(goal_orientation_bin); + _a_star->setGoal( + mx_goal, my_goal, static_cast(goal_orientation_bin_int), + _goal_heading_mode, _coarse_search_resolution); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _clock->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + // Corner case of start and goal being on the same cell + if (std::floor(mx_start) == std::floor(mx_goal) && + std::floor(my_start) == std::floor(my_goal) && + start_orientation_bin_int == goal_orientation_bin_int) + { + pose.pose = start.pose; + pose.pose.orientation = goal.pose.orientation; + plan.poses.push_back(pose); + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + auto msg = std::make_unique(plan); + _raw_plan_publisher->publish(std::move(msg)); + } + + return plan; + } + + // Compute plan + NodeHybrid::CoordinateVector path; + int num_iterations = 0; + std::string error; + std::unique_ptr>> expansions = nullptr; + if (_debug_visualizations) { + expansions = std::make_unique>>(); + } + + if (!_a_star->createPath( + path, num_iterations, + _tolerance / static_cast(costmap->getResolution()), cancel_checker, expansions.get())) + { + if (_debug_visualizations) { + auto msg = std::make_unique(); + geometry_msgs::msg::Pose msg_pose; + msg->header.stamp = _clock->now(); + msg->header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg->poses.push_back(msg_pose); + } + _expansions_publisher->publish(std::move(msg)); + } + + if (num_iterations == 1) { + throw std::runtime_error("Start occupied"); + } + + if (num_iterations < _a_star->getMaxIterations()) { + throw std::runtime_error("No valid path could be found"); + } else { + throw std::runtime_error("Exceeded maximum iterations"); + } + } + + // Convert to world coordinates + plan.poses.reserve(path.size()); + for (int i = path.size() - 1; i >= 0; --i) { + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); + pose.pose.orientation = getWorldOrientation(path[i].theta); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + auto msg = std::make_unique(plan); + _raw_plan_publisher->publish(std::move(msg)); + } + + if (_debug_visualizations) { + // Publish expansions for debug + auto now = _clock->now(); + auto msg = std::make_unique(); + geometry_msgs::msg::Pose msg_pose; + msg->header.stamp = now; + msg->header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg->poses.push_back(msg_pose); + } + _expansions_publisher->publish(std::move(msg)); + + if (_planned_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _planned_footprints_publisher->publish(std::move(marker_array)); + + // Publish planned footprints for debug + marker_array = std::make_unique(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); + } + _planned_footprints_publisher->publish(std::move(marker_array)); + } + } + + // Find how much time we have left to do smoothing + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + double time_remaining = _max_planning_time - static_cast(time_span.count()); + +#ifdef BENCHMARK_TESTING + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + + // Smooth plan + if (_smoother && num_iterations > 1) { + _smoother->smooth(plan, costmap, time_remaining); + } + +#ifdef BENCHMARK_TESTING + steady_clock::time_point c = steady_clock::now(); + duration time_span2 = duration_cast>(c - b); + std::cout << "It took " << time_span2.count() * 1000 << + " milliseconds to smooth path." << std::endl; +#endif + + if (_debug_visualizations) { + if (_smoothed_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _smoothed_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); + auto now = _clock->now(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); + } + _smoothed_footprints_publisher->publish(std::move(marker_array)); + } + } + + return plan; +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp new file mode 100644 index 00000000..7438c3d8 --- /dev/null +++ b/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp @@ -0,0 +1,436 @@ +// Copyright (c) 2021, Samsung Research America +// +// 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. Reserved. + +#include +#include + +#include +#include +#include + +#include "angles/angles.h" + +#include "tf2/utils.hpp" + +#include "athena_smac_planner/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" + +namespace athena_smac_planner +{ +using namespace nav2_util::geometry_utils; // NOLINT +using namespace std::chrono; // NOLINT +using smoother_utils::PathSegment; + +Smoother::Smoother(const SmootherParams & params) +{ + tolerance_ = params.tolerance_; + max_its_ = params.max_its_; + data_w_ = params.w_data_; + smooth_w_ = params.w_smooth_; + is_holonomic_ = params.holonomic_; + do_refinement_ = params.do_refinement_; + refinement_num_ = params.refinement_num_; +} + +void Smoother::initialize(const double & min_turning_radius) +{ + min_turning_rad_ = min_turning_radius; + state_space_ = std::make_unique(min_turning_rad_); +} + +bool Smoother::smooth( + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time) +{ + // by-pass path orientations approximation when skipping smac smoother + if (max_its_ == 0) { + return false; + } + + steady_clock::time_point start = steady_clock::now(); + double time_remaining = max_time; + bool success = true, reversing_segment; + nav_msgs::msg::Path curr_path_segment; + curr_path_segment.header = path.header; + std::vector path_segments = smoother_utils::findDirectionalPathSegments(path); + + for (unsigned int i = 0; i != path_segments.size(); i++) { + if (path_segments[i].end - path_segments[i].start > 10) { + // Populate path segment + curr_path_segment.poses.clear(); + std::copy( + path.poses.begin() + path_segments[i].start, + path.poses.begin() + path_segments[i].end + 1, + std::back_inserter(curr_path_segment.poses)); + + // Make sure we're still able to smooth with time remaining + steady_clock::time_point now = steady_clock::now(); + time_remaining = max_time - duration_cast>(now - start).count(); + refinement_ctr_ = 0; + + // Smooth path segment naively + const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose; + const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back().pose; + bool local_success = + smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining); + success = success && local_success; + + // Enforce boundary conditions + if (!is_holonomic_ && local_success) { + enforceStartBoundaryConditions(start_pose, curr_path_segment, costmap, reversing_segment); + enforceEndBoundaryConditions(goal_pose, curr_path_segment, costmap, reversing_segment); + } + + // Assemble the path changes to the main path + std::copy( + curr_path_segment.poses.begin(), + curr_path_segment.poses.end(), + path.poses.begin() + path_segments[i].start); + } + } + + return success; +} + +bool Smoother::smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time) +{ + steady_clock::time_point a = steady_clock::now(); + rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time); + + int its = 0; + double change = tolerance_; + const unsigned int & path_size = path.poses.size(); + double x_i, y_i, y_m1, y_ip1, y_i_org; + unsigned int mx, my; + + nav_msgs::msg::Path new_path = path; + nav_msgs::msg::Path last_path = path; + + while (change >= tolerance_) { + its += 1; + change = 0.0; + + // Make sure the smoothing function will converge + if (its >= max_its_) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Number of iterations has exceeded limit of %i.", max_its_); + path = last_path; + smoother_utils::updateApproximatePathOrientations(path, reversing_segment); + return false; + } + + // Make sure still have time left to process + steady_clock::time_point b = steady_clock::now(); + rclcpp::Duration timespan(duration_cast>(b - a)); + if (timespan > max_dur) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing time exceeded allowed duration of %0.2f.", max_time); + path = last_path; + smoother_utils::updateApproximatePathOrientations(path, reversing_segment); + return false; + } + + for (unsigned int i = 1; i != path_size - 1; i++) { + for (unsigned int j = 0; j != 2; j++) { + x_i = getFieldByDim(path.poses[i], j); + y_i = getFieldByDim(new_path.poses[i], j); + y_m1 = getFieldByDim(new_path.poses[i - 1], j); + y_ip1 = getFieldByDim(new_path.poses[i + 1], j); + y_i_org = y_i; + + // Smooth based on local 3 point neighborhood and original data locations + y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i)); + setFieldByDim(new_path.poses[i], j, y_i); + change += abs(y_i - y_i_org); + } + + // validate update is admissible, only checks cost if a valid costmap pointer is provided + float cost = 0.0; + if (costmap) { + costmap->worldToMap( + getFieldByDim(new_path.poses[i], 0), + getFieldByDim(new_path.poses[i], 1), + mx, my); + cost = static_cast(costmap->getCost(mx, my)); + } + + if (cost > MAX_NON_OBSTACLE_COST && cost != UNKNOWN_COST) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing process resulted in an infeasible collision. " + "Returning the last path before the infeasibility was introduced."); + path = last_path; + smoother_utils::updateApproximatePathOrientations(path, reversing_segment); + return false; + } + } + + last_path = new_path; + } + + // Let's do additional refinement, it shouldn't take more than a couple milliseconds + // but really puts the path quality over the top. + if (do_refinement_ && refinement_ctr_ < refinement_num_) { + refinement_ctr_++; + smoothImpl(new_path, reversing_segment, costmap, max_time); + } + + smoother_utils::updateApproximatePathOrientations(new_path, reversing_segment); + path = new_path; + return true; +} + +double Smoother::getFieldByDim( + const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim) +{ + if (dim == 0) { + return msg.pose.position.x; + } else if (dim == 1) { + return msg.pose.position.y; + } else { + return msg.pose.position.z; + } +} + +void Smoother::setFieldByDim( + geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, + const double & value) +{ + if (dim == 0) { + msg.pose.position.x = value; + } else if (dim == 1) { + msg.pose.position.y = value; + } else { + msg.pose.position.z = value; + } +} + +unsigned int Smoother::findShortestBoundaryExpansionIdx( + const BoundaryExpansions & boundary_expansions) +{ + // Check which is valid with the minimum integrated length such that + // shorter end-points away that are infeasible to achieve without + // a loop-de-loop are punished + double min_length = 1e9; + int shortest_boundary_expansion_idx = 1e9; + for (unsigned int idx = 0; idx != boundary_expansions.size(); idx++) { + if (boundary_expansions[idx].expansion_path_length0.0 && + boundary_expansions[idx].expansion_path_length > 0.0) + { + min_length = boundary_expansions[idx].expansion_path_length; + shortest_boundary_expansion_idx = idx; + } + } + + return shortest_boundary_expansion_idx; +} + +void Smoother::findBoundaryExpansion( + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & end, + BoundaryExpansion & expansion, + const nav2_costmap_2d::Costmap2D * costmap) +{ + ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_); + + from[0] = start.position.x; + from[1] = start.position.y; + from[2] = tf2::getYaw(start.orientation); + to[0] = end.position.x; + to[1] = end.position.y; + to[2] = tf2::getYaw(end.orientation); + + double d = state_space_->distance(from(), to()); + // If this path is too long compared to the original, then this is probably + // a loop-de-loop, treat as invalid as to not deviate too far from the original path. + // 2.0 selected from prinicipled choice of boundary test points + // r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be + // approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI. + // For all but the last backup test point, a loop would be approximately + // 2x greater than any of the selections. + if (d > 2.0 * expansion.original_path_length) { + return; + } + + std::vector reals; + double theta(0.0), x(0.0), y(0.0); + double x_m = start.position.x; + double y_m = start.position.y; + + // Get intermediary poses + for (double i = 0; i <= expansion.path_end_idx; i++) { + state_space_->interpolate(from(), to(), i / expansion.path_end_idx, s()); + reals = s.reals(); + // Make sure in range [0, 2PI) + theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; + theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; + x = reals[0]; + y = reals[1]; + + // Check for collision + unsigned int mx, my; + costmap->worldToMap(x, y, mx, my); + if (static_cast(costmap->getCost(mx, my)) >= INSCRIBED_COST) { + expansion.in_collision = true; + } + + // Integrate path length + expansion.expansion_path_length += hypot(x - x_m, y - y_m); + x_m = x; + y_m = y; + + // Store point + expansion.pts.emplace_back(x, y, theta); + } +} + +template +BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, IteratorT end) +{ + std::vector distances = { + min_turning_rad_, // Radius + 2.0 * min_turning_rad_, // Diameter + M_PI * min_turning_rad_, // 50% Circumference + 2.0 * M_PI * min_turning_rad_ // Circumference + }; + + BoundaryExpansions boundary_expansions; + boundary_expansions.resize(distances.size()); + double curr_dist = 0.0; + double x_last = start->pose.position.x; + double y_last = start->pose.position.y; + geometry_msgs::msg::Point pt; + unsigned int curr_dist_idx = 0; + + for (IteratorT iter = start; iter != end; iter++) { + pt = iter->pose.position; + curr_dist += hypot(pt.x - x_last, pt.y - y_last); + x_last = pt.x; + y_last = pt.y; + + if (curr_dist >= distances[curr_dist_idx]) { + boundary_expansions[curr_dist_idx].path_end_idx = iter - start; + boundary_expansions[curr_dist_idx].original_path_length = curr_dist; + curr_dist_idx++; + } + + if (curr_dist_idx == boundary_expansions.size()) { + break; + } + } + + return boundary_expansions; +} + +void Smoother::enforceStartBoundaryConditions( + const geometry_msgs::msg::Pose & start_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment) +{ + // Find range of points for testing + BoundaryExpansions boundary_expansions = + generateBoundaryExpansionPoints(path.poses.begin(), path.poses.end()); + + // Generate the motion model and metadata from start -> test points + for (unsigned int i = 0; i != boundary_expansions.size(); i++) { + BoundaryExpansion & expansion = boundary_expansions[i]; + if (expansion.path_end_idx == 0.0) { + continue; + } + + if (!reversing_segment) { + findBoundaryExpansion( + start_pose, path.poses[expansion.path_end_idx].pose, expansion, + costmap); + } else { + findBoundaryExpansion( + path.poses[expansion.path_end_idx].pose, start_pose, expansion, + costmap); + } + } + + // Find the shortest kinematically feasible boundary expansion + unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); + if (best_expansion_idx > boundary_expansions.size()) { + return; + } + + // Override values to match curve + BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; + if (reversing_segment) { + std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); + } + for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { + path.poses[i].pose.position.x = best_expansion.pts[i].x; + path.poses[i].pose.position.y = best_expansion.pts[i].y; + path.poses[i].pose.orientation = orientationAroundZAxis(best_expansion.pts[i].theta); + } +} + +void Smoother::enforceEndBoundaryConditions( + const geometry_msgs::msg::Pose & end_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment) +{ + // Find range of points for testing + BoundaryExpansions boundary_expansions = + generateBoundaryExpansionPoints(path.poses.rbegin(), path.poses.rend()); + + // Generate the motion model and metadata from start -> test points + unsigned int expansion_starting_idx; + for (unsigned int i = 0; i != boundary_expansions.size(); i++) { + BoundaryExpansion & expansion = boundary_expansions[i]; + if (expansion.path_end_idx == 0.0) { + continue; + } + expansion_starting_idx = path.poses.size() - expansion.path_end_idx - 1; + if (!reversing_segment) { + findBoundaryExpansion(path.poses[expansion_starting_idx].pose, end_pose, expansion, costmap); + } else { + findBoundaryExpansion(end_pose, path.poses[expansion_starting_idx].pose, expansion, costmap); + } + } + + // Find the shortest kinematically feasible boundary expansion + unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); + if (best_expansion_idx > boundary_expansions.size()) { + return; + } + + // Override values to match curve + BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; + if (reversing_segment) { + std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); + } + expansion_starting_idx = path.poses.size() - best_expansion.path_end_idx - 1; + for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { + path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x; + path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y; + path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis( + best_expansion.pts[i].theta); + } +} + +} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt b/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt new file mode 100644 index 00000000..8b0d98b2 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.8) +project(dem_costmap) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED) + +set(dependencies + rclcpp + nav_msgs + sensor_msgs + cv_bridge + OpenCV +) + +add_executable(map_node src/map_node.cpp) +target_include_directories(map_node PUBLIC + $ + $ + ${OpenCV_INCLUDE_DIRS}) + +target_compile_features(map_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + map_node SYSTEM + ${dependencies} +) + +target_link_libraries(map_node ${OpenCV_LIBRARIES}) + +install(TARGETS map_node + DESTINATION lib/${PROJECT_NAME}) + +# Install maps directory +install(DIRECTORY maps/ + DESTINATION share/${PROJECT_NAME}/maps) + +# Install launch directory +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch) + +# Install config directory +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/subsystems/minimal_navigation/dem_costmap/LICENSE b/src/subsystems/minimal_navigation/dem_costmap/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml b/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml new file mode 100644 index 00000000..60bafdb3 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml @@ -0,0 +1,8 @@ +dem_costmap_converter: + ros__parameters: + dem_file_path: "" + map_resolution: 1.0 + max_passable_slope_degrees: 15.0 + output_frame: map + origin_x: -400.0 + origin_y: -400.0 \ No newline at end of file diff --git a/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py b/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py new file mode 100644 index 00000000..8b03a15d --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + pkg_dir = get_package_share_directory('dem_costmap') + config_file = os.path.join(pkg_dir, 'config', 'dem_costmap.yaml') + dem_file = os.path.join(pkg_dir, 'maps', 'north_site800m.tif') + + dem_node = Node( + package='dem_costmap', + executable='map_node', + name='dem_costmap_converter', + output='screen', + parameters=[ + config_file, + { + 'dem_file_path': dem_file + } + ] + ) + + return LaunchDescription([ + dem_node + ]) diff --git a/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif b/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif new file mode 100644 index 00000000..ec4e9fca --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:122777571140e3b8484bc334cdbdf3247eacf3a9c06a6624ec7cdc340476a3a3 +size 2559156 diff --git a/src/subsystems/minimal_navigation/dem_costmap/package.xml b/src/subsystems/minimal_navigation/dem_costmap/package.xml new file mode 100644 index 00000000..00fe8e66 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/package.xml @@ -0,0 +1,22 @@ + + + + dem_costmap + 0.0.0 + DEM to Costmap converter for terrain-based navigation + mdurrani + Apache-2.0 + + ament_cmake + rclcpp + nav_msgs + sensor_msgs + cv_bridge + libopencv-dev + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp b/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp new file mode 100644 index 00000000..637faf71 --- /dev/null +++ b/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp @@ -0,0 +1,236 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +class DEMCostmapConverter : public rclcpp::Node +{ +public: + DEMCostmapConverter() : Node("dem_costmap_converter") + { + // Declare parameters + this->declare_parameter("dem_file_path", ""); + this->declare_parameter("map_resolution", 1.0); // meters per pixel + this->declare_parameter("max_passable_slope_degrees", 15.0); + this->declare_parameter("output_frame", "map"); + this->declare_parameter("origin_x", 0.0); + this->declare_parameter("origin_y", 0.0); + + // Get parameters + std::string dem_file_path = this->get_parameter("dem_file_path").as_string(); + map_resolution_ = this->get_parameter("map_resolution").as_double(); + max_passable_slope_degrees_ = this->get_parameter("max_passable_slope_degrees").as_double(); + output_frame_ = this->get_parameter("output_frame").as_string(); + origin_x_ = this->get_parameter("origin_x").as_double(); + origin_y_ = this->get_parameter("origin_y").as_double(); + + // Create publisher with transient_local QoS for static maps + costmap_publisher_ = this->create_publisher( + "map", rclcpp::QoS(1).transient_local()); + + RCLCPP_INFO(this->get_logger(), "DEM Costmap Converter initialized"); + RCLCPP_INFO(this->get_logger(), "DEM file: %s", dem_file_path.c_str()); + RCLCPP_INFO(this->get_logger(), "Resolution: %.2f m/pixel", map_resolution_); + RCLCPP_INFO(this->get_logger(), "Max passable slope: %.1f degrees", max_passable_slope_degrees_); + + // Load and process DEM + costmap_ready_ = false; + if (!dem_file_path.empty()) { + loadAndProcessDEM(dem_file_path); + } else { + RCLCPP_ERROR(this->get_logger(), "No DEM file path provided. Use param 'dem_file_path'"); + RCLCPP_ERROR(this->get_logger(), "Node will not publish costmap without valid DEM file"); + } + + // Publish at 1Hz + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&DEMCostmapConverter::publishCostmap, this)); + } + +private: + void loadAndProcessDEM(const std::string& file_path) + { + RCLCPP_INFO(this->get_logger(), "Loading DEM from: %s", file_path.c_str()); + + // Check if file exists + if (!std::filesystem::exists(file_path)) { + RCLCPP_ERROR(this->get_logger(), "DEM file does not exist: %s", file_path.c_str()); + return; + } + + // Load DEM using OpenCV (supports TIFF) + cv::Mat dem = cv::imread(file_path, cv::IMREAD_ANYDEPTH | cv::IMREAD_GRAYSCALE); + + if (dem.empty()) { + RCLCPP_ERROR(this->get_logger(), "Failed to load DEM file: %s", file_path.c_str()); + return; + } + + RCLCPP_INFO(this->get_logger(), "DEM loaded successfully: %dx%d pixels", dem.cols, dem.rows); + + // Convert to float for calculations + cv::Mat dem_float; + dem.convertTo(dem_float, CV_32F); + + // Calculate slope map + cv::Mat slope_map = calculateSlope(dem_float); + + // Convert slope to costmap + costmap_msg_ = createCostmapFromSlope(slope_map, dem.cols, dem.rows); + costmap_ready_ = true; + + RCLCPP_INFO(this->get_logger(), "Costmap generated successfully"); + } + + cv::Mat calculateSlope(const cv::Mat& dem) + { + RCLCPP_INFO(this->get_logger(), "Calculating slope map..."); + + cv::Mat grad_x, grad_y; + cv::Mat slope_radians = cv::Mat::zeros(dem.size(), CV_32F); + + // Calculate gradients using Sobel operator + cv::Sobel(dem, grad_x, CV_32F, 1, 0, 3); + cv::Sobel(dem, grad_y, CV_32F, 0, 1, 3); + + // Scale gradients by resolution to get proper slope + grad_x = grad_x / (8.0 * map_resolution_); // Sobel scale factor is 8 + grad_y = grad_y / (8.0 * map_resolution_); + + // Calculate slope magnitude + for (int i = 0; i < dem.rows; ++i) { + for (int j = 0; j < dem.cols; ++j) { + float dx = grad_x.at(i, j); + float dy = grad_y.at(i, j); + float slope_rad = std::atan(std::sqrt(dx*dx + dy*dy)); + slope_radians.at(i, j) = slope_rad; + } + } + + // Convert to degrees + cv::Mat slope_degrees; + slope_radians *= (180.0 / M_PI); + slope_degrees = slope_radians; + + return slope_degrees; + } + + nav_msgs::msg::OccupancyGrid createCostmapFromSlope(const cv::Mat& slope_map, int width, int height) + { + RCLCPP_INFO(this->get_logger(), "Converting slope to costmap..."); + + nav_msgs::msg::OccupancyGrid costmap; + costmap.header.frame_id = output_frame_; + costmap.header.stamp = this->get_clock()->now(); + + // Set map info + costmap.info.resolution = map_resolution_; + costmap.info.width = width; + costmap.info.height = height; + costmap.info.origin.position.x = origin_x_; + costmap.info.origin.position.y = origin_y_; + costmap.info.origin.position.z = 0.0; + costmap.info.origin.orientation.w = 1.0; + + // Calculate and log costmap bounds + double costmap_max_x = origin_x_ + (width * map_resolution_); + double costmap_max_y = origin_y_ + (height * map_resolution_); + + RCLCPP_INFO(this->get_logger(), "Costmap bounds:"); + RCLCPP_INFO(this->get_logger(), " Origin: (%.2f, %.2f)", origin_x_, origin_y_); + RCLCPP_INFO(this->get_logger(), " Size: %dx%d pixels", width, height); + RCLCPP_INFO(this->get_logger(), " Resolution: %.2f m/pixel", map_resolution_); + RCLCPP_INFO(this->get_logger(), " Bounds: X[%.2f, %.2f], Y[%.2f, %.2f]", + origin_x_, costmap_max_x, origin_y_, costmap_max_y); + RCLCPP_INFO(this->get_logger(), " Total coverage: %.2f x %.2f meters", + width * map_resolution_, height * map_resolution_); + + // Resize data array + costmap.data.resize(width * height); + + // Convert slope to cost values following nav2 costmap2d architecture + int lethal_count = 0; + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + float slope_deg = slope_map.at(i, j); + int cost = slopeToCost(slope_deg); + + // OpenCV uses (row, col) but OccupancyGrid uses (x, y) + // Need to flip Y coordinate for ROS convention + int ros_i = height - 1 - i; + costmap.data[ros_i * width + j] = cost; + + if (cost >= 99) lethal_count++; + } + } + + RCLCPP_INFO(this->get_logger(), + "Costmap conversion complete. Lethal/high-cost cells: %d/%d (%.1f%%)", + lethal_count, width * height, + 100.0 * lethal_count / (width * height)); + + return costmap; + } + + int slopeToCost(float slope_degrees) + { + // Nav2 costmap2d cost values: + // 0: Free space + // 1-252: Scaled costs + // 253: Possibly circumscribed + // 254: Lethal obstacle (impassable) + // 255: No information / unknown + + if (slope_degrees >= max_passable_slope_degrees_) { + return 254; // Lethal - impassable + } else if (slope_degrees >= 10.0) { + // High cost: linear scale from 150-252 for 10-15 degrees + float ratio = (slope_degrees - 10.0) / (max_passable_slope_degrees_ - 10.0); + return static_cast(150 + ratio * 102); + } else if (slope_degrees >= 5.0) { + // Medium cost: linear scale from 50-149 for 5-10 degrees + float ratio = (slope_degrees - 5.0) / 5.0; + return static_cast(50 + ratio * 99); + } else { + // Low cost: linear scale from 0-49 for 0-5 degrees + float ratio = slope_degrees / 5.0; + return static_cast(ratio * 49); + } + } + + + void publishCostmap() + { + if (costmap_ready_) { + costmap_msg_.header.stamp = this->get_clock()->now(); + costmap_publisher_->publish(costmap_msg_); + } else { + RCLCPP_DEBUG(this->get_logger(), "Costmap not ready, skipping publish"); + } + } + + // Member variables + rclcpp::Publisher::SharedPtr costmap_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + nav_msgs::msg::OccupancyGrid costmap_msg_; + bool costmap_ready_; + + double map_resolution_; + double max_passable_slope_degrees_; + std::string output_frame_; + double origin_x_; + double origin_y_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt b/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt new file mode 100644 index 00000000..209d45e6 --- /dev/null +++ b/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(gps_pose_publisher) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(msgs REQUIRED) +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") +find_package(GeographicLib REQUIRED) + +add_executable(gps_pose_publisher_node src/gps_pose_publisher_node.cpp) +target_include_directories(gps_pose_publisher_node PRIVATE ${GeographicLib_INCLUDE_DIRS}) +target_link_libraries(gps_pose_publisher_node + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${std_msgs_TARGETS} + tf2_ros::tf2_ros + tf2::tf2 + ${msgs_TARGETS} + ${GeographicLib_LIBRARIES} +) + +install(TARGETS gps_pose_publisher_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml b/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml new file mode 100644 index 00000000..9442c5f1 --- /dev/null +++ b/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml @@ -0,0 +1,24 @@ + + + + gps_pose_publisher + 0.1.0 + Converts GPS fix + heading into a map-frame PoseStamped and TF broadcast + UMD Loop + Apache-2.0 + + ament_cmake + + rclcpp + geometry_msgs + sensor_msgs + std_msgs + tf2_ros + tf2 + msgs + geographiclib + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp b/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp new file mode 100644 index 00000000..10d0291c --- /dev/null +++ b/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp @@ -0,0 +1,192 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "std_msgs/msg/string.hpp" +#include "msgs/msg/heading.hpp" +#include "tf2_ros/transform_broadcaster.hpp" +#include "msgs/srv/lat_lon_to_enu.hpp" + +#include + +class GpsPosePublisher : public rclcpp::Node +{ +public: + explicit GpsPosePublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("gps_pose_publisher", options) + { + declare_parameter("heading_topic", std::string("/gps/heading")); + declare_parameter("use_start_gate_ref", false); + declare_parameter("origin_lat", std::numeric_limits::quiet_NaN()); + declare_parameter("origin_lon", std::numeric_limits::quiet_NaN()); + declare_parameter("origin_alt", 0.0); + + tf_broadcaster_ = std::make_shared(*this); + + pose_pub_ = create_publisher("/robot_pose", 10); + status_pub_ = create_publisher("/gps_status", 10); + + fix_sub_ = create_subscription( + "/gps/fix", rclcpp::SensorDataQoS(), + [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { onFix(msg); }); + + std::string heading_topic; + get_parameter("heading_topic", heading_topic); + heading_sub_ = create_subscription( + heading_topic, rclcpp::SensorDataQoS(), + [this](const msgs::msg::Heading::SharedPtr msg) { + heading_enu_rad_ = (90.0 - msg->compass_bearing) * M_PI / 180.0; + has_heading_ = true; + }); + + // Pre-set origin from params if provided (non-NaN overrides first-fix logic) + const double param_lat = get_parameter("origin_lat").as_double(); + const double param_lon = get_parameter("origin_lon").as_double(); + const double param_alt = get_parameter("origin_alt").as_double(); + if (!std::isnan(param_lat) && !std::isnan(param_lon)) { + setOrigin(param_lat, param_lon, param_alt); + } + + bool use_start_gate_ref; + get_parameter("use_start_gate_ref", use_start_gate_ref); + if (use_start_gate_ref) { + start_gate_sub_ = create_subscription( + "/start_gate_ref", 1, + [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { + if (!origin_set_) { + setOrigin(msg->latitude, msg->longitude, msg->altitude); + } + }); + } + + latlon_srv_ = create_service( + "~/latlon_to_enu", + [this]( + const msgs::srv::LatLonToENU::Request::SharedPtr req, + msgs::srv::LatLonToENU::Response::SharedPtr res) + { + if (!origin_set_) { + RCLCPP_ERROR(get_logger(), "latlon_to_enu called before origin is set"); + res->x = 0.0; res->y = 0.0; res->z = 0.0; + return; + } + double x, y, z; + projection_.Forward(req->lat, req->lon, 0.0, x, y, z); + res->x = x; res->y = y; res->z = z; + }); + } + +private: + void setOrigin(double lat, double lon, double alt) + { + projection_.Reset(lat, lon, alt); + origin_set_ = true; + RCLCPP_INFO(get_logger(), "GPS origin set: lat=%.8f lon=%.8f alt=%.3f", lat, lon, alt); + } + + void onFix(const sensor_msgs::msg::NavSatFix::SharedPtr msg) + { + if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + std_msgs::msg::String s; + s.data = "NO_FIX"; + status_pub_->publish(s); + return; + } + + if (!origin_set_) { + bool use_start_gate_ref; + get_parameter("use_start_gate_ref", use_start_gate_ref); + if (!use_start_gate_ref) { + setOrigin(msg->latitude, msg->longitude, msg->altitude); + } else { + return; + } + } + + double x, y, z; + projection_.Forward(msg->latitude, msg->longitude, msg->altitude, x, y, z); + + // Publish fix quality + heading source + std::string quality; + switch (msg->status.status) { + case sensor_msgs::msg::NavSatStatus::STATUS_FIX: quality = "GPS"; break; + case sensor_msgs::msg::NavSatStatus::STATUS_SBAS_FIX: quality = "SBAS"; break; + case sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX: quality = "GBAS"; break; + default: quality = "FIX"; break; + } + std_msgs::msg::String s; + s.data = "FIX quality=" + quality + " heading=" + (has_heading_ ? "compass" : "none"); + status_pub_->publish(s); + + if (!has_heading_) { + return; + } + + geometry_msgs::msg::PoseStamped pose; + pose.header.stamp = msg->header.stamp; + pose.header.frame_id = "map"; + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = z; + + const double half = heading_enu_rad_ * 0.5; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = std::sin(half); + pose.pose.orientation.w = std::cos(half); + + pose_pub_->publish(pose); + + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = msg->header.stamp; + tf.header.frame_id = "map"; + tf.child_frame_id = "base_link"; + tf.transform.translation.x = x; + tf.transform.translation.y = y; + tf.transform.translation.z = z; + tf.transform.rotation = pose.pose.orientation; + tf_broadcaster_->sendTransform(tf); + } + + std::shared_ptr tf_broadcaster_; + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::Subscription::SharedPtr fix_sub_; + rclcpp::Subscription::SharedPtr heading_sub_; + rclcpp::Subscription::SharedPtr start_gate_sub_; + rclcpp::Service::SharedPtr latlon_srv_; + + GeographicLib::LocalCartesian projection_; + bool origin_set_{false}; + bool has_heading_{false}; + double heading_enu_rad_{0.0}; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt b/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt new file mode 100644 index 00000000..57406adb --- /dev/null +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.8) +project(minimal_nav_bringup) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml new file mode 100644 index 00000000..0227280c --- /dev/null +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml @@ -0,0 +1,92 @@ +# nav_params.yaml — single parameter file passed to all navigation nodes. +# See PLAN.md §7 for full documentation. + +# ── Global ────────────────────────────────────────────────────────────────── +use_sim_time: false + +# ── gps_pose_publisher ─────────────────────────────────────────────────────── +gps_pose_publisher: + ros__parameters: + use_sim_time: false + heading_topic: "/gps/heading" + origin_lat: .nan # auto-set from first fix when NaN + origin_lon: .nan + origin_alt: 0.0 + use_start_gate_ref: true # wait for /start_gate_ref NavSatFix to set origin + +# ── mission_executive ──────────────────────────────────────────────────────── +mission_executive: + ros__parameters: + use_sim_time: false + velocity_zero_threshold: 0.05 # m/s — applied to /odom twist (ZED SDK) + arrival_hold_time: 1.0 # seconds of continuous zero velocity + replan_distance_m: 3.0 # cross-track error threshold to trigger replan + latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" + +# ── global_planner (athena_smac_planner) ───────────────────────────────────── +global_planner: + ros__parameters: + use_sim_time: false + geotiff_path: "" + grid_size_m: 500.0 + grid_resolution_m: 1.0 + obstacle_threshold: 50 + min_turning_r: 1.2 # must match mppi_runner + allow_reverse: false + +global_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 1.0 + publish_frequency: 0.5 + transform_tolerance: 1.0 # generous — GPS-only localization has jitter + rolling_window: false + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_topic: /map + subscribe_to_updates: true + map_subscribe_transient_local: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 1.0 # >= robot_radius (0.45 m) + +# ── mppi_runner (ackermann_mppi) ───────────────────────────────────────────── +mppi_runner: + ros__parameters: + use_sim_time: false + controller_frequency: 20.0 + motion_model: "Ackermann" + min_turning_r: 1.2 + vx_max: 3.0 + vx_min: 0.0 + wz_max: 1.0 + time_steps: 56 + batch_size: 1000 + model_dt: 0.05 + critics: + - ConstraintCritic + - GoalCritic + - GoalAngleCritic + - PathFollowCritic + - PathAlignCritic + - PreferForwardCritic + # Local costmap — owned by mppi_runner as a child Costmap2DROS node + local_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + robot_radius: 0.45 + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 1.0 + rolling_window: true + width: 20 + height: 20 + resolution: 0.1 + plugins: [] # Phase 2a: add ObstacleLayer + InflationLayer diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py b/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py new file mode 100644 index 00000000..1edbd225 --- /dev/null +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +# Copyright (c) 2025 UMD Loop +# +# 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. + +"""nav.launch.py — Full Athena navigation stack (GPS-only, Nav2-free). + +Launch order / node summary +─────────────────────────── + athena_gps : Pixhawk GPS + heading bridge (from athena_gps/gps_launch.py) + gps_pose_publisher : WGS84→ENU, /robot_pose, map→base_link TF + map_node : DEM GeoTIFF → nav_msgs/OccupancyGrid /map + global_planner : Hybrid-A* planner, /goal_pose → /global_path + ackermann_mppi : MPPI local controller, /global_path → /cmd_vel_nav + mission_executive : State machine, action/service operator interface + cmd_vel_stamper : TwistStamped bridge → /rear_ackermann_controller/reference +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # ── Arguments ──────────────────────────────────────────────────────────── + sim_arg = DeclareLaunchArgument( + 'sim', + default_value='false', + choices=['true', 'false'], + description='Use simulation GPS bridge instead of real hardware', + ) + + sim = LaunchConfiguration('sim') + + # ── Config ─────────────────────────────────────────────────────────────── + nav_bringup_dir = get_package_share_directory('minimal_nav_bringup') + nav_params_file = os.path.join(nav_bringup_dir, 'config', 'nav_params.yaml') + + athena_map_dir = get_package_share_directory('dem_costmap') + dem_file = os.path.join(athena_map_dir, 'maps', 'north_site800m.tif') + + # ── GPS hardware / sim bridge ───────────────────────────────────────────── + gps_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory('athena_gps'), + 'launch', + 'gps_launch.py', + ) + ), + launch_arguments={'sim': sim}.items(), + ) + + # ── gps_pose_publisher ──────────────────────────────────────────────────── + gps_pose_publisher_node = Node( + package='gps_pose_publisher', + executable='gps_pose_publisher_node', + name='gps_pose_publisher', + output='screen', + parameters=[nav_params_file], + ) + + # ── map_node (DEM → OccupancyGrid) ──────────────────────────────────────── + map_node = Node( + package='dem_costmap', + executable='map_node', + name='map_node', + output='screen', + parameters=[ + nav_params_file, + {'dem_file_path': dem_file}, + ], + ) + + # ── global_planner (Hybrid-A* via athena_smac_planner) ─────────────────── + global_planner_node = Node( + package='athena_smac_planner', + executable='global_planner_node', + name='global_planner', + output='screen', + parameters=[nav_params_file], + ) + + # ── ackermann_mppi (MPPI local controller) ──────────────────────────────── + # /odom remapped to /odom/ground_truth (ZED SDK visual-inertial odometry) + ackermann_mppi_node = Node( + package='ackermann_mppi', + executable='ackermann_mppi_node', + name='mppi_runner', + output='screen', + parameters=[nav_params_file], + remappings=[ + ('/odom', '/odom/ground_truth'), + ], + ) + + # ── mission_executive (state machine) ──────────────────────────────────── + # /odom remapped to /odom/ground_truth for stop detection + mission_executive_node = Node( + package='mission_executive', + executable='mission_executive_node', + name='mission_executive', + output='screen', + parameters=[nav_params_file], + remappings=[ + ('/odom', '/odom/ground_truth'), + ], + ) + + # ── twist_stamper: /cmd_vel_nav → /rear_ackermann_controller/reference ──── + twist_stamper_node = Node( + package='twist_stamper', + executable='twist_stamper', + name='cmd_vel_stamper', + output='screen', + parameters=[{'use_sim_time': sim}], + remappings=[ + ('cmd_vel_in', '/cmd_vel_nav'), + ('cmd_vel_out', '/rear_ackermann_controller/reference'), + ], + ) + + return LaunchDescription([ + sim_arg, + gps_launch, + gps_pose_publisher_node, + map_node, + global_planner_node, + ackermann_mppi_node, + mission_executive_node, + twist_stamper_node, + ]) diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml b/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml new file mode 100644 index 00000000..71d3ccc1 --- /dev/null +++ b/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml @@ -0,0 +1,23 @@ + + + + minimal_nav_bringup + 0.1.0 + Launch and configuration files for the full Athena navigation stack + UMD Loop + Apache-2.0 + + ament_cmake + + gps_pose_publisher + dem_costmap + athena_smac_planner + ackermann_mppi + mission_executive + athena_gps + twist_stamper + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt b/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt new file mode 100644 index 00000000..d3f37c8b --- /dev/null +++ b/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(mission_executive) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(msgs REQUIRED) + +add_executable(mission_executive_node src/mission_executive_node.cpp) +target_compile_features(mission_executive_node PUBLIC cxx_std_17) +target_link_libraries(mission_executive_node + rclcpp::rclcpp + rclcpp_action::rclcpp_action + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_msgs_TARGETS} + ${std_srvs_TARGETS} + ${msgs_TARGETS} +) + +install(TARGETS mission_executive_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/subsystems/minimal_navigation/mission_executive/package.xml b/src/subsystems/minimal_navigation/mission_executive/package.xml new file mode 100644 index 00000000..49fb99be --- /dev/null +++ b/src/subsystems/minimal_navigation/mission_executive/package.xml @@ -0,0 +1,23 @@ + + + + mission_executive + 0.1.0 + State-machine node that manages target sequencing, abort/return, teleop mode, and arrival detection for the Athena navigation stack + UMD Loop + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + nav_msgs + std_msgs + std_srvs + msgs + + + ament_cmake + + diff --git a/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp b/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp new file mode 100644 index 00000000..97c8e998 --- /dev/null +++ b/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp @@ -0,0 +1,784 @@ +// Copyright (c) 2025 UMD Loop +// +// 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 +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/string.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include "msgs/action/navigate_to_target.hpp" +#include "msgs/msg/active_target.hpp" +#include "msgs/msg/nav_status.hpp" +#include "msgs/msg/planner_event.hpp" +#include "msgs/srv/lat_lon_to_enu.hpp" +#include "msgs/srv/set_target.hpp" + +using namespace std::chrono_literals; + +// ── State ────────────────────────────────────────────────────────────────── + +enum class State +{ + IDLE, + NAVIGATING, + ARRIVING, + STOPPED_AT_TARGET, + ABORTING, + RETURNING, + STOPPED_AT_RETURN, + TELEOP +}; + +static std::string stateToStr(State s) +{ + switch (s) { + case State::IDLE: return "IDLE"; + case State::NAVIGATING: return "NAVIGATING"; + case State::ARRIVING: return "ARRIVING"; + case State::STOPPED_AT_TARGET: return "STOPPED_AT_TARGET"; + case State::ABORTING: return "ABORTING"; + case State::RETURNING: return "RETURNING"; + case State::STOPPED_AT_RETURN: return "STOPPED_AT_RETURN"; + case State::TELEOP: return "TELEOP"; + default: return "UNKNOWN"; + } +} + +// ── Node ─────────────────────────────────────────────────────────────────── + +class MissionExecutive : public rclcpp::Node +{ +public: + using NavAction = msgs::action::NavigateToTarget; + using GoalHandle = rclcpp_action::ServerGoalHandle; + + // Target entry stored in the registry and as the active target + struct TargetEntry + { + std::string id; + double x_m{0.0}; + double y_m{0.0}; + uint8_t target_type{0}; + uint8_t goal_source{0}; // GPS=0, METER=1 + double tolerance_m{3.0}; + bool visited{false}; + geometry_msgs::msg::PoseStamped goal_enu; + }; + + explicit MissionExecutive(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("mission_executive", options) + { + declare_parameter("velocity_zero_threshold", 0.05); + declare_parameter("arrival_hold_time", 1.0); + declare_parameter("replan_distance_m", 3.0); + declare_parameter("latlon_to_enu_service", + std::string("/gps_pose_publisher/latlon_to_enu")); + // §8: launch file should remap to /odom/ground_truth when using ZED ground-truth odometry + declare_parameter("odom_topic", std::string("/odom")); + + velocity_zero_threshold_ = get_parameter("velocity_zero_threshold").as_double(); + arrival_hold_time_ = get_parameter("arrival_hold_time").as_double(); + replan_distance_m_ = get_parameter("replan_distance_m").as_double(); + const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); + const auto odom_topic = get_parameter("odom_topic").as_string(); + + // Reentrant group — allows the action server / service client to block + // inside handleAccepted() while other callbacks still run on other threads. + reentrant_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + // ── Publishers ────────────────────────────────────────────────────────── + goal_pub_ = create_publisher("/goal_pose", 10); + nav_enabled_pub_ = create_publisher( + "/nav_enabled", rclcpp::QoS(1).reliable()); + nav_mode_pub_ = create_publisher("/nav_mode", 10); + active_target_pub_ = create_publisher("/active_target", 10); + nav_status_pub_ = create_publisher( + "/nav_status", rclcpp::QoS(1).reliable()); + + // ── Service client ────────────────────────────────────────────────────── + latlon_client_ = create_client( + latlon_svc, + rmw_qos_profile_services_default, + reentrant_group_); + + // ── Action server ─────────────────────────────────────────────────────── + action_server_ = rclcpp_action::create_server( + this, + "~/navigate_to_target", + [this](const rclcpp_action::GoalUUID &, std::shared_ptr goal) { + return handleGoal(goal); + }, + [this](std::shared_ptr gh) { + return handleCancel(gh); + }, + [this](std::shared_ptr gh) { + handleAccepted(gh); + }, + rcl_action_server_get_default_options(), + reentrant_group_); + + // ── Services ──────────────────────────────────────────────────────────── + abort_srv_ = create_service( + "~/abort", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr res) + { + std::lock_guard lk(mutex_); + if (state_ == State::IDLE || state_ == State::TELEOP || + state_ == State::ABORTING || + state_ == State::STOPPED_AT_TARGET || state_ == State::STOPPED_AT_RETURN) + { + res->success = false; + res->message = "Nothing to abort in state " + stateToStr(state_); + return; + } + transition(State::ABORTING, "~/abort service called"); + res->success = true; + res->message = "Aborting"; + }); + + set_target_srv_ = create_service( + "~/set_target", + [this]( + const msgs::srv::SetTarget::Request::SharedPtr req, + msgs::srv::SetTarget::Response::SharedPtr res) + { + onSetTarget(req, res); + }, + rmw_qos_profile_services_default, + reentrant_group_); + + teleop_srv_ = create_service( + "~/teleop", + [this]( + const std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res) + { + std::lock_guard lk(mutex_); + if (req->data) { + // TELEOP_ON — valid from any state + transition(State::TELEOP, "teleop enabled"); + res->success = true; + res->message = "Teleop ON"; + } else { + // TELEOP_OFF — only from TELEOP + if (state_ == State::TELEOP) { + transition(State::IDLE, "teleop disabled"); + res->success = true; + res->message = "Teleop OFF"; + } else { + res->success = false; + res->message = "Not in TELEOP state"; + } + } + }); + + // ── Subscriptions ──────────────────────────────────────────────────────── + robot_pose_sub_ = create_subscription( + "/robot_pose", 10, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + std::lock_guard lk(mutex_); + robot_pose_ = *msg; + // Arrival check first: once we transition to ARRIVING, + // checkCrossTrackError's state guard prevents a spurious replan. + checkArrival(); + checkCrossTrackError(); + }); + + odom_sub_ = create_subscription( + odom_topic, 10, + [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lk(mutex_); + const auto & lin = msg->twist.twist.linear; + robot_speed_ = std::hypot(lin.x, lin.y); + checkStopDetection(); + }); + + planner_event_sub_ = create_subscription( + "/planner_event", 10, + [this](const msgs::msg::PlannerEvent::SharedPtr msg) { + std::lock_guard lk(mutex_); + last_planner_event_ = msg->event; + if (msg->event == msgs::msg::PlannerEvent::PLAN_FAILED) { + onPlanFailed(); + } + }); + + auto transient_qos = rclcpp::QoS(1).transient_local(); + global_path_sub_ = create_subscription( + "/global_path", transient_qos, + [this](const nav_msgs::msg::Path::SharedPtr msg) { + std::lock_guard lk(mutex_); + global_path_ = *msg; + }); + + // ── 2 Hz status timer ──────────────────────────────────────────────────── + status_timer_ = create_wall_timer( + 500ms, + [this]() { + std::lock_guard lk(mutex_); + publishStatus(); + checkActionResult(); + }); + + // Publish initial safe state + publishNavEnabled(false); + publishNavMode(); + + RCLCPP_INFO(get_logger(), "MissionExecutive ready — state: IDLE"); + } + +private: + // ── State transitions ───────────────────────────────────────────────────── + + // Must be called with mutex_ held. + void transition(State new_state, const std::string & reason) + { + RCLCPP_INFO(get_logger(), "[mission_executive] %s → %s: %s", + stateToStr(state_).c_str(), stateToStr(new_state).c_str(), reason.c_str()); + state_ = new_state; + + const bool nav_active = + state_ == State::NAVIGATING || + state_ == State::ARRIVING || + state_ == State::RETURNING; + publishNavEnabled(nav_active); + publishNavMode(); + publishStatus(); + + if (state_ == State::TELEOP || !nav_active) { + // Clear stop-detection tracking on any non-navigating transition + low_speed_tracking_ = false; + } + } + + // ── Internal publish helpers (no lock needed — caller holds mutex) ───────── + + void publishNavEnabled(bool enabled) + { + std_msgs::msg::Bool msg; + msg.data = enabled; + nav_enabled_pub_->publish(msg); + } + + void publishNavMode() + { + std_msgs::msg::String msg; + switch (state_) { + case State::TELEOP: + msg.data = "teleop"; break; + case State::NAVIGATING: + case State::ARRIVING: + case State::RETURNING: + msg.data = "autonomous"; break; + default: + msg.data = "stopped"; break; + } + nav_mode_pub_->publish(msg); + } + + void publishActiveTarget() + { + if (!active_target_.has_value()) return; + msgs::msg::ActiveTarget at; + at.target_id = active_target_->id; + at.target_type = active_target_->target_type; + at.tolerance_m = active_target_->tolerance_m; + at.goal_enu = active_target_->goal_enu; + at.goal_source = active_target_->goal_source; + at.status = stateToStr(state_); + active_target_pub_->publish(at); + } + + void publishStatus() + { + msgs::msg::NavStatus s; + s.state = stateToStr(state_); + if (active_target_.has_value()) { + s.active_target_id = active_target_->id; + s.active_target_type = active_target_->target_type; + s.goal_source = active_target_->goal_source; + } + s.distance_to_goal_m = distToGoal(); + s.cross_track_error_m = computeCrossTrackError(); + s.heading_error_rad = computeHeadingError(); + s.robot_speed_mps = robot_speed_; + s.is_return = is_return_; + s.last_planner_event = last_planner_event_; + nav_status_pub_->publish(s); + } + + // ── Geometry helpers ────────────────────────────────────────────────────── + + // Both return -1.0 if data is unavailable. Called with mutex_ held. + + static double quaternionToYaw(const geometry_msgs::msg::Quaternion & q) + { + const double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); + const double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); + return std::atan2(siny_cosp, cosy_cosp); + } + + double computeHeadingError() const + { + if (!robot_pose_.has_value() || !active_target_.has_value()) return 0.0; + const double yaw = quaternionToYaw(robot_pose_->pose.orientation); + const double dx = active_target_->goal_enu.pose.position.x - robot_pose_->pose.position.x; + const double dy = active_target_->goal_enu.pose.position.y - robot_pose_->pose.position.y; + const double bearing = std::atan2(dy, dx); + double err = bearing - yaw; + while (err > M_PI) err -= 2.0 * M_PI; + while (err < -M_PI) err += 2.0 * M_PI; + return err; + } + + double distToGoal() const + { + if (!robot_pose_.has_value() || !active_target_.has_value()) return -1.0; + const double dx = + robot_pose_->pose.position.x - active_target_->goal_enu.pose.position.x; + const double dy = + robot_pose_->pose.position.y - active_target_->goal_enu.pose.position.y; + return std::hypot(dx, dy); + } + + double computeCrossTrackError() const + { + if (!robot_pose_.has_value() || !global_path_.has_value()) return -1.0; + const auto & poses = global_path_->poses; + if (poses.size() < 2) return -1.0; + + const double rx = robot_pose_->pose.position.x; + const double ry = robot_pose_->pose.position.y; + double min_dist = std::numeric_limits::max(); + + for (size_t i = 0; i < poses.size() - 1; ++i) { + const double ax = poses[i].pose.position.x, ay = poses[i].pose.position.y; + const double bx = poses[i+1].pose.position.x, by = poses[i+1].pose.position.y; + const double dx = bx - ax, dy = by - ay; + const double len2 = dx*dx + dy*dy; + double dist; + if (len2 < 1e-10) { + dist = std::hypot(rx - ax, ry - ay); + } else { + const double t = std::clamp(((rx-ax)*dx + (ry-ay)*dy) / len2, 0.0, 1.0); + dist = std::hypot(rx - (ax + t*dx), ry - (ay + t*dy)); + } + min_dist = std::min(min_dist, dist); + } + return min_dist; + } + + // ── Subscription callbacks (mutex_ already held by caller) ──────────────── + + void checkCrossTrackError() + { + if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; + if (!active_target_.has_value()) return; + + const double xte = computeCrossTrackError(); + if (xte < 0.0 || xte <= replan_distance_m_) return; + + RCLCPP_INFO(get_logger(), + "[mission_executive] replan triggered: xtrack=%.2fm", xte); + goal_pub_->publish(active_target_->goal_enu); + } + + void checkArrival() + { + // Transition NAVIGATING/RETURNING → ARRIVING when within tolerance + if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; + if (!active_target_.has_value()) return; + + const double d = distToGoal(); + if (d < 0.0 || d >= active_target_->tolerance_m) return; + + transition(State::ARRIVING, + "within tolerance (d=" + std::to_string(d) + "m)"); + } + + void checkStopDetection() + { + // Transition ARRIVING → STOPPED_AT_TARGET / STOPPED_AT_RETURN + if (state_ != State::ARRIVING) return; + + if (robot_speed_ < velocity_zero_threshold_) { + if (!low_speed_tracking_) { + low_speed_start_ = now(); + low_speed_tracking_ = true; + } else { + const double held = (now() - low_speed_start_).seconds(); + if (held >= arrival_hold_time_) { + low_speed_tracking_ = false; + if (is_return_) { + transition(State::STOPPED_AT_RETURN, "velocity held < threshold"); + } else { + // Mark target as visited so RETURN is permitted later + if (active_target_.has_value()) { + auto it = target_registry_.find(active_target_->id); + if (it != target_registry_.end()) { + it->second.visited = true; + } + } + transition(State::STOPPED_AT_TARGET, "velocity held < threshold"); + } + } + } + } else { + low_speed_tracking_ = false; + } + } + + void onPlanFailed() + { + // Called with mutex_ held + if (state_ == State::NAVIGATING || state_ == State::RETURNING) { + RCLCPP_WARN(get_logger(), + "[mission_executive] PLAN_FAILED — transitioning to ABORTING"); + transition(State::ABORTING, "PLAN_FAILED received"); + } + } + + // ── Action result dispatch (called from the 2 Hz status timer) ──────────── + // Must be called with mutex_ held. + + void checkActionResult() + { + if (!active_goal_handle_) return; + + auto result = std::make_shared(); + + // Cancel requested externally — result already sent, go straight to IDLE. + // (ABORTING is only for the case where the result hasn't been sent yet.) + if (active_goal_handle_->is_canceling()) { + result->success = false; + result->message = "Cancelled"; + active_goal_handle_->canceled(result); + active_goal_handle_ = nullptr; + transition(State::IDLE, "action cancelled"); + return; + } + + switch (state_) { + case State::STOPPED_AT_TARGET: + case State::STOPPED_AT_RETURN: + result->success = true; + result->message = "Arrived at target"; + active_goal_handle_->succeed(result); + active_goal_handle_ = nullptr; + return; + + case State::ABORTING: + result->success = false; + result->message = "Aborted"; + active_goal_handle_->abort(result); + active_goal_handle_ = nullptr; + if (queued_goal_handle_) { + // Promote queued goal — go directly to NAVIGATING/RETURNING (§3.2.2) + active_target_ = queued_entry_; + is_return_ = queued_is_return_; + active_goal_handle_ = queued_goal_handle_; + queued_goal_handle_ = nullptr; + transition(is_return_ ? State::RETURNING : State::NAVIGATING, + "queued goal dequeued after abort"); + goal_pub_->publish(active_target_->goal_enu); + publishActiveTarget(); + } else { + transition(State::IDLE, "abort complete"); + } + return; + + default: + break; + } + + // Still executing — push feedback + auto fb = std::make_shared(); + fb->distance_to_goal_m = distToGoal(); + fb->cross_track_error_m = computeCrossTrackError(); + fb->state = stateToStr(state_); + active_goal_handle_->publish_feedback(fb); + } + + // ── Action server callbacks ─────────────────────────────────────────────── + + rclcpp_action::GoalResponse handleGoal( + std::shared_ptr /*goal*/) + { + // Accept all goals; teleop/state guards are applied in handleAccepted. + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handleCancel(std::shared_ptr /*gh*/) + { + return rclcpp_action::CancelResponse::ACCEPT; + } + + // Runs on a thread from the MultiThreadedExecutor (reentrant group). + // May block briefly for the latlon_to_enu service call. + void handleAccepted(std::shared_ptr goal_handle) + { + const auto goal = goal_handle->get_goal(); + + // ── Resolve ENU coordinates ──────────────────────────────────────────── + TargetEntry entry; + const bool is_ret = goal->is_return; + + if (!goal->target_id.empty()) { + // Look up pre-registered target + std::lock_guard lk(mutex_); + auto it = target_registry_.find(goal->target_id); + if (it == target_registry_.end()) { + auto res = std::make_shared(); + res->success = false; + res->message = "Unknown target_id: " + goal->target_id; + goal_handle->abort(res); + return; + } + if (is_ret && !it->second.visited) { + auto res = std::make_shared(); + res->success = false; + res->message = "Target not yet visited — cannot RETURN"; + goal_handle->abort(res); + return; + } + entry = it->second; + } else { + // Inline goal — no target_id + entry.id = ""; + entry.target_type = goal->target_type; + entry.tolerance_m = goal->tolerance_m; + entry.goal_source = goal->goal_type; + + if (goal->goal_type == NavAction::Goal::GPS) { + if (!latlon_client_->wait_for_service(3s)) { + auto res = std::make_shared(); + res->success = false; + res->message = "latlon_to_enu service not available"; + goal_handle->abort(res); + return; + } + auto req = std::make_shared(); + req->lat = goal->lat; + req->lon = goal->lon; + auto future = latlon_client_->async_send_request(req); + // Blocking wait is safe here: MultiThreadedExecutor + reentrant group + // means other callbacks continue on other threads. + if (future.wait_for(5s) != std::future_status::ready) { + auto res = std::make_shared(); + res->success = false; + res->message = "latlon_to_enu service timeout"; + goal_handle->abort(res); + return; + } + auto resp = future.get(); + entry.x_m = resp->x; + entry.y_m = resp->y; + } else { + entry.x_m = goal->x_m; + entry.y_m = goal->y_m; + } + + entry.goal_enu.header.frame_id = "map"; + entry.goal_enu.header.stamp = now(); + entry.goal_enu.pose.position.x = entry.x_m; + entry.goal_enu.pose.position.y = entry.y_m; + entry.goal_enu.pose.orientation.w = 1.0; + } + + // ── Apply state transition ───────────────────────────────────────────── + { + std::lock_guard lk(mutex_); + + if (state_ == State::TELEOP) { + auto res = std::make_shared(); + res->success = false; + res->message = "Cannot navigate — currently in TELEOP mode"; + goal_handle->abort(res); + return; + } + + // §3.2.2: During ABORTING, queue the goal rather than preempting the abort + if (state_ == State::ABORTING) { + // Preempt any previously queued goal + if (queued_goal_handle_ && queued_goal_handle_->is_active()) { + auto old_res = std::make_shared(); + old_res->success = false; + old_res->message = "Preempted by newer queued goal"; + queued_goal_handle_->abort(old_res); + } + queued_goal_handle_ = goal_handle; + queued_entry_ = entry; + queued_is_return_ = is_ret; + RCLCPP_INFO(get_logger(), + "[mission_executive] goal queued — currently ABORTING"); + return; + } + + // Preempt any existing active goal (guard: abort() throws if not active) + if (active_goal_handle_ && active_goal_handle_->is_active()) { + auto old_res = std::make_shared(); + old_res->success = false; + old_res->message = "Preempted by new goal"; + active_goal_handle_->abort(old_res); + active_goal_handle_ = nullptr; + } + + active_target_ = entry; + is_return_ = is_ret; + active_goal_handle_ = goal_handle; + + transition(is_ret ? State::RETURNING : State::NAVIGATING, + is_ret ? "RETURN action accepted" : "GO_TO action accepted"); + + // Trigger global planner + goal_pub_->publish(active_target_->goal_enu); + publishActiveTarget(); + } + } + + // ── SetTarget service ───────────────────────────────────────────────────── + + void onSetTarget( + const msgs::srv::SetTarget::Request::SharedPtr req, + msgs::srv::SetTarget::Response::SharedPtr res) + { + TargetEntry entry; + entry.id = req->target_id; + entry.target_type = req->target_type; + entry.tolerance_m = req->tolerance_m; + entry.goal_source = req->goal_type; + + if (req->goal_type == msgs::srv::SetTarget::Request::GPS) { + if (!latlon_client_->wait_for_service(3s)) { + res->success = false; + res->message = "latlon_to_enu service not available"; + return; + } + auto conv = std::make_shared(); + conv->lat = req->lat; + conv->lon = req->lon; + auto future = latlon_client_->async_send_request(conv); + if (future.wait_for(5s) != std::future_status::ready) { + res->success = false; + res->message = "latlon_to_enu service timeout"; + return; + } + auto resp = future.get(); + entry.x_m = resp->x; + entry.y_m = resp->y; + } else { + entry.x_m = req->x_m; + entry.y_m = req->y_m; + } + + entry.goal_enu.header.frame_id = "map"; + entry.goal_enu.header.stamp = now(); + entry.goal_enu.pose.position.x = entry.x_m; + entry.goal_enu.pose.position.y = entry.y_m; + entry.goal_enu.pose.orientation.w = 1.0; + + { + std::lock_guard lk(mutex_); + target_registry_[entry.id] = entry; + } + + res->success = true; + res->message = "Target '" + entry.id + "' registered"; + RCLCPP_INFO(get_logger(), + "Target '%s' registered: (%.2f, %.2f) tol=%.2fm", + entry.id.c_str(), entry.x_m, entry.y_m, entry.tolerance_m); + } + + // ── Member data ─────────────────────────────────────────────────────────── + + std::mutex mutex_; + State state_{State::IDLE}; + + std::unordered_map target_registry_; + std::optional active_target_; + bool is_return_{false}; + + // Goal queued while ABORTING (§3.2.2 — GO_TO/RETURN during ABORTING are "queued") + std::shared_ptr queued_goal_handle_; + TargetEntry queued_entry_; + bool queued_is_return_{false}; + + // Sensor cache + std::optional robot_pose_; + std::optional global_path_; + double robot_speed_{0.0}; + rclcpp::Time low_speed_start_{0, 0, RCL_ROS_TIME}; + bool low_speed_tracking_{false}; + uint8_t last_planner_event_{0}; + + // Parameters + double velocity_zero_threshold_{0.05}; + double arrival_hold_time_{1.0}; + double replan_distance_m_{3.0}; + + // ROS interfaces + rclcpp::CallbackGroup::SharedPtr reentrant_group_; + + rclcpp::Publisher::SharedPtr goal_pub_; + rclcpp::Publisher::SharedPtr nav_enabled_pub_; + rclcpp::Publisher::SharedPtr nav_mode_pub_; + rclcpp::Publisher::SharedPtr active_target_pub_; + rclcpp::Publisher::SharedPtr nav_status_pub_; + + rclcpp_action::Server::SharedPtr action_server_; + std::shared_ptr active_goal_handle_; + + rclcpp::Service::SharedPtr abort_srv_; + rclcpp::Service::SharedPtr set_target_srv_; + rclcpp::Service::SharedPtr teleop_srv_; + + rclcpp::Subscription::SharedPtr robot_pose_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr planner_event_sub_; + rclcpp::Subscription::SharedPtr global_path_sub_; + + rclcpp::Client::SharedPtr latlon_client_; + + rclcpp::TimerBase::SharedPtr status_timer_; +}; + +// ── main ────────────────────────────────────────────────────────────────── + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/minimal_navigation/todo.md b/src/subsystems/minimal_navigation/todo.md new file mode 100644 index 00000000..284b0975 --- /dev/null +++ b/src/subsystems/minimal_navigation/todo.md @@ -0,0 +1,69 @@ +# Navigation Stack — Implementation Progress + +## Legend +- [x] Done +- [ ] Not started + +--- + +## Plan 1 — `gps_pose_publisher` ✅ +- [x] New package: `gps_pose_publisher/` +- [x] WGS84 → ENU via `GeographicLib::LocalCartesian` +- [x] Publishes `/robot_pose` (PoseStamped, frame: map) +- [x] Broadcasts `map → base_link` TF +- [x] Publishes `/gps_status` (String) +- [x] Service `~/latlon_to_enu` (msgs/srv/LatLonToENU) +- [x] `use_start_gate_ref` param — waits for `/start_gate_ref` NavSatFix if true +- [x] Heading conversion: compass deg → ENU rad (`(90 - deg) * π/180`) +- [x] Only publishes once both GPS fix AND heading are available + +## Plan 2 — `ackermann_mppi` modifications ✅ +- [x] Input topic renamed: `/plan` → `/global_path` (transient_local QoS) +- [x] `nav_enabled_topic` param (default `/nav_enabled`) +- [x] Subscribe `std_msgs/Bool` on nav_enabled topic +- [x] `controlLoop()` short-circuits (zero cmd_vel, DEBUG log) when nav disabled +- [x] `std::atomic nav_enabled_` — thread-safe with MultiThreadedExecutor + +## Plan 3 — `athena_smac_planner` + `global_planner` ✅ +- [x] Package rename: `nav2_smac_planner` → `athena_smac_planner` (package.xml + CMakeLists.txt) +- [x] Library rename: `nav2_smac_planner` → `athena_smac_planner` +- [x] Removed nonexistent plugin XML exports from package.xml +- [x] `global_planner_node.cpp` — subscribes `/goal_pose`, `/robot_pose` +- [x] Owns `Costmap2DROS` under `"global_costmap"` namespace (StaticLayer from `/map`) +- [x] Uses `SmacPlannerHybrid::createPlan()` directly (no lifecycle) +- [x] Publishes `/global_path` (transient_local) and `/planner_event` +- [x] Emits `PLAN_FAILED` event on exception; `PLAN_SUCCEEDED` on success +- [x] WARN if `/goal_pose` arrives before `/robot_pose` +- [x] MultiThreadedExecutor with node + costmap node + +--- + +## Plan 4 — `mission_executive` ✅ +- [x] Package: `mission_executive/` (package.xml, CMakeLists.txt) +- [x] Action server `~/navigate_to_target` (NavigateToTarget) +- [x] Services: `~/abort` (Trigger), `~/set_target` (SetTarget), `~/teleop` (SetBool) +- [x] Publishes: `/goal_pose`, `/nav_enabled`, `/nav_mode`, `/active_target`, `/nav_status` +- [x] Subscribes: `/robot_pose`, `/odom`, `/planner_event`, `/global_path` +- [x] States: IDLE, NAVIGATING, ARRIVING, STOPPED_AT_TARGET, ABORTING, RETURNING, STOPPED_AT_RETURN, TELEOP +- [x] Full transition table from PLAN.md §3.2.2 +- [x] Calls `gps_pose_publisher/latlon_to_enu` for coordinate conversion +- [x] MultiThreadedExecutor + reentrant callback group (deadlock-safe) +- [x] Cross-track error replanning: replan when xtrack > `replan_distance_m` +- [x] Stop detection: `|twist.linear| < velocity_zero_threshold` held for `arrival_hold_time` seconds +- [x] Target registry via `~/set_target`; RETURN requires visited target +- [x] Preemption: new GO_TO aborts current active goal + +## Plan 5 — `nav.launch.py` + unified build ✅ +- [x] Package: `nav_bringup/` (package.xml, CMakeLists.txt) +- [x] `config/nav_params.yaml` — single param file for all nodes (from PLAN.md §7) +- [x] `launch/nav.launch.py` — includes `athena_gps/launch/gps_launch.py` +- [x] `/odom` → `/odom/ground_truth` remapped for `mission_executive` + `ackermann_mppi` +- [x] twist_stamper node: `cmd_vel_nav` → `/rear_ackermann_controller/reference` +- [x] Launches: `gps_pose_publisher`, `map_node`, `global_planner`, `ackermann_mppi`, `mission_executive` +- [x] Single `nav_params.yaml` passed to all nodes + +--- + +## Notes +- `msgs` package (at `src/msgs/`) already contains all required message/service/action definitions +- Phase 2 extensions (obstacle layers, ArUco, object detection) are deferred per PLAN.md §5 From 1c36814ea1e813e506872049b30de9e80ab2d213 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sat, 11 Apr 2026 16:17:27 -0400 Subject: [PATCH 16/21] remove old minimal Nav --- src/subsystems/minimal_navigation/PLAN.md | 508 ---- .../ackermann_mppi/CMakeLists.txt | 121 - .../include/ackermann_mppi/critic_data.hpp | 85 - .../ackermann_mppi/critic_function.hpp | 120 - .../critics/constraint_critic.hpp | 46 - .../ackermann_mppi/critics/goal_critic.hpp | 39 - .../critics/obstacles_critic.hpp | 61 - .../critics/path_align_critic.hpp | 43 - .../critics/path_follow_critic.hpp | 40 - .../ackermann_mppi/models/constraints.hpp | 47 - .../models/control_sequence.hpp | 50 - .../models/optimizer_settings.hpp | 47 - .../include/ackermann_mppi/models/path.hpp | 46 - .../include/ackermann_mppi/models/state.hpp | 55 - .../ackermann_mppi/models/trajectories.hpp | 46 - .../include/ackermann_mppi/motion_models.hpp | 133 - .../include/ackermann_mppi/optimizer.hpp | 238 -- .../ackermann_mppi/tools/noise_generator.hpp | 97 - .../include/ackermann_mppi/tools/utils.hpp | 387 --- .../ackermann_mppi/package.xml | 36 - .../src/ackermann_mppi_node.cpp | 301 -- .../src/critics/constraint_critic.cpp | 65 - .../src/critics/goal_critic.cpp | 55 - .../src/critics/obstacles_critic.cpp | 214 -- .../src/critics/path_align_critic.cpp | 141 - .../src/critics/path_follow_critic.cpp | 69 - .../ackermann_mppi/src/noise_generator.cpp | 103 - .../ackermann_mppi/src/optimizer.cpp | 486 ---- .../athena_smac_planner/CMakeLists.txt | 114 - .../include/athena_smac_planner/a_star.hpp | 323 --- .../analytic_expansion.hpp | 200 -- .../athena_smac_planner/collision_checker.hpp | 139 - .../include/athena_smac_planner/constants.hpp | 105 - .../costmap_downsampler.hpp | 99 - .../distance_heuristic.hpp | 77 - .../athena_smac_planner/goal_manager.hpp | 289 -- .../athena_smac_planner/node_basic.hpp | 69 - .../athena_smac_planner/node_hybrid.hpp | 386 --- .../obstacle_heuristic.hpp | 96 - .../smac_planner_hybrid.hpp | 111 - .../include/athena_smac_planner/smoother.hpp | 208 -- .../thirdparty/robin_hood.h | 2539 ----------------- .../include/athena_smac_planner/types.hpp | 257 -- .../include/athena_smac_planner/utils.hpp | 231 -- .../athena_smac_planner/package.xml | 31 - .../athena_smac_planner/src/a_star.cpp | 526 ---- .../src/analytic_expansion.cpp | 449 --- .../src/collision_checker.cpp | 196 -- .../src/costmap_downsampler.cpp | 127 - .../src/distance_heuristic.cpp | 157 - .../src/global_planner_node.cpp | 159 -- .../athena_smac_planner/src/node_basic.cpp | 54 - .../athena_smac_planner/src/node_hybrid.cpp | 540 ---- .../src/obstacle_heuristic.cpp | 230 -- .../src/smac_planner_hybrid.cpp | 478 ---- .../athena_smac_planner/src/smoother.cpp | 436 --- .../dem_costmap/CMakeLists.txt | 65 - .../minimal_navigation/dem_costmap/LICENSE | 202 -- .../dem_costmap/config/dem_costmap.yaml | 8 - .../dem_costmap/launch/dem_costmap.launch.py | 30 - .../dem_costmap/maps/north_site800m.tif | 3 - .../dem_costmap/package.xml | 22 - .../dem_costmap/src/map_node.cpp | 236 -- .../gps_pose_publisher/CMakeLists.txt | 32 - .../gps_pose_publisher/package.xml | 24 - .../src/gps_pose_publisher_node.cpp | 192 -- .../minimal_nav_bringup/CMakeLists.txt | 10 - .../config/nav_params.yaml | 92 - .../minimal_nav_bringup/launch/nav.launch.py | 147 - .../minimal_nav_bringup/package.xml | 23 - .../mission_executive/CMakeLists.txt | 33 - .../mission_executive/package.xml | 23 - .../src/mission_executive_node.cpp | 784 ----- src/subsystems/minimal_navigation/todo.md | 69 - 74 files changed, 14300 deletions(-) delete mode 100644 src/subsystems/minimal_navigation/PLAN.md delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/package.xml delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp delete mode 100644 src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/package.xml delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp delete mode 100644 src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp delete mode 100644 src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt delete mode 100644 src/subsystems/minimal_navigation/dem_costmap/LICENSE delete mode 100644 src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml delete mode 100644 src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py delete mode 100644 src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif delete mode 100644 src/subsystems/minimal_navigation/dem_costmap/package.xml delete mode 100644 src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp delete mode 100644 src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt delete mode 100644 src/subsystems/minimal_navigation/gps_pose_publisher/package.xml delete mode 100644 src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp delete mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt delete mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml delete mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py delete mode 100644 src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml delete mode 100644 src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt delete mode 100644 src/subsystems/minimal_navigation/mission_executive/package.xml delete mode 100644 src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp delete mode 100644 src/subsystems/minimal_navigation/todo.md diff --git a/src/subsystems/minimal_navigation/PLAN.md b/src/subsystems/minimal_navigation/PLAN.md deleted file mode 100644 index 09d8b107..00000000 --- a/src/subsystems/minimal_navigation/PLAN.md +++ /dev/null @@ -1,508 +0,0 @@ -# Athena Navigation Stack — Implementation Spec - -A GPS-only, Nav2-free navigation system in a single ROS 2 package. No IMU-GNSS fusion — localization is GPS + compass heading only. - - ---- - -## 1. Architecture Overview - -Four nodes form the runtime pipeline: - -``` -gps_pose_publisher → mission_executive → global_planner → mppi_runner - │ │ │ │ - map→base_link TF state machine Hybrid-A* MPPI control - /robot_pose /goal_pose /global_path /cmd_vel -``` - -**TF tree:** `map → base_link` (no `odom` frame — see §1.2). - -### 1.1 Existing Code - -| Source | Role | Required Changes | -|--------|------|------------------| -| `ackermann_mppi/` | Local planner / path follower | Rename `/plan` → `/global_path`; add `/nav_enabled` guard; add costmap debug publisher | -| `smac/` | Global planner (Hybrid-A\*) | Rename package to `athena_smac_planner`; integrate `athena_map` costmap input | -| `athena_map/` | Publishes `nav_msgs/OccupancyGrid` on `/map` | Ensure `frame_id: "map"`, `transient_local` QoS, and resolution matches `global_costmap` (0.5 m). We control this node — adapt as needed for clean integration. | -| `athena_planner/` | BT nodes (ArUco pose extraction, etc.) | Reuse in Phase 2 | -| `aruco_detection/` | ArUco refinement (~80% complete) | Wire in Phase 2c | - -**Do not use `localizer/`** — IMU-GNSS fusion is explicitly excluded from the localization path. - -### 1.2 TF Convention — Intentional `odom` Omission - -REP-105 expects `map → odom → base_link`. This stack uses `map → base_link` directly because there is no fused odometry source suitable for the `odom` frame. `gps_pose_publisher` broadcasts this transform. - -The ZED SDK provides `/odom` (`nav_msgs/Odometry`) with visual-inertial velocity estimates. This is used **only** for measured velocity in stop detection (§3.2.4) — it does not participate in localization or TF. - -### 1.3 Package Rename: `smac/` → `athena_smac_planner` - -The vendored `smac/` package declared `nav2_smac_planner` in `package.xml`, colliding with the system-installed `ros-$ROS_DISTRO-nav2-smac-planner`. Fix: rename to `athena_smac_planner` in both `package.xml` and `CMakeLists.txt`. - ---- - -## 2. Operator Interface - -All operator commands use ROS 2 services and actions (the legacy `OperatorCmd.msg` topic is retired). - -| Command | Interface | Type | Rationale | -|---------|-----------|------|-----------| -| `GO_TO` | Action | `NavigateToTarget` | Long-running; preemptable; provides distance feedback | -| `RETURN` | Action | `NavigateToTarget` | Same semantics as `GO_TO` (with `is_return=true`) | -| `ABORT` | Service | `std_srvs/Trigger` | Instant; needs confirmation | -| `SET_TARGET` | Service | `SetTarget` | Pre-register a target by ID for later navigation | -| `TELEOP_ON` / `TELEOP_OFF` | Service | `std_srvs/SetBool` | Needs ack; returns success + optional message | - -**Intended workflow:** Use `SetTarget` to pre-load named targets (with coordinates, type, and tolerance) into `mission_executive`'s target registry. Then use `NavigateToTarget` with just a `target_id` (+ `is_return` flag) to begin navigation. `NavigateToTarget` also accepts inline coordinates for ad-hoc goals that don't need pre-registration — when `target_id` is empty, the coordinates in the goal message are used directly. - -All custom types live in the `navigation_msgs` package (see §6). - ---- - -## 3. Node Specifications - -### 3.1 `gps_pose_publisher` - -Converts raw GPS + heading into a `map`-frame pose and broadcasts `map → base_link`. - -**Subscriptions:** - -| Topic | Type | Notes | -|-------|------|-------| -| `/gps/fix` | `sensor_msgs/NavSatFix` | | -| `/gps/heading` (configurable) | `std_msgs/Float64` | Input always in degrees. Internally converted to ENU radians (0 = East, CCW+). | - -**Publications:** - -| Topic | Type | Notes | -|-------|------|-------| -| `/robot_pose` | `geometry_msgs/PoseStamped` | Frame: `map` | -| `/gps_status` | `std_msgs/String` | Fix quality + heading source each update | -| TF: `map → base_link` | | | - -**Services:** - -| Service | Type | Purpose | -|---------|------|---------| -| `~/latlon_to_enu` | `LatLonToENU` | Centralizes WGS84 → ENU projection | - -**Core logic:** - -- WGS84 → ENU via `GeographicLib::LocalCartesian`: - ```cpp - GeographicLib::LocalCartesian proj(lat0, lon0, alt0); - proj.Forward(lat, lon, alt, e, n, u); - ``` -- Origin set from first fix, or from `/start_gate_ref` if `use_start_gate_ref: true`. -- Heading conversion: input degrees → ENU radians, then `tf2::Quaternion::setRPY(0, 0, heading_rad)`. - ---- - -### 3.2 `mission_executive` - -Operator-driven state machine managing target sequencing, abort/return, mode switching, and arrival detection. - -#### 3.2.1 States - -`IDLE`, `NAVIGATING`, `ARRIVING`, `STOPPED_AT_TARGET`, `ABORTING`, `RETURNING`, `STOPPED_AT_RETURN`, `TELEOP`. - -`TELEOP_ON` from any state → `TELEOP`. `TELEOP_OFF` → `IDLE` (never auto-resumes navigation). - -#### 3.2.2 Transition Table - -| State | GO_TO | ABORT | RETURN | `PLAN_FAILED` | TELEOP_ON | TELEOP_OFF | -|-------|-------|-------|--------|---------------|-----------|------------| -| `IDLE` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | -| `NAVIGATING` | → `NAVIGATING` (preempt) | → `ABORTING` | ignore | → `ABORTING` | → `TELEOP` | ignore | -| `ARRIVING` | → `NAVIGATING` (cancel) | → `ABORTING` | ignore | ignore | → `TELEOP` | ignore | -| `STOPPED_AT_TARGET` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | -| `ABORTING` | queued | ignore | queued | ignore | → `TELEOP` | ignore | -| `RETURNING` | → `NAVIGATING` (preempt) | → `ABORTING` | ignore | → `ABORTING` | → `TELEOP` | ignore | -| `STOPPED_AT_RETURN` | → `NAVIGATING` | ignore | → `RETURNING`\* | ignore | → `TELEOP` | ignore | -| `TELEOP` | ignore | ignore | ignore | ignore | ignore | → `IDLE` | - -\* `RETURN` requires the target to have been previously visited. Otherwise the action server returns failure immediately. - -#### 3.2.3 `PLAN_FAILED` Handling - -Subscribes to `/planner_event` (`PlannerEvent.msg`). On `event == PLAN_FAILED`: - -- In `NAVIGATING` or `RETURNING`: transition to `ABORTING`, publish `/nav_enabled = false`. -- In all other states: ignore (the executive is authoritative on state). - -#### 3.2.4 Stopped Detection - -Arrival confirmation uses **measured velocity from `/odom`** (published by the ZED SDK — not commanded `/cmd_vel`). The robot is considered stopped when `|twist.linear| < velocity_zero_threshold` is held for `arrival_hold_time` seconds. - -#### 3.2.5 Replanning - -The global planner does **not** self-trigger replans. `mission_executive` computes the robot's cross-track error (perpendicular distance from `/robot_pose` to the nearest segment of the current `/global_path`). When this exceeds `replan_distance_m`, the executive re-publishes `/goal_pose` to trigger a fresh plan. This keeps replanning fully observable via the `/goal_pose` topic stream. - -The executive caches the latest `/global_path` (subscribed with `transient_local` QoS) for this computation. - -#### 3.2.6 Executor Configuration - -`mission_executive` calls `gps_pose_publisher`'s `latlon_to_enu` service during `GO_TO` goal conversion. To avoid deadlock from blocking on `future.get()` inside a callback, **use a `MultiThreadedExecutor` with a reentrant callback group** for the service client. - -#### 3.2.7 Interfaces - -**Subscriptions:** - -| Topic | Type | Purpose | -|-------|------|---------| -| `/robot_pose` | `PoseStamped` | Current position | -| `/odom` | `nav_msgs/Odometry` | Measured velocity for stop detection (ZED SDK) | -| `/planner_event` | `PlannerEvent` | `PLAN_FAILED` triggers abort | -| `/global_path` | `nav_msgs/Path` | Cached for cross-track error (QoS: `transient_local`) | - -**Action servers:** - -| Name | Type | Handles | -|------|------|---------| -| `~/navigate_to_target` | `NavigateToTarget` | `GO_TO`, `RETURN` | - -**Services:** - -| Name | Type | Purpose | -|------|------|---------| -| `~/abort` | `std_srvs/Trigger` | Transition to `ABORTING` | -| `~/set_target` | `SetTarget` | Register/update a named target in the registry | - -**Publications:** - -| Topic | Type | Notes | -|-------|------|-------| -| `/goal_pose` | `PoseStamped` | Always ENU / `map` frame; consumed by global planner | -| `/nav_enabled` | `std_msgs/Bool` | `false` during teleop/stop; MPPI respects this | -| `/nav_mode` | `std_msgs/String` | `"autonomous"` / `"teleop"` / `"stopped"` | -| `/active_target` | `ActiveTarget` | Target metadata + status | -| `/nav_status` | `NavStatus` | 2 Hz + on every state change | - -#### 3.2.8 Target Types - -| Type | Tolerance | Notes | -|------|-----------|-------| -| `GNSS_ONLY` | 3.0 m | GPS-sourced goal | -| `ARUCO_POST` | 2.0 m | GNSS approach → ArUco refines (Phase 2c) | -| `OBJECT` | stop on detection | First two: GNSS < 3 m; third: < 10 m | -| `LOCAL` | user-defined | Meter-sourced goal; no GPS required | - ---- - -### 3.3 `global_planner` (Hybrid-A\* via `athena_smac_planner`) - -Wraps the vendored SMAC planner with `athena_map` costmap input. Plans **only** on receipt of a new `/goal_pose` — no self-triggered replanning. - -**Startup behavior:** On receiving a `/goal_pose` before a valid `/robot_pose` has been received, the planner logs a `WARN` and waits (does not publish `PLAN_FAILED`). Planning proceeds once both pose and map are available. This handles the GPS cold-start window gracefully. - -**Subscriptions:** - -| Topic | Type | Notes | -|-------|------|-------| -| `/goal_pose` | `PoseStamped` | Triggers planning | -| `/robot_pose` | `PoseStamped` | Planner start pose — must be received before planning | -| `/map` | `nav_msgs/OccupancyGrid` | Feeds `StaticLayer` (QoS: `transient_local`) | - -**Publications:** - -| Topic | Type | QoS | Notes | -|-------|------|-----|-------| -| `/global_path` | `nav_msgs/Path` | `transient_local` | Consumed by MPPI + mission_executive | -| `/planner_event` | `PlannerEvent` | | Enum-based event (see §6) | - ---- - -### 3.4 `mppi_runner` (existing `ackermann_mppi`) - -Minimal changes to the existing package. - -**Changes required:** - -1. Rename input topic `/plan` → `/global_path` -2. Subscribe to `/nav_enabled` — skip `evalControl()` when false -3. Add costmap debug publisher via `nav2_costmap_2d::CostmapPublisher` - -**Local costmap** is owned by `mppi_runner` as a child `Costmap2DROS` node, configured under the `local_costmap` namespace nested within `mppi_runner` parameters (see §7). - -**Subscriptions:** `/robot_pose`, `/odom`, `/global_path`, `/goal_pose`, `/nav_enabled` - -**Publications:** `/cmd_vel` (`TwistStamped`) - -**Control loop:** Timer at `controller_frequency` Hz (default 20). Calls `evalControl()` only when pose + path + goal are all available **and** `/nav_enabled` is true. - -**Initial critics:** -`ConstraintCritic`, `GoalCritic`, `GoalAngleCritic`, `PathFollowCritic`, `PathAlignCritic`, `PreferForwardCritic` - ---- - -## 4. Debugging & Observability - -### 4.1 `/nav_status` (published by `mission_executive`) - -Published at 2 Hz and on every state change. See §6 for full message definition. - -### 4.2 Logging Conventions - -| Event | Level | Format | -|-------|-------|--------| -| State transition | `INFO` | `[mission_executive] → : ` | -| Replan trigger | `INFO` | `[mission_executive] replan triggered: xtrack=X.Xm` | -| `evalControl()` skip | `DEBUG` | (fires at 20 Hz — never INFO) | -| GPS origin set | `INFO` | `[gps_pose_publisher] origin set: lat=X lon=Y alt=Z` | -| `PLAN_FAILED` received | `WARN` | `[mission_executive] PLAN_FAILED — transitioning to ABORTING` | -| Planner waiting for fix | `WARN` | `[global_planner] goal received but no robot_pose yet — waiting` | -| Service/action rejection | `WARN` | Reason logged; error returned to caller | - -### 4.3 Topic QoS - -| Topic | QoS | -|-------|-----| -| `/global_path` | `transient_local` (rviz2/Foxglove sees current path on connect) | -| `/map` | `transient_local` (from `athena_map`) | -| `/nav_status` | `keep_last(1)`, reliable | - ---- - -## 5. Extension Phases - -| Phase | Feature | Effort | -|-------|---------|--------| -| **2a** | Depth camera obstacle avoidance | Config only — add `ObstacleLayer` + `InflationLayer` to MPPI local costmap plugins, enable `CostCritic` + `ObstaclesCritic` | -| **2b** | GeoTIFF terrain obstacles | Config only — set `geotiff_path` on `global_planner` | -| **2c** | ArUco post refinement | Wire `correction_node` output to override `/goal_pose` when `ARUCO_POST` target is within ~10 m. Reuse `GetArucoPose` BT node. | -| **2d** | Object detection | `yolo_ros/` detects objects within tolerance → publish to C2 overlay → `mission_executive` transitions to `STOPPED_AT_TARGET` | -| **2e** | Spiral/lawnmower search | `SpiralCoverageAction` BT node (Archimedean spiral, $r = 15$ m, spacing $= 2$ m). Sub-mode when target reached via GNSS but not visually acquired. | - ---- - -## 6. Custom Message / Service / Action Definitions - -All defined in the `navigation_msgs` package. - -### Services - -``` -# LatLonToENU.srv -float64 lat -float64 lon ---- -float64 x -float64 y -float64 z -``` - -``` -# SetTarget.srv — pre-register a named target -string target_id -float64 lat # GPS goal (WGS84) — ignored if goal_type == METER -float64 lon -float64 x_m # Meter goal (ENU) — ignored if goal_type == GPS -float64 y_m -uint8 goal_type # GPS=0, METER=1 -uint8 GPS=0 -uint8 METER=1 -uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 -uint8 GNSS_ONLY=0 -uint8 ARUCO_POST=1 -uint8 OBJECT=2 -uint8 LOCAL=3 -float64 tolerance_m ---- -bool success -string message -``` - -### Actions - -``` -# NavigateToTarget.action -# --- Goal --- -# If target_id is non-empty, coordinates are looked up from the registry -# (previously loaded via SetTarget). If target_id is empty, inline -# coordinates are used as an ad-hoc goal. -string target_id # empty string = use inline coordinates -float64 lat # ad-hoc GPS goal (ignored if target_id set) -float64 lon -float64 x_m # ad-hoc meter goal (ignored if target_id set) -float64 y_m -uint8 goal_type # GPS=0, METER=1 -uint8 GPS=0 -uint8 METER=1 -uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 -uint8 GNSS_ONLY=0 -uint8 ARUCO_POST=1 -uint8 OBJECT=2 -uint8 LOCAL=3 -float64 tolerance_m # only used for ad-hoc goals; registry targets use stored tolerance -bool is_return ---- -# --- Result --- -bool success -string message ---- -# --- Feedback --- -float64 distance_to_goal_m -float64 cross_track_error_m -string state -``` - -### Messages - -``` -# PlannerEvent.msg -uint8 NEW_GOAL=0 -uint8 PLANNING=1 -uint8 PLAN_SUCCEEDED=2 -uint8 PLAN_FAILED=3 -uint8 event -``` - -``` -# ActiveTarget.msg -string target_id -uint8 target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 -float64 tolerance_m -geometry_msgs/PoseStamped goal_enu -uint8 goal_source # GPS=0, METER=1 -string status # NAVIGATING, SEARCHING, ARRIVED, ABORTED -``` - -``` -# NavStatus.msg -string state # IDLE, NAVIGATING, ARRIVING, STOPPED_AT_TARGET, - # ABORTING, RETURNING, STOPPED_AT_RETURN, TELEOP -string active_target_id -uint8 active_target_type # GNSS_ONLY=0, ARUCO_POST=1, OBJECT=2, LOCAL=3 -uint8 goal_source # GPS=0, METER=1 -float64 distance_to_goal_m # -1.0 if no active goal -float64 cross_track_error_m # -1.0 if no active path -float64 heading_error_rad -float64 robot_speed_mps # from /odom twist (ZED SDK) -bool is_return -uint8 last_planner_event # last PlannerEvent.event value -``` - ---- - -## 7. Parameter Reference (`nav_params.yaml`) - -```yaml -# ── Global ────────────────────────────────────────────── -use_sim_time: false # propagated to all nodes via launch - -# ── Robot geometry (referenced by costmaps + planner) ─── -robot_radius: &robot_radius 0.45 # meters — inscribed radius - -# ── gps_pose_publisher ───────────────────────────────── -gps_pose_publisher: - ros__parameters: - use_sim_time: false - heading_topic: "/gps/heading" - origin_lat: NaN # auto-set from first fix if NaN - origin_lon: NaN - origin_alt: 0.0 - use_start_gate_ref: true # if true, /start_gate_ref sets origin - -# ── mission_executive ────────────────────────────────── -mission_executive: - ros__parameters: - use_sim_time: false - default_targets: [] - velocity_zero_threshold: 0.05 # m/s — applied to /odom twist - arrival_hold_time: 1.0 # seconds of confirmed zero velocity - replan_distance_m: 3.0 # cross-track error threshold to trigger replan - latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" - -# ── global_planner (athena_smac_planner) ─────────────── -global_planner: - ros__parameters: - use_sim_time: false - geotiff_path: "" - grid_size_m: 500.0 - grid_resolution_m: 0.5 - obstacle_threshold: 50 - min_turning_r: 1.2 # must match mppi_runner - allow_reverse: false - -global_costmap: - ros__parameters: - use_sim_time: false - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 1.0 - publish_frequency: 0.5 - transform_tolerance: 1.0 # generous — GPS-only localization has jitter - rolling_window: false - plugins: ["static_layer", "inflation_layer"] - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_topic: /map - subscribe_to_updates: true - map_subscribe_transient_local: true - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 1.0 # >= robot_radius - -# ── mppi_runner (ackermann_mppi) ─────────────────────── -mppi_runner: - ros__parameters: - use_sim_time: false - controller_frequency: 20.0 - motion_model: "Ackermann" - min_turning_r: 1.2 - vx_max: 3.0 - vx_min: 0.0 - wz_max: 1.0 - time_steps: 56 - batch_size: 1000 - model_dt: 0.05 - critics: - - ConstraintCritic - - GoalCritic - - GoalAngleCritic - - PathFollowCritic - - PathAlignCritic - - PreferForwardCritic - # Local costmap — child Costmap2DROS node owned by mppi_runner - local_costmap: - ros__parameters: - use_sim_time: false - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 5.0 - publish_frequency: 2.0 - transform_tolerance: 1.0 - rolling_window: true - width: 20 - height: 20 - resolution: 0.1 - plugins: [] # Phase 2a: add ObstacleLayer + InflationLayer -``` - ---- - -## 8. Implementation Order - -1. **`gps_pose_publisher`** — full implementation; validates TF + ENU projection before anything else runs. -2. **`ackermann_mppi` modifications** — topic rename, `/nav_enabled` guard, costmap debug publisher. -3. **`global_planner` / `athena_smac_planner`** — package rename, `athena_map` integration, costmap wiring. -4. **`mission_executive`** — state machine, action/service servers, all operator commands. -5. **`nav.launch.py`** + `CMakeLists.txt` + `package.xml` — single-package build and launch. - -In the final launch file, you need to launch main/athena-code/src/subsystems/navigation/athena_gps/launch/gps_launch.py, and we will use the /odom/ground_truth topic for odom. you also need to map the output using a twist stamper like - - twist_stamper_node = Node( - package='twist_stamper', - executable='twist_stamper', - name='cmd_vel_stamper', - parameters=[{'use_sim_time': sim}], - remappings=[ - ('cmd_vel_in', '/cmd_vel_nav'), - ('cmd_vel_out', '/rear_ackermann_controller/reference'), - ], - ) - diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt b/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt deleted file mode 100644 index 8b73412f..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/CMakeLists.txt +++ /dev/null @@ -1,121 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ackermann_mppi) - -find_package(ament_cmake REQUIRED) -find_package(angles REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(builtin_interfaces REQUIRED) - -include_directories( - include - ${EIGEN3_INCLUDE_DIR} -) - -include(CheckCXXCompilerFlag) -check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA) -if(COMPILER_SUPPORTS_FMA) - add_compile_options(-mfma) -endif() - -# --- Core optimizer library (no ROS node, no pluginlib) --- -add_library(ackermann_mppi_core SHARED - src/noise_generator.cpp - src/optimizer.cpp - src/critics/constraint_critic.cpp - src/critics/goal_critic.cpp - src/critics/obstacles_critic.cpp - src/critics/path_align_critic.cpp - src/critics/path_follow_critic.cpp -) - -# Enable C++17 (required for inline constexpr, structured bindings, etc.) -target_compile_features(ackermann_mppi_core PUBLIC cxx_std_17) - -# Apply -O3 only for Release and RelWithDebInfo builds so Debug stays debuggable. -# Do not force -O3 unconditionally — it disrespects the developer's chosen build type. -target_compile_options(ackermann_mppi_core PUBLIC - $<$:-O3> - $<$:-O3> -) - -target_include_directories(ackermann_mppi_core - PUBLIC - "$" - "$") - -target_link_libraries(ackermann_mppi_core PUBLIC - angles::angles - ${geometry_msgs_TARGETS} - nav2_costmap_2d::layers - nav2_costmap_2d::nav2_costmap_2d_core - ${nav_msgs_TARGETS} - rclcpp::rclcpp - ${std_msgs_TARGETS} - tf2::tf2 - tf2_geometry_msgs::tf2_geometry_msgs - tf2_ros::tf2_ros - ${visualization_msgs_TARGETS} - ${builtin_interfaces_TARGETS} -) - -# --- ROS2 node executable --- -add_executable(ackermann_mppi_node - src/ackermann_mppi_node.cpp -) - -target_compile_options(ackermann_mppi_node PRIVATE - $<$:-O3> - $<$:-O3> -) - -target_include_directories(ackermann_mppi_node PRIVATE - "$") - -target_link_libraries(ackermann_mppi_node - ackermann_mppi_core - rclcpp::rclcpp - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - nav2_costmap_2d::nav2_costmap_2d_core - ${nav_msgs_TARGETS} - ${geometry_msgs_TARGETS} -) - -# --- Install --- -install(TARGETS ackermann_mppi_core ackermann_mppi_node - EXPORT ackermann_mppi - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -ament_export_targets(ackermann_mppi HAS_LIBRARY_TARGET) -ament_export_dependencies( - angles - geometry_msgs - nav2_costmap_2d - nav_msgs - rclcpp - std_msgs - tf2 - tf2_geometry_msgs - tf2_ros - visualization_msgs - Eigen3 -) -ament_export_include_directories(include/${PROJECT_NAME}) - -ament_package() diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp deleted file mode 100644 index 14e89c51..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_data.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITIC_DATA_HPP_ -#define ACKERMANN_MPPI__CRITIC_DATA_HPP_ - -#include - -#include -#include -#include - -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/models/trajectories.hpp" -#include "ackermann_mppi/models/path.hpp" -#include "ackermann_mppi/motion_models.hpp" - -namespace mppi -{ - -/** - * @struct mppi::CriticData - * @brief Data passed to critics for scoring. GoalChecker replaced with a simple - * distance tolerance float — the optimizer sets this from its parameters. - * - * Has an explicit constructor to prevent silent wrong-field binding when using - * aggregate initialization in C++17 (designated initializers require C++20). - */ -struct CriticData -{ - const models::State & state; - const models::Trajectories & trajectories; - const models::Path & path; - const geometry_msgs::msg::Pose & goal; - - Eigen::ArrayXf & costs; - float & model_dt; - - bool fail_flag{false}; - - // Replaces nav2_core::GoalChecker — set by Optimizer::prepare() from settings - float goal_dist_tolerance{0.25f}; - - std::shared_ptr motion_model; - std::optional> path_pts_valid; - std::optional furthest_reached_path_point; - std::vector trajectories_in_collision; - - // Explicit constructor so call sites use named parameters, preventing silent - // mis-binding if fields are reordered. - CriticData( - const models::State & state_in, - const models::Trajectories & trajectories_in, - const models::Path & path_in, - const geometry_msgs::msg::Pose & goal_in, - Eigen::ArrayXf & costs_in, - float & model_dt_in, - float goal_dist_tolerance_in = 0.25f, - std::shared_ptr motion_model_in = nullptr) - : state(state_in), - trajectories(trajectories_in), - path(path_in), - goal(goal_in), - costs(costs_in), - model_dt(model_dt_in), - fail_flag(false), - goal_dist_tolerance(goal_dist_tolerance_in), - motion_model(std::move(motion_model_in)) {} -}; - -} // namespace mppi - -#endif // ACKERMANN_MPPI__CRITIC_DATA_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp deleted file mode 100644 index 90cff7f6..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critic_function.hpp +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ -#define ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" - -#include "ackermann_mppi/critic_data.hpp" - -namespace mppi::critics -{ - -struct CollisionCost -{ - float cost{0}; - bool using_footprint{false}; -}; - -/** - * @class mppi::critics::CriticFunction - * @brief Abstract base for MPPI trajectory scoring critics. - * - * Compared to the original nav2 version: - * - Uses rclcpp::Node::SharedPtr instead of LifecycleNode::WeakPtr - * - No ParametersHandler dependency — uses a simple getParam() helper inline - * - Instantiated directly by the Optimizer (no pluginlib) - */ -class CriticFunction -{ -public: - CriticFunction() = default; - virtual ~CriticFunction() = default; - - /** - * @brief Configure critic on startup. - * @param node ROS2 node (used for parameter access and logging) - * @param parent_name Name of the parent optimizer (for reading shared params) - * @param name Name of this critic (used as param namespace) - * @param costmap_ros Costmap for collision/cost queries - */ - void on_configure( - rclcpp::Node::SharedPtr node, - const std::string & parent_name, - const std::string & name, - std::shared_ptr costmap_ros) - { - node_ = node; - logger_ = node_->get_logger(); - name_ = name; - parent_name_ = parent_name; - costmap_ros_ = costmap_ros; - costmap_ = costmap_ros_->getCostmap(); - - // Declare and read the common 'enabled' parameter - declare_param(name_ + ".enabled", true); - node_->get_parameter(name_ + ".enabled", enabled_); - - initialize(); - } - - virtual void score(CriticData & data) = 0; - virtual void initialize() = 0; - - std::string getName() { return name_; } - -protected: - /** - * @brief Declare a parameter with a default value if not already declared. - */ - template - void declare_param(const std::string & full_name, T default_value) - { - if (!node_->has_parameter(full_name)) { - node_->declare_parameter(full_name, default_value); - } - } - - /** - * @brief Get a parameter accessor scoped to a given namespace. - * - * Usage (same pattern as original ParametersHandler::getParamGetter): - * auto getParam = getParamGetter(name_); - * getParam(weight_, "cost_weight", 5.0f); - */ - auto getParamGetter(const std::string & ns) - { - return [this, ns](auto & setting, const std::string & name, auto default_value) { - std::string full_name = ns.empty() ? name : ns + "." + name; - declare_param(full_name, default_value); - node_->get_parameter(full_name, setting); - }; - } - - bool enabled_{true}; - std::string name_, parent_name_; - rclcpp::Node::SharedPtr node_; - std::shared_ptr costmap_ros_; - nav2_costmap_2d::Costmap2D * costmap_{nullptr}; - rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITIC_FUNCTION_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp deleted file mode 100644 index ba4901a3..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/constraint_critic.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ - -#include "ackermann_mppi/critic_function.hpp" - -namespace mppi::critics -{ - -/** - * @class ConstraintCritic (Ackermann-only) - * @brief Penalizes trajectories that violate velocity and turning radius constraints. - * - * For Ackermann: penalizes vx outside [vx_min, vx_max] and steering ratios - * (|vx|/|wz|) that would require a tighter turn than min_turning_r. - */ -class ConstraintCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - unsigned int power_{0}; - float weight_{0}; - // min_turning_r is vehicle geometry — fixed. Velocity limits are read from - // data.motion_model->getConstraints() in score() so speed-limit changes take effect. - float min_turning_r_{0.2f}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__CONSTRAINT_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp deleted file mode 100644 index f8d69ed1..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/goal_critic.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ - -#include "ackermann_mppi/critic_function.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -namespace mppi::critics -{ - -class GoalCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - unsigned int power_{0}; - float weight_{0}; - float threshold_to_consider_{0}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__GOAL_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp deleted file mode 100644 index 25a9ca14..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/obstacles_critic.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ - -#include -#include - -#include "nav2_costmap_2d/footprint_collision_checker.hpp" -#include "nav2_costmap_2d/inflation_layer.hpp" -#include "ackermann_mppi/critic_function.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -namespace mppi::critics -{ - -class ObstaclesCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - inline bool inCollision(float cost) const; - inline CollisionCost costAtPose(float x, float y, float theta); - inline float distanceToObstacle(const CollisionCost & cost); - float findCircumscribedCost(std::shared_ptr costmap); - -protected: - nav2_costmap_2d::FootprintCollisionChecker - collision_checker_{nullptr}; - - bool consider_footprint_{true}; - float collision_cost_{0}; - float inflation_scale_factor_{0}, inflation_radius_{0}; - float possible_collision_cost_; - float collision_margin_distance_; - float near_goal_distance_; - float circumscribed_cost_{0}, circumscribed_radius_{0}; - - unsigned int power_{0}; - float repulsion_weight_, critical_weight_{0}; - std::string inflation_layer_name_; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__OBSTACLES_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp deleted file mode 100644 index db17564e..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_align_critic.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) 2023 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. - -#ifndef ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ - -#include "ackermann_mppi/critic_function.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -namespace mppi::critics -{ - -class PathAlignCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - size_t offset_from_furthest_{0}; - int trajectory_point_step_{0}; - float threshold_to_consider_{0}; - float max_path_occupancy_ratio_{0}; - bool use_path_orientations_{false}; - unsigned int power_{0}; - float weight_{0}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__PATH_ALIGN_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp deleted file mode 100644 index ef5485b7..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/critics/path_follow_critic.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ -#define ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ - -#include "ackermann_mppi/critic_function.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -namespace mppi::critics -{ - -class PathFollowCritic : public CriticFunction -{ -public: - void initialize() override; - void score(CriticData & data) override; - -protected: - float threshold_to_consider_{0}; - size_t offset_from_furthest_{0}; - unsigned int power_{0}; - float weight_{0}; -}; - -} // namespace mppi::critics - -#endif // ACKERMANN_MPPI__CRITICS__PATH_FOLLOW_CRITIC_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp deleted file mode 100644 index 8f54c8d6..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/constraints.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ -#define ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ - -namespace mppi::models -{ - -/** - * @struct mppi::models::ControlConstraints - * @brief Constraints on control (Ackermann: no vy/ay terms) - */ -struct ControlConstraints -{ - float vx_max; - float vx_min; - float wz; - float ax_max; - float ax_min; - float az_max; -}; - -/** - * @struct mppi::models::SamplingStd - * @brief Noise standard deviations for MPPI sampling (Ackermann: vx and wz only) - */ -struct SamplingStd -{ - float vx; - float wz; -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__CONSTRAINTS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp deleted file mode 100644 index b4781787..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/control_sequence.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ -#define ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ - -#include - -namespace mppi::models -{ - -/** - * @struct mppi::models::Control - * @brief A single control step (Ackermann: vx and wz only) - */ -struct Control -{ - float vx, wz; -}; - -/** - * @struct mppi::models::ControlSequence - * @brief A control sequence over time (Ackermann: vx and wz only) - */ -struct ControlSequence -{ - Eigen::ArrayXf vx; - Eigen::ArrayXf wz; - - void reset(unsigned int time_steps) - { - vx.setZero(time_steps); - wz.setZero(time_steps); - } -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__CONTROL_SEQUENCE_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp deleted file mode 100644 index af0ad621..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/optimizer_settings.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ -#define ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ - -#include -#include "ackermann_mppi/models/constraints.hpp" - -namespace mppi::models -{ - -/** - * @struct mppi::models::OptimizerSettings - * @brief Settings for the optimizer to use - */ -struct OptimizerSettings -{ - models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; - models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; - models::SamplingStd sampling_std{0.0f, 0.0f}; - float model_dt{0.0f}; - float temperature{0.0f}; - float gamma{0.0f}; - unsigned int batch_size{0u}; - unsigned int time_steps{0u}; - unsigned int iteration_count{0u}; - bool shift_control_sequence{false}; - size_t retry_attempt_limit{0}; - bool open_loop{false}; - bool visualize{false}; -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__OPTIMIZER_SETTINGS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp deleted file mode 100644 index a9171d78..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/path.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__PATH_HPP_ -#define ACKERMANN_MPPI__MODELS__PATH_HPP_ - -#include - -namespace mppi::models -{ - -/** - * @struct mppi::models::Path - * @brief Path represented as a tensor - */ -struct Path -{ - Eigen::ArrayXf x; - Eigen::ArrayXf y; - Eigen::ArrayXf yaws; - - /** - * @brief Reset path data - */ - void reset(unsigned int size) - { - x.setZero(size); - y.setZero(size); - yaws.setZero(size); - } -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__PATH_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp deleted file mode 100644 index cb5018f4..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/state.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__STATE_HPP_ -#define ACKERMANN_MPPI__MODELS__STATE_HPP_ - -#include - -#include -#include - - -namespace mppi::models -{ - -/** - * @struct mppi::models::State - * @brief State information: velocities, controls, poses, speed - */ -struct State -{ - // Propagated velocities: [batch_size x time_steps] - Eigen::ArrayXXf vx; // longitudinal velocity - Eigen::ArrayXXf wz; // angular velocity (vy is always zero for Ackermann) - - // Noised controls: [batch_size x time_steps] - Eigen::ArrayXXf cvx; - Eigen::ArrayXXf cwz; - - geometry_msgs::msg::PoseStamped pose; - geometry_msgs::msg::Twist speed; - float local_path_length; - - void reset(unsigned int batch_size, unsigned int time_steps) - { - vx.setZero(batch_size, time_steps); - wz.setZero(batch_size, time_steps); - cvx.setZero(batch_size, time_steps); - cwz.setZero(batch_size, time_steps); - } -}; -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__STATE_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp deleted file mode 100644 index 8284872a..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/models/trajectories.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ -#define ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ - -#include - -namespace mppi::models -{ - -/** - * @class mppi::models::Trajectories - * @brief Candidate Trajectories - */ -struct Trajectories -{ - Eigen::ArrayXXf x; - Eigen::ArrayXXf y; - Eigen::ArrayXXf yaws; - - /** - * @brief Reset state data - */ - void reset(unsigned int batch_size, unsigned int time_steps) - { - x.setZero(batch_size, time_steps); - y.setZero(batch_size, time_steps); - yaws.setZero(batch_size, time_steps); - } -}; - -} // namespace mppi::models - -#endif // ACKERMANN_MPPI__MODELS__TRAJECTORIES_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp deleted file mode 100644 index 87487587..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/motion_models.hpp +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2025 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. - -#ifndef ACKERMANN_MPPI__MOTION_MODELS_HPP_ -#define ACKERMANN_MPPI__MOTION_MODELS_HPP_ - -#include -#include -#include - -#include "ackermann_mppi/models/control_sequence.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/models/constraints.hpp" - -namespace mppi -{ - -// Forward declaration used in utils.hpp to avoid circular include -namespace utils -{ -float clamp(const float lower_bound, const float upper_bound, const float input); -} - -/** - * @class mppi::MotionModel - * @brief Abstract motion model base class - */ -class MotionModel -{ -public: - MotionModel() = default; - virtual ~MotionModel() = default; - - void initialize(const models::ControlConstraints & control_constraints, float model_dt) - { - control_constraints_ = control_constraints; - model_dt_ = model_dt; - } - - /** - * @brief Propagate velocities forward one step using acceleration constraints. - * Batch operation: state matrices are [batch_size x time_steps]. - */ - virtual void predict(models::State & state) - { - float max_delta_vx = model_dt_ * control_constraints_.ax_max; - float min_delta_vx = model_dt_ * control_constraints_.ax_min; - float max_delta_wz = model_dt_ * control_constraints_.az_max; - - unsigned int n_cols = state.vx.cols(); - for (unsigned int i = 1; i < n_cols; i++) { - auto lower_bound_vx = (state.vx.col(i - 1) > 0).select( - state.vx.col(i - 1) + min_delta_vx, - state.vx.col(i - 1) - max_delta_vx); - auto upper_bound_vx = (state.vx.col(i - 1) > 0).select( - state.vx.col(i - 1) + max_delta_vx, - state.vx.col(i - 1) - min_delta_vx); - - state.cvx.col(i - 1) = state.cvx.col(i - 1) - .cwiseMax(lower_bound_vx) - .cwiseMin(upper_bound_vx); - state.vx.col(i) = state.cvx.col(i - 1); - - state.cwz.col(i - 1) = state.cwz.col(i - 1) - .cwiseMax(state.wz.col(i - 1) - max_delta_wz) - .cwiseMin(state.wz.col(i - 1) + max_delta_wz); - state.wz.col(i) = state.cwz.col(i - 1); - } - } - - virtual bool isHolonomic() = 0; - - /** - * @brief Apply model-specific hard constraints to the optimal control sequence. - * Called after softmax weighting, before returning the command. - */ - virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {} - - /** - * @brief Return the currently active constraints (may differ from base if speed limit is set). - * Critics should read from this instead of caching their own copies. - */ - const models::ControlConstraints & getConstraints() const { return control_constraints_; } - -protected: - float model_dt_{0.0}; - models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; -}; - -/** - * @class mppi::AckermannMotionModel - * @brief Ackermann steering motion model. - * - * Key constraint: angular velocity is bounded by |wz| <= |vx| / min_turning_r - * This prevents commanding steering angles that are physically impossible at a given speed. - */ -class AckermannMotionModel : public MotionModel -{ -public: - explicit AckermannMotionModel(float min_turning_r = 0.2f) - : min_turning_r_(min_turning_r) {} - - bool isHolonomic() override { return false; } - - void applyConstraints(models::ControlSequence & control_sequence) override - { - const auto wz_constrained = control_sequence.vx.abs() / min_turning_r_; - control_sequence.wz = control_sequence.wz - .max(-wz_constrained) - .min(wz_constrained); - } - - float getMinTurningRadius() const { return min_turning_r_; } - -private: - float min_turning_r_{0.2f}; -}; - -} // namespace mppi - -#endif // ACKERMANN_MPPI__MOTION_MODELS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp deleted file mode 100644 index bfcd2e41..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/optimizer.hpp +++ /dev/null @@ -1,238 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2025 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. - -#ifndef ACKERMANN_MPPI__OPTIMIZER_HPP_ -#define ACKERMANN_MPPI__OPTIMIZER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "tf2_ros/buffer.hpp" - -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/path.hpp" - -#include "ackermann_mppi/models/optimizer_settings.hpp" -#include "ackermann_mppi/motion_models.hpp" -#include "ackermann_mppi/critic_data.hpp" -#include "ackermann_mppi/models/state.hpp" -#include "ackermann_mppi/models/trajectories.hpp" -#include "ackermann_mppi/models/path.hpp" -#include "ackermann_mppi/tools/noise_generator.hpp" -#include "ackermann_mppi/tools/utils.hpp" - -// Forward declarations for critic types -namespace mppi::critics -{ -class CriticFunction; -class PathFollowCritic; -class PathAlignCritic; -class GoalCritic; -class ObstaclesCritic; -class ConstraintCritic; -} - -namespace mppi -{ - -/** - * @class mppi::Optimizer - * @brief Ackermann-specific MPPI optimizer. - * - * Differences from the nav2_mppi_controller Optimizer: - * - Initialized with rclcpp::Node::SharedPtr (not LifecycleNode) - * - Critics are created directly (no pluginlib) - * - No OptimalTrajectoryValidator — simple fallback on fail_flag - * - Motion model hardcoded to AckermannMotionModel - * - GoalChecker replaced with a distance threshold parameter - * - Parameters read directly via node->declare_parameter / node->get_parameter - */ -class Optimizer -{ -public: - // Both constructor and destructor defined in optimizer.cpp so critic forward declarations are sufficient here - Optimizer(); - ~Optimizer(); - - /** - * @brief Initialize the optimizer. - * @param node ROS2 node for parameters, logging, and clock - * @param name Parameter namespace prefix (e.g. "mppi") - * @param costmap_ros Costmap for obstacle avoidance critics - * @param tf_buffer TF buffer for robot pose lookups - */ - void initialize( - rclcpp::Node::SharedPtr node, - const std::string & name, - std::shared_ptr costmap_ros, - std::shared_ptr tf_buffer); - - void shutdown(); - - /** - * @brief Run MPPI to compute the next control command. - * @param robot_pose Current robot pose (in costmap frame) - * @param robot_speed Current robot velocity (from odometry) - * @param plan Global path to follow - * @param goal Goal pose (last pose of plan, or explicit goal) - * @return [TwistStamped command, optimal trajectory as [time_steps x 3] (x, y, yaw)] - * @throws std::runtime_error if all trajectories are in collision after retries - */ - std::tuple evalControl( - const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal); - - // --- Accessors for visualization / debugging --- - - models::Trajectories & getGeneratedTrajectories() { return generated_trajectories_; } - Eigen::ArrayXXf getOptimizedTrajectory(); - const models::ControlSequence & getOptimalControlSequence() { return control_sequence_; } - const Eigen::ArrayXf & getCosts() const { return costs_; } - - /** - * @brief Per-critic cost breakdown from last evalControl call. - * Each entry is (critic_name, cost_delta_array[batch_size]). - */ - const std::vector> & getCriticCosts() const - { - return critic_costs_; - } - - const std::vector & getCollisionFlags() const - { - return critics_data_.trajectories_in_collision; - } - - /** - * @brief Scale max speeds by a ratio (e.g. from a speed zone costmap filter). - * @param speed_limit Absolute speed OR percentage of max speed - * @param percentage True if speed_limit is a percentage [0–100] - */ - void setSpeedLimit(double speed_limit, bool percentage); - - void reset(bool reset_dynamic_speed_limits = true); - - bool isSpeedLimitActive() const; - - const models::OptimizerSettings & getSettings() const { return settings_; } - -protected: - void optimize(); - void evalTrajectoriesScores(); - - void prepare( - const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal); - - void getParams(); - void loadCritics(); - - void shiftControlSequence(); - void generateNoisedTrajectories(); - void applyControlSequenceConstraints(); - void updateStateVelocities(models::State & state) const; - void updateInitialStateVelocities(models::State & state) const; - void propagateStateVelocitiesFromInitials(models::State & state) const; - - void integrateStateVelocities( - models::Trajectories & trajectories, - const models::State & state) const; - - void integrateStateVelocities( - Eigen::Array & trajectory, - const Eigen::ArrayXXf & sequence) const; - - void updateControlSequence(); - - geometry_msgs::msg::TwistStamped - getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time & stamp); - - bool fallback(bool fail); - - size_t fallback_counter_{0}; // member — not static, so reset() clears it correctly - - template - void declareParam(const std::string & full_name, T default_value) - { - if (!node_->has_parameter(full_name)) { - node_->declare_parameter(full_name, default_value); - } - } - - auto getParamGetter(const std::string & ns) - { - return [this, ns](auto & setting, const std::string & name, auto default_value) { - std::string full_name = ns.empty() ? name : ns + "." + name; - declareParam(full_name, default_value); - node_->get_parameter(full_name, setting); - }; - } - -protected: - rclcpp::Node::SharedPtr node_; - std::shared_ptr costmap_ros_; - nav2_costmap_2d::Costmap2D * costmap_{nullptr}; - std::string name_; - std::string base_frame_; - std::shared_ptr tf_buffer_; - - std::shared_ptr motion_model_; - - // Critics owned directly (no pluginlib) - std::vector> critics_; - std::vector> critic_costs_; - - NoiseGenerator noise_generator_; - - models::OptimizerSettings settings_; - float goal_dist_tolerance_{0.25f}; - - models::State state_; - models::ControlSequence control_sequence_; - std::array control_history_; - models::Trajectories generated_trajectories_; - models::Path path_; - geometry_msgs::msg::Pose goal_; - Eigen::ArrayXf costs_; - - CriticData critics_data_{ - state_, - generated_trajectories_, - path_, - goal_, - costs_, - settings_.model_dt, - goal_dist_tolerance_}; - - rclcpp::Logger logger_{rclcpp::get_logger("AckermannMPPI")}; - geometry_msgs::msg::Twist last_command_vel_; -}; - -} // namespace mppi - -#endif // ACKERMANN_MPPI__OPTIMIZER_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp deleted file mode 100644 index c4b69720..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/noise_generator.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ -#define ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ - -#include - -#include -#include -#include -#include -#include - -#include "ackermann_mppi/models/optimizer_settings.hpp" -#include "ackermann_mppi/models/control_sequence.hpp" -#include "ackermann_mppi/models/state.hpp" - -namespace mppi -{ - -/** - * @class mppi::NoiseGenerator - * @brief Generates Gaussian-perturbed control sequences for MPPI sampling. - * - * When regenerate_noises=true (set via optimizer param), noise generation runs in a - * background thread that pre-generates noise for the *next* iteration while the - * current one scores trajectories. This hides latency on slower hardware. - * - * When regenerate_noises=false (default), noise is generated synchronously each call. - */ -class NoiseGenerator -{ -public: - NoiseGenerator() = default; - - /** - * @brief Initialize noise generator. - * @param settings Optimizer settings (batch_size, time_steps, sampling_std) - * @param regenerate_noises If true, run noise generation in background thread - */ - void initialize( - mppi::models::OptimizerSettings & settings, - bool regenerate_noises = false); - - void shutdown(); - - /** - * @brief Signal the background thread to generate next iteration's noises. - * No-op when regenerate_noises=false. - */ - void generateNextNoises(); - - /** - * @brief Apply noises to the current optimal control sequence to get noised controls. - * Stores in state.cvx / state.cwz for use by the motion model predict(). - */ - void setNoisedControls( - models::State & state, - const models::ControlSequence & control_sequence); - - void reset(mppi::models::OptimizerSettings & settings); - -protected: - void noiseThread(); - void generateNoisedControls(); - - Eigen::ArrayXXf noises_vx_; - Eigen::ArrayXXf noises_wz_; - - std::default_random_engine generator_; - std::normal_distribution ndistribution_vx_; - std::normal_distribution ndistribution_wz_; - - mppi::models::OptimizerSettings settings_; - bool regenerate_noises_{false}; - - std::thread noise_thread_; - std::condition_variable noise_cond_; - std::mutex noise_lock_; - bool active_{false}, ready_{false}; -}; - -} // namespace mppi - -#endif // ACKERMANN_MPPI__TOOLS__NOISE_GENERATOR_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp b/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp deleted file mode 100644 index f499ddea..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/include/ackermann_mppi/tools/utils.hpp +++ /dev/null @@ -1,387 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2023 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. - -#ifndef ACKERMANN_MPPI__TOOLS__UTILS_HPP_ -#define ACKERMANN_MPPI__TOOLS__UTILS_HPP_ - -#include - -#include -#include -#include -#include -#include - -#include "angles/angles.h" -#include "tf2/utils.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/path.hpp" -#include "visualization_msgs/msg/marker_array.hpp" -#include "std_msgs/msg/color_rgba.hpp" - -#include "rclcpp/rclcpp.hpp" - -#include "ackermann_mppi/models/optimizer_settings.hpp" -#include "ackermann_mppi/models/control_sequence.hpp" -#include "ackermann_mppi/models/path.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include "ackermann_mppi/critic_data.hpp" - -namespace mppi::utils -{ - -// Use constexpr instead of #define to avoid macro pollution and enable type checking. -inline constexpr float M_PIF = 3.141592653589793238462643383279502884e+00F; -inline constexpr float M_PIF_2 = 1.5707963267948966e+00F; - -inline geometry_msgs::msg::Pose createPose(double x, double y, double z) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = z; - pose.orientation.w = 1; - pose.orientation.x = 0; - pose.orientation.y = 0; - pose.orientation.z = 0; - return pose; -} - -inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z) -{ - geometry_msgs::msg::Vector3 scale; - scale.x = x; - scale.y = y; - scale.z = z; - return scale; -} - -inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a) -{ - std_msgs::msg::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - color.a = a; - return color; -} - -inline visualization_msgs::msg::Marker createMarker( - int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale, - const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns) -{ - using visualization_msgs::msg::Marker; - Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = rclcpp::Time(0, 0); - marker.ns = ns; - marker.id = id; - marker.type = Marker::SPHERE; - marker.action = Marker::ADD; - marker.pose = pose; - marker.scale = scale; - marker.color = color; - return marker; -} - -inline geometry_msgs::msg::TwistStamped toTwistStamped( - float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame) -{ - geometry_msgs::msg::TwistStamped twist; - twist.header.frame_id = frame; - twist.header.stamp = stamp; - twist.twist.linear.x = vx; - twist.twist.angular.z = wz; - return twist; -} - -/** - * @brief Convert path to a tensor of (x, y, yaw) arrays - */ -inline models::Path toTensor(const nav_msgs::msg::Path & path) -{ - auto result = models::Path{}; - result.reset(path.poses.size()); - for (size_t i = 0; i < path.poses.size(); ++i) { - result.x(i) = path.poses[i].pose.position.x; - result.y(i) = path.poses[i].pose.position.y; - result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation); - } - return result; -} - -/** - * @brief Get the last pose in the path tensor - */ -inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) -{ - const unsigned int path_last_idx = path.x.size() - 1; - tf2::Quaternion q; - q.setRPY(0.0, 0.0, path.yaws(path_last_idx)); - geometry_msgs::msg::Pose p; - p.position.x = path.x(path_last_idx); - p.position.y = path.y(path_last_idx); - p.orientation = tf2::toMsg(q); - return p; -} - -template -auto normalize_angles(const T & angles) -{ - return (angles + M_PIF).unaryExpr( - [&](const float x) { - float remainder = std::fmod(x, 2.0f * M_PIF); - return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF; - }); -} - -template -auto shortest_angular_distance(const F & from, const T & to) -{ - return normalize_angles(to - from); -} - -/** - * @brief Find the path point index furthest along the path that any trajectory in the batch - * reaches (by closest-point mapping). Used by PathFollow and PathAlign critics. - * - * Vectorized: outer loop iterates over path points (n_path), inner computation is a - * batch-wide Eigen array op over all trajectories simultaneously. This is significantly - * faster than the naive scalar O(n_batch × n_path) nested loop because each iteration - * of the outer loop operates on a [batch_size] SIMD-vectorizable array. - */ -inline size_t findPathFurthestReachedPoint(const CriticData & data) -{ - const int last_col = data.trajectories.x.cols() - 1; - // End-positions of all trajectories: [batch_size] arrays. - const auto traj_x = data.trajectories.x.col(last_col); - const auto traj_y = data.trajectories.y.col(last_col); - - const Eigen::Index n_batch = traj_x.rows(); - const Eigen::Index n_path = static_cast(data.path.x.size()); - - // Per-trajectory: closest path index found so far and its squared distance. - Eigen::ArrayXf min_dist2 = Eigen::ArrayXf::Constant(n_batch, std::numeric_limits::max()); - Eigen::ArrayXi best_idx = Eigen::ArrayXi::Zero(n_batch); - - for (Eigen::Index j = 0; j != n_path; ++j) { - // Vectorized over all batch_size trajectories at once. - const Eigen::ArrayXf dx = traj_x - data.path.x(j); - const Eigen::ArrayXf dy = traj_y - data.path.y(j); - const Eigen::ArrayXf d2 = dx * dx + dy * dy; - - const auto better = (d2 < min_dist2).eval(); - min_dist2 = better.select(d2, min_dist2); - best_idx = better.select(Eigen::ArrayXi::Constant(n_batch, static_cast(j)), best_idx); - - // Early exit: already found a trajectory that reaches the last path point. - if (best_idx.maxCoeff() == static_cast(n_path) - 1) { - break; - } - } - - return static_cast(best_idx.maxCoeff()); -} - -inline void setPathFurthestPointIfNotSet(CriticData & data) -{ - if (!data.furthest_reached_path_point) { - data.furthest_reached_path_point = findPathFurthestReachedPoint(data); - } -} - -/** - * @brief Check which path points are collision-free according to the costmap. - * Stores results in data.path_pts_valid for reuse across critics. - */ -inline void findPathCosts( - CriticData & data, - std::shared_ptr costmap_ros) -{ - auto * costmap = costmap_ros->getCostmap(); - unsigned int map_x, map_y; - const size_t path_segments_count = data.path.x.size() - 1; - data.path_pts_valid = std::vector(path_segments_count, false); - const bool tracking_unknown = costmap_ros->getLayeredCostmap()->isTrackingUnknown(); - - for (unsigned int idx = 0; idx < path_segments_count; idx++) { - if (!costmap->worldToMap(data.path.x(idx), data.path.y(idx), map_x, map_y)) { - (*data.path_pts_valid)[idx] = false; - continue; - } - switch (costmap->getCost(map_x, map_y)) { - case (nav2_costmap_2d::LETHAL_OBSTACLE): - (*data.path_pts_valid)[idx] = false; - continue; - case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): - (*data.path_pts_valid)[idx] = false; - continue; - case (nav2_costmap_2d::NO_INFORMATION): - (*data.path_pts_valid)[idx] = tracking_unknown ? true : false; - continue; - } - (*data.path_pts_valid)[idx] = true; - } -} - -inline void setPathCostsIfNotSet( - CriticData & data, - std::shared_ptr costmap_ros) -{ - if (!data.path_pts_valid) { - findPathCosts(data, costmap_ros); - } -} - -inline float posePointAngle( - const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) -{ - float pose_x = pose.position.x; - float pose_y = pose.position.y; - float pose_yaw = tf2::getYaw(pose.orientation); - float yaw = atan2f(point_y - pose_y, point_x - pose_x); - - if (!forward_preference) { - return std::min( - fabs(angles::shortest_angular_distance(yaw, pose_yaw)), - fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); - } - return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); -} - -/** - * @brief Apply Savitzky-Golay smoothing filter to the optimal control sequence. - * Uses a 9-point quadratic filter to smooth sharp changes between control steps. - */ -inline void savitskyGolayFilter( - models::ControlSequence & control_sequence, - std::array & control_history, - const models::OptimizerSettings & settings) -{ - // Standard 9-point quadratic Savitzky-Golay smoothing coefficients (window = ±4). - // Denominator 231 is the normalization factor for this window/polynomial order. - // Reference: Savitzky & Golay, Analytical Chemistry, 1964, Table I (m=4, n=2). - Eigen::Array filter = { - -21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, -21.0f}; - filter /= 231.0f; - - const unsigned int num_sequences = control_sequence.vx.size() - 1; - // Skip smoothing for very short horizons: the 9-point window needs at least - // 5 look-ahead points (indices 0..4), so sequences shorter than 20 provide - // too little context for meaningful smoothing. - if (num_sequences < 20) {return;} - - auto applyFilter = [&](const Eigen::Array & data) -> float { - return (data * filter).eval().sum(); - }; - - auto applyFilterOverAxis = - [&](Eigen::ArrayXf & sequence, const Eigen::ArrayXf & initial_sequence, - const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void - { - float pt_m4 = hist_0, pt_m3 = hist_1, pt_m2 = hist_2, pt_m1 = hist_3; - float pt = initial_sequence(0); - float pt_p1 = initial_sequence(1), pt_p2 = initial_sequence(2); - float pt_p3 = initial_sequence(3), pt_p4 = initial_sequence(4); - - for (unsigned int idx = 0; idx != num_sequences; idx++) { - sequence(idx) = applyFilter( - {pt_m4, pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3, pt_p4}); - pt_m4 = pt_m3; pt_m3 = pt_m2; pt_m2 = pt_m1; pt_m1 = pt; - pt = pt_p1; pt_p1 = pt_p2; pt_p2 = pt_p3; pt_p3 = pt_p4; - pt_p4 = (idx + 5 < num_sequences) ? - initial_sequence(idx + 5) : initial_sequence(num_sequences); - } - }; - - const models::ControlSequence initial_control_sequence = control_sequence; - applyFilterOverAxis( - control_sequence.vx, initial_control_sequence.vx, - control_history[0].vx, control_history[1].vx, - control_history[2].vx, control_history[3].vx); - applyFilterOverAxis( - control_sequence.wz, initial_control_sequence.wz, - control_history[0].wz, control_history[1].wz, - control_history[2].wz, control_history[3].wz); - - unsigned int offset = settings.shift_control_sequence ? 1 : 0; - control_history[0] = control_history[1]; - control_history[1] = control_history[2]; - control_history[2] = control_history[3]; - control_history[3] = {control_sequence.vx(offset), control_sequence.wz(offset)}; -} - -inline unsigned int findClosestPathPt( - const std::vector & vec, const float dist, const unsigned int init = 0u) -{ - float distim1 = init != 0u ? vec[init] : 0.0f; - float disti = 0.0f; - const unsigned int size = vec.size(); - for (unsigned int i = init + 1; i != size; i++) { - disti = vec[i]; - if (disti > dist) { - if (i > 0 && dist - distim1 < disti - dist) {return i - 1;} - return i; - } - distim1 = disti; - } - return size - 1; -} - -struct Pose2D { float x, y, theta; }; - -/** - * @brief Shift columns of an Eigen Array left (-1) or right (1) by one place. - * Used in trajectory integration and control sequence shifting. - */ -inline void shiftColumnsByOnePlace(Eigen::Ref e, int direction) -{ - int size = e.size(); - if (size == 1) {return;} - if (abs(direction) != 1) { - throw std::logic_error("Invalid direction, only 1 and -1 are valid values."); - } - if ((e.cols() == 1 || e.rows() == 1) && size > 1) { - auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1; - auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1; - while (start_ptr != end_ptr) { - *(start_ptr + direction) = *start_ptr; - start_ptr -= direction; - } - *(start_ptr + direction) = *start_ptr; - } else { - auto start_ptr = direction == 1 ? - e.data() + size - 2 * e.rows() : e.data() + e.rows(); - auto end_ptr = direction == 1 ? - e.data() : e.data() + size - e.rows(); - auto span = e.rows(); - while (start_ptr != end_ptr) { - std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); - start_ptr -= (direction * span); - } - std::copy(start_ptr, start_ptr + span, start_ptr + direction * span); - } -} - -inline float clamp(const float lower_bound, const float upper_bound, const float input) -{ - return std::min(upper_bound, std::max(input, lower_bound)); -} - -} // namespace mppi::utils - -#endif // ACKERMANN_MPPI__TOOLS__UTILS_HPP_ diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/package.xml b/src/subsystems/minimal_navigation/ackermann_mppi/package.xml deleted file mode 100644 index cf682381..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - ackermann_mppi - 0.1.0 - - Standalone MPPI controller for Ackermann steering robots. - A minimal extraction of nav2_mppi_controller with the nav2 plugin - scaffolding removed — no pluginlib, no LifecycleNode dependency, - no GoalChecker, no OptimalTrajectoryValidator. - Runs as a plain rclcpp::Node. - - UMD Loop - Apache-2.0 - - ament_cmake - - angles - builtin_interfaces - geometry_msgs - nav2_costmap_2d - nav_msgs - rclcpp - std_msgs - tf2 - tf2_geometry_msgs - tf2_ros - visualization_msgs - - eigen3_cmake_module - Eigen3 - - - ament_cmake - - diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp deleted file mode 100644 index 730e1081..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/ackermann_mppi_node.cpp +++ /dev/null @@ -1,301 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 - -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" - -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "std_msgs/msg/bool.hpp" - -#include "ackermann_mppi/optimizer.hpp" - -/** - * @class AckermannMPPINode - * @brief Standalone ROS2 node that runs MPPI for an Ackermann steering vehicle. - * - * Inputs: - * /global_path (nav_msgs/msg/Path) — global path to follow - * /odom (nav_msgs/msg/Odometry) — robot velocity feedback - * - * Outputs: - * /cmd_vel (geometry_msgs/msg/Twist) — velocity command - * - * The node also owns a Costmap2DROS instance (subscribes to sensor topics - * independently). Configure it via the standard nav2 costmap parameters. - * - * Key parameters (under the node's namespace): - * controller_frequency (double, default 10.0) — control loop rate in Hz - * plan_timeout (double, default 2.0) — seconds before stale plan triggers stop - * mppi.model_dt (float, default 0.05) — must equal 1/controller_frequency - * mppi.batch_size (int, default 1000) - * mppi.time_steps (int, default 56) - * mppi.vx_max (float, default 0.5) - * mppi.vx_min (float, default -0.35) - * mppi.wz_max (float, default 1.9) - * mppi.AckermannConstraints.min_turning_r (float, default 0.2) - * ... (see optimizer.cpp getParams() for full list) - */ -class AckermannMPPINode : public rclcpp::Node -{ -public: - explicit AckermannMPPINode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("ackermann_mppi", options) - { - // Declare parameters in constructor so they're available before configure() - declare_parameter("controller_frequency", 10.0); - declare_parameter("plan_timeout", 2.0); - declare_parameter("odom_topic", std::string("/odom")); - declare_parameter("plan_topic", std::string("/global_path")); - declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); - declare_parameter("cmd_vel_topic", std::string("/cmd_vel")); - declare_parameter("robot_base_frame", std::string("base_link")); - declare_parameter("global_frame", std::string("map")); - } - - /** - * @brief Finish setup after construction (requires shared_from_this()). - * Call this immediately after make_shared(). - */ - void configure() - { - RCLCPP_INFO(get_logger(), "AckermannMPPINode configuring..."); - - // Cache frame names — these never change at runtime. - get_parameter("global_frame", global_frame_); - get_parameter("robot_base_frame", base_frame_); - get_parameter("plan_timeout", plan_timeout_s_); - - // TF buffer and listener - tf_ = std::make_shared(get_clock()); - tf_listener_ = std::make_shared(*tf_); - - // Costmap2DROS manages its own ROS2 lifecycle (sensors, inflation layers, etc). - // It needs to be spun in the same executor as this node. - // Parameters are read from the "local_costmap" namespace (standard nav2 costmap params). - costmap_ros_ = std::make_shared( - "local_costmap", - get_namespace(), // parent namespace - "local_costmap" // local namespace - ); - // Transfer the TF buffer so the costmap shares our TF instance - costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", get_parameter("use_sim_time").as_bool())); - costmap_ros_->configure(); - costmap_ros_->activate(); - - optimizer_.initialize(shared_from_this(), "mppi", costmap_ros_, tf_); - - double controller_frequency; - get_parameter("controller_frequency", controller_frequency); - - // Subscriptions - std::string odom_topic, plan_topic, nav_enabled_topic; - get_parameter("odom_topic", odom_topic); - get_parameter("plan_topic", plan_topic); - get_parameter("nav_enabled_topic", nav_enabled_topic); - - // NOTE: both callbacks write shared state that is read by the control timer. - // The mutexes (plan_mutex_, odom_mutex_) below protect against the data race - // in the MultiThreadedExecutor where these run in parallel with controlLoop(). - odom_sub_ = create_subscription( - odom_topic, rclcpp::SensorDataQoS(), - [this](const nav_msgs::msg::Odometry::SharedPtr msg) { - std::lock_guard lock(odom_mutex_); - last_odom_ = *msg; - }); - - plan_sub_ = create_subscription( - plan_topic, rclcpp::QoS(1).transient_local(), - [this](const nav_msgs::msg::Path::SharedPtr msg) { - if (!msg->poses.empty()) { - std::lock_guard lock(plan_mutex_); - current_plan_ = *msg; - last_plan_time_ = now(); - } - }); - - nav_enabled_sub_ = create_subscription( - nav_enabled_topic, 1, - [this](const std_msgs::msg::Bool::SharedPtr msg) { - nav_enabled_.store(msg->data); - }); - - // Publisher - std::string cmd_vel_topic; - get_parameter("cmd_vel_topic", cmd_vel_topic); - cmd_vel_pub_ = create_publisher(cmd_vel_topic, 1); - - // Control loop timer - const auto period = std::chrono::duration(1.0 / controller_frequency); - control_timer_ = create_wall_timer(period, [this]() { controlLoop(); }); - - RCLCPP_INFO( - get_logger(), - "AckermannMPPINode configured. Control loop: %.1f Hz, plan timeout: %.1f s", - controller_frequency, plan_timeout_s_); - } - - std::shared_ptr getCostmapROS() - { - return costmap_ros_; - } - - void deactivate() - { - control_timer_->cancel(); - optimizer_.shutdown(); - costmap_ros_->deactivate(); - costmap_ros_->cleanup(); - } - -private: - void controlLoop() - { - if (!nav_enabled_.load()) { - RCLCPP_DEBUG(get_logger(), "Navigation disabled — sending zero velocity."); - cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); - return; - } - - // --- Snapshot shared state under locks --- - nav_msgs::msg::Path plan; - nav_msgs::msg::Odometry odom; - rclcpp::Time plan_time; - { - std::lock_guard lock(plan_mutex_); - if (current_plan_.poses.empty()) { - return; // No plan received yet - } - plan = current_plan_; - plan_time = last_plan_time_; - } - { - std::lock_guard lock(odom_mutex_); - odom = last_odom_; - } - - // --- Staleness check --- - const double plan_age = (now() - plan_time).seconds(); - if (plan_age > plan_timeout_s_) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 2000, - "Plan is stale (%.1f s old, timeout %.1f s) — sending zero velocity.", - plan_age, plan_timeout_s_); - cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); - optimizer_.reset(); - return; - } - - // --- Look up robot pose in the global frame --- - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header.frame_id = global_frame_; - robot_pose.header.stamp = now(); - - try { - auto transform = tf_->lookupTransform( - global_frame_, base_frame_, - tf2::TimePointZero); - robot_pose.pose.position.x = transform.transform.translation.x; - robot_pose.pose.position.y = transform.transform.translation.y; - robot_pose.pose.position.z = transform.transform.translation.z; - robot_pose.pose.orientation = transform.transform.rotation; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 1000, - "Could not get robot pose: %s", ex.what()); - return; - } - - // Robot speed from odometry - geometry_msgs::msg::Twist robot_speed; - robot_speed.linear.x = odom.twist.twist.linear.x; - robot_speed.linear.y = odom.twist.twist.linear.y; - robot_speed.angular.z = odom.twist.twist.angular.z; - - // Use the last pose of the plan as the goal - const auto & goal = plan.poses.back().pose; - - try { - auto [cmd, optimal_traj] = optimizer_.evalControl( - robot_pose, robot_speed, plan, goal); - cmd_vel_pub_->publish(cmd.twist); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR(get_logger(), "MPPI failed: %s — sending zero velocity.", ex.what()); - cmd_vel_pub_->publish(geometry_msgs::msg::Twist{}); - optimizer_.reset(); - } - } - - // TF - std::shared_ptr tf_; - std::shared_ptr tf_listener_; - - // Costmap (sensor integration + inflation) - std::shared_ptr costmap_ros_; - - // MPPI optimizer - mppi::Optimizer optimizer_; - - // ROS interfaces - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr plan_sub_; - rclcpp::Subscription::SharedPtr nav_enabled_sub_; - rclcpp::Publisher::SharedPtr cmd_vel_pub_; - rclcpp::TimerBase::SharedPtr control_timer_; - - std::atomic nav_enabled_{true}; - - // Shared state — protected by corresponding mutexes. - // Written by subscription callbacks, read by control timer; all run in a - // MultiThreadedExecutor so concurrent access is real. - std::mutex plan_mutex_; - std::mutex odom_mutex_; - nav_msgs::msg::Path current_plan_; - nav_msgs::msg::Odometry last_odom_; - rclcpp::Time last_plan_time_{0, 0, RCL_ROS_TIME}; - - // Cached parameters (never change after configure()) - std::string global_frame_{"map"}; - std::string base_frame_{"base_link"}; - double plan_timeout_s_{2.0}; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - node->configure(); - - // Use a multi-threaded executor so the Costmap2DROS lifecycle node - // (which has its own callbacks for sensor data) runs alongside the control loop. - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.add_node(node->getCostmapROS()->get_node_base_interface()); - executor.spin(); - - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp deleted file mode 100644 index 3b985feb..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/constraint_critic.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 "ackermann_mppi/critics/constraint_critic.hpp" - -namespace mppi::critics -{ - -void ConstraintCritic::initialize() -{ - auto getParam = getParamGetter(name_); - auto getParentParam = getParamGetter(parent_name_); - - getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 4.0f); - - // min_turning_r is vehicle geometry — fixed at init. - // vx_min/vx_max are read live from data.motion_model in score() so speed limits take effect. - getParentParam(min_turning_r_, "AckermannConstraints.min_turning_r", 0.2f); - - RCLCPP_INFO( - logger_, "ConstraintCritic (Ackermann) instantiated with %d power and %f weight.", - power_, weight_); -} - -void ConstraintCritic::score(CriticData & data) -{ - if (!enabled_) {return;} - - // Read velocity limits live so that setSpeedLimit() changes take effect immediately. - const auto & c = data.motion_model->getConstraints(); - const float max_vel = c.vx_max; - // Preserve original sign convention: vx_min is typically negative for reverse. - const float min_vel = (c.vx_min > 0.0f ? 1.0f : -1.0f) * std::abs(c.vx_min); - - auto & vx = data.state.vx; - auto & wz = data.state.wz; - - // Penalize steering ratio violations: |wz| <= |vx| / min_turning_r - constexpr float kEpsilon = 1e-6f; - auto wz_safe = wz.abs().max(kEpsilon); - auto out_of_turning_rad_motion = (min_turning_r_ - (vx.abs() / wz_safe)).max(0.0f); - - if (power_ > 1u) { - data.costs += ((((vx - max_vel).max(0.0f) + (min_vel - vx).max(0.0f) + - out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * - weight_).pow(power_).eval(); - } else { - data.costs += ((((vx - max_vel).max(0.0f) + (min_vel - vx).max(0.0f) + - out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_).eval(); - } -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp deleted file mode 100644 index 27c9baa7..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/goal_critic.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 "ackermann_mppi/critics/goal_critic.hpp" - -namespace mppi::critics -{ - -void GoalCritic::initialize() -{ - auto getParam = getParamGetter(name_); - getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 5.0f); - getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); - - RCLCPP_INFO( - logger_, "GoalCritic instantiated with %d power and %f weight.", - power_, weight_); -} - -void GoalCritic::score(CriticData & data) -{ - // Only activate when near the goal to provide terminal cost - if (!enabled_ || data.state.local_path_length > threshold_to_consider_) { - return; - } - - geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path); - const auto goal_x = goal.position.x; - const auto goal_y = goal.position.y; - - const auto delta_x = data.trajectories.x - goal_x; - const auto delta_y = data.trajectories.y - goal_y; - - if (power_ > 1u) { - data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * - weight_).pow(power_); - } else { - data.costs += (((delta_x.square() + delta_y.square()).sqrt()).rowwise().mean() * - weight_).eval(); - } -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp deleted file mode 100644 index 128204e2..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/obstacles_critic.cpp +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2023 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. - -#include -#include "ackermann_mppi/critics/obstacles_critic.hpp" - -namespace mppi::critics -{ - -void ObstaclesCritic::initialize() -{ - auto getParam = getParamGetter(name_); - getParam(consider_footprint_, "consider_footprint", false); - getParam(power_, "cost_power", 1); - getParam(repulsion_weight_, "repulsion_weight", 1.5f); - getParam(critical_weight_, "critical_weight", 20.0f); - getParam(collision_cost_, "collision_cost", 100000.0f); - getParam(collision_margin_distance_, "collision_margin_distance", 0.10f); - getParam(near_goal_distance_, "near_goal_distance", 0.5f); - getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); - - collision_checker_.setCostmap(costmap_); - possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - - if (possible_collision_cost_ < 1.0f) { - RCLCPP_ERROR( - logger_, - "Inflation layer either not found or inflation radius is not set sufficiently for " - "non-circular collision checking. Set inflation_radius >= half of the robot's largest " - "cross-section. This will substantially impact run-time performance."); - } - - RCLCPP_INFO( - logger_, - "ObstaclesCritic instantiated with %d power and %f / %f weights. " - "Collision check based on %s cost.", - power_, critical_weight_, repulsion_weight_, - consider_footprint_ ? "footprint" : "circular"); -} - -float ObstaclesCritic::findCircumscribedCost( - std::shared_ptr costmap) -{ - double result = -1.0; - const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); - if (static_cast(circum_radius) == circumscribed_radius_) { - return circumscribed_cost_; - } - - std::shared_ptr inflation_layer; - for (auto & layer : *costmap->getLayeredCostmap()->getPlugins()) { - auto candidate = std::dynamic_pointer_cast(layer); - if (candidate && - (inflation_layer_name_.empty() || candidate->getName() == inflation_layer_name_)) - { - inflation_layer = candidate; - break; - } - } - if (inflation_layer != nullptr) { - const double resolution = costmap->getCostmap()->getResolution(); - result = inflation_layer->computeCost(circum_radius / resolution); - const std::string & ln = inflation_layer->getName(); - rclcpp::Parameter p; - if (costmap->get_parameter(ln + ".cost_scaling_factor", p)) { - inflation_scale_factor_ = static_cast(p.as_double()); - } - if (costmap->get_parameter(ln + ".inflation_radius", p)) { - inflation_radius_ = static_cast(p.as_double()); - } - } else { - RCLCPP_WARN( - logger_, - "No inflation layer found. Cannot use costmap potential field for fast collision " - "pre-screening. Only absolute collisions will be detected."); - } - - circumscribed_radius_ = static_cast(circum_radius); - circumscribed_cost_ = static_cast(result); - return circumscribed_cost_; -} - -float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) -{ - const float scale_factor = inflation_scale_factor_; - const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); - // nav2 inflation layer maps INSCRIBED_INFLATED_OBSTACLE (254) - 1 = 253 to the inscribed - // radius. We invert the exponential cost formula to recover metric distance. - constexpr float kNavInscribedCostThreshold = 253.0f; - float dist_to_obj = - (scale_factor * min_radius - logf(cost.cost) + logf(kNavInscribedCostThreshold)) / - scale_factor; - if (!cost.using_footprint) { - dist_to_obj -= min_radius; - } - return dist_to_obj; -} - -void ObstaclesCritic::score(CriticData & data) -{ - if (!enabled_) {return;} - - if (consider_footprint_) { - possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - } - - bool near_goal = data.state.local_path_length < near_goal_distance_; - - Eigen::ArrayXf raw_cost = Eigen::ArrayXf::Zero(data.costs.size()); - Eigen::ArrayXf repulsive_cost = Eigen::ArrayXf::Zero(data.costs.size()); - - const unsigned int traj_len = data.trajectories.x.cols(); - const unsigned int batch_size = data.trajectories.x.rows(); - bool all_trajectories_collide = true; - - auto & collisions = data.trajectories_in_collision; - const bool track_collisions = !collisions.empty(); - - for (unsigned int i = 0; i != batch_size; i++) { - bool trajectory_collide = false; - float traj_cost = 0.0f; - const auto & traj = data.trajectories; - CollisionCost pose_cost; - - for (unsigned int j = 0; j != traj_len; j++) { - pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); - if (pose_cost.cost < 1.0f) {continue;} - - if (inCollision(pose_cost.cost)) { - trajectory_collide = true; - break; - } - - if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {continue;} - - const float dist_to_obj = distanceToObstacle(pose_cost); - if (dist_to_obj < collision_margin_distance_) { - traj_cost += (collision_margin_distance_ - dist_to_obj); - } - if (!near_goal) { - repulsive_cost[i] += inflation_radius_ - dist_to_obj; - } - } - - if (!trajectory_collide) {all_trajectories_collide = false;} - raw_cost(i) = trajectory_collide ? collision_cost_ : traj_cost; - if (trajectory_collide && track_collisions) {collisions[i] = true;} - } - - auto repulsive_cost_normalized = (repulsive_cost - repulsive_cost.minCoeff()) / traj_len; - - if (power_ > 1u) { - data.costs += - ((critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized)) - .pow(power_); - } else { - data.costs += - (critical_weight_ * raw_cost) + (repulsion_weight_ * repulsive_cost_normalized); - } - - data.fail_flag = all_trajectories_collide; -} - -bool ObstaclesCritic::inCollision(float cost) const -{ - bool is_tracking_unknown = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); - using namespace nav2_costmap_2d; // NOLINT - switch (static_cast(cost)) { - case (LETHAL_OBSTACLE): - return true; - case (INSCRIBED_INFLATED_OBSTACLE): - return consider_footprint_ ? false : true; - case (NO_INFORMATION): - return is_tracking_unknown ? false : true; - } - return false; -} - -CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) -{ - CollisionCost collision_cost; - float & cost = collision_cost.cost; - collision_cost.using_footprint = false; - - unsigned int x_i, y_i; - if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { - cost = nav2_costmap_2d::NO_INFORMATION; - return collision_cost; - } - cost = collision_checker_.pointCost(x_i, y_i); - - if (consider_footprint_ && - (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) - { - cost = static_cast(collision_checker_.footprintCostAtPose( - x, y, theta, costmap_ros_->getRobotFootprint())); - collision_cost.using_footprint = true; - } - return collision_cost; -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp deleted file mode 100644 index be4813a5..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_align_critic.cpp +++ /dev/null @@ -1,141 +0,0 @@ -// Copyright (c) 2023 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. - -#include "ackermann_mppi/critics/path_align_critic.hpp" - -namespace mppi::critics -{ - -void PathAlignCritic::initialize() -{ - auto getParam = getParamGetter(name_); - getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 10.0f); - getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); - getParam(offset_from_furthest_, "offset_from_furthest", 20); - getParam(trajectory_point_step_, "trajectory_point_step", 4); - getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); - getParam(use_path_orientations_, "use_path_orientations", false); - - RCLCPP_INFO( - logger_, "PathAlignCritic instantiated with %d power and %f weight", - power_, weight_); -} - -void PathAlignCritic::score(CriticData & data) -{ - if (!enabled_ || data.state.local_path_length < threshold_to_consider_) {return;} - - utils::setPathFurthestPointIfNotSet(data); - const size_t path_segments_count = *data.furthest_reached_path_point; - float path_segments_flt = static_cast(path_segments_count); - if (path_segments_count < offset_from_furthest_) {return;} - - utils::setPathCostsIfNotSet(data, costmap_ros_); - std::vector & path_pts_valid = *data.path_pts_valid; - float invalid_ctr = 0.0f; - for (size_t i = 0; i < path_segments_count; i++) { - if (!path_pts_valid[i]) {invalid_ctr += 1.0f;} - if (invalid_ctr / path_segments_flt > max_path_occupancy_ratio_ && invalid_ctr > 2.0f) { - return; - } - } - - const size_t batch_size = data.trajectories.x.rows(); - Eigen::ArrayXf cost(data.costs.rows()); - cost.setZero(); - - std::vector path_integrated_distances(path_segments_count, 0.0f); - std::vector path(path_segments_count); - float dx = 0.0f, dy = 0.0f; - for (unsigned int i = 1; i != path_segments_count; i++) { - auto & pose = path[i - 1]; - pose.x = data.path.x(i - 1); - pose.y = data.path.y(i - 1); - pose.theta = data.path.yaws(i - 1); - dx = data.path.x(i) - pose.x; - dy = data.path.y(i) - pose.y; - path_integrated_distances[i] = path_integrated_distances[i - 1] + sqrtf(dx * dx + dy * dy); - } - - auto & final_pose = path[path_segments_count - 1]; - final_pose.x = data.path.x(path_segments_count - 1); - final_pose.y = data.path.y(path_segments_count - 1); - final_pose.theta = data.path.yaws(path_segments_count - 1); - - int strided_traj_rows = data.trajectories.x.rows(); - int strided_traj_cols = - static_cast(floor((data.trajectories.x.cols() - 1) / trajectory_point_step_)) + 1; - int outer_stride = strided_traj_rows * trajectory_point_step_; - - const auto T_x = Eigen::Map>( - data.trajectories.x.data(), - strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto T_y = Eigen::Map>( - data.trajectories.y.data(), - strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto T_yaw = Eigen::Map>( - data.trajectories.yaws.data(), - strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1)); - const auto traj_sampled_size = T_x.cols(); - - float summed_path_dist = 0.0f, dyaw = 0.0f; - unsigned int num_samples = 0u, path_pt = 0u; - float traj_integrated_distance = 0.0f; - float Tx_m1, Ty_m1; - - for (size_t t = 0; t < batch_size; ++t) { - summed_path_dist = 0.0f; - num_samples = 0u; - traj_integrated_distance = 0.0f; - path_pt = 0u; - Tx_m1 = T_x(t, 0); - Ty_m1 = T_y(t, 0); - - for (int p = 1; p < traj_sampled_size; p++) { - const float Tx = T_x(t, p); - const float Ty = T_y(t, p); - dx = Tx - Tx_m1; - dy = Ty - Ty_m1; - Tx_m1 = Tx; - Ty_m1 = Ty; - traj_integrated_distance += sqrtf(dx * dx + dy * dy); - path_pt = utils::findClosestPathPt(path_integrated_distances, traj_integrated_distance, - path_pt); - - if (path_pts_valid[path_pt]) { - const auto & pose = path[path_pt]; - dx = pose.x - Tx; - dy = pose.y - Ty; - num_samples++; - if (use_path_orientations_) { - dyaw = angles::shortest_angular_distance(pose.theta, T_yaw(t, p)); - summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); - } else { - summed_path_dist += sqrtf(dx * dx + dy * dy); - } - } - } - cost(t) = num_samples > 0u ? - summed_path_dist / static_cast(num_samples) : 0.0f; - } - - if (power_ > 1u) { - data.costs += (cost * weight_).pow(power_).eval(); - } else { - data.costs += (cost * weight_).eval(); - } -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp deleted file mode 100644 index d6e1207e..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/critics/path_follow_critic.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 "ackermann_mppi/critics/path_follow_critic.hpp" -#include - -namespace mppi::critics -{ - -void PathFollowCritic::initialize() -{ - auto getParam = getParamGetter(name_); - getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); - getParam(offset_from_furthest_, "offset_from_furthest", 6); - getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 5.0f); -} - -void PathFollowCritic::score(CriticData & data) -{ - if (!enabled_) {return;} - - if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) { - return; - } - - utils::setPathFurthestPointIfNotSet(data); - utils::setPathCostsIfNotSet(data, costmap_ros_); - const size_t path_size = data.path.x.size() - 1; - - auto offsetted_idx = std::min( - *data.furthest_reached_path_point + offset_from_furthest_, path_size); - - // Drive to first valid (non-obstructed) path point at or after the lookahead - bool valid = false; - while (!valid && offsetted_idx < path_size - 1) { - valid = (*data.path_pts_valid)[offsetted_idx]; - if (!valid) {offsetted_idx++;} - } - - const auto path_x = data.path.x(offsetted_idx); - const auto path_y = data.path.y(offsetted_idx); - - const int rightmost_idx = data.trajectories.x.cols() - 1; - const auto last_x = data.trajectories.x.col(rightmost_idx); - const auto last_y = data.trajectories.y.col(rightmost_idx); - - const auto delta_x = last_x - path_x; - const auto delta_y = last_y - path_y; - - if (power_ > 1u) { - data.costs += (((delta_x.square() + delta_y.square()).sqrt()) * weight_).pow(power_); - } else { - data.costs += ((delta_x.square() + delta_y.square()).sqrt()) * weight_; - } -} - -} // namespace mppi::critics diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp deleted file mode 100644 index 2160d7f2..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/noise_generator.cpp +++ /dev/null @@ -1,103 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// -// 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 "ackermann_mppi/tools/noise_generator.hpp" - -namespace mppi -{ - -void NoiseGenerator::initialize( - mppi::models::OptimizerSettings & settings, - bool regenerate_noises) -{ - settings_ = settings; - regenerate_noises_ = regenerate_noises; - active_ = true; - - ndistribution_vx_ = std::normal_distribution(0.0f, settings_.sampling_std.vx); - ndistribution_wz_ = std::normal_distribution(0.0f, settings_.sampling_std.wz); - - if (regenerate_noises_) { - noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); - } else { - generateNoisedControls(); - } -} - -void NoiseGenerator::shutdown() -{ - active_ = false; - ready_ = true; - noise_cond_.notify_all(); - if (noise_thread_.joinable()) { - noise_thread_.join(); - } -} - -void NoiseGenerator::generateNextNoises() -{ - { - std::unique_lock guard(noise_lock_); - ready_ = true; - } - noise_cond_.notify_all(); -} - -void NoiseGenerator::setNoisedControls( - models::State & state, - const models::ControlSequence & control_sequence) -{ - std::unique_lock guard(noise_lock_); - state.cvx = noises_vx_.rowwise() + control_sequence.vx.transpose(); - state.cwz = noises_wz_.rowwise() + control_sequence.wz.transpose(); -} - -void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings) -{ - settings_ = settings; - - { - std::unique_lock guard(noise_lock_); - noises_vx_.setZero(settings_.batch_size, settings_.time_steps); - noises_wz_.setZero(settings_.batch_size, settings_.time_steps); - ready_ = true; - } - - if (regenerate_noises_) { - noise_cond_.notify_all(); - } else { - generateNoisedControls(); - } -} - -void NoiseGenerator::noiseThread() -{ - do { - std::unique_lock guard(noise_lock_); - noise_cond_.wait(guard, [this]() {return ready_;}); - ready_ = false; - generateNoisedControls(); - } while (active_); -} - -void NoiseGenerator::generateNoisedControls() -{ - auto & s = settings_; - noises_vx_ = Eigen::ArrayXXf::NullaryExpr( - s.batch_size, s.time_steps, [&]() {return ndistribution_vx_(generator_);}); - noises_wz_ = Eigen::ArrayXXf::NullaryExpr( - s.batch_size, s.time_steps, [&]() {return ndistribution_wz_(generator_);}); -} - -} // namespace mppi diff --git a/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp b/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp deleted file mode 100644 index 17d75f66..00000000 --- a/src/subsystems/minimal_navigation/ackermann_mppi/src/optimizer.cpp +++ /dev/null @@ -1,486 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2025 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. - -#include "ackermann_mppi/optimizer.hpp" - -#include -#include -#include -#include -#include -#include - -#include "ackermann_mppi/critics/path_follow_critic.hpp" -#include "ackermann_mppi/critics/path_align_critic.hpp" -#include "ackermann_mppi/critics/goal_critic.hpp" -#include "ackermann_mppi/critics/obstacles_critic.hpp" -#include "ackermann_mppi/critics/constraint_critic.hpp" - -namespace mppi -{ - -Optimizer::Optimizer() = default; - -Optimizer::~Optimizer() -{ - shutdown(); -} - -void Optimizer::initialize( - rclcpp::Node::SharedPtr node, - const std::string & name, - std::shared_ptr costmap_ros, - std::shared_ptr tf_buffer) -{ - node_ = node; - name_ = name; - costmap_ros_ = costmap_ros; - costmap_ = costmap_ros_->getCostmap(); - tf_buffer_ = tf_buffer; - logger_ = node_->get_logger(); - base_frame_ = costmap_ros_->getBaseFrameID(); - - getParams(); - loadCritics(); - - bool regenerate_noises = false; - declareParam(name_ + ".regenerate_noises", false); - node_->get_parameter(name_ + ".regenerate_noises", regenerate_noises); - noise_generator_.initialize(settings_, regenerate_noises); - - reset(); -} - -void Optimizer::loadCritics() -{ - // Direct instantiation — add or remove critics here to change scoring behavior. - // Order matters: critics run in order and can set fail_flag to short-circuit the rest. - auto add = [&](auto * raw, const std::string & short_name) { - critics_.emplace_back(raw); - const std::string full_name = name_ + "." + short_name; - critics_.back()->on_configure(node_, name_, full_name, costmap_ros_); - RCLCPP_INFO(logger_, "Loaded critic: %s", short_name.c_str()); - }; - - add(new critics::ConstraintCritic(), "ConstraintCritic"); - add(new critics::ObstaclesCritic(), "ObstaclesCritic"); - add(new critics::PathFollowCritic(), "PathFollowCritic"); - add(new critics::PathAlignCritic(), "PathAlignCritic"); - add(new critics::GoalCritic(), "GoalCritic"); -} - -void Optimizer::shutdown() -{ - noise_generator_.shutdown(); -} - -void Optimizer::getParams() -{ - auto & s = settings_; - auto getParam = getParamGetter(name_); - - getParam(s.model_dt, "model_dt", 0.05f); - getParam(s.time_steps, "time_steps", 56); - getParam(s.batch_size, "batch_size", 1000); - getParam(s.iteration_count, "iteration_count", 1); - getParam(s.temperature, "temperature", 0.3f); - getParam(s.gamma, "gamma", 0.015f); - getParam(s.base_constraints.vx_max, "vx_max", 0.5f); - getParam(s.base_constraints.vx_min, "vx_min", -0.35f); - getParam(s.base_constraints.wz, "wz_max", 1.9f); - getParam(s.base_constraints.ax_max, "ax_max", 3.0f); - getParam(s.base_constraints.ax_min, "ax_min", -3.0f); - getParam(s.base_constraints.az_max, "az_max", 3.5f); - getParam(s.sampling_std.vx, "vx_std", 0.2f); - getParam(s.sampling_std.wz, "wz_std", 0.4f); - getParam(s.retry_attempt_limit, "retry_attempt_limit", 1); - getParam(s.open_loop, "open_loop", false); - getParam(s.visualize, "visualize", false); - getParam(goal_dist_tolerance_, "goal_dist_tolerance", 0.25f); - - s.base_constraints.ax_max = fabs(s.base_constraints.ax_max); - if (s.base_constraints.ax_min > 0.0f) { - s.base_constraints.ax_min = -s.base_constraints.ax_min; - RCLCPP_WARN(logger_, "ax_min sign incorrect, setting negative."); - } - - float min_turning_r = 0.2f; - getParam(min_turning_r, "AckermannConstraints.min_turning_r", 0.2f); - motion_model_ = std::make_shared(min_turning_r); - - s.constraints = s.base_constraints; - - // Determine if control period matches model_dt to enable sequence shifting - double controller_frequency = 10.0; - declareParam("controller_frequency", 10.0); - node_->get_parameter("controller_frequency", controller_frequency); - const double controller_period = 1.0 / controller_frequency; - constexpr double eps = 1e-6; - if (std::abs(controller_period - s.model_dt) < eps) { - s.shift_control_sequence = true; - RCLCPP_INFO(logger_, "Control sequence shifting enabled (controller_period == model_dt)."); - } else if (controller_period > s.model_dt + eps) { - RCLCPP_WARN( - logger_, - "controller_frequency (%.2f Hz, period=%.4f s) > model_dt (%.4f s). " - "Set controller_frequency = 1/model_dt for best performance.", - controller_frequency, controller_period, s.model_dt); - } -} - -void Optimizer::reset(bool reset_dynamic_speed_limits) -{ - state_.reset(settings_.batch_size, settings_.time_steps); - control_sequence_.reset(settings_.time_steps); - control_history_.fill({}); - - if (settings_.open_loop) { - last_command_vel_ = geometry_msgs::msg::Twist(); - } - - if (reset_dynamic_speed_limits) { - settings_.constraints = settings_.base_constraints; - } - - costs_.setZero(settings_.batch_size); - generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); - noise_generator_.reset(settings_); - motion_model_->initialize(settings_.constraints, settings_.model_dt); - - // Update critic_data_ references (they hold refs to member variables) - critics_data_.goal_dist_tolerance = goal_dist_tolerance_; - - RCLCPP_INFO(logger_, "Optimizer reset"); -} - -bool Optimizer::isSpeedLimitActive() const -{ - const auto & base = settings_.base_constraints; - const auto & curr = settings_.constraints; - return base.vx_max != curr.vx_max || - base.vx_min != curr.vx_min || - base.wz != curr.wz; -} - -std::tuple Optimizer::evalControl( - const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal) -{ - prepare(robot_pose, robot_speed, plan, goal); - Eigen::ArrayXXf optimal_trajectory; - - do { - optimize(); - optimal_trajectory = getOptimizedTrajectory(); - } while (fallback(critics_data_.fail_flag)); - - auto control = getControlFromSequenceAsTwist(plan.header.stamp); - last_command_vel_ = control.twist; - - if (settings_.shift_control_sequence) { - shiftControlSequence(); - } - - return std::make_tuple(control, optimal_trajectory); -} - -void Optimizer::optimize() -{ - for (size_t i = 0; i < settings_.iteration_count; ++i) { - generateNoisedTrajectories(); - evalTrajectoriesScores(); - updateControlSequence(); - } -} - -void Optimizer::evalTrajectoriesScores() -{ - critic_costs_.clear(); - - for (auto & critic : critics_) { - if (critics_data_.fail_flag) {break;} - - if (settings_.visualize) { - Eigen::ArrayXf costs_before = critics_data_.costs; - critic->score(critics_data_); - critic_costs_.emplace_back(critic->getName(), critics_data_.costs - costs_before); - } else { - critic->score(critics_data_); - } - } -} - -bool Optimizer::fallback(bool fail) -{ - if (!fail) { - fallback_counter_ = 0; - return false; - } - - reset(false); - - if (++fallback_counter_ > settings_.retry_attempt_limit) { - fallback_counter_ = 0; - throw std::runtime_error("AckermannMPPI: all trajectories in collision, no valid control."); - } - return true; -} - -void Optimizer::prepare( - const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::Twist & robot_speed, - const nav_msgs::msg::Path & plan, - const geometry_msgs::msg::Pose & goal) -{ - state_.pose = robot_pose; - state_.speed = settings_.open_loop ? last_command_vel_ : robot_speed; - - // Compute approximate path length (sum of Euclidean segment distances) - float path_length = 0.0f; - for (size_t i = 1; i < plan.poses.size(); ++i) { - float dx = plan.poses[i].pose.position.x - plan.poses[i - 1].pose.position.x; - float dy = plan.poses[i].pose.position.y - plan.poses[i - 1].pose.position.y; - path_length += sqrtf(dx * dx + dy * dy); - } - state_.local_path_length = path_length; - - path_ = utils::toTensor(plan); - costs_.setZero(settings_.batch_size); - goal_ = goal; - - critics_data_.fail_flag = false; - critics_data_.motion_model = motion_model_; - critics_data_.furthest_reached_path_point.reset(); - critics_data_.path_pts_valid.reset(); - critics_data_.trajectories_in_collision.clear(); -} - -void Optimizer::shiftControlSequence() -{ - auto size = control_sequence_.vx.size(); - utils::shiftColumnsByOnePlace(control_sequence_.vx, -1); - utils::shiftColumnsByOnePlace(control_sequence_.wz, -1); - control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2); - control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2); -} - -void Optimizer::generateNoisedTrajectories() -{ - noise_generator_.setNoisedControls(state_, control_sequence_); - noise_generator_.generateNextNoises(); - updateStateVelocities(state_); - integrateStateVelocities(generated_trajectories_, state_); -} - -void Optimizer::applyControlSequenceConstraints() -{ - auto & s = settings_; - - float max_delta_vx = s.model_dt * s.constraints.ax_max; - float min_delta_vx = s.model_dt * s.constraints.ax_min; - float max_delta_wz = s.model_dt * s.constraints.az_max; - - float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, - control_sequence_.vx(0)); - float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0)); - control_sequence_.vx(0) = vx_last; - control_sequence_.wz(0) = wz_last; - - for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { - float & vx_curr = control_sequence_.vx(i); - vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr); - if (vx_last > 0) { - vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr); - } else { - vx_curr = utils::clamp(vx_last - max_delta_vx, vx_last - min_delta_vx, vx_curr); - } - vx_last = vx_curr; - - float & wz_curr = control_sequence_.wz(i); - wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr); - wz_curr = utils::clamp(wz_last - max_delta_wz, wz_last + max_delta_wz, wz_curr); - wz_last = wz_curr; - } - - motion_model_->applyConstraints(control_sequence_); -} - -void Optimizer::updateStateVelocities(models::State & state) const -{ - updateInitialStateVelocities(state); - propagateStateVelocitiesFromInitials(state); -} - -void Optimizer::updateInitialStateVelocities(models::State & state) const -{ - state.vx.col(0) = static_cast(state.speed.linear.x); - state.wz.col(0) = static_cast(state.speed.angular.z); - // vy is always zero for Ackermann (non-holonomic) -} - -void Optimizer::propagateStateVelocitiesFromInitials(models::State & state) const -{ - motion_model_->predict(state); -} - -void Optimizer::integrateStateVelocities( - Eigen::Array & trajectory, - const Eigen::ArrayXXf & sequence) const -{ - float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); - - const auto vx = sequence.col(0); - const auto wz = sequence.col(1); - auto traj_x = trajectory.col(0); - auto traj_y = trajectory.col(1); - auto traj_yaws = trajectory.col(2); - - const size_t n_size = traj_yaws.size(); - if (n_size == 0) {return;} - - float last_yaw = initial_yaw; - for (size_t i = 0; i != n_size; i++) { - last_yaw += wz(i) * settings_.model_dt; - traj_yaws(i) = last_yaw; - } - - Eigen::ArrayXf yaw_cos = traj_yaws.cos(); - Eigen::ArrayXf yaw_sin = traj_yaws.sin(); - utils::shiftColumnsByOnePlace(yaw_cos, 1); - utils::shiftColumnsByOnePlace(yaw_sin, 1); - yaw_cos(0) = cosf(initial_yaw); - yaw_sin(0) = sinf(initial_yaw); - - float last_x = state_.pose.pose.position.x; - float last_y = state_.pose.pose.position.y; - for (size_t i = 0; i != n_size; i++) { - last_x += vx(i) * yaw_cos(i) * settings_.model_dt; - last_y += vx(i) * yaw_sin(i) * settings_.model_dt; - traj_x(i) = last_x; - traj_y(i) = last_y; - } -} - -void Optimizer::integrateStateVelocities( - models::Trajectories & trajectories, - const models::State & state) const -{ - auto initial_yaw = static_cast(tf2::getYaw(state.pose.pose.orientation)); - const size_t n_cols = trajectories.yaws.cols(); - - Eigen::ArrayXf last_yaws = Eigen::ArrayXf::Constant(trajectories.yaws.rows(), initial_yaw); - for (size_t i = 0; i != n_cols; i++) { - last_yaws += state.wz.col(i) * settings_.model_dt; - trajectories.yaws.col(i) = last_yaws; - } - - Eigen::ArrayXXf yaw_cos = trajectories.yaws.cos(); - Eigen::ArrayXXf yaw_sin = trajectories.yaws.sin(); - utils::shiftColumnsByOnePlace(yaw_cos, 1); - utils::shiftColumnsByOnePlace(yaw_sin, 1); - yaw_cos.col(0) = cosf(initial_yaw); - yaw_sin.col(0) = sinf(initial_yaw); - - // Ackermann: no vy term - auto dx = (state.vx * yaw_cos).eval(); - auto dy = (state.vx * yaw_sin).eval(); - - Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant( - trajectories.x.rows(), state.pose.pose.position.x); - Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant( - trajectories.y.rows(), state.pose.pose.position.y); - - for (size_t i = 0; i != n_cols; i++) { - last_x += dx.col(i) * settings_.model_dt; - last_y += dy.col(i) * settings_.model_dt; - trajectories.x.col(i) = last_x; - trajectories.y.col(i) = last_y; - } -} - -Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() -{ - // Build [time_steps x 2] sequence (vx, wz) and integrate to poses - Eigen::ArrayXXf sequence(settings_.time_steps, 2); - Eigen::Array trajectory(settings_.time_steps, 3); - - sequence.col(0) = control_sequence_.vx; - sequence.col(1) = control_sequence_.wz; - - integrateStateVelocities(trajectory, sequence); - return trajectory; -} - -void Optimizer::updateControlSequence() -{ - auto & s = settings_; - - auto vx_T = control_sequence_.vx.transpose(); - auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; - const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx); - costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); - - if (s.sampling_std.wz > 0.0f) { - auto wz_T = control_sequence_.wz.transpose(); - auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; - const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz); - costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); - } - - auto costs_normalized = costs_ - costs_.minCoeff(); - const float inv_temp = 1.0f / s.temperature; - auto softmaxes = (-inv_temp * costs_normalized).exp().eval(); - softmaxes /= softmaxes.sum(); - - auto softmax_mat = softmaxes.matrix(); - control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat; - control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat; - - utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); - applyControlSequenceConstraints(); -} - -geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( - const builtin_interfaces::msg::Time & stamp) -{ - unsigned int offset = settings_.shift_control_sequence ? 1 : 0; - return utils::toTwistStamped( - control_sequence_.vx(offset), - control_sequence_.wz(offset), - stamp, base_frame_); -} - -void Optimizer::setSpeedLimit(double speed_limit, bool percentage) -{ - auto & s = settings_; - // nav2 costmap speed filter sentinel: 255.0 means "no limit active". - // Matches nav2_costmap_2d::NO_SPEED_LIMIT from filter_values.hpp. - constexpr double NO_SPEED_LIMIT = 255.0; - if (speed_limit == NO_SPEED_LIMIT) { - s.constraints.vx_max = s.base_constraints.vx_max; - s.constraints.vx_min = s.base_constraints.vx_min; - s.constraints.wz = s.base_constraints.wz; - } else { - double ratio = percentage ? speed_limit / 100.0 : speed_limit / s.base_constraints.vx_max; - s.constraints.vx_max = s.base_constraints.vx_max * ratio; - s.constraints.vx_min = s.base_constraints.vx_min * ratio; - s.constraints.wz = s.base_constraints.wz * ratio; - } - motion_model_->initialize(settings_.constraints, settings_.model_dt); -} - -} // namespace mppi diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt b/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt deleted file mode 100644 index 80e5cd00..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/CMakeLists.txt +++ /dev/null @@ -1,114 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(athena_smac_planner) - -find_package(ament_cmake REQUIRED) -find_package(angles REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav2_common REQUIRED) -find_package(nav2_core REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(nav2_smoother REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(nlohmann_json REQUIRED) -find_package(ompl REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(msgs REQUIRED) - -nav2_package() - -if(MSVC) - add_compile_definitions(_USE_MATH_DEFINES) -else() - add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) -endif() - -set(library_name athena_smac_planner) - -add_library(${library_name} SHARED - src/a_star.cpp - src/analytic_expansion.cpp - src/collision_checker.cpp - src/costmap_downsampler.cpp - src/node_basic.cpp - src/node_hybrid.cpp - src/obstacle_heuristic.cpp - src/distance_heuristic.cpp - src/smoother.cpp - src/smac_planner_hybrid.cpp -) -target_include_directories(${library_name} - PUBLIC - "$" - "$" - ${OMPL_INCLUDE_DIRS} - ${nav2_core_INCLUDE_DIRS} - ${nav2_smoother_INCLUDE_DIRS} -) -target_link_libraries(${library_name} PUBLIC - ${geometry_msgs_TARGETS} - nav2_costmap_2d::layers - nav2_costmap_2d::nav2_costmap_2d_core - ${nav_msgs_TARGETS} - nlohmann_json::nlohmann_json - ${OMPL_LIBRARIES} - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle - tf2::tf2 - tf2_ros::tf2_ros - ${visualization_msgs_TARGETS} -) -target_link_libraries(${library_name} PRIVATE - angles::angles -) - -install(TARGETS ${library_name} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -add_executable(global_planner_node src/global_planner_node.cpp) -target_include_directories(global_planner_node PRIVATE - "$" - ${OMPL_INCLUDE_DIRS} - ${nav2_core_INCLUDE_DIRS} -) -target_link_libraries(global_planner_node - ${library_name} - rclcpp::rclcpp - ${geometry_msgs_TARGETS} - ${nav_msgs_TARGETS} - nav2_costmap_2d::nav2_costmap_2d_core - ${msgs_TARGETS} -) - -install(TARGETS global_planner_node - DESTINATION lib/${PROJECT_NAME} -) - -ament_export_include_directories(include/${PROJECT_NAME}) -ament_export_libraries(${library_name}) -ament_export_dependencies( - geometry_msgs - nav2_core - nav2_costmap_2d - nav_msgs - nlohmann_json - ompl - rclcpp - rclcpp_lifecycle - tf2 - tf2_ros - visualization_msgs -) -ament_export_targets(${PROJECT_NAME}) -ament_package() diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp deleted file mode 100644 index fc587fb8..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/a_star.hpp +++ /dev/null @@ -1,323 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__A_STAR_HPP_ -#define ATHENA_SMAC_PLANNER__A_STAR_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_core/exceptions.hpp" - -#include "athena_smac_planner/thirdparty/robin_hood.h" -#include "athena_smac_planner/analytic_expansion.hpp" -#include "athena_smac_planner/node_hybrid.hpp" -#include "athena_smac_planner/node_basic.hpp" -#include "athena_smac_planner/goal_manager.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/constants.hpp" - -namespace athena_smac_planner -{ - -/** - * @class athena_smac_planner::AStarAlgorithm - * @brief An A* implementation for planning in a costmap. Templated based on the Node type. - */ -template -class AStarAlgorithm -{ -public: - typedef NodeT * NodePtr; - typedef robin_hood::unordered_node_map Graph; - typedef std::vector NodeVector; - typedef std::pair> NodeElement; - typedef typename NodeT::Coordinates Coordinates; - typedef typename NodeT::CoordinateVector CoordinateVector; - typedef typename NodeVector::iterator NeighborIterator; - typedef std::function NodeGetter; - typedef GoalManager GoalManagerT; - using NodeContext = typename NodeT::NodeContext; - - - /** - * @struct athena_smac_planner::NodeComparator - * @brief Node comparison for priority queue sorting - */ - struct NodeComparator - { - bool operator()(const NodeElement & a, const NodeElement & b) const - { - return a.first > b.first; - } - }; - - typedef std::priority_queue, NodeComparator> NodeQueue; - - /** - * @brief A constructor for athena_smac_planner::AStarAlgorithm - */ - explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); - - /** - * @brief A destructor for athena_smac_planner::AStarAlgorithm - */ - ~AStarAlgorithm(); - - /** - * @brief Initialization of the planner with defaults - * @param allow_unknown Allow search in unknown space, good for navigation while mapping - * @param max_iterations Maximum number of iterations to use while expanding search - * @param max_on_approach_iterations Maximum number of iterations before returning a valid - * path once within thresholds to refine path - * comes at more compute time but smoother paths. - * @param terminal_checking_interval Number of iterations to check if the task has been canceled or - * or planning time exceeded - * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns - * false after this timeout - * @param lookup_table_size Size of the lookup table to store heuristic values - * @param dim_3_size Number of quantization bins - */ - void initialize( - const bool & allow_unknown, - int & max_iterations, - const int & max_on_approach_iterations, - const int & terminal_checking_interval, - const double & max_planning_time, - const float & lookup_table_size, - const unsigned int & dim_3_size); - - /** - * @brief Creating path from given costmap, start, and goal - * @param path Reference to a vector of indices of generated path - * @param num_iterations Reference to number of iterations to create plan - * @param tolerance Reference to tolerance in costmap nodes - * @param cancel_checker Function to check if the task has been canceled - * @param expansions_log Optional expansions logged for debug - * @return if plan was successful - */ - bool createPath( - CoordinateVector & path, int & num_iterations, const float & tolerance, - std::function cancel_checker, - std::vector> * expansions_log = nullptr); - - /** - * @brief Update the search info used by the algorithm - * @param search_info Search info to update - */ - void setSearchInfo(const SearchInfo & search_info) {_search_info = search_info;} - - /** - * @brief Sets the collision checker to use - * @param collision_checker Collision checker to use for checking state validity - */ - void setCollisionChecker(GridCollisionChecker * collision_checker); - - /** - * @brief Set the goal for planning, as a node index - * @param mx The node X index of the goal - * @param my The node Y index of the goal - * @param dim_3 The node dim_3 index of the goal - * @param goal_heading_mode The goal heading mode to use - * @param coarse_search_resolution The resolution to search for goal heading - */ - void setGoal( - const float & mx, - const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT, - const int & coarse_search_resolution = 1); - - /** - * @brief Set the starting pose for planning, as a node index - * @param mx The node X index of the goal - * @param my The node Y index of the goal - * @param dim_3 The node dim_3 index of the goal - */ - void setStart( - const float & mx, - const float & my, - const unsigned int & dim_3); - - /** - * @brief Get maximum number of iterations to plan - * @return Reference to Maximum iterations parameter - */ - int & getMaxIterations(); - - /** - * @brief Get pointer reference to starting node - * @return Node pointer reference to starting node - */ - NodePtr & getStart(); - - /** - * @brief Get maximum number of on-approach iterations after within threshold - * @return Reference to Maximum on-approach iterations parameter - */ - int & getOnApproachMaxIterations(); - - /** - * @brief Get tolerance, in node nodes - * @return Reference to tolerance parameter - */ - float & getToleranceHeuristic(); - - /** - * @brief Get size of graph in X - * @return Size in X - */ - unsigned int & getSizeX(); - - /** - * @brief Get size of graph in Y - * @return Size in Y - */ - unsigned int & getSizeY(); - - /** - * @brief Get number of angle quantization bins (SE2) or Z coordinate (XYZ) - * @return Number of angle bins / Z dimension - */ - unsigned int & getSizeDim3(); - - /** - * @brief Get the resolution of the coarse search - * @return Size of the goals to expand - */ - unsigned int getCoarseSearchResolution(); - - /** - * @brief Get the goals manager class - * @return Goal manager class - */ - GoalManagerT getGoalManager(); - - /** - * @brief Get pointer to shared node context - * @return Node context pointer - */ - NodeContext * getContext(); - -protected: - /** - * @brief Get pointer to next goal in open set - * @return Node pointer reference to next heuristically scored node - */ - inline NodePtr getNextNode(); - - /** - * @brief Add a node to the open set - * @param cost The cost to sort into the open set of the node - * @param node Node pointer reference to add to open set - */ - inline void addNode(const float & cost, NodePtr & node); - - /** - * @brief Adds node to graph - * @param index Node index to add - */ - inline NodePtr addToGraph(const uint64_t & index); - - /** - * @brief Get cost of heuristic of node - * @param node Node pointer to get heuristic for - * @return Heuristic cost for node - */ - inline float getHeuristicCost(const NodePtr & node); - - /** - * @brief Check if inputs to planner are valid - * @return Are valid - */ - inline bool areInputsValid(); - - /** - * @brief Get the closest path within tolerance if available - * @param path Vector of coordinates to fill with path - * @return if a valid path was found within tolerance - */ - inline bool getClosestPathWithinTolerance(CoordinateVector & path); - - /** - * @brief Clear heuristic queue of nodes to search - */ - inline void clearQueue(); - - /** - * @brief Clear graph of nodes searched - */ - inline void clearGraph(); - - /** - * @brief Get index at coordinates - * @param x X coordinate of point - * @param y Y coordinate of point - * @param dim3 Z coordinate / theta bin of point - * @return Index - */ - inline uint64_t getIndex( - const unsigned int & x, const unsigned int & y, const unsigned int & dim3); - - /** - * @brief Check if node has been visited - * @param current_node Node to check if visited - * @return if node has been visited - */ - inline bool onVisitationCheckNode(const NodePtr & node); - - /** - * @brief Populate a debug log of expansions for Hybrid-A* for visualization - * @param node Node expanded - * @param expansions_log Log to add not expanded to - */ - inline void populateExpansionsLog( - const NodePtr & node, std::vector> * expansions_log); - - bool _traverse_unknown; - bool _is_initialized; - int _max_iterations; - int _max_on_approach_iterations; - int _terminal_checking_interval; - double _max_planning_time; - float _tolerance; - unsigned int _x_size; - unsigned int _y_size; - unsigned int _dim3_size; - unsigned int _coarse_search_resolution; - SearchInfo _search_info; - - NodePtr _start; - GoalManagerT _goal_manager; - Graph _graph; - NodeQueue _queue; - - MotionModel _motion_model; - NodeHeuristicPair _best_heuristic_node; - - GridCollisionChecker * _collision_checker; - nav2_costmap_2d::Costmap2D * _costmap; - std::unique_ptr> _expander; - std::shared_ptr _shared_ctx; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__A_STAR_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp deleted file mode 100644 index 1c3263cc..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/analytic_expansion.hpp +++ /dev/null @@ -1,200 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ -#define ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "athena_smac_planner/node_hybrid.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/constants.hpp" - -namespace athena_smac_planner -{ - -template -class AnalyticExpansion -{ -public: - typedef NodeT * NodePtr; - typedef std::vector NodeVector; - typedef typename NodeT::Coordinates Coordinates; - typedef std::function NodeGetter; - typedef typename NodeT::CoordinateVector CoordinateVector; - using NodeContext = typename NodeT::NodeContext; - - /** - * @struct athena_smac_planner::AnalyticExpansion::AnalyticExpansionNodes - * @brief Analytic expansion nodes and associated metadata - */ - struct AnalyticExpansionNode - { - AnalyticExpansionNode( - NodePtr & node_in, - Coordinates & initial_coords_in, - Coordinates & proposed_coords_in) - : node(node_in), - initial_coords(initial_coords_in), - proposed_coords(proposed_coords_in) - { - } - - NodePtr node; - Coordinates initial_coords; - Coordinates proposed_coords; - }; - - /** - * @struct AnalyticExpansionNodes - * @brief Analytic expansion nodes and associated metadata - * - * This structure holds a collection of analytic expansion nodes and the number of direction - * changes encountered during the expansion. - */ - struct AnalyticExpansionNodes - { - AnalyticExpansionNodes() = default; - - void add( - NodePtr & node, - Coordinates & initial_coords, - Coordinates & proposed_coords) - { - nodes.emplace_back(node, initial_coords, proposed_coords); - } - - void setDirectionChanges(int changes) - { - direction_changes = changes; - } - - std::vector nodes; - int direction_changes{0}; - }; - - /** - * @brief Constructor for analytic expansion object - */ - AnalyticExpansion( - const MotionModel & motion_model, - const SearchInfo & search_info, - const bool & traverse_unknown, - const unsigned int & dim_3_size); - - /** - * @brief Sets the collision checker and costmap to use in expansion validation - * @param collision_checker Collision checker to use - */ - void setCollisionChecker(GridCollisionChecker * collision_checker); - - /** - * @brief Sets the shared context to use - * @param ctx Shared context pointer - */ - void setContext(NodeContext * ctx); - - /** - * @brief Attempt an analytic path completion - * @param node The node to start the analytic path from - * @param coarse_check_goals Coarse list of goals nodes to plan to - * @param fine_check_goals Fine list of goals nodes to plan to - * @param goals_coords vector of goal coordinates to plan to - * @param getter Gets a node at a set of coordinates - * @param iterations Iterations to run over - * @param closest_distance Closest distance to goal - * @return Node pointer reference to goal node with the best score out of the goals node if - * successful, else return nullptr - */ - NodePtr tryAnalyticExpansion( - const NodePtr & current_node, - const NodeVector & coarse_check_goals, - const NodeVector & fine_check_goals, - const CoordinateVector & goals_coords, - const NodeGetter & getter, int & iterations, - int & closest_distance); - - /** - * @brief Perform an analytic path expansion to the goal - * @param node The node to start the analytic path from - * @param goal The goal node to plan to - * @param getter The function object that gets valid nodes from the graph - * @param state_space State space to use for computing analytic expansions - * @return A set of analytically expanded nodes to the goal from current node, if possible - */ - AnalyticExpansionNodes getAnalyticPath( - const NodePtr & node, const NodePtr & goal, - const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space); - - /** - * @brief Refined analytic path from the current node to the goal - * @param node The node to start the analytic path from. Node head may - * change as a result of refinement - * @param goal_node The goal node to plan to - * @param getter The function object that gets valid nodes from the graph - * @param analytic_nodes The set of analytic nodes to refine - * @return The score of the refined path - */ - float refineAnalyticPath( - NodePtr & node, - const NodePtr & goal_node, - const NodeGetter & getter, - AnalyticExpansionNodes & analytic_nodes); - - /** - * @brief Takes final analytic expansion and appends to current expanded node - * @param node The node to start the analytic path from - * @param goal The goal node to plan to - * @param expanded_nodes Expanded nodes to append to end of current search path - * @return Node pointer to goal node if successful, else return nullptr - */ - NodePtr setAnalyticPath( - const NodePtr & node, const NodePtr & goal, - const AnalyticExpansionNodes & expanded_nodes); - - /** - * @brief Counts the number of direction changes in a Reeds-Shepp path - * @param path The Reeds-Shepp path to count direction changes in - * @return The number of direction changes in the path - */ - int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path); - - /** - * @brief Takes an expanded nodes to clean up, if necessary, of any state - * information that may be polluting it from a prior search iteration - * @param expanded_nodes Expanded node to clean up from search - */ - void cleanNode(const NodePtr & nodes); - -protected: - MotionModel _motion_model; - SearchInfo _search_info; - bool _traverse_unknown; - unsigned int _dim_3_size; - GridCollisionChecker * _collision_checker; - std::list> _detached_nodes; - NodeContext * _ctx = nullptr; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp deleted file mode 100644 index ebd5023f..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/collision_checker.hpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#include -#include - -#include "nav2_costmap_2d/footprint_collision_checker.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "athena_smac_planner/constants.hpp" -#include "rclcpp/rclcpp.hpp" - -#ifndef ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ -#define ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ - -namespace athena_smac_planner -{ - -/** - * @class athena_smac_planner::GridCollisionChecker - * @brief A costmap grid collision checker - */ -class GridCollisionChecker - : public nav2_costmap_2d::FootprintCollisionChecker -{ -public: - /** - * @brief A constructor for athena_smac_planner::GridCollisionChecker - * for use when regular bin intervals are appropriate - * @param costmap The costmap to collision check against - * @param num_quantizations The number of quantizations to precompute footprint - * @param node Node to extract clock and logger from - * orientations for to speed up collision checking - */ - GridCollisionChecker( - std::shared_ptr costmap, - unsigned int num_quantizations, - rclcpp::Node::SharedPtr node); - - /** - * @brief A constructor for athena_smac_planner::GridCollisionChecker - * for use when irregular bin intervals are appropriate - * @param costmap The costmap to collision check against - * @param angles The vector of possible angle bins to precompute for - * orientations for to speed up collision checking, in radians - */ - // GridCollisionChecker( - // nav2_costmap_2d::Costmap2D * costmap, - // std::vector & angles); - - /** - * @brief Set the footprint to use with collision checker - * @param footprint The footprint to collision check against - * @param radius Whether or not the footprint is a circle and use radius collision checking - */ - void setFootprint( - const nav2_costmap_2d::Footprint & footprint, - const bool & radius, - const double & possible_collision_cost); - - /** - * @brief Check if in collision with costmap and footprint at pose - * @param x X coordinate of pose to check against - * @param y Y coordinate of pose to check against - * @param theta Angle bin number of pose to check against (NOT radians) - * @param traverse_unknown Whether or not to traverse in unknown space - * @return boolean if in collision or not. - */ - bool inCollision( - const float & x, - const float & y, - const float & theta, - const bool & traverse_unknown); - - /** - * @brief Check if in collision with costmap and footprint at pose - * @param i Index to search collision status of - * @param traverse_unknown Whether or not to traverse in unknown space - * @return boolean if in collision or not. - */ - bool inCollision( - const unsigned int & i, - const bool & traverse_unknown); - - /** - * @brief Get cost at footprint pose in costmap - * @return the cost at the pose in costmap - */ - float getCost(); - - /** - * @brief Get the angles of the precomputed footprint orientations - * @return the ordered vector of angles corresponding to footprints - */ - std::vector & getPrecomputedAngles() - { - return angles_; - } - - /** - * @brief Get costmap ros object for inflation layer params - * @return Costmap ros - */ - std::shared_ptr getCostmapROS() {return costmap_ros_;} - - /** - * @brief Check if value outside the range - * @param min Minimum value of the range - * @param max Maximum value of the range - * @param value the value to check if it is within the range - * @return boolean if in range or not - */ - bool outsideRange(const unsigned int & max, const float & value); - -protected: - std::shared_ptr costmap_ros_; - std::vector oriented_footprints_; - nav2_costmap_2d::Footprint unoriented_footprint_; - float center_cost_; - bool footprint_is_radius_{false}; - std::vector angles_; - float possible_collision_cost_{-1}; - rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; - rclcpp::Clock::SharedPtr clock_; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__COLLISION_CHECKER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp deleted file mode 100644 index 78e145ef..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/constants.hpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ -#define ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ - -#include - -namespace athena_smac_planner -{ -enum class MotionModel -{ - UNKNOWN = 0, - TWOD = 1, - DUBIN = 2, - REEDS_SHEPP = 3, - STATE_LATTICE = 4, -}; - -enum class GoalHeadingMode -{ - UNKNOWN = 0, - DEFAULT = 1, - BIDIRECTIONAL = 2, - ALL_DIRECTION = 3, -}; - -inline std::string toString(const MotionModel & n) -{ - switch (n) { - case MotionModel::TWOD: - return "2D"; - case MotionModel::DUBIN: - return "Dubin"; - case MotionModel::REEDS_SHEPP: - return "Reeds-Shepp"; - case MotionModel::STATE_LATTICE: - return "State Lattice"; - default: - return "Unknown"; - } -} - -inline MotionModel fromString(const std::string & n) -{ - if (n == "2D") { - return MotionModel::TWOD; - } else if (n == "DUBIN") { - return MotionModel::DUBIN; - } else if (n == "REEDS_SHEPP") { - return MotionModel::REEDS_SHEPP; - } else if (n == "STATE_LATTICE") { - return MotionModel::STATE_LATTICE; - } else { - return MotionModel::UNKNOWN; - } -} - -inline std::string toString(const GoalHeadingMode & n) -{ - switch (n) { - case GoalHeadingMode::DEFAULT: - return "DEFAULT"; - case GoalHeadingMode::BIDIRECTIONAL: - return "BIDIRECTIONAL"; - case GoalHeadingMode::ALL_DIRECTION: - return "ALL_DIRECTION"; - default: - return "Unknown"; - } -} - -inline GoalHeadingMode fromStringToGH(const std::string & n) -{ - if (n == "DEFAULT") { - return GoalHeadingMode::DEFAULT; - } else if (n == "BIDIRECTIONAL") { - return GoalHeadingMode::BIDIRECTIONAL; - } else if (n == "ALL_DIRECTION") { - return GoalHeadingMode::ALL_DIRECTION; - } else { - return GoalHeadingMode::UNKNOWN; - } -} - -const float UNKNOWN_COST = 255.0; -const float OCCUPIED_COST = 254.0; -const float INSCRIBED_COST = 253.0; -const float MAX_NON_OBSTACLE_COST = 252.0; -const float FREE_COST = 0; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__CONSTANTS_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp deleted file mode 100644 index 00d49f3f..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/costmap_downsampler.hpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright (c) 2020, Carlos Luis -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ -#define ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ - -#include - -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "athena_smac_planner/constants.hpp" - -namespace athena_smac_planner -{ - -/** - * @class athena_smac_planner::CostmapDownsampler - * @brief A costmap downsampler for more efficient path planning - */ -class CostmapDownsampler -{ -public: - /** - * @brief A constructor for CostmapDownsampler - */ - CostmapDownsampler(); - - /** - * @brief A destructor for CostmapDownsampler - */ - ~CostmapDownsampler(); - - /** - * @brief Configure the downsampled costmap object - * @param costmap The costmap we want to downsample - * @param downsampling_factor Multiplier for the costmap resolution - * @param use_min_cost_neighbor If true, min function is used instead of max for downsampling - */ - void on_configure( - nav2_costmap_2d::Costmap2D * const costmap, - const unsigned int & downsampling_factor, - const bool & use_min_cost_neighbor = false); - - /** - * @brief Cleanup the downsampled costmap - */ - void on_cleanup(); - - /** - * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap - * @param downsampling_factor Multiplier for the costmap resolution - * @return A ptr to the downsampled costmap - */ - nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor); - - /** - * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version - */ - void resizeCostmap(); - -protected: - /** - * @brief Update the sizes X-Y of the costmap and its downsampled version - */ - void updateCostmapSize(); - - /** - * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell - * @param new_mx The X-coordinate of the cell in the new costmap - * @param new_my The Y-coordinate of the cell in the new costmap - */ - void setCostOfCell( - const unsigned int & new_mx, - const unsigned int & new_my); - - unsigned int _size_x; - unsigned int _size_y; - unsigned int _downsampled_size_x; - unsigned int _downsampled_size_y; - unsigned int _downsampling_factor; - bool _use_min_cost_neighbor; - float _downsampled_resolution; - nav2_costmap_2d::Costmap2D * _costmap; - std::unique_ptr _downsampled_costmap; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp deleted file mode 100644 index 3a74bf2e..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/distance_heuristic.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright (c) 2026, 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ -#define ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ - -#include "athena_smac_planner/constants.hpp" -#include "athena_smac_planner/types.hpp" - -namespace athena_smac_planner -{ -struct HybridMotionTable; -class NodeHybrid; - -/** - * @class athena_smac_planner::DistanceHeuristic - * @brief Distance Heuristic implementation for graph, Hybrid-A* - */ -template -class DistanceHeuristic -{ -public: - /** - * @brief A constructor for athena_smac_planner::DistanceHeuristic - */ - DistanceHeuristic() {} - - /** - * @brief Compute the SE2 distance heuristic - * @param lookup_table_dim Size, in costmap pixels, of the - * each lookup table dimension to populate - * @param motion_model Motion model to use for state space - * @param dim_3_size Number of quantization bins for caching - * @param search_info Info containing minimum radius to use - */ - template - void precomputeDistanceHeuristic( - const float & lookup_table_dim, - const MotionModel & motion_model, - const unsigned int & dim_3_size, - const SearchInfo & search_info, - MotionTableT & motion_table); - - /** - * @brief Compute the Distance heuristic - * @param node_coords Coordinates to get heuristic at - * @param goal_coords Coordinates to compute heuristic to - * @param obstacle_heuristic Value of the obstacle heuristic to compute - * additional motion heuristics if required - * @return heuristic Heuristic value - */ - template - float getDistanceHeuristic( - const Coordinates & node_coords, - const Coordinates & goal_coords, - const float & obstacle_heuristic, - MotionTableT & motion_table); - -protected: - // Dubin / Reeds-Shepp lookup and size for dereferencing - LookupTable dist_heuristic_lookup_table_; - float size_lookup_; -}; - -} // namespace athena_smac_planner -#endif // ATHENA_SMAC_PLANNER__DISTANCE_HEURISTIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp deleted file mode 100644 index 4c2c1d87..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/goal_manager.hpp +++ /dev/null @@ -1,289 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// Copyright (c) 2024, Stevedan Ogochukwu Omodolor Omodia -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ -#define ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ - -#include -#include -#include -#include - -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/node_hybrid.hpp" -#include "athena_smac_planner/node_basic.hpp" -#include "athena_smac_planner/collision_checker.hpp" - - -namespace athena_smac_planner -{ - -/** -* @class athena_smac_planner::GoalManager -* @brief Responsible for managing multiple variables storing information on the goal -*/ -template -class GoalManager -{ -public: - typedef NodeT * NodePtr; - typedef std::vector NodeVector; - typedef std::unordered_set NodeSet; - typedef std::vector> GoalStateVector; - typedef typename NodeT::Coordinates Coordinates; - typedef typename NodeT::CoordinateVector CoordinateVector; - using NodeContext = typename NodeT::NodeContext; - - /** - * @brief Constructor: Initializes empty goal state. sets and coordinate lists. - */ - GoalManager() - : _goals_set(NodeSet()), - _goals_state(GoalStateVector()), - _goals_coordinate(CoordinateVector()), - _ref_goal_coord(Coordinates()) - { - } - - /** - * @brief Destructor for the GoalManager - */ - ~GoalManager() = default; - - /** - * @brief Sets the node context for goal nodes - * @param ctx Pointer to the NodeContext - */ - void setContext(NodeContext * ctx) - { - _ctx = ctx; - } - - /** - * @brief Checks if the goals set is empty - * @return true if the goals set is empty - */ - bool goalsIsEmpty() - { - return _goals_state.empty(); - } - - /** - * @brief Adds goal to the goal vector - *@param goal Reference to the NodePtr - */ - void addGoal(NodePtr & goal) - { - _goals_state.push_back({goal, true}); - } - - /** - * @brief Clears all internal goal data, including goals, states, and coordinates. - */ - void clear() - { - _goals_set.clear(); - _goals_state.clear(); - _goals_coordinate.clear(); - } - - /** - * @brief Populates coarse and fine goal lists for analytic expansion. - * @param coarse_check_goals Output list of goals for coarse search expansion. - * @param fine_check_goals Output list of goals for fine search refinement. - * @param coarse_search_resolution Number of fine goals per coarse goal. - */ - void prepareGoalsForAnalyticExpansion( - NodeVector & coarse_check_goals, NodeVector & fine_check_goals, - int coarse_search_resolution) - { - for (unsigned int i = 0; i < _goals_state.size(); i++) { - if (_goals_state[i].is_valid) { - if (i % coarse_search_resolution == 0) { - coarse_check_goals.push_back(_goals_state[i].goal); - } else { - fine_check_goals.push_back(_goals_state[i].goal); - } - } - } - } - - /** - * @brief Checks if zone within the radius of a node is feasible. Returns true if - * there's at least one non-lethal cell within the node radius. - * @param node Input node. - * @param radius Search radius. - * @param collision_checker Collision checker to validate nearby nodes. - * @param traverse_unknown Flag whether traversal through unknown space is allowed. - * @return true - * @return false - */ - bool isZoneValid( - const NodePtr node, const float & radius, GridCollisionChecker * collision_checker, - const bool & traverse_unknown) const - { - if (radius < 1) { - return false; - } - - const auto size_x = collision_checker->getCostmap()->getSizeInCellsX(); - const auto size_y = collision_checker->getCostmap()->getSizeInCellsY(); - - auto getIndexFromPoint = [this](const Coordinates & point) { - const auto mx = static_cast(point.x); - const auto my = static_cast(point.y); - const auto angle = static_cast(point.theta); - return NodeT::getIndex(mx, my, angle, _ctx->motion_table.size_x, - _ctx->motion_table.num_angle_quantization); - }; - - const Coordinates & center_point = node->pose; - const float min_x = std::max(0.0f, std::floor(center_point.x - radius)); - const float min_y = std::max(0.0f, std::floor(center_point.y - radius)); - const float max_x = - std::min(static_cast(size_x - 1), std::ceil(center_point.x + radius)); - const float max_y = - std::min(static_cast(size_y - 1), std::ceil(center_point.y + radius)); - const float radius_sq = radius * radius; - - Coordinates m; - for (m.x = min_x; m.x <= max_x; m.x += 1.0f) { - for (m.y = min_y; m.y <= max_y; m.y += 1.0f) { - const float dx = m.x - center_point.x; - const float dy = m.y - center_point.y; - - if (dx * dx + dy * dy > radius_sq) { - continue; - } - - NodeT current_node(getIndexFromPoint(m), _ctx); - current_node.setPose(m); - - if (current_node.isNodeValid(traverse_unknown, collision_checker)) { - return true; - } - } - } - - return false; - } - - /** - * @brief Filters and marks invalid goals based on collision checking and tolerance thresholds. - * - * Stores only valid (or tolerably infeasible) goals into internal goal sets and coordinates. - * - * @param tolerance Heuristic tolerance allowed for infeasible goals. - * @param collision_checker Collision checker to validate goal positions. - * @param traverse_unknown Flag whether traversal through unknown space is allowed. - */ - void removeInvalidGoals( - const float & tolerance, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown) - { - // Make sure that there was a goal clear before this was run - if (!_goals_set.empty() || !_goals_coordinate.empty()) { - throw std::runtime_error( - "Goal set should be cleared before calling " - "removeinvalidgoals"); - } - for (unsigned int i = 0; i < _goals_state.size(); i++) { - if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) || - isZoneValid(_goals_state[i].goal, tolerance, collision_checker, traverse_unknown)) - { - _goals_state[i].is_valid = true; - _goals_set.insert(_goals_state[i].goal); - _goals_coordinate.push_back(_goals_state[i].goal->pose); - } else { - _goals_state[i].is_valid = false; - } - } - } - - /** - * @brief Check if a given node is part of the goal set. - * @param node Node pointer to check. - * @return if node matches any goal in the goal set. - */ - inline bool isGoal(const NodePtr & node) - { - return _goals_set.find(node) != _goals_set.end(); - } - - /** - * @brief Get pointer reference to goals set vector - * @return unordered_set of node pointers reference to the goals nodes - */ - inline NodeSet & getGoalsSet() - { - return _goals_set; - } - - /** - * @brief Get pointer reference to goals state - * @return vector of node pointers reference to the goals state - */ - inline GoalStateVector & getGoalsState() - { - return _goals_state; - } - - /** - * @brief Get pointer reference to goals coordinates - * @return vector of goals coordinates reference to the goals coordinates - */ - inline CoordinateVector & getGoalsCoordinates() - { - return _goals_coordinate; - } - - /** - * @brief Set the Reference goal coordinate - * @param coord Coordinates to set as Reference goal - */ - inline void setRefGoalCoordinates(const Coordinates & coord) - { - _ref_goal_coord = coord; - } - - /** - * @brief Checks whether the Reference goal coordinate has changed. - * @param coord Coordinates to compare with the current Reference goal coordinate. - * @return true if the Reference goal coordinate has changed, false otherwise. - */ - inline bool hasGoalChanged(const Coordinates & coord) - { - /** - * Note: This function checks if the goal has changed. This has to be done with - * the coordinates not the Node pointer because the Node pointer - * can be reused for different goals, but the coordinates will always - * be unique for each goal. - */ - return _ref_goal_coord != coord; - } - -protected: - NodeSet _goals_set; - GoalStateVector _goals_state; - CoordinateVector _goals_coordinate; - Coordinates _ref_goal_coord; - NodeContext * _ctx = nullptr; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__GOAL_MANAGER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp deleted file mode 100644 index 70f464c9..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_basic.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ -#define ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ - -#include "athena_smac_planner/constants.hpp" -#include "athena_smac_planner/node_hybrid.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/collision_checker.hpp" - -namespace athena_smac_planner -{ - -/** - * @class athena_smac_planner::NodeBasic - * @brief NodeBasic implementation for priority queue insertion - */ -template -class NodeBasic -{ -public: - /** - * @brief A constructor for athena_smac_planner::NodeBasic - * @param index The index of this node for self-reference - */ - explicit NodeBasic(const uint64_t new_index) - : graph_node_ptr(nullptr), - index(new_index) - { - } - - /** - * @brief Take a NodeBasic and populate it with any necessary state - * cached in the queue for NodeT. - * @param node NodeT ptr to populate metadata into NodeBasic - */ - void populateSearchNode(NodeT * & node); - - /** - * @brief Take a NodeBasic and populate it with any necessary state - * cached in the queue for NodeTs. - * @param node Search node (basic) object to initialize internal node - * with state - */ - void processSearchNode(); - - typename NodeT::Coordinates pose; // Used by NodeHybrid - NodeT * graph_node_ptr; - uint64_t index; - unsigned int motion_index; - bool backward; - TurnDirection turn_dir; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp deleted file mode 100644 index b9ed6092..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/node_hybrid.hpp +++ /dev/null @@ -1,386 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ -#define ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ - -#include -#include -#include -#include - -#include "ompl/base/StateSpace.h" - -#include "athena_smac_planner/constants.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/collision_checker.hpp" -#include "athena_smac_planner/costmap_downsampler.hpp" -#include "athena_smac_planner/obstacle_heuristic.hpp" -#include "athena_smac_planner/distance_heuristic.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/inflation_layer.hpp" - -namespace athena_smac_planner -{ - -// Must forward declare -class NodeHybrid; - -/** - * @struct athena_smac_planner::HybridMotionTable - * @brief A table of motion primitives and related functions - */ -struct HybridMotionTable -{ - /** - * @brief A constructor for athena_smac_planner::HybridMotionTable - */ - HybridMotionTable() {} - - /** - * @brief Initializing using Dubin model - * @param size_x_in Size of costmap in X - * @param size_y_in Size of costmap in Y - * @param angle_quantization_in Size of costmap in bin sizes - * @param search_info Parameters for searching - */ - void initDubin( - unsigned int & size_x_in, - unsigned int & size_y_in, - unsigned int & angle_quantization_in, - SearchInfo & search_info); - - /** - * @brief Initializing using Reeds-Shepp model - * @param size_x_in Size of costmap in X - * @param size_y_in Size of costmap in Y - * @param angle_quantization_in Size of costmap in bin sizes - * @param search_info Parameters for searching - */ - void initReedsShepp( - unsigned int & size_x_in, - unsigned int & size_y_in, - unsigned int & angle_quantization_in, - SearchInfo & search_info); - - /** - * @brief Get projections of motion models - * @param node Ptr to NodeHybrid - * @return A set of motion poses - */ - MotionPoses getProjections(const NodeHybrid * node); - - /** - * @brief Get the angular bin to use from a raw orientation - * @param theta Angle in radians - * @return bin index of closest angle to request - */ - unsigned int getClosestAngularBin(const double & theta); - - /** - * @brief Get the raw orientation from an angular bin - * @param bin_idx Index of the bin - * @return Raw orientation in radians - */ - float getAngleFromBin(const unsigned int & bin_idx); - - /** - * @brief Get the angle scaled across bins from a raw orientation - * @param theta Angle in radians - * @return angle scaled across bins - */ - double getAngle(const double & theta); - - MotionModel motion_model = MotionModel::UNKNOWN; - MotionPoses projections; - unsigned int size_x; - unsigned int num_angle_quantization; - float num_angle_quantization_float; - float min_turning_radius; - float bin_size; - float change_penalty; - float non_straight_penalty; - float cost_penalty; - float reverse_penalty; - float travel_distance_reward; - bool downsample_obstacle_heuristic; - bool use_quadratic_cost_penalty; - bool allow_primitive_interpolation; - ompl::base::StateSpacePtr state_space; - std::vector> delta_xs; - std::vector> delta_ys; - std::vector trig_values; - std::vector travel_costs; -}; - -/** - * @class athena_smac_planner::NodeHybrid - * @brief NodeHybrid implementation for graph, Hybrid-A* - */ -class NodeHybrid -{ -public: - typedef NodeHybrid * NodePtr; - typedef std::unique_ptr> Graph; - typedef std::vector NodeVector; - using Coordinates = athena_smac_planner::Coordinates; - typedef std::vector CoordinateVector; - - struct NodeContext - { - /** - * @brief A constructor for athena_smac_planner::NodeContext - */ - NodeContext() - { - obstacle_heuristic = std::make_unique(); - distance_heuristic = std::make_unique>(); - } - HybridMotionTable motion_table; - std::unique_ptr obstacle_heuristic; - std::unique_ptr> distance_heuristic; - }; - /** - * @brief A constructor for athena_smac_planner::NodeHybrid - * @param index The index of this node for self-reference - */ - explicit NodeHybrid(const uint64_t index, NodeContext * ctx); - - /** - * @brief A destructor for athena_smac_planner::NodeHybrid - */ - ~NodeHybrid(); - - /** - * @brief operator== for comparisons - * @param NodeHybrid right hand side node reference - * @return If cell indices are equal - */ - bool operator==(const NodeHybrid & rhs) - { - return this->_index == rhs._index; - } - - /** - * @brief setting continuous coordinate search poses (in partial-cells) - * @param Pose pose - */ - inline void setPose(const Coordinates & pose_in) - { - pose = pose_in; - } - - /** - * @brief Reset method for new search - */ - void reset(); - - /** - * @brief Gets the accumulated cost at this node - * @return accumulated cost - */ - inline float getAccumulatedCost() - { - return _accumulated_cost; - } - - /** - * @brief Sets the accumulated cost at this node - * @param reference to accumulated cost - */ - inline void setAccumulatedCost(const float & cost_in) - { - _accumulated_cost = cost_in; - } - - /** - * @brief Sets the motion primitive index used to achieve node in search - * @param reference to motion primitive idx - */ - inline void setMotionPrimitiveIndex(const unsigned int & idx, const TurnDirection & turn_dir) - { - _motion_primitive_index = idx; - _turn_dir = turn_dir; - } - - /** - * @brief Gets the motion primitive index used to achieve node in search - * @return reference to motion primitive idx - */ - inline unsigned int & getMotionPrimitiveIndex() - { - return _motion_primitive_index; - } - - /** - * @brief Gets the motion primitive turning direction used to achieve node in search - * @return reference to motion primitive turning direction - */ - inline TurnDirection & getTurnDirection() - { - return _turn_dir; - } - - /** - * @brief Gets the costmap cost at this node - * @return costmap cost - */ - inline float getCost() - { - return _cell_cost; - } - - /** - * @brief Gets if cell has been visited in search - * @param If cell was visited - */ - inline bool wasVisited() - { - return _was_visited; - } - - /** - * @brief Sets if cell has been visited in search - */ - inline void visited() - { - _was_visited = true; - } - - /** - * @brief Gets cell index - * @return Reference to cell index - */ - inline uint64_t getIndex() - { - return _index; - } - - /** - * @brief Check if this node is valid - * @param traverse_unknown If we can explore unknown nodes on the graph - * @param collision_checker: Collision checker object - * @return whether this node is valid and collision free - */ - bool isNodeValid( - const bool & traverse_unknown, - GridCollisionChecker * collision_checker); - - /** - * @brief Get traversal cost of parent node to child node - * @param child Node pointer to child - * @return traversal cost - */ - float getTraversalCost(const NodePtr & child); - - /** - * @brief Get index at coordinates - * @param x X coordinate of point - * @param y Y coordinate of point - * @param angle Theta coordinate of point - * @param width Width of costmap - * @param angle_quantization Number of theta bins - * @return Index - */ - static inline uint64_t getIndex( - const unsigned int & x, const unsigned int & y, const unsigned int & angle, - const unsigned int & width, const unsigned int & angle_quantization) - { - return static_cast(angle) + static_cast(x) * - static_cast(angle_quantization) + - static_cast(y) * static_cast(width) * - static_cast(angle_quantization); - } - - /** - * @brief Get coordinates at index - * @param index Index of point - * @param width Width of costmap - * @param angle_quantization Theta size of costmap - * @return Coordinates - */ - static inline Coordinates getCoords( - const uint64_t & index, - const unsigned int & width, const unsigned int & angle_quantization) - { - return Coordinates( - (index / angle_quantization) % width, // x - index / (angle_quantization * width), // y - index % angle_quantization); // theta - } - - /** - * @brief Get cost of heuristic of node - * @param node Node index current - * @param node Node index of new - * @return Heuristic cost between the nodes - */ - float getHeuristicCost( - const Coordinates & node_coords, - const CoordinateVector & goals_coords); - - /** - * @brief Initialize motion models - * @param motion_model Motion model enum to use - * @param size_x Size of X of graph - * @param size_y Size of y of graph - * @param angle_quantization Size of theta bins of graph - * @param search_info Search info to use - */ - static void initMotionModel( - NodeContext * ctx, - const MotionModel & motion_model, - unsigned int & size_x, - unsigned int & size_y, - unsigned int & angle_quantization, - SearchInfo & search_info); - - /** - * @brief Retrieve all valid neighbors of a node. - * @param validity_checker Functor for state validity checking - * @param collision_checker Collision checker to use - * @param traverse_unknown If unknown costs are valid to traverse - * @param neighbors Vector of neighbors to be filled - */ - void getNeighbors( - std::function & validity_checker, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown, - NodeVector & neighbors); - - /** - * @brief Set the starting pose for planning, as a node index - * @param path Reference to a vector of indices of generated path - * @return whether the path was able to be backtraced - */ - bool backtracePath(CoordinateVector & path); - - NodeHybrid * parent; - Coordinates pose; - -private: - float _cell_cost; - float _accumulated_cost; - uint64_t _index; - bool _was_visited; - unsigned int _motion_primitive_index; - TurnDirection _turn_dir; - bool _is_node_valid{false}; - NodeContext * _ctx = nullptr; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__NODE_HYBRID_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp deleted file mode 100644 index 2a439d02..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/obstacle_heuristic.hpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright (c) 2026, 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ -#define ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ - -#include -#include -#include -#include "athena_smac_planner/constants.hpp" -#include "athena_smac_planner/types.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" - -namespace athena_smac_planner -{ - -typedef std::pair ObstacleHeuristicElement; -struct ObstacleHeuristicComparator -{ - bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const - { - return a.first > b.first; - } -}; - -typedef std::vector ObstacleHeuristicQueue; - -/** - * @class athena_smac_planner::ObstacleHeuristic - * @brief Obstacle Heuristic implementation for graph, Hybrid-A* - */ -class ObstacleHeuristic -{ -public: - /** - * @brief A constructor for athena_smac_planner::ObstacleHeuristic - */ - ObstacleHeuristic() {} - - /** - * @brief A destructor for athena_smac_planner::ObstacleHeuristic - */ - ~ObstacleHeuristic() {} - - /** - * @brief Compute the wavefront heuristic - * @param costmap Costmap to use - * @param goal_coords Coordinates to start heuristic expansion at - */ - void resetObstacleHeuristic( - std::shared_ptr costmap_ros_i, - const unsigned int & start_x, const unsigned int & start_y, - const unsigned int & goal_x, const unsigned int & goal_y, - const bool downsample_obstacle_heuristic); - - /** - * @brief Compute the Obstacle heuristic - * @param node_coords Coordinates to get heuristic at - * @param goal_coords Coordinates to compute heuristic to - * @return heuristic Heuristic value - */ - float getObstacleHeuristic( - const Coordinates & node_coords, - const float & cost_penalty, - const bool use_quadratic_cost_penalty, - const bool downsample_obstacle_heuristic); - - inline float distanceHeuristic2D( - const uint64_t idx, const unsigned int size_x, - const unsigned int target_x, const unsigned int target_y) - { - int dx = static_cast(idx % size_x) - static_cast(target_x); - int dy = static_cast(idx / size_x) - static_cast(target_y); - return std::sqrt(dx * dx + dy * dy); - } - -protected: - LookupTable obstacle_heuristic_lookup_table_; - ObstacleHeuristicQueue obstacle_heuristic_queue_; - std::shared_ptr costmap_ros; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__OBSTACLE_HEURISTIC_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp deleted file mode 100644 index c5a2cd84..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smac_planner_hybrid.hpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ -#define ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ - -#include -#include -#include -#include - -#include "athena_smac_planner/a_star.hpp" -#include "athena_smac_planner/smoother.hpp" -#include "athena_smac_planner/utils.hpp" -#include "athena_smac_planner/costmap_downsampler.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_array.hpp" -#include "visualization_msgs/msg/marker_array.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2/utils.hpp" - -namespace athena_smac_planner -{ - -class SmacPlannerHybrid -{ -public: - /** - * @brief Constructor — initializes the planner from ROS parameters. - * @param node Shared ptr to an rclcpp::Node for parameter reading and publisher creation. - * @param costmap_ros Costmap2DROS providing the collision costmap. - * @param name Parameter namespace prefix (default "SmacPlannerHybrid"). - */ - SmacPlannerHybrid( - rclcpp::Node::SharedPtr node, - std::shared_ptr costmap_ros, - const std::string & name = "SmacPlannerHybrid"); - - /** - * @brief Destructor - */ - ~SmacPlannerHybrid(); - - /** - * @brief Create a path from start to goal. - * @param start Start pose in map frame. - * @param goal Goal pose in map frame. - * @param cancel_checker Callable that returns true when planning should abort. - * @return nav_msgs::msg::Path of the generated path. - */ - nav_msgs::msg::Path createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal, - std::function cancel_checker); - -protected: - std::unique_ptr> _a_star; - GridCollisionChecker _collision_checker; - std::unique_ptr _smoother; - rclcpp::Clock::SharedPtr _clock; - rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")}; - nav2_costmap_2d::Costmap2D * _costmap; - std::shared_ptr _costmap_ros; - std::unique_ptr _costmap_downsampler; - std::string _global_frame, _name; - float _lookup_table_dim; - float _tolerance; - bool _downsample_costmap; - int _downsampling_factor; - double _angle_bin_size; - unsigned int _angle_quantizations; - bool _allow_unknown; - int _max_iterations; - int _max_on_approach_iterations; - int _terminal_checking_interval; - SearchInfo _search_info; - double _max_planning_time; - double _lookup_table_size; - double _minimum_turning_radius_global_coords; - bool _debug_visualizations; - std::string _motion_model_for_search; - MotionModel _motion_model; - GoalHeadingMode _goal_heading_mode; - int _coarse_search_resolution; - rclcpp::Publisher::SharedPtr _raw_plan_publisher; - rclcpp::Publisher::SharedPtr - _planned_footprints_publisher; - rclcpp::Publisher::SharedPtr - _smoothed_footprints_publisher; - rclcpp::Publisher::SharedPtr - _expansions_publisher; - std::mutex _mutex; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp deleted file mode 100644 index 06997543..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/smoother.hpp +++ /dev/null @@ -1,208 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ -#define ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ - -#include - -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "athena_smac_planner/types.hpp" -#include "athena_smac_planner/constants.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav_msgs/msg/path.hpp" -#include "ompl/base/StateSpace.h" - -namespace athena_smac_planner -{ - -/** - * @struct athena_smac_planner::BoundaryPoints - * @brief Set of boundary condition points from expansion - */ -struct BoundaryPoints -{ - /** - * @brief A constructor for BoundaryPoints - */ - BoundaryPoints(double & x_in, double & y_in, double & theta_in) - : x(x_in), y(y_in), theta(theta_in) - {} - - double x; - double y; - double theta; -}; - -/** - * @struct athena_smac_planner::BoundaryExpansion - * @brief Boundary expansion state - */ -struct BoundaryExpansion -{ - double path_end_idx{0.0}; - double expansion_path_length{0.0}; - double original_path_length{0.0}; - std::vector pts; - bool in_collision{false}; -}; - -typedef std::vector BoundaryExpansions; -typedef std::vector::iterator PathIterator; -typedef std::vector::reverse_iterator ReversePathIterator; - -/** - * @class athena_smac_planner::Smoother - * @brief A path smoother implementation - */ -class Smoother -{ -public: - /** - * @brief A constructor for athena_smac_planner::Smoother - */ - explicit Smoother(const SmootherParams & params); - - /** - * @brief A destructor for athena_smac_planner::Smoother - */ - ~Smoother() {} - - /** - * @brief Initialization of the smoother - * @param min_turning_radius Minimum turning radius (m) - * @param motion_model Motion model type - */ - void initialize( - const double & min_turning_radius); - - /** - * @brief Smoother API method - * @param path Reference to path - * @param costmap Pointer to minimal costmap - * @param max_time Maximum time to compute, stop early if over limit - * @return If smoothing was successful - */ - bool smooth( - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time); - -protected: - /** - * @brief Smoother method - does the smoothing on a segment - * @param path Reference to path - * @param reversing_segment Return if this is a reversing segment - * @param costmap Pointer to minimal costmap - * @param max_time Maximum time to compute, stop early if over limit - * @return If smoothing was successful - */ - bool smoothImpl( - nav_msgs::msg::Path & path, - bool & reversing_segment, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time); - - /** - * @brief Get the field value for a given dimension - * @param msg Current pose to sample - * @param dim Dimension ID of interest - * @return dim value - */ - inline double getFieldByDim( - const geometry_msgs::msg::PoseStamped & msg, - const unsigned int & dim); - - /** - * @brief Set the field value for a given dimension - * @param msg Current pose to sample - * @param dim Dimension ID of interest - * @param value to set the dimension to for the pose - */ - inline void setFieldByDim( - geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, - const double & value); - - /** - * @brief Enforced minimum curvature boundary conditions on plan output - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param start_pose Start pose of the feasible path to maintain - * @param path Path to modify for curvature constraints on start / end of path - * @param costmap Costmap to check for collisions - * @param reversing_segment Whether this path segment is in reverse - */ - void enforceStartBoundaryConditions( - const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment); - - /** - * @brief Enforced minimum curvature boundary conditions on plan output - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param end_pose End pose of the feasible path to maintain - * @param path Path to modify for curvature constraints on start / end of path - * @param costmap Costmap to check for collisions - * @param reversing_segment Whether this path segment is in reverse - */ - void enforceEndBoundaryConditions( - const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment); - - /** - * @brief Given a set of boundary expansion, find the one which is shortest - * such that it is least likely to contain a loop-de-loop when working with - * close-by primitive markers. Instead, select a further away marker which - * generates a shorter ` - * @param boundary_expansions Set of boundary expansions - * @return Idx of the shorest boundary expansion option - */ - unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions & boundary_expansions); - - /** - * @brief Populate a motion model expansion from start->end into expansion - * @param start Start pose of the feasible path to maintain - * @param end End pose of the feasible path to maintain - * @param expansion Expansion object to populate - * @param costmap Costmap to check for collisions - * @param reversing_segment Whether this path segment is in reverse - */ - void findBoundaryExpansion( - const geometry_msgs::msg::Pose & start, - const geometry_msgs::msg::Pose & end, - BoundaryExpansion & expansion, - const nav2_costmap_2d::Costmap2D * costmap); - - /** - * @brief Generates boundary expansions with end idx at least strategic - * distances away, using either Reverse or (Forward) Path Iterators. - * @param start iterator to start search in path for - * @param end iterator to end search for - * @return Boundary expansions with end idxs populated - */ - template - BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end); - - double min_turning_rad_, tolerance_, data_w_, smooth_w_; - int max_its_, refinement_ctr_, refinement_num_; - bool is_holonomic_, do_refinement_; - MotionModel motion_model_; - ompl::base::StateSpacePtr state_space_; -}; - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h deleted file mode 100644 index c6b0fd60..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/thirdparty/robin_hood.h +++ /dev/null @@ -1,2539 +0,0 @@ -// Copyright (c) 2018-2021 Martin Ankerl -// ______ _____ ______ _________ -// ______________ ___ /_ ___(_)_______ ___ /_ ______ ______ ______ / -// __ ___/_ __ \__ __ \__ / __ __ \ __ __ \_ __ \_ __ \_ __ / -// _ / / /_/ /_ /_/ /_ / _ / / / _ / / // /_/ // /_/ // /_/ / -// /_/ \____/ /_.___/ /_/ /_/ /_/ ________/_/ /_/ \____/ \____/ \__,_/ -// _/_____/ -// -// Fast & memory efficient hashtable based on robin hood hashing for C++11/14/17/20 -// https://github.com/martinus/robin-hood-hashing -// -// Licensed under the MIT License . -// SPDX-License-Identifier: MIT -// Copyright (c) 2018-2021 Martin Ankerl -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ -#define NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ - -// see https://semver.org/ -#define ROBIN_HOOD_VERSION_MAJOR 3 // for incompatible API changes -#define ROBIN_HOOD_VERSION_MINOR 11 // for adding functionality in a backwards-compatible manner -#define ROBIN_HOOD_VERSION_PATCH 5 // for backwards-compatible bug fixes - -#include -#include -#include -#include -#include -#include // only to support hash of smart pointers -#include -#include -#include -#include -#include -#if __cplusplus >= 201703L -# include -#endif -/* *INDENT-OFF* */ - -// #define ROBIN_HOOD_LOG_ENABLED -#ifdef ROBIN_HOOD_LOG_ENABLED -# include -# define ROBIN_HOOD_LOG(...) \ - std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; -#else -# define ROBIN_HOOD_LOG(x) -#endif - -// #define ROBIN_HOOD_TRACE_ENABLED -#ifdef ROBIN_HOOD_TRACE_ENABLED -# include -# define ROBIN_HOOD_TRACE(...) \ - std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; -#else -# define ROBIN_HOOD_TRACE(x) -#endif - -// #define ROBIN_HOOD_COUNT_ENABLED -#ifdef ROBIN_HOOD_COUNT_ENABLED -# include -# define ROBIN_HOOD_COUNT(x) ++counts().x; -namespace robin_hood { -struct Counts { - uint64_t shiftUp{}; - uint64_t shiftDown{}; -}; -inline std::ostream& operator<<(std::ostream& os, Counts const& c) { - return os << c.shiftUp << " shiftUp" << std::endl << c.shiftDown << " shiftDown" << std::endl; -} - -static Counts& counts() { - static Counts counts{}; - return counts; -} -} // namespace robin_hood -#else -# define ROBIN_HOOD_COUNT(x) -#endif - -// all non-argument macros should use this facility. See -// https://www.fluentcpp.com/2019/05/28/better-macros-better-flags/ -#define ROBIN_HOOD(x) ROBIN_HOOD_PRIVATE_DEFINITION_##x() - -// mark unused members with this macro -#define ROBIN_HOOD_UNUSED(identifier) - -// bitness -#if SIZE_MAX == UINT32_MAX -# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 32 -#elif SIZE_MAX == UINT64_MAX -# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 -#else -# error Unsupported bitness -#endif - -// endianness -#ifdef _MSC_VER -# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() 1 -# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() 0 -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() \ - (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) -# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) -#endif - -// inline -#ifdef _MSC_VER -# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __declspec(noinline) -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __attribute__((noinline)) -#endif - -// exceptions -#if !defined(__cpp_exceptions) && !defined(__EXCEPTIONS) && !defined(_CPPUNWIND) -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 0 -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 1 -#endif - -// count leading/trailing bits -#if !defined(ROBIN_HOOD_DISABLE_INTRINSICS) -# ifdef _MSC_VER -# if ROBIN_HOOD(BITNESS) == 32 -# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward -# else -# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward64 -# endif -# include -# pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD)) -# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ - [](size_t mask) noexcept -> int { \ - unsigned long index; \ // NOLINT - return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast(index) \ - : ROBIN_HOOD(BITNESS); \ - }(x) -# else -# if ROBIN_HOOD(BITNESS) == 32 -# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzl -# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzl -# else -# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzll -# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzll -# endif -# define ROBIN_HOOD_COUNT_LEADING_ZEROES(x) ((x) ? ROBIN_HOOD(CLZ)(x) : ROBIN_HOOD(BITNESS)) -# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) ((x) ? ROBIN_HOOD(CTZ)(x) : ROBIN_HOOD(BITNESS)) -# endif -#endif - -// fallthrough -#ifndef __has_cpp_attribute // For backwards compatibility -# define __has_cpp_attribute(x) 0 -#endif -#if __has_cpp_attribute(clang::fallthrough) -# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[clang::fallthrough]] -#elif __has_cpp_attribute(gnu::fallthrough) -# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[gnu::fallthrough]] -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() -#endif - -// likely/unlikely -#ifdef _MSC_VER -# define ROBIN_HOOD_LIKELY(condition) condition -# define ROBIN_HOOD_UNLIKELY(condition) condition -#else -# define ROBIN_HOOD_LIKELY(condition) __builtin_expect(condition, 1) -# define ROBIN_HOOD_UNLIKELY(condition) __builtin_expect(condition, 0) -#endif - -// detect if native wchar_t type is available in MSVC -#ifdef _MSC_VER -# ifdef _NATIVE_WCHAR_T_DEFINED -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 -# else -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 0 -# endif -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 -#endif - -// detect if MSVC supports the pair(std::piecewise_construct_t,...) constructor being constexpr -#ifdef _MSC_VER -# if _MSC_VER <= 1900 -# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 1 -# else -# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 -# endif -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 -#endif - -// workaround missing "is_trivially_copyable" in g++ < 5.0 -// See https://stackoverflow.com/a/31798726/48181 -#if defined(__GNUC__) && __GNUC__ < 5 -# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) __has_trivial_copy(__VA_ARGS__) -#else -# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) std::is_trivially_copyable<__VA_ARGS__>::value -#endif - -// helpers for C++ versions, see https://gcc.gnu.org/onlinedocs/cpp/Standard-Predefined-Macros.html -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX() __cplusplus -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX98() 199711L -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX11() 201103L -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX14() 201402L -#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX17() 201703L - -#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) -# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() [[nodiscard]] -#else -# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() -#endif - -namespace robin_hood { - -#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) -# define ROBIN_HOOD_STD std -#else - -// c++11 compatibility layer -namespace ROBIN_HOOD_STD { -template -struct alignment_of - : std::integral_constant::type)> {}; - -template -class integer_sequence { -public: - using value_type = T; - static_assert(std::is_integral::value, "not integral type"); - static constexpr std::size_t size() noexcept { - return sizeof...(Ints); - } -}; -template -using index_sequence = integer_sequence; - -namespace detail_ { -template -struct IntSeqImpl { - using TValue = T; - static_assert(std::is_integral::value, "not integral type"); - static_assert(Begin >= 0 && Begin < End, "unexpected argument (Begin<0 || Begin<=End)"); - - template - struct IntSeqCombiner; - - template - struct IntSeqCombiner, integer_sequence> { - using TResult = integer_sequence; - }; - - using TResult = - typename IntSeqCombiner::TResult, - typename IntSeqImpl::TResult>::TResult; -}; - -template -struct IntSeqImpl { - using TValue = T; - static_assert(std::is_integral::value, "not integral type"); - static_assert(Begin >= 0, "unexpected argument (Begin<0)"); - using TResult = integer_sequence; -}; - -template -struct IntSeqImpl { - using TValue = T; - static_assert(std::is_integral::value, "not integral type"); - static_assert(Begin >= 0, "unexpected argument (Begin<0)"); - using TResult = integer_sequence; -}; -} // namespace detail_ - -template -using make_integer_sequence = typename detail_::IntSeqImpl::TResult; - -template -using make_index_sequence = make_integer_sequence; - -template -using index_sequence_for = make_index_sequence; - -} // namespace ROBIN_HOOD_STD - -#endif - -namespace detail { - -// make sure we static_cast to the correct type for hash_int -#if ROBIN_HOOD(BITNESS) == 64 -using SizeT = uint64_t; -#else -using SizeT = uint32_t; -#endif - -template -T rotr(T x, unsigned k) { - return (x >> k) | (x << (8U * sizeof(T) - k)); -} - -// This cast gets rid of warnings like "cast from 'uint8_t*' {aka 'unsigned char*'} to -// 'uint64_t*' {aka 'long unsigned int*'} increases required alignment of target type". Use with -// care! -template -inline T reinterpret_cast_no_cast_align_warning(void* ptr) noexcept { - return reinterpret_cast(ptr); -} - -template -inline T reinterpret_cast_no_cast_align_warning(void const* ptr) noexcept { - return reinterpret_cast(ptr); -} - -// make sure this is not inlined as it is slow and dramatically enlarges code, thus making other -// inlinings more difficult. Throws are also generally the slow path. -template -[[noreturn]] ROBIN_HOOD(NOINLINE) -#if ROBIN_HOOD(HAS_EXCEPTIONS) - void doThrow(Args&&... args) { - throw E(std::forward(args)...); -} -#else - void doThrow(Args&&... ROBIN_HOOD_UNUSED(args) /*unused*/) { - abort(); -} -#endif - -template -T* assertNotNull(T* t, Args&&... args) { - if (ROBIN_HOOD_UNLIKELY(nullptr == t)) { - doThrow(std::forward(args)...); - } - return t; -} - -template -inline T unaligned_load(void const* ptr) noexcept { - // using memcpy so we don't get into unaligned load problems. - // compiler should optimize this very well anyways. - T t; - std::memcpy(&t, ptr, sizeof(T)); - return t; -} - -// Allocates bulks of memory for objects of type T. This deallocates the memory in the destructor, -// and keeps a linked list of the allocated memory around. Overhead per allocation is the size of a -// pointer. -template -class BulkPoolAllocator { -public: - BulkPoolAllocator() noexcept = default; - - // does not copy anything, just creates a new allocator. - BulkPoolAllocator(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept - : mHead(nullptr) - , mListForFree(nullptr) {} - - BulkPoolAllocator(BulkPoolAllocator&& o) noexcept - : mHead(o.mHead) - , mListForFree(o.mListForFree) { - o.mListForFree = nullptr; - o.mHead = nullptr; - } - - BulkPoolAllocator& operator=(BulkPoolAllocator&& o) noexcept { - reset(); - mHead = o.mHead; - mListForFree = o.mListForFree; - o.mListForFree = nullptr; - o.mHead = nullptr; - return *this; - } - - BulkPoolAllocator& - operator=(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept { - // does not do anything - return *this; - } - - ~BulkPoolAllocator() noexcept { - reset(); - } - - // Deallocates all allocated memory. - void reset() noexcept { - while (mListForFree) { - T* tmp = *mListForFree; - ROBIN_HOOD_LOG("std::free") - std::free(mListForFree); - mListForFree = reinterpret_cast_no_cast_align_warning(tmp); - } - mHead = nullptr; - } - - // allocates, but does NOT initialize. Use in-place new constructor, e.g. - // T* obj = pool.allocate(); - // ::new (static_cast(obj)) T(); - T* allocate() { - T* tmp = mHead; - if (!tmp) { - tmp = performAllocation(); - } - - mHead = *reinterpret_cast_no_cast_align_warning(tmp); - return tmp; - } - - // does not actually deallocate but puts it in store. - // make sure you have already called the destructor! e.g. with - // obj->~T(); - // pool.deallocate(obj); - void deallocate(T* obj) noexcept { - *reinterpret_cast_no_cast_align_warning(obj) = mHead; - mHead = obj; - } - - // Adds an already allocated block of memory to the allocator. This allocator is from now on - // responsible for freeing the data (with free()). If the provided data is not large enough to - // make use of, it is immediately freed. Otherwise it is reused and freed in the destructor. - void addOrFree(void* ptr, const size_t numBytes) noexcept { - // calculate number of available elements in ptr - if (numBytes < ALIGNMENT + ALIGNED_SIZE) { - // not enough data for at least one element. Free and return. - ROBIN_HOOD_LOG("std::free") - std::free(ptr); - } else { - ROBIN_HOOD_LOG("add to buffer") - add(ptr, numBytes); - } - } - - void swap(BulkPoolAllocator& other) noexcept { - using std::swap; - swap(mHead, other.mHead); - swap(mListForFree, other.mListForFree); - } - -private: - // iterates the list of allocated memory to calculate how many to alloc next. - // Recalculating this each time saves us a size_t member. - // This ignores the fact that memory blocks might have been added manually with addOrFree. In - // practice, this should not matter much. - ROBIN_HOOD(NODISCARD) size_t calcNumElementsToAlloc() const noexcept { - auto tmp = mListForFree; - size_t numAllocs = MinNumAllocs; - - while (numAllocs * 2 <= MaxNumAllocs && tmp) { - auto x = reinterpret_cast(tmp); // NOLINT - tmp = *x; - numAllocs *= 2; - } - - return numAllocs; - } - - // WARNING: Underflow if numBytes < ALIGNMENT! This is guarded in addOrFree(). - void add(void* ptr, const size_t numBytes) noexcept { - const size_t numElements = (numBytes - ALIGNMENT) / ALIGNED_SIZE; - auto data = reinterpret_cast(ptr); - - // link free list - auto x = reinterpret_cast(data); - *x = mListForFree; - mListForFree = data; - - // create linked list for newly allocated data - auto* const headT = - reinterpret_cast_no_cast_align_warning(reinterpret_cast(ptr) + ALIGNMENT); - - auto* const head = reinterpret_cast(headT); - - // Visual Studio compiler automatically unrolls this loop, which is pretty cool - for (size_t i = 0; i < numElements; ++i) { - *reinterpret_cast_no_cast_align_warning(head + i * ALIGNED_SIZE) = - head + (i + 1) * ALIGNED_SIZE; - } - - // last one points to 0 - *reinterpret_cast_no_cast_align_warning(head + (numElements - 1) * ALIGNED_SIZE) = - mHead; - mHead = headT; - } - - // Called when no memory is available (mHead == 0). - // Don't inline this slow path. - ROBIN_HOOD(NOINLINE) T* performAllocation() { - size_t const numElementsToAlloc = calcNumElementsToAlloc(); - - // alloc new memory: [prev |T, T, ... T] - size_t const bytes = ALIGNMENT + ALIGNED_SIZE * numElementsToAlloc; - ROBIN_HOOD_LOG("std::malloc " << bytes << " = " << ALIGNMENT << " + " << ALIGNED_SIZE - << " * " << numElementsToAlloc) - add(assertNotNull(std::malloc(bytes)), bytes); - return mHead; - } - - // enforce byte alignment of the T's -#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) - static constexpr size_t ALIGNMENT = - (std::max)(std::alignment_of::value, std::alignment_of::value); -#else - static const size_t ALIGNMENT = - (ROBIN_HOOD_STD::alignment_of::value > ROBIN_HOOD_STD::alignment_of::value) - ? ROBIN_HOOD_STD::alignment_of::value - : +ROBIN_HOOD_STD::alignment_of::value; // the + is for walkarround -#endif - - static constexpr size_t ALIGNED_SIZE = ((sizeof(T) - 1) / ALIGNMENT + 1) * ALIGNMENT; - - static_assert(MinNumAllocs >= 1, "MinNumAllocs"); - static_assert(MaxNumAllocs >= MinNumAllocs, "MaxNumAllocs"); - static_assert(ALIGNED_SIZE >= sizeof(T*), "ALIGNED_SIZE"); - static_assert(0 == (ALIGNED_SIZE % sizeof(T*)), "ALIGNED_SIZE mod"); - static_assert(ALIGNMENT >= sizeof(T*), "ALIGNMENT"); - - T* mHead{nullptr}; - T** mListForFree{nullptr}; -}; - -template -struct NodeAllocator; - -// dummy allocator that does nothing -template -struct NodeAllocator { - // we are not using the data, so just free it. - void addOrFree(void* ptr, size_t ROBIN_HOOD_UNUSED(numBytes)/*unused*/) noexcept { - ROBIN_HOOD_LOG("std::free") - std::free(ptr); - } -}; - -template -struct NodeAllocator : public BulkPoolAllocator {}; - -// c++14 doesn't have is_nothrow_swappable, and clang++ 6.0.1 doesn't like it either, so I'm making -// my own here. -namespace swappable { -#if ROBIN_HOOD(CXX) < ROBIN_HOOD(CXX17) -using std::swap; -template -struct nothrow { - static const bool value = noexcept(swap(std::declval(), std::declval())); -}; -#else -template -struct nothrow { - static const bool value = std::is_nothrow_swappable::value; -}; -#endif -} // namespace swappable - -} // namespace detail - -struct is_transparent_tag {}; - -// A custom pair implementation is used in the map because std::pair is not is_trivially_copyable, -// which means it would not be allowed to be used in std::memcpy. This struct is copiable, which is -// also tested. -template -struct pair { - using first_type = T1; - using second_type = T2; - - template ::value && - std::is_default_constructible::value>::type> - constexpr pair() noexcept(noexcept(U1()) && noexcept(U2())) - : first() - , second() {} - - // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. - explicit constexpr pair(std::pair const& o) noexcept( - noexcept(T1(std::declval())) && noexcept(T2(std::declval()))) - : first(o.first) - , second(o.second) {} - - // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. - explicit constexpr pair(std::pair&& o) noexcept(noexcept( - T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) - : first(std::move(o.first)) - , second(std::move(o.second)) {} - - constexpr pair(T1&& a, T2&& b) noexcept(noexcept( - T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) - : first(std::move(a)) - , second(std::move(b)) {} - - template - constexpr pair(U1&& a, U2&& b) noexcept(noexcept(T1(std::forward( - std::declval()))) && noexcept(T2(std::forward(std::declval())))) - : first(std::forward(a)) - , second(std::forward(b)) {} - - template - // MSVC 2015 produces error "C2476: ‘constexpr’ constructor does not initialize all members" - // if this constructor is constexpr -#if !ROBIN_HOOD(BROKEN_CONSTEXPR) - constexpr -#endif - pair(std::piecewise_construct_t /*unused*/, std::tuple a, - std::tuple - b) noexcept(noexcept(pair(std::declval&>(), - std::declval&>(), - ROBIN_HOOD_STD::index_sequence_for(), - ROBIN_HOOD_STD::index_sequence_for()))) - : pair(a, b, ROBIN_HOOD_STD::index_sequence_for(), - ROBIN_HOOD_STD::index_sequence_for()) { - } - - // constructor called from the std::piecewise_construct_t ctor - template - pair( - std::tuple& a, std::tuple& b, - ROBIN_HOOD_STD::index_sequence /*unused*/, - ROBIN_HOOD_STD::index_sequence /*unused*/) noexcept( - noexcept(T1(std::forward(std::get( - std::declval&>()))...)) && noexcept(T2(std:: - forward(std::get( - std::declval&>()))...))) - : first(std::forward(std::get(a))...) - , second(std::forward(std::get(b))...) { - // make visual studio compiler happy about warning about unused a & b. - // Visual studio's pair implementation disables warning 4100. - (void)a; - (void)b; - } - - void swap(pair& o) noexcept((detail::swappable::nothrow::value) && - (detail::swappable::nothrow::value)) { - using std::swap; - swap(first, o.first); - swap(second, o.second); - } - - T1 first; // NOLINT - T2 second; // NOLINT -}; - -template -inline void swap(pair& a, pair& b) noexcept( - noexcept(std::declval&>().swap(std::declval&>()))) { - a.swap(b); -} - -template -inline constexpr bool operator==(pair const& x, pair const& y) { - return (x.first == y.first) && (x.second == y.second); -} -template -inline constexpr bool operator!=(pair const& x, pair const& y) { - return !(x == y); -} -template -inline constexpr bool operator<(pair const& x, pair const& y) noexcept(noexcept( - std::declval() < std::declval()) && noexcept(std::declval() < - std::declval())) { - return x.first < y.first || (!(y.first < x.first) && x.second < y.second); -} -template -inline constexpr bool operator>(pair const& x, pair const& y) { - return y < x; -} -template -inline constexpr bool operator<=(pair const& x, pair const& y) { - return !(x > y); -} -template -inline constexpr bool operator>=(pair const& x, pair const& y) { - return !(x < y); -} - -inline size_t hash_bytes(void const* ptr, size_t len) noexcept { - static constexpr uint64_t m = UINT64_C(0xc6a4a7935bd1e995); - static constexpr uint64_t seed = UINT64_C(0xe17a1465); - static constexpr unsigned int r = 47; - - auto const* const data64 = static_cast(ptr); - uint64_t h = seed ^ (len * m); - - size_t const n_blocks = len / 8; - for (size_t i = 0; i < n_blocks; ++i) { - auto k = detail::unaligned_load(data64 + i); - - k *= m; - k ^= k >> r; - k *= m; - - h ^= k; - h *= m; - } - - auto const* const data8 = reinterpret_cast(data64 + n_blocks); - switch (len & 7U) { - case 7: - h ^= static_cast(data8[6]) << 48U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 6: - h ^= static_cast(data8[5]) << 40U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 5: - h ^= static_cast(data8[4]) << 32U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 4: - h ^= static_cast(data8[3]) << 24U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 3: - h ^= static_cast(data8[2]) << 16U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 2: - h ^= static_cast(data8[1]) << 8U; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - case 1: - h ^= static_cast(data8[0]); - h *= m; - ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH - default: - break; - } - - h ^= h >> r; - - // not doing the final step here, because this will be done by keyToIdx anyways - // h *= m; - // h ^= h >> r; - return static_cast(h); -} - -inline size_t hash_int(uint64_t x) noexcept { - // tried lots of different hashes, let's stick with murmurhash3. It's simple, fast, well tested, - // and doesn't need any special 128bit operations. - x ^= x >> 33U; - x *= UINT64_C(0xff51afd7ed558ccd); - x ^= x >> 33U; - - // not doing the final step here, because this will be done by keyToIdx anyways - // x *= UINT64_C(0xc4ceb9fe1a85ec53); - // x ^= x >> 33U; - return static_cast(x); -} - -// A thin wrapper around std::hash, performing an additional simple mixing step of the result. -template -struct hash : public std::hash { - size_t operator()(T const& obj) const - noexcept(noexcept(std::declval>().operator()(std::declval()))) { - // call base hash - auto result = std::hash::operator()(obj); - // return mixed of that, to be save against identity has - return hash_int(static_cast(result)); - } -}; - -template -struct hash> { - size_t operator()(std::basic_string const& str) const noexcept { - return hash_bytes(str.data(), sizeof(CharT) * str.size()); - } -}; - -#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) -template -struct hash> { - size_t operator()(std::basic_string_view const& sv) const noexcept { - return hash_bytes(sv.data(), sizeof(CharT) * sv.size()); - } -}; -#endif - -template -struct hash { - size_t operator()(T* ptr) const noexcept { - return hash_int(reinterpret_cast(ptr)); - } -}; - -template -struct hash> { - size_t operator()(std::unique_ptr const& ptr) const noexcept { - return hash_int(reinterpret_cast(ptr.get())); - } -}; - -template -struct hash> { - size_t operator()(std::shared_ptr const& ptr) const noexcept { - return hash_int(reinterpret_cast(ptr.get())); - } -}; - -template -struct hash::value>::type> { - size_t operator()(Enum e) const noexcept { - using Underlying = typename std::underlying_type::type; - return hash{}(static_cast(e)); - } -}; - -#define ROBIN_HOOD_HASH_INT(T) \ - template <> \ - struct hash { \ - size_t operator()(T const& obj) const noexcept { \ - return hash_int(static_cast(obj)); \ - } \ - } - -#if defined(__GNUC__) && !defined(__clang__) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wuseless-cast" -#endif -// see https://en.cppreference.com/w/cpp/utility/hash -ROBIN_HOOD_HASH_INT(bool); -ROBIN_HOOD_HASH_INT(char); -ROBIN_HOOD_HASH_INT(signed char); -ROBIN_HOOD_HASH_INT(unsigned char); -ROBIN_HOOD_HASH_INT(char16_t); -ROBIN_HOOD_HASH_INT(char32_t); -#if ROBIN_HOOD(HAS_NATIVE_WCHART) -ROBIN_HOOD_HASH_INT(wchar_t); -#endif -ROBIN_HOOD_HASH_INT(short); // NOLINT -ROBIN_HOOD_HASH_INT(unsigned short); // NOLINT -ROBIN_HOOD_HASH_INT(int); -ROBIN_HOOD_HASH_INT(unsigned int); -ROBIN_HOOD_HASH_INT(long); // NOLINT -ROBIN_HOOD_HASH_INT(long long); // NOLINT -ROBIN_HOOD_HASH_INT(unsigned long); // NOLINT -ROBIN_HOOD_HASH_INT(unsigned long long); // NOLINT -#if defined(__GNUC__) && !defined(__clang__) -# pragma GCC diagnostic pop -#endif -namespace detail { - -template -struct void_type { - using type = void; -}; - -template -struct has_is_transparent : public std::false_type {}; - -template -struct has_is_transparent::type> - : public std::true_type {}; - -// using wrapper classes for hash and key_equal prevents the diamond problem when the same type -// is used. see https://stackoverflow.com/a/28771920/48181 -template -struct WrapHash : public T { - WrapHash() = default; - explicit WrapHash(T const& o) noexcept(noexcept(T(std::declval()))) - : T(o) {} -}; - -template -struct WrapKeyEqual : public T { - WrapKeyEqual() = default; - explicit WrapKeyEqual(T const& o) noexcept(noexcept(T(std::declval()))) - : T(o) {} -}; - -// A highly optimized hashmap implementation, using the Robin Hood algorithm. -// -// In most cases, this map should be usable as a drop-in replacement for std::unordered_map, but -// be about 2x faster in most cases and require much less allocations. -// -// This implementation uses the following memory layout: -// -// [Node, Node, ... Node | info, info, ... infoSentinel ] -// -// * Node: either a DataNode that directly has the std::pair as member, -// or a DataNode with a pointer to std::pair. Which DataNode representation to use -// depends on how fast the swap() operation is. Heuristically, this is automatically chosen -// based on sizeof(). there are always 2^n Nodes. -// -// * info: Each Node in the map has a corresponding info byte, so there are 2^n info bytes. -// Each byte is initialized to 0, meaning the corresponding Node is empty. Set to 1 means the -// corresponding node contains data. Set to 2 means the corresponding Node is filled, but it -// actually belongs to the previous position and was pushed out because that place is already -// taken. -// -// * infoSentinel: Sentinel byte set to 1, so that iterator's ++ can stop at end() without the -// need for a idx variable. -// -// According to STL, order of templates has effect on throughput. That's why I've moved the -// boolean to the front. -// https://www.reddit.com/r/cpp/comments/ahp6iu/compile_time_binary_size_reductions_and_cs_future/eeguck4/ -template -class Table - : public WrapHash, - public WrapKeyEqual, - detail::NodeAllocator< - typename std::conditional< - std::is_void::value, Key, - robin_hood::pair::type, T>>::type, - 4, 16384, IsFlat> { -public: - static constexpr bool is_flat = IsFlat; - static constexpr bool is_map = !std::is_void::value; - static constexpr bool is_set = !is_map; - static constexpr bool is_transparent = - has_is_transparent::value && has_is_transparent::value; - - using key_type = Key; - using mapped_type = T; - using value_type = typename std::conditional< - is_set, Key, - robin_hood::pair::type, T>>::type; - using size_type = size_t; - using hasher = Hash; - using key_equal = KeyEqual; - using Self = Table; - -private: - static_assert(MaxLoadFactor100 > 10 && MaxLoadFactor100 < 100, - "MaxLoadFactor100 needs to be >10 && < 100"); - - using WHash = WrapHash; - using WKeyEqual = WrapKeyEqual; - - // configuration defaults - - // make sure we have 8 elements, needed to quickly rehash mInfo - static constexpr size_t InitialNumElements = sizeof(uint64_t); - static constexpr uint32_t InitialInfoNumBits = 5; - static constexpr uint8_t InitialInfoInc = 1U << InitialInfoNumBits; - static constexpr size_t InfoMask = InitialInfoInc - 1U; - static constexpr uint8_t InitialInfoHashShift = 0; - using DataPool = detail::NodeAllocator; - - // type needs to be wider than uint8_t. - using InfoType = uint32_t; - - // DataNode //////////////////////////////////////////////////////// - - // Primary template for the data node. We have special implementations for small and big - // objects. For large objects it is assumed that swap() is fairly slow, so we allocate these - // on the heap so swap merely swaps a pointer. - template - class DataNode {}; - - // Small: just allocate on the stack. - template - class DataNode final { - public: - template - explicit DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, Args&&... args) noexcept( - noexcept(value_type(std::forward(args)...))) - : mData(std::forward(args)...) {} - - DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept( - std::is_nothrow_move_constructible::value) - : mData(std::move(n.mData)) {} - - // doesn't do anything - void destroy(M& ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {} - void destroyDoNotDeallocate() noexcept {} - - value_type const* operator->() const noexcept { - return &mData; - } - value_type* operator->() noexcept { - return &mData; - } - - const value_type& operator*() const noexcept { - return mData; - } - - value_type& operator*() noexcept { - return mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() noexcept { - return mData.first; - } - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() noexcept { - return mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type - getFirst() const noexcept { - return mData.first; - } - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() const noexcept { - return mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getSecond() noexcept { - return mData.second; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getSecond() const noexcept { - return mData.second; - } - - void swap(DataNode& o) noexcept( - noexcept(std::declval().swap(std::declval()))) { - mData.swap(o.mData); - } - - private: - value_type mData; - }; - - // big object: allocate on heap. - template - class DataNode { - public: - template - explicit DataNode(M& map, Args&&... args) - : mData(map.allocate()) { - ::new (static_cast(mData)) value_type(std::forward(args)...); - } - - DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept - : mData(std::move(n.mData)) {} - - void destroy(M& map) noexcept { - // don't deallocate, just put it into list of datapool. - mData->~value_type(); - map.deallocate(mData); - } - - void destroyDoNotDeallocate() noexcept { - mData->~value_type(); - } - - value_type const* operator->() const noexcept { - return mData; - } - - value_type* operator->() noexcept { - return mData; - } - - const value_type& operator*() const { - return *mData; - } - - value_type& operator*() { - return *mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() noexcept { - return mData->first; - } - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() noexcept { - return *mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type - getFirst() const noexcept { - return mData->first; - } - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getFirst() const noexcept { - return *mData; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getSecond() noexcept { - return mData->second; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::type getSecond() const noexcept { - return mData->second; - } - - void swap(DataNode& o) noexcept { - using std::swap; - swap(mData, o.mData); - } - - private: - value_type* mData; - }; - - using Node = DataNode; - - // helpers for insertKeyPrepareEmptySpot: extract first entry (only const required) - ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(Node const& n) const noexcept { - return n.getFirst(); - } - - // in case we have void mapped_type, we are not using a pair, thus we just route k through. - // No need to disable this because it's just not used if not applicable. - ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(key_type const& k) const noexcept { - return k; - } - - // in case we have non-void mapped_type, we have a standard robin_hood::pair - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::value, key_type const&>::type - getFirstConst(value_type const& vt) const noexcept { - return vt.first; - } - - // Cloner ////////////////////////////////////////////////////////// - - template - struct Cloner; - - // fast path: Just copy data, without allocating anything. - template - struct Cloner { - void operator()(M const& source, M& target) const { - auto const* const src = reinterpret_cast(source.mKeyVals); - auto* tgt = reinterpret_cast(target.mKeyVals); - auto const numElementsWithBuffer = target.calcNumElementsWithBuffer(target.mMask + 1); - std::copy(src, src + target.calcNumBytesTotal(numElementsWithBuffer), tgt); - } - }; - - template - struct Cloner { - void operator()(M const& s, M& t) const { - auto const numElementsWithBuffer = t.calcNumElementsWithBuffer(t.mMask + 1); - std::copy(s.mInfo, s.mInfo + t.calcNumBytesInfo(numElementsWithBuffer), t.mInfo); - - for (size_t i = 0; i < numElementsWithBuffer; ++i) { - if (t.mInfo[i]) { - ::new (static_cast(t.mKeyVals + i)) Node(t, *s.mKeyVals[i]); - } - } - } - }; - - // Destroyer /////////////////////////////////////////////////////// - - template - struct Destroyer {}; - - template - struct Destroyer { - void nodes(M& m) const noexcept { - m.mNumElements = 0; - } - - void nodesDoNotDeallocate(M& m) const noexcept { - m.mNumElements = 0; - } - }; - - template - struct Destroyer { - void nodes(M& m) const noexcept { - m.mNumElements = 0; - // clear also resets mInfo to 0, that's sometimes not necessary. - auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); - - for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { - if (0 != m.mInfo[idx]) { - Node& n = m.mKeyVals[idx]; - n.destroy(m); - n.~Node(); - } - } - } - - void nodesDoNotDeallocate(M& m) const noexcept { - m.mNumElements = 0; - // clear also resets mInfo to 0, that's sometimes not necessary. - auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); - for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { - if (0 != m.mInfo[idx]) { - Node& n = m.mKeyVals[idx]; - n.destroyDoNotDeallocate(); - n.~Node(); - } - } - } - }; - - // Iter //////////////////////////////////////////////////////////// - - struct fast_forward_tag {}; - - // generic iterator for both const_iterator and iterator. - template - class Iter { - private: - using NodePtr = typename std::conditional::type; - - public: - using difference_type = std::ptrdiff_t; - using value_type = typename Self::value_type; - using reference = typename std::conditional::type; - using pointer = typename std::conditional::type; - using iterator_category = std::forward_iterator_tag; - - // default constructed iterator can be compared to itself, but WON'T return true when - // compared to end(). - Iter() = default; - - // Rule of zero: nothing specified. The conversion constructor is only enabled for - // iterator to const_iterator, so it doesn't accidentally work as a copy ctor. - - // Conversion constructor from iterator to const_iterator. - template ::type> - Iter(Iter const& other) noexcept - : mKeyVals(other.mKeyVals) - , mInfo(other.mInfo) {} - - Iter(NodePtr valPtr, uint8_t const* infoPtr) noexcept - : mKeyVals(valPtr) - , mInfo(infoPtr) {} - - Iter(NodePtr valPtr, uint8_t const* infoPtr, - fast_forward_tag ROBIN_HOOD_UNUSED(tag) /*unused*/) noexcept - : mKeyVals(valPtr) - , mInfo(infoPtr) { - fastForward(); - } - - template ::type> - Iter& operator=(Iter const& other) noexcept { - mKeyVals = other.mKeyVals; - mInfo = other.mInfo; - return *this; - } - - // prefix increment. Undefined behavior if we are at end()! - Iter& operator++() noexcept { - mInfo++; - mKeyVals++; - fastForward(); - return *this; - } - - Iter operator++(int) noexcept { - Iter tmp = *this; - ++(*this); - return tmp; - } - - reference operator*() const { - return **mKeyVals; - } - - pointer operator->() const { - return &**mKeyVals; - } - - template - bool operator==(Iter const& o) const noexcept { - return mKeyVals == o.mKeyVals; - } - - template - bool operator!=(Iter const& o) const noexcept { - return mKeyVals != o.mKeyVals; - } - - private: - // fast forward to the next non-free info byte - // I've tried a few variants that don't depend on intrinsics, but unfortunately they are - // quite a bit slower than this one. So I've reverted that change again. See map_benchmark. - void fastForward() noexcept { - size_t n = 0; - while (0U == (n = detail::unaligned_load(mInfo))) { - mInfo += sizeof(size_t); - mKeyVals += sizeof(size_t); - } -#if defined(ROBIN_HOOD_DISABLE_INTRINSICS) - // we know for certain that within the next 8 bytes we'll find a non-zero one. - if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { - mInfo += 4; - mKeyVals += 4; - } - if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { - mInfo += 2; - mKeyVals += 2; - } - if (ROBIN_HOOD_UNLIKELY(0U == *mInfo)) { - mInfo += 1; - mKeyVals += 1; - } -#else -# if ROBIN_HOOD(LITTLE_ENDIAN) - auto inc = ROBIN_HOOD_COUNT_TRAILING_ZEROES(n) / 8; -# else - auto inc = ROBIN_HOOD_COUNT_LEADING_ZEROES(n) / 8; -# endif - mInfo += inc; - mKeyVals += inc; -#endif - } - - friend class Table; - NodePtr mKeyVals{nullptr}; - uint8_t const* mInfo{nullptr}; - }; - - //////////////////////////////////////////////////////////////////// - - // highly performance relevant code. - // Lower bits are used for indexing into the array (2^n size) - // The upper 1-5 bits need to be a reasonable good hash, to save comparisons. - template - void keyToIdx(HashKey&& key, size_t* idx, InfoType* info) const { - // In addition to whatever hash is used, add another mul & shift so we get better hashing. - // This serves as a bad hash prevention, if the given data is - // badly mixed. - auto h = static_cast(WHash::operator()(key)); - - h *= mHashMultiplier; - h ^= h >> 33U; - - // the lower InitialInfoNumBits are reserved for info. - *info = mInfoInc + static_cast((h & InfoMask) >> mInfoHashShift); - *idx = (static_cast(h) >> InitialInfoNumBits) & mMask; - } - - // forwards the index by one, wrapping around at the end - void next(InfoType* info, size_t* idx) const noexcept { - *idx = *idx + 1; - *info += mInfoInc; - } - - void nextWhileLess(InfoType* info, size_t* idx) const noexcept { - // unrolling this by hand did not bring any speedups. - while (*info < mInfo[*idx]) { - next(info, idx); - } - } - - // Shift everything up by one element. Tries to move stuff around. - void - shiftUp(size_t startIdx, - size_t const insertion_idx) noexcept(std::is_nothrow_move_assignable::value) { - auto idx = startIdx; - ::new (static_cast(mKeyVals + idx)) Node(std::move(mKeyVals[idx - 1])); - while (--idx != insertion_idx) { - mKeyVals[idx] = std::move(mKeyVals[idx - 1]); - } - - idx = startIdx; - while (idx != insertion_idx) { - ROBIN_HOOD_COUNT(shiftUp) - mInfo[idx] = static_cast(mInfo[idx - 1] + mInfoInc); - if (ROBIN_HOOD_UNLIKELY(mInfo[idx] + mInfoInc > 0xFF)) { - mMaxNumElementsAllowed = 0; - } - --idx; - } - } - - void shiftDown(size_t idx) noexcept(std::is_nothrow_move_assignable::value) { - // until we find one that is either empty or has zero offset. - // TODO(martinus) we don't need to move everything, just the last one for the same - // bucket. - mKeyVals[idx].destroy(*this); - - // until we find one that is either empty or has zero offset. - while (mInfo[idx + 1] >= 2 * mInfoInc) { - ROBIN_HOOD_COUNT(shiftDown) - mInfo[idx] = static_cast(mInfo[idx + 1] - mInfoInc); - mKeyVals[idx] = std::move(mKeyVals[idx + 1]); - ++idx; - } - - mInfo[idx] = 0; - // don't destroy, we've moved it - // mKeyVals[idx].destroy(*this); - mKeyVals[idx].~Node(); - } - - // copy of find(), except that it returns iterator instead of const_iterator. - template - ROBIN_HOOD(NODISCARD) - size_t findIdx(Other const& key) const { - size_t idx{}; - InfoType info{}; - keyToIdx(key, &idx, &info); - - do { - // unrolling this twice gives a bit of a speedup. More unrolling did not help. - if (info == mInfo[idx] && - ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { - return idx; - } - next(&info, &idx); - if (info == mInfo[idx] && - ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { - return idx; - } - next(&info, &idx); - } while (info <= mInfo[idx]); - - // nothing found! - return mMask == 0 ? 0 - : static_cast(std::distance( - mKeyVals, reinterpret_cast_no_cast_align_warning(mInfo))); - } - - void cloneData(const Table& o) { - Cloner()(o, *this); - } - - // inserts a keyval that is guaranteed to be new, e.g. when the hashmap is resized. - // @return True on success, false if something went wrong - void insert_move(Node&& keyval) { - // we don't retry, fail if overflowing - // don't need to check max num elements - if (0 == mMaxNumElementsAllowed && !try_increase_info()) { - throwOverflowError(); - } - - size_t idx{}; - InfoType info{}; - keyToIdx(keyval.getFirst(), &idx, &info); - - // skip forward. Use <= because we are certain that the element is not there. - while (info <= mInfo[idx]) { - idx = idx + 1; - info += mInfoInc; - } - - // key not found, so we are now exactly where we want to insert it. - auto const insertion_idx = idx; - auto const insertion_info = static_cast(info); - if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { - mMaxNumElementsAllowed = 0; - } - - // find an empty spot - while (0 != mInfo[idx]) { - next(&info, &idx); - } - - auto& l = mKeyVals[insertion_idx]; - if (idx == insertion_idx) { - ::new (static_cast(&l)) Node(std::move(keyval)); - } else { - shiftUp(idx, insertion_idx); - l = std::move(keyval); - } - - // put at empty spot - mInfo[insertion_idx] = insertion_info; - - ++mNumElements; - } - -public: - using iterator = Iter; - using const_iterator = Iter; - - Table() noexcept(noexcept(Hash()) && noexcept(KeyEqual())) - : WHash() - , WKeyEqual() { - ROBIN_HOOD_TRACE(this) - } - - // Creates an empty hash map. Nothing is allocated yet, this happens at the first insert. - // This tremendously speeds up ctor & dtor of a map that never receives an element. The - // penalty is paid at the first insert, and not before. Lookup of this empty map works - // because everybody points to DummyInfoByte::b. parameter bucket_count is dictated by the - // standard, but we can ignore it. - explicit Table( - size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/, const Hash& h = Hash{}, - const KeyEqual& equal = KeyEqual{}) noexcept(noexcept(Hash(h)) && noexcept(KeyEqual(equal))) - : WHash(h) - , WKeyEqual(equal) { - ROBIN_HOOD_TRACE(this) - } - - template - Table(Iter first, Iter last, size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, - const Hash& h = Hash{}, const KeyEqual& equal = KeyEqual{}) - : WHash(h) - , WKeyEqual(equal) { - ROBIN_HOOD_TRACE(this) - insert(first, last); - } - - Table(std::initializer_list initlist, - size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, const Hash& h = Hash{}, - const KeyEqual& equal = KeyEqual{}) - : WHash(h) - , WKeyEqual(equal) { - ROBIN_HOOD_TRACE(this) - insert(initlist.begin(), initlist.end()); - } - - Table(Table&& o) noexcept - : WHash(std::move(static_cast(o))) - , WKeyEqual(std::move(static_cast(o))) - , DataPool(std::move(static_cast(o))) { - ROBIN_HOOD_TRACE(this) - if (o.mMask) { - mHashMultiplier = std::move(o.mHashMultiplier); - mKeyVals = std::move(o.mKeyVals); - mInfo = std::move(o.mInfo); - mNumElements = std::move(o.mNumElements); - mMask = std::move(o.mMask); - mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); - mInfoInc = std::move(o.mInfoInc); - mInfoHashShift = std::move(o.mInfoHashShift); - // set other's mask to 0 so its destructor won't do anything - o.init(); - } - } - - Table& operator=(Table&& o) noexcept { - ROBIN_HOOD_TRACE(this) - if (&o != this) { - if (o.mMask) { - // only move stuff if the other map actually has some data - destroy(); - mHashMultiplier = std::move(o.mHashMultiplier); - mKeyVals = std::move(o.mKeyVals); - mInfo = std::move(o.mInfo); - mNumElements = std::move(o.mNumElements); - mMask = std::move(o.mMask); - mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); - mInfoInc = std::move(o.mInfoInc); - mInfoHashShift = std::move(o.mInfoHashShift); - WHash::operator=(std::move(static_cast(o))); - WKeyEqual::operator=(std::move(static_cast(o))); - DataPool::operator=(std::move(static_cast(o))); - - o.init(); - - } else { - // nothing in the other map => just clear us. - clear(); - } - } - return *this; - } - - Table(const Table& o) - : WHash(static_cast(o)) - , WKeyEqual(static_cast(o)) - , DataPool(static_cast(o)) { - ROBIN_HOOD_TRACE(this) - if (!o.empty()) { - // not empty: create an exact copy. it is also possible to just iterate through all - // elements and insert them, but copying is probably faster. - - auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); - auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); - - ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" - << numElementsWithBuffer << ")") - mHashMultiplier = o.mHashMultiplier; - mKeyVals = static_cast( - detail::assertNotNull(std::malloc(numBytesTotal))); - // no need for calloc because clonData does memcpy - mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); - mNumElements = o.mNumElements; - mMask = o.mMask; - mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; - mInfoInc = o.mInfoInc; - mInfoHashShift = o.mInfoHashShift; - cloneData(o); - } - } - - // Creates a copy of the given map. Copy constructor of each entry is used. - // Not sure why clang-tidy thinks this doesn't handle self assignment, it does - Table& operator=(Table const& o) { - ROBIN_HOOD_TRACE(this) - if (&o == this) { - // prevent assigning of itself - return *this; - } - - // we keep using the old allocator and not assign the new one, because we want to keep - // the memory available. when it is the same size. - if (o.empty()) { - if (0 == mMask) { - // nothing to do, we are empty too - return *this; - } - - // not empty: destroy what we have there - // clear also resets mInfo to 0, that's sometimes not necessary. - destroy(); - init(); - WHash::operator=(static_cast(o)); - WKeyEqual::operator=(static_cast(o)); - DataPool::operator=(static_cast(o)); - - return *this; - } - - // clean up old stuff - Destroyer::value>{}.nodes(*this); - - if (mMask != o.mMask) { - // no luck: we don't have the same array size allocated, so we need to realloc. - if (0 != mMask) { - // only deallocate if we actually have data! - ROBIN_HOOD_LOG("std::free") - std::free(mKeyVals); - } - - auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); - auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); - ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" - << numElementsWithBuffer << ")") - mKeyVals = static_cast( - detail::assertNotNull(std::malloc(numBytesTotal))); - - // no need for calloc here because cloneData performs a memcpy. - mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); - // sentinel is set in cloneData - } - WHash::operator=(static_cast(o)); - WKeyEqual::operator=(static_cast(o)); - DataPool::operator=(static_cast(o)); - mHashMultiplier = o.mHashMultiplier; - mNumElements = o.mNumElements; - mMask = o.mMask; - mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; - mInfoInc = o.mInfoInc; - mInfoHashShift = o.mInfoHashShift; - cloneData(o); - - return *this; - } - - // Swaps everything between the two maps. - void swap(Table& o) { - ROBIN_HOOD_TRACE(this) - using std::swap; - swap(o, *this); - } - - // Clears all data, without resizing. - void clear() { - ROBIN_HOOD_TRACE(this) - if (empty()) { - // don't do anything! also important because we don't want to write to - // DummyInfoByte::b, even though we would just write 0 to it. - return; - } - - Destroyer::value>{}.nodes(*this); - - auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); - // clear everything, then set the sentinel again - uint8_t const z = 0; - std::fill(mInfo, mInfo + calcNumBytesInfo(numElementsWithBuffer), z); - mInfo[numElementsWithBuffer] = 1; - - mInfoInc = InitialInfoInc; - mInfoHashShift = InitialInfoHashShift; - } - - // Destroys the map and all it's contents. - ~Table() { - ROBIN_HOOD_TRACE(this) - destroy(); - } - - // Checks if both tables contain the same entries. Order is irrelevant. - bool operator==(const Table& other) const { - ROBIN_HOOD_TRACE(this) - if (other.size() != size()) { - return false; - } - for (auto const& otherEntry : other) { - if (!has(otherEntry)) { - return false; - } - } - - return true; - } - - bool operator!=(const Table& other) const { - ROBIN_HOOD_TRACE(this) - return !operator==(other); - } - - template - typename std::enable_if::value, Q&>::type operator[](const key_type& key) { - ROBIN_HOOD_TRACE(this) - auto idxAndState = insertKeyPrepareEmptySpot(key); - switch (idxAndState.second) { - case InsertionState::key_found: - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) - Node(*this, std::piecewise_construct, std::forward_as_tuple(key), - std::forward_as_tuple()); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, - std::forward_as_tuple(key), std::forward_as_tuple()); - break; - - case InsertionState::overflow_error: - throwOverflowError(); - } - - return mKeyVals[idxAndState.first].getSecond(); - } - - template - typename std::enable_if::value, Q&>::type operator[](key_type&& key) { - ROBIN_HOOD_TRACE(this) - auto idxAndState = insertKeyPrepareEmptySpot(key); - switch (idxAndState.second) { - case InsertionState::key_found: - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) - Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), - std::forward_as_tuple()); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = - Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), - std::forward_as_tuple()); - break; - - case InsertionState::overflow_error: - throwOverflowError(); - } - - return mKeyVals[idxAndState.first].getSecond(); - } - - template - void insert(Iter first, Iter last) { - for (; first != last; ++first) { - // value_type ctor needed because this might be called with std::pair's - insert(value_type(*first)); - } - } - - void insert(std::initializer_list ilist) { - for (auto&& vt : ilist) { - insert(std::move(vt)); - } - } - - template - std::pair emplace(Args&&... args) { - ROBIN_HOOD_TRACE(this) - Node n{*this, std::forward(args)...}; - auto idxAndState = insertKeyPrepareEmptySpot(getFirstConst(n)); - switch (idxAndState.second) { - case InsertionState::key_found: - n.destroy(*this); - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) Node(*this, std::move(n)); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = std::move(n); - break; - - case InsertionState::overflow_error: - n.destroy(*this); - throwOverflowError(); - break; - } - - return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), - InsertionState::key_found != idxAndState.second); - } - - template - iterator emplace_hint(const_iterator position, Args&&... args) { - (void)position; - return emplace(std::forward(args)...).first; - } - - template - std::pair try_emplace(const key_type& key, Args&&... args) { - return try_emplace_impl(key, std::forward(args)...); - } - - template - std::pair try_emplace(key_type&& key, Args&&... args) { - return try_emplace_impl(std::move(key), std::forward(args)...); - } - - template - iterator try_emplace(const_iterator hint, const key_type& key, Args&&... args) { - (void)hint; - return try_emplace_impl(key, std::forward(args)...).first; - } - - template - iterator try_emplace(const_iterator hint, key_type&& key, Args&&... args) { - (void)hint; - return try_emplace_impl(std::move(key), std::forward(args)...).first; - } - - template - std::pair insert_or_assign(const key_type& key, Mapped&& obj) { - return insertOrAssignImpl(key, std::forward(obj)); - } - - template - std::pair insert_or_assign(key_type&& key, Mapped&& obj) { - return insertOrAssignImpl(std::move(key), std::forward(obj)); - } - - template - iterator insert_or_assign(const_iterator hint, const key_type& key, Mapped&& obj) { - (void)hint; - return insertOrAssignImpl(key, std::forward(obj)).first; - } - - template - iterator insert_or_assign(const_iterator hint, key_type&& key, Mapped&& obj) { - (void)hint; - return insertOrAssignImpl(std::move(key), std::forward(obj)).first; - } - - std::pair insert(const value_type& keyval) { - ROBIN_HOOD_TRACE(this) - return emplace(keyval); - } - - iterator insert(const_iterator hint, const value_type& keyval) { - (void)hint; - return emplace(keyval).first; - } - - std::pair insert(value_type&& keyval) { - return emplace(std::move(keyval)); - } - - iterator insert(const_iterator hint, value_type&& keyval) { - (void)hint; - return emplace(std::move(keyval)).first; - } - - // Returns 1 if key is found, 0 otherwise. - size_t count(const key_type& key) const { // NOLINT - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { - return 1; - } - return 0; - } - - template - typename std::enable_if::type count(const OtherKey& key) const { - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { - return 1; - } - return 0; - } - - bool contains(const key_type& key) const { // NOLINT - return 1U == count(key); - } - - template - typename std::enable_if::type contains(const OtherKey& key) const { - return 1U == count(key); - } - - // Returns a reference to the value found for key. - // Throws std::out_of_range if element cannot be found - template - typename std::enable_if::value, Q&>::type at(key_type const& key) { - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { - doThrow("key not found"); - } - return kv->getSecond(); - } - - // Returns a reference to the value found for key. - // Throws std::out_of_range if element cannot be found - template - typename std::enable_if::value, Q const&>::type at(key_type const& key) const { - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { - doThrow("key not found"); - } - return kv->getSecond(); - } - - const_iterator find(const key_type& key) const { // NOLINT - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return const_iterator{mKeyVals + idx, mInfo + idx}; - } - - template - const_iterator find(const OtherKey& key, is_transparent_tag /*unused*/) const { - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return const_iterator{mKeyVals + idx, mInfo + idx}; - } - - template - typename std::enable_if::type // NOLINT - find(const OtherKey& key) const { // NOLINT - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return const_iterator{mKeyVals + idx, mInfo + idx}; - } - - iterator find(const key_type& key) { - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return iterator{mKeyVals + idx, mInfo + idx}; - } - - template - iterator find(const OtherKey& key, is_transparent_tag/*unused*/) { - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return iterator{mKeyVals + idx, mInfo + idx}; - } - - template - typename std::enable_if::type find(const OtherKey& key) { - ROBIN_HOOD_TRACE(this) - const size_t idx = findIdx(key); - return iterator{mKeyVals + idx, mInfo + idx}; - } - - iterator begin() { - ROBIN_HOOD_TRACE(this) - if (empty()) { - return end(); - } - return iterator(mKeyVals, mInfo, fast_forward_tag{}); - } - const_iterator begin() const { // NOLINT - ROBIN_HOOD_TRACE(this) - return cbegin(); - } - const_iterator cbegin() const { // NOLINT - ROBIN_HOOD_TRACE(this) - if (empty()) { - return cend(); - } - return const_iterator(mKeyVals, mInfo, fast_forward_tag{}); - } - - iterator end() { - ROBIN_HOOD_TRACE(this) - // no need to supply valid info pointer: end() must not be dereferenced, and only node - // pointer is compared. - return iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; - } - const_iterator end() const { // NOLINT - ROBIN_HOOD_TRACE(this) - return cend(); - } - const_iterator cend() const { // NOLINT - ROBIN_HOOD_TRACE(this) - return const_iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; - } - - iterator erase(const_iterator pos) { - ROBIN_HOOD_TRACE(this) - // its safe to perform const cast here - return erase(iterator{const_cast(pos.mKeyVals), const_cast(pos.mInfo)}); - } - - // Erases element at pos, returns iterator to the next element. - iterator erase(iterator pos) { - ROBIN_HOOD_TRACE(this) - // we assume that pos always points to a valid entry, and not end(). - auto const idx = static_cast(pos.mKeyVals - mKeyVals); - - shiftDown(idx); - --mNumElements; - - if (*pos.mInfo) { - // we've backward shifted, return this again - return pos; - } - - // no backward shift, return next element - return ++pos; - } - - size_t erase(const key_type& key) { - ROBIN_HOOD_TRACE(this) - size_t idx{}; - InfoType info{}; - keyToIdx(key, &idx, &info); - - // check while info matches with the source idx - do { - if (info == mInfo[idx] && WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { - shiftDown(idx); - --mNumElements; - return 1; - } - next(&info, &idx); - } while (info <= mInfo[idx]); - - // nothing found to delete - return 0; - } - - // reserves space for the specified number of elements. Makes sure the old data fits. - // exactly the same as reserve(c). - void rehash(size_t c) { - // forces a reserve - reserve(c, true); - } - - // reserves space for the specified number of elements. Makes sure the old data fits. - // Exactly the same as rehash(c). Use rehash(0) to shrink to fit. - void reserve(size_t c) { - // reserve, but don't force rehash - reserve(c, false); - } - - // If possible reallocates the map to a smaller one. This frees the underlying table. - // Does not do anything if load_factor is too large for decreasing the table's size. - void compact() { - ROBIN_HOOD_TRACE(this) - auto newSize = InitialNumElements; - while (calcMaxNumElementsAllowed(newSize) < mNumElements && newSize != 0) { - newSize *= 2; - } - if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { - throwOverflowError(); - } - - ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") - - // only actually do anything when the new size is bigger than the old one. This prevents to - // continuously allocate for each reserve() call. - if (newSize < mMask + 1) { - rehashPowerOfTwo(newSize, true); - } - } - - size_type size() const noexcept { // NOLINT - ROBIN_HOOD_TRACE(this) - return mNumElements; - } - - size_type max_size() const noexcept { // NOLINT - ROBIN_HOOD_TRACE(this) - return static_cast(-1); - } - - ROBIN_HOOD(NODISCARD) bool empty() const noexcept { - ROBIN_HOOD_TRACE(this) - return 0 == mNumElements; - } - - float max_load_factor() const noexcept { // NOLINT - ROBIN_HOOD_TRACE(this) - return MaxLoadFactor100 / 100.0F; - } - - // Average number of elements per bucket. Since we allow only 1 per bucket - float load_factor() const noexcept { // NOLINT - ROBIN_HOOD_TRACE(this) - return static_cast(size()) / static_cast(mMask + 1); - } - - ROBIN_HOOD(NODISCARD) size_t mask() const noexcept { - ROBIN_HOOD_TRACE(this) - return mMask; - } - - ROBIN_HOOD(NODISCARD) size_t calcMaxNumElementsAllowed(size_t maxElements) const noexcept { - if (ROBIN_HOOD_LIKELY(maxElements <= (std::numeric_limits::max)() / 100)) { - return maxElements * MaxLoadFactor100 / 100; - } - - // we might be a bit imprecise, but since maxElements is quite large that doesn't matter - return (maxElements / 100) * MaxLoadFactor100; - } - - ROBIN_HOOD(NODISCARD) size_t calcNumBytesInfo(size_t numElements) const noexcept { - // we add a uint64_t, which houses the sentinel (first byte) and padding so we can load - // 64bit types. - return numElements + sizeof(uint64_t); - } - - ROBIN_HOOD(NODISCARD) - size_t calcNumElementsWithBuffer(size_t numElements) const noexcept { - auto maxNumElementsAllowed = calcMaxNumElementsAllowed(numElements); - return numElements + (std::min)(maxNumElementsAllowed, (static_cast(0xFF))); - } - - // calculation only allowed for 2^n values - ROBIN_HOOD(NODISCARD) size_t calcNumBytesTotal(size_t numElements) const { -#if ROBIN_HOOD(BITNESS) == 64 - return numElements * sizeof(Node) + calcNumBytesInfo(numElements); -#else - // make sure we're doing 64bit operations, so we are at least safe against 32bit overflows. - auto const ne = static_cast(numElements); - auto const s = static_cast(sizeof(Node)); - auto const infos = static_cast(calcNumBytesInfo(numElements)); - - auto const total64 = ne * s + infos; - auto const total = static_cast(total64); - - if (ROBIN_HOOD_UNLIKELY(static_cast(total) != total64)) { - throwOverflowError(); - } - return total; -#endif - } - -private: - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::value, bool>::type has(const value_type& e) const { - ROBIN_HOOD_TRACE(this) - auto it = find(e.first); - return it != end() && it->second == e.second; - } - - template - ROBIN_HOOD(NODISCARD) - typename std::enable_if::value, bool>::type has(const value_type& e) const { - ROBIN_HOOD_TRACE(this) - return find(e) != end(); - } - - void reserve(size_t c, bool forceRehash) { - ROBIN_HOOD_TRACE(this) - auto const minElementsAllowed = (std::max)(c, mNumElements); - auto newSize = InitialNumElements; - while (calcMaxNumElementsAllowed(newSize) < minElementsAllowed && newSize != 0) { - newSize *= 2; - } - if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { - throwOverflowError(); - } - - ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") - - // only actually do anything when the new size is bigger than the old one. This prevents to - // continuously allocate for each reserve() call. - if (forceRehash || newSize > mMask + 1) { - rehashPowerOfTwo(newSize, false); - } - } - - // reserves space for at least the specified number of elements. - // only works if numBuckets if power of two - // True on success, false otherwise - void rehashPowerOfTwo(size_t numBuckets, bool forceFree) { - ROBIN_HOOD_TRACE(this) - - Node* const oldKeyVals = mKeyVals; - uint8_t const* const oldInfo = mInfo; - - const size_t oldMaxElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); - - // resize operation: move stuff - initData(numBuckets); - if (oldMaxElementsWithBuffer > 1) { - for (size_t i = 0; i < oldMaxElementsWithBuffer; ++i) { - if (oldInfo[i] != 0) { - // might throw an exception, which is really bad since we are in the middle of - // moving stuff. - insert_move(std::move(oldKeyVals[i])); - // destroy the node but DON'T destroy the data. - oldKeyVals[i].~Node(); - } - } - - // this check is not necessary as it's guarded by the previous if, but it helps - // silence g++'s overeager "attempt to free a non-heap object 'map' - // [-Werror=free-nonheap-object]" warning. - if (oldKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { - // don't destroy old data: put it into the pool instead - if (forceFree) { - std::free(oldKeyVals); - } else { - DataPool::addOrFree(oldKeyVals, calcNumBytesTotal(oldMaxElementsWithBuffer)); - } - } - } - } - - ROBIN_HOOD(NOINLINE) void throwOverflowError() const { -#if ROBIN_HOOD(HAS_EXCEPTIONS) - throw std::overflow_error("robin_hood::map overflow"); -#else - abort(); -#endif - } - - template - std::pair try_emplace_impl(OtherKey&& key, Args&&... args) { - ROBIN_HOOD_TRACE(this) - auto idxAndState = insertKeyPrepareEmptySpot(key); - switch (idxAndState.second) { - case InsertionState::key_found: - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) Node( - *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), - std::forward_as_tuple(std::forward(args)...)); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, - std::forward_as_tuple(std::forward(key)), - std::forward_as_tuple(std::forward(args)...)); - break; - - case InsertionState::overflow_error: - throwOverflowError(); - break; - } - - return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), - InsertionState::key_found != idxAndState.second); - } - - template - std::pair insertOrAssignImpl(OtherKey&& key, Mapped&& obj) { - ROBIN_HOOD_TRACE(this) - auto idxAndState = insertKeyPrepareEmptySpot(key); - switch (idxAndState.second) { - case InsertionState::key_found: - mKeyVals[idxAndState.first].getSecond() = std::forward(obj); - break; - - case InsertionState::new_node: - ::new (static_cast(&mKeyVals[idxAndState.first])) Node( - *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), - std::forward_as_tuple(std::forward(obj))); - break; - - case InsertionState::overwrite_node: - mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, - std::forward_as_tuple(std::forward(key)), - std::forward_as_tuple(std::forward(obj))); - break; - - case InsertionState::overflow_error: - throwOverflowError(); - break; - } - - return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), - InsertionState::key_found != idxAndState.second); - } - - void initData(size_t max_elements) { - mNumElements = 0; - mMask = max_elements - 1; - mMaxNumElementsAllowed = calcMaxNumElementsAllowed(max_elements); - - auto const numElementsWithBuffer = calcNumElementsWithBuffer(max_elements); - - // malloc & zero mInfo. Faster than calloc everything. - auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); - ROBIN_HOOD_LOG("std::calloc " << numBytesTotal << " = calcNumBytesTotal(" - << numElementsWithBuffer << ")") - mKeyVals = reinterpret_cast( - detail::assertNotNull(std::malloc(numBytesTotal))); - mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); - std::memset(mInfo, 0, numBytesTotal - numElementsWithBuffer * sizeof(Node)); - - // set sentinel - mInfo[numElementsWithBuffer] = 1; - - mInfoInc = InitialInfoInc; - mInfoHashShift = InitialInfoHashShift; - } - - enum class InsertionState { overflow_error, key_found, new_node, overwrite_node }; - - // Finds key, and if not already present prepares a spot where to pot the key & value. - // This potentially shifts nodes out of the way, updates mInfo and number of inserted - // elements, so the only operation left to do is create/assign a new node at that spot. - template - std::pair insertKeyPrepareEmptySpot(OtherKey&& key) { - for (int i = 0; i < 256; ++i) { - size_t idx{}; - InfoType info{}; - keyToIdx(key, &idx, &info); - nextWhileLess(&info, &idx); - - // while we potentially have a match - while (info == mInfo[idx]) { - if (WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { - // key already exists, do NOT insert. - // see http://en.cppreference.com/w/cpp/container/unordered_map/insert - return std::make_pair(idx, InsertionState::key_found); - } - next(&info, &idx); - } - - // unlikely that this evaluates to true - if (ROBIN_HOOD_UNLIKELY(mNumElements >= mMaxNumElementsAllowed)) { - if (!increase_size()) { - return std::make_pair(size_t(0), InsertionState::overflow_error); - } - continue; - } - - // key not found, so we are now exactly where we want to insert it. - auto const insertion_idx = idx; - auto const insertion_info = info; - if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { - mMaxNumElementsAllowed = 0; - } - - // find an empty spot - while (0 != mInfo[idx]) { - next(&info, &idx); - } - - if (idx != insertion_idx) { - shiftUp(idx, insertion_idx); - } - // put at empty spot - mInfo[insertion_idx] = static_cast(insertion_info); - ++mNumElements; - return std::make_pair(insertion_idx, idx == insertion_idx - ? InsertionState::new_node - : InsertionState::overwrite_node); - } - - // enough attempts failed, so finally give up. - return std::make_pair(size_t(0), InsertionState::overflow_error); - } - - bool try_increase_info() { - ROBIN_HOOD_LOG("mInfoInc=" << mInfoInc << ", numElements=" << mNumElements - << ", maxNumElementsAllowed=" - << calcMaxNumElementsAllowed(mMask + 1)) - if (mInfoInc <= 2) { - // need to be > 2 so that shift works (otherwise undefined behavior!) - return false; - } - // we got space left, try to make info smaller - mInfoInc = static_cast(mInfoInc >> 1U); - - // remove one bit of the hash, leaving more space for the distance info. - // This is extremely fast because we can operate on 8 bytes at once. - ++mInfoHashShift; - auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); - - for (size_t i = 0; i < numElementsWithBuffer; i += 8) { - auto val = unaligned_load(mInfo + i); - val = (val >> 1U) & UINT64_C(0x7f7f7f7f7f7f7f7f); - std::memcpy(mInfo + i, &val, sizeof(val)); - } - // update sentinel, which might have been cleared out! - mInfo[numElementsWithBuffer] = 1; - - mMaxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); - return true; - } - - // True if resize was possible, false otherwise - bool increase_size() { - // nothing allocated yet? just allocate InitialNumElements - if (0 == mMask) { - initData(InitialNumElements); - return true; - } - - auto const maxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); - if (mNumElements < maxNumElementsAllowed && try_increase_info()) { - return true; - } - - ROBIN_HOOD_LOG("mNumElements=" << mNumElements << ", maxNumElementsAllowed=" - << maxNumElementsAllowed << ", load=" - << (static_cast(mNumElements) * 100.0 / - (static_cast(mMask) + 1))) - - if (mNumElements * 2 < calcMaxNumElementsAllowed(mMask + 1)) { - // we have to resize, even though there would still be plenty of space left! - // Try to rehash instead. Delete freed memory so we don't steadyily increase mem in case - // we have to rehash a few times - nextHashMultiplier(); - rehashPowerOfTwo(mMask + 1, true); - } else { - // we've reached the capacity of the map, so the hash seems to work nice. Keep using it. - rehashPowerOfTwo((mMask + 1) * 2, false); - } - return true; - } - - void nextHashMultiplier() { - // adding an *even* number, so that the multiplier will always stay odd. This is necessary - // so that the hash stays a mixing function (and thus doesn't have any information loss). - mHashMultiplier += UINT64_C(0xc4ceb9fe1a85ec54); - } - - void destroy() { - if (0 == mMask) { - // don't deallocate! - return; - } - - Destroyer::value>{} - .nodesDoNotDeallocate(*this); - - // This protection against not deleting mMask shouldn't be needed as it's sufficiently - // protected with the 0==mMask check, but I have this anyways because g++ 7 otherwise - // reports a compile error: attempt to free a non-heap object 'fm' - // [-Werror=free-nonheap-object] - if (mKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { - ROBIN_HOOD_LOG("std::free") - std::free(mKeyVals); - } - } - - void init() noexcept { - mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); - mInfo = reinterpret_cast(&mMask); - mNumElements = 0; - mMask = 0; - mMaxNumElementsAllowed = 0; - mInfoInc = InitialInfoInc; - mInfoHashShift = InitialInfoHashShift; - } - - // members are sorted so no padding occurs - uint64_t mHashMultiplier = UINT64_C(0xc4ceb9fe1a85ec53); // 8 byte 8 - Node* mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); // 8 byte 16 - uint8_t* mInfo = reinterpret_cast(&mMask); // 8 byte 24 - size_t mNumElements = 0; // 8 byte 32 - size_t mMask = 0; // 8 byte 40 - size_t mMaxNumElementsAllowed = 0; // 8 byte 48 - InfoType mInfoInc = InitialInfoInc; // 4 byte 52 - InfoType mInfoHashShift = InitialInfoHashShift; // 4 byte 56 - // 16 byte 56 if NodeAllocator -}; - -} // namespace detail - -// map - -template , - typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> -using unordered_flat_map = detail::Table; - -template , - typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> -using unordered_node_map = detail::Table; - -template , - typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> -using unordered_map = - detail::Table) <= sizeof(size_t) * 6 && - std::is_nothrow_move_constructible>::value && - std::is_nothrow_move_assignable>::value, - MaxLoadFactor100, Key, T, Hash, KeyEqual>; - -// set - -template , typename KeyEqual = std::equal_to, - size_t MaxLoadFactor100 = 80> -using unordered_flat_set = detail::Table; - -template , typename KeyEqual = std::equal_to, - size_t MaxLoadFactor100 = 80> -using unordered_node_set = detail::Table; - -template , typename KeyEqual = std::equal_to, - size_t MaxLoadFactor100 = 80> -using unordered_set = detail::Table < sizeof(Key) <= sizeof(size_t) * 6 && - std::is_nothrow_move_constructible::value && - std::is_nothrow_move_assignable::value, - MaxLoadFactor100, Key, void, Hash, KeyEqual>; - -} // namespace robin_hood -/* *INDENT-ON* */ - -#endif // NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp deleted file mode 100644 index f8aa72ff..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/types.hpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__TYPES_HPP_ -#define ATHENA_SMAC_PLANNER__TYPES_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -namespace athena_smac_planner -{ - -typedef std::pair NodeHeuristicPair; -typedef std::vector LookupTable; -typedef std::pair TrigValues; - -/** - * @struct athena_smac_planner::SearchInfo - * @brief Search properties and penalties - */ -struct SearchInfo -{ - float minimum_turning_radius{8.0}; - float non_straight_penalty{1.05}; - float change_penalty{0.0}; - float reverse_penalty{2.0}; - float cost_penalty{2.0}; - float retrospective_penalty{0.015}; - float rotation_penalty{5.0}; - float analytic_expansion_ratio{3.5}; - float analytic_expansion_max_length{60.0}; - float analytic_expansion_max_cost{200.0}; - bool analytic_expansion_max_cost_override{false}; - std::string lattice_filepath; - bool cache_obstacle_heuristic{false}; - bool allow_reverse_expansion{false}; - bool allow_primitive_interpolation{false}; - bool downsample_obstacle_heuristic{true}; - bool use_quadratic_cost_penalty{false}; -}; - -/** - * @struct athena_smac_planner::SmootherParams - * @brief Parameters for the smoother - */ -struct SmootherParams -{ - /** - * @brief A constructor for athena_smac_planner::SmootherParams - */ - SmootherParams() - : holonomic_(false) - { - } - - /** - * @brief Get params from ROS parameter - * @param node Ptr to node - * @param name Name of plugin - */ - void get(rclcpp::Node * node, const std::string & name) - { - std::string prefix = name + ".smoother."; - auto dp = [node, &prefix](const std::string & p, auto def) { - if (!node->has_parameter(prefix + p)) { - node->declare_parameter(prefix + p, def); - } - return node->get_parameter(prefix + p).get_value(); - }; - tolerance_ = dp("tolerance", 1e-10); - max_its_ = dp("max_iterations", 1000); - w_data_ = dp("w_data", 0.2); - w_smooth_ = dp("w_smooth", 0.3); - do_refinement_ = dp("do_refinement", true); - refinement_num_ = dp("refinement_num", 2); - } - - double tolerance_; - int max_its_; - double w_data_; - double w_smooth_; - bool holonomic_; - bool do_refinement_; - int refinement_num_; -}; - -/** - * @struct athena_smac_planner::TurnDirection - * @brief A struct with the motion primitive's direction embedded - */ -enum class TurnDirection -{ - UNKNOWN = 0, - FORWARD = 1, - LEFT = 2, - RIGHT = 3, - REVERSE = 4, - REV_LEFT = 5, - REV_RIGHT = 6 -}; - -/** - * @struct athena_smac_planner::MotionPose - * @brief A struct for poses in motion primitives - */ -struct MotionPose -{ - /** - * @brief A constructor for athena_smac_planner::MotionPose - */ - MotionPose() {} - - /** - * @brief A constructor for athena_smac_planner::MotionPose - * @param x X pose - * @param y Y pose - * @param theta Angle of pose - * @param TurnDirection Direction of the primitive's turn - */ - MotionPose(const float & x, const float & y, const float & theta, const TurnDirection & turn_dir) - : _x(x), _y(y), _theta(theta), _turn_dir(turn_dir) - {} - - MotionPose operator-(const MotionPose & p2) - { - return MotionPose( - this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta, TurnDirection::UNKNOWN); - } - - float _x; - float _y; - float _theta; - TurnDirection _turn_dir; -}; - -typedef std::vector MotionPoses; - -/** - * @struct athena_smac_planner::LatticeMetadata - * @brief A struct of all lattice metadata - */ -struct LatticeMetadata -{ - float min_turning_radius; - float grid_resolution; - unsigned int number_of_headings; - std::vector heading_angles; - unsigned int number_of_trajectories; - std::string motion_model; -}; - -/** - * @struct athena_smac_planner::MotionPrimitive - * @brief A struct of all motion primitive data - */ -struct MotionPrimitive -{ - unsigned int trajectory_id; - float start_angle; - float end_angle; - float turning_radius; - float trajectory_length; - float arc_length; - float straight_length; - bool left_turn; - MotionPoses poses; -}; - -/** - * @struct athena_smac_planner::GoalState - * @brief A struct to store the goal state - */ -template -struct GoalState -{ - NodeT * goal = nullptr; - bool is_valid = true; -}; - -typedef std::vector MotionPrimitives; -typedef std::vector MotionPrimitivePtrs; - -/** - * @class athena_smac_planner::Coordinates - * @brief Implementation of coordinate2d structure - */ -struct Coordinates2D -{ - Coordinates2D() {} - Coordinates2D(const float & x_in, const float & y_in) - : x(x_in), y(y_in) - {} - - inline bool operator==(const Coordinates2D & rhs) const - { - return this->x == rhs.x && this->y == rhs.y; - } - - inline bool operator!=(const Coordinates2D & rhs) const - { - return !(*this == rhs); - } - - float x, y; -}; - -/** - * @class athena_smac_planner::Coordinates - * @brief Implementation of coordinate structure - */ -struct Coordinates -{ - /** - * @brief A constructor for athena_smac_planner::NodeHybrid::Coordinates - */ - Coordinates() {} - - /** - * @brief A constructor for athena_smac_planner::NodeHybrid::Coordinates - * @param x_in X coordinate - * @param y_in Y coordinate - * @param theta_in Theta coordinate - */ - Coordinates(const float & x_in, const float & y_in, const float & theta_in) - : x(x_in), y(y_in), theta(theta_in) - {} - - inline bool operator==(const Coordinates & rhs) const - { - return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; - } - - inline bool operator!=(const Coordinates & rhs) const - { - return !(*this == rhs); - } - - float x, y, theta; -}; -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__TYPES_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp b/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp deleted file mode 100644 index 494f68d4..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/include/athena_smac_planner/utils.hpp +++ /dev/null @@ -1,231 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// Copyright (c) 2023, 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. Reserved. - -#ifndef ATHENA_SMAC_PLANNER__UTILS_HPP_ -#define ATHENA_SMAC_PLANNER__UTILS_HPP_ - -#include -#include -#include - -#include "nlohmann/json.hpp" -#include "geometry_msgs/msg/quaternion.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "tf2/utils.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/inflation_layer.hpp" -#include "visualization_msgs/msg/marker_array.hpp" -#include "athena_smac_planner/types.hpp" -#include - -namespace athena_smac_planner -{ - -/** -* @brief Create an Eigen Vector2D of world poses from continuous map coords -* @param mx float of map X coordinate -* @param my float of map Y coordinate -* @param costmap Costmap pointer -* @return Eigen::Vector2d eigen vector of the generated path -*/ -inline geometry_msgs::msg::Pose getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) -{ - geometry_msgs::msg::Pose msg; - msg.position.x = - static_cast(costmap->getOriginX()) + mx * costmap->getResolution(); - msg.position.y = - static_cast(costmap->getOriginY()) + my * costmap->getResolution(); - return msg; -} - -/** -* @brief Create quaternion from radians -* @param theta continuous bin coordinates angle -* @return quaternion orientation in map frame -*/ -inline geometry_msgs::msg::Quaternion getWorldOrientation( - const float & theta) -{ - // theta is in radians already - tf2::Quaternion q; - q.setEuler(0.0, 0.0, theta); - return tf2::toMsg(q); -} - -/** -* @brief Find the min cost of the inflation decay function for which the robot MAY be -* in collision in any orientation -* @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) -* @return double circumscribed cost, any higher than this and need to do full footprint collision checking -* since some element of the robot could be in collision -*/ -inline double findCircumscribedCost(std::shared_ptr costmap) -{ - double result = -1.0; - std::vector>::iterator layer; - - // check if the costmap has an inflation layer - std::shared_ptr inflation_layer = nullptr; - for (auto & layer : *costmap->getLayeredCostmap()->getPlugins()) { - auto candidate = std::dynamic_pointer_cast(layer); - if (candidate) { - inflation_layer = candidate; - break; - } - } - if (inflation_layer != nullptr) { - double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); - double resolution = costmap->getCostmap()->getResolution(); - result = static_cast(inflation_layer->computeCost(circum_radius / resolution)); - } else { - RCLCPP_WARN( - rclcpp::get_logger("computeCircumscribedCost"), - "No inflation layer found in costmap configuration. " - "If this is an SE2-collision checking plugin, it cannot use costmap potential " - "field to speed up collision checking by only checking the full footprint " - "when robot is within possibly-inscribed radius of an obstacle. This may " - "significantly slow down planning times!"); - } - - return result; -} - -/** - * @brief convert json to lattice metadata - * @param[in] json json object - * @param[out] lattice meta data - */ -inline void fromJsonToMetaData(const nlohmann::json & json, LatticeMetadata & lattice_metadata) -{ - json.at("turning_radius").get_to(lattice_metadata.min_turning_radius); - json.at("grid_resolution").get_to(lattice_metadata.grid_resolution); - json.at("num_of_headings").get_to(lattice_metadata.number_of_headings); - json.at("heading_angles").get_to(lattice_metadata.heading_angles); - json.at("number_of_trajectories").get_to(lattice_metadata.number_of_trajectories); - json.at("motion_model").get_to(lattice_metadata.motion_model); -} - -/** - * @brief convert json to pose - * @param[in] json json object - * @param[out] pose - */ -inline void fromJsonToPose(const nlohmann::json & json, MotionPose & pose) -{ - pose._x = json[0]; - pose._y = json[1]; - pose._theta = json[2]; -} - -/** - * @brief convert json to motion primitive - * @param[in] json json object - * @param[out] motion primitive - */ -inline void fromJsonToMotionPrimitive( - const nlohmann::json & json, MotionPrimitive & motion_primitive) -{ - json.at("trajectory_id").get_to(motion_primitive.trajectory_id); - json.at("start_angle_index").get_to(motion_primitive.start_angle); - json.at("end_angle_index").get_to(motion_primitive.end_angle); - json.at("trajectory_radius").get_to(motion_primitive.turning_radius); - json.at("trajectory_length").get_to(motion_primitive.trajectory_length); - json.at("arc_length").get_to(motion_primitive.arc_length); - json.at("straight_length").get_to(motion_primitive.straight_length); - json.at("left_turn").get_to(motion_primitive.left_turn); - - for (unsigned int i = 0; i < json["poses"].size(); i++) { - MotionPose pose; - fromJsonToPose(json["poses"][i], pose); - motion_primitive.poses.push_back(pose); - } -} - -/** - * @brief transform footprint into edges - * @param[in] robot position , orientation and footprint - * @param[out] robot footprint edges - */ -inline std::vector transformFootprintToEdges( - const geometry_msgs::msg::Pose & pose, - const std::vector & footprint) -{ - const double & x = pose.position.x; - const double & y = pose.position.y; - const double & yaw = tf2::getYaw(pose.orientation); - const double sin_yaw = sin(yaw); - const double cos_yaw = cos(yaw); - - std::vector out_footprint; - out_footprint.resize(2 * footprint.size()); - for (unsigned int i = 0; i < footprint.size(); i++) { - out_footprint[2 * i].x = x + cos_yaw * footprint[i].x - sin_yaw * footprint[i].y; - out_footprint[2 * i].y = y + sin_yaw * footprint[i].x + cos_yaw * footprint[i].y; - if (i == 0) { - out_footprint.back().x = out_footprint[i].x; - out_footprint.back().y = out_footprint[i].y; - } else { - out_footprint[2 * i - 1].x = out_footprint[2 * i].x; - out_footprint[2 * i - 1].y = out_footprint[2 * i].y; - } - } - return out_footprint; -} - -/** - * @brief initializes marker to visualize shape of linestring - * @param edge edge to mark of footprint - * @param i marker ID - * @param frame_id frame of the marker - * @param timestamp timestamp of the marker - * @return marker populated - */ -inline visualization_msgs::msg::Marker createMarker( - const std::vector edge, - unsigned int i, const std::string & frame_id, const rclcpp::Time & timestamp) -{ - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = timestamp; - marker.frame_locked = false; - marker.ns = "planned_footprint"; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.type = visualization_msgs::msg::Marker::LINE_LIST; - marker.lifetime = rclcpp::Duration(0, 0); - - marker.id = i; - for (auto & point : edge) { - marker.points.push_back(point); - } - - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.05; - marker.scale.y = 0.05; - marker.scale.z = 0.05; - marker.color.r = 0.0f; - marker.color.g = 0.0f; - marker.color.b = 1.0f; - marker.color.a = 1.3f; - return marker; -} - - -} // namespace athena_smac_planner - -#endif // ATHENA_SMAC_PLANNER__UTILS_HPP_ diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/package.xml b/src/subsystems/minimal_navigation/athena_smac_planner/package.xml deleted file mode 100644 index fbae2a06..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - athena_smac_planner - 1.4.0 - Vendored Hybrid-A* planner (SMAC) for the Athena navigation stack - UMD Loop - Apache-2.0 - - ament_cmake - nav2_common - - angles - eigen - geometry_msgs - msgs - nav2_core - nav2_costmap_2d - nav_msgs - nlohmann-json-dev - ompl - rclcpp - rclcpp_lifecycle - tf2_ros - tf2 - visualization_msgs - - - ament_cmake - - diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp deleted file mode 100644 index 9f2ce25f..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/a_star.cpp +++ /dev/null @@ -1,526 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// -// 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. Reserved. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "athena_smac_planner/a_star.hpp" -using namespace std::chrono; // NOLINT - -namespace athena_smac_planner -{ - -template -AStarAlgorithm::AStarAlgorithm( - const MotionModel & motion_model, - const SearchInfo & search_info) -: _traverse_unknown(true), - _is_initialized(false), - _max_iterations(0), - _terminal_checking_interval(5000), - _max_planning_time(0), - _x_size(0), - _y_size(0), - _search_info(search_info), - _start(nullptr), - _goal_manager(GoalManagerT()), - _motion_model(motion_model) -{ - _graph.reserve(100000); -} - -template -AStarAlgorithm::~AStarAlgorithm() -{ -} - -template -void AStarAlgorithm::initialize( - const bool & allow_unknown, - int & max_iterations, - const int & max_on_approach_iterations, - const int & terminal_checking_interval, - const double & max_planning_time, - const float & lookup_table_size, - const unsigned int & dim_3_size) -{ - _traverse_unknown = allow_unknown; - _max_iterations = max_iterations; - _max_on_approach_iterations = max_on_approach_iterations; - _terminal_checking_interval = terminal_checking_interval; - _max_planning_time = max_planning_time; - if (!_is_initialized) { - _shared_ctx = std::make_shared(); - _shared_ctx->distance_heuristic->precomputeDistanceHeuristic(lookup_table_size, _motion_model, - dim_3_size, - _search_info, _shared_ctx->motion_table); - } - _is_initialized = true; - _dim3_size = dim_3_size; - _expander = std::make_unique>( - _motion_model, _search_info, _traverse_unknown, _dim3_size); -} - -template -void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision_checker) -{ - _collision_checker = collision_checker; - _costmap = collision_checker->getCostmap(); - unsigned int x_size = _costmap->getSizeInCellsX(); - unsigned int y_size = _costmap->getSizeInCellsY(); - - clearGraph(); - - if (getSizeX() != x_size || getSizeY() != y_size) { - _x_size = x_size; - _y_size = y_size; - } - - // Always refresh the motion model so dynamic penalty parameters take effect immediately - NodeT::initMotionModel(_shared_ctx.get(), _motion_model, _x_size, _y_size, _dim3_size, - _search_info); - - // Always set context pointers to ensure newly allocated objects get their contexts restored - _goal_manager.setContext(_shared_ctx.get()); - _expander->setContext(_shared_ctx.get()); - _expander->setCollisionChecker(_collision_checker); -} - -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( - const uint64_t & index) -{ - auto iter = _graph.find(index); - if (iter != _graph.end()) { - return &(iter->second); - } - - return &(_graph.emplace(index, NodeT(index, _shared_ctx.get())).first->second); -} - -template -void AStarAlgorithm::setStart( - const float & mx, - const float & my, - const unsigned int & dim_3) -{ - _start = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - dim_3)); - _start->setPose(Coordinates(mx, my, dim_3)); -} - -template -void AStarAlgorithm::populateExpansionsLog( - const NodePtr & node, - std::vector> * expansions_log) -{ - typename NodeT::Coordinates coords = node->pose; - expansions_log->emplace_back( - _costmap->getOriginX() + ((coords.x + 0.5) * _costmap->getResolution()), - _costmap->getOriginY() + ((coords.y + 0.5) * _costmap->getResolution()), - _shared_ctx->motion_table.getAngleFromBin(coords.theta)); -} - -template -void AStarAlgorithm::setGoal( - const float & mx, - const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & goal_heading_mode, - const int & coarse_search_resolution) -{ - // Default to minimal resolution unless overridden for ALL_DIRECTION - _coarse_search_resolution = 1; - - _goal_manager.clear(); - Coordinates ref_goal_coord(mx, my, static_cast(dim_3)); - - if (!_search_info.cache_obstacle_heuristic || - _goal_manager.hasGoalChanged(ref_goal_coord)) - { - if (!_start) { - throw std::runtime_error("Start must be set before goal."); - } - - _shared_ctx->obstacle_heuristic->resetObstacleHeuristic( - _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my, - _shared_ctx->motion_table.downsample_obstacle_heuristic); - } - - _goal_manager.setRefGoalCoordinates(ref_goal_coord); - - unsigned int num_bins = _shared_ctx->motion_table.num_angle_quantization; - // set goal based on heading mode - switch (goal_heading_mode) { - case GoalHeadingMode::DEFAULT: { - // add a single goal node with single heading - auto goal = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - dim_3)); - goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); - _goal_manager.addGoal(goal); - break; - } - - case GoalHeadingMode::BIDIRECTIONAL: { - // Add two goals, one for each direction - // add goal in original direction - auto goal = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - dim_3)); - goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); - _goal_manager.addGoal(goal); - - // Add goal node in opposite (180°) direction - unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; - auto opposite_goal = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - opposite_heading)); - opposite_goal->setPose( - typename NodeT::Coordinates(mx, my, static_cast(opposite_heading))); - _goal_manager.addGoal(opposite_goal); - break; - } - - case GoalHeadingMode::ALL_DIRECTION: { - // Set the coarse search resolution only for all direction - _coarse_search_resolution = coarse_search_resolution; - - // Add goal nodes for all headings - for (unsigned int i = 0; i < num_bins; ++i) { - auto goal = addToGraph( - getIndex( - static_cast(mx), - static_cast(my), - i)); - goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(i))); - _goal_manager.addGoal(goal); - } - break; - } - case GoalHeadingMode::UNKNOWN: - throw std::runtime_error("Goal heading is UNKNOWN."); - } -} - -template -bool AStarAlgorithm::areInputsValid() -{ - // Check if graph was filled in - if (_graph.empty()) { - throw std::runtime_error("Failed to compute path, no costmap given."); - } - - // Check if points were filled in - if (!_start || _goal_manager.goalsIsEmpty()) { - throw std::runtime_error("Failed to compute path, no valid start or goal given."); - } - - // remove invalid goals - _goal_manager.removeInvalidGoals(getToleranceHeuristic(), _collision_checker, _traverse_unknown); - - // Check if ending point is valid - if (_goal_manager.getGoalsSet().empty()) { - throw nav2_core::PlannerException("Goal was in lethal cost"); - } - - // Note: We do not check the if the start is valid because it is cleared - return true; -} - -template -bool AStarAlgorithm::getClosestPathWithinTolerance(CoordinateVector & path) -{ - if (_best_heuristic_node.first < getToleranceHeuristic()) { - _graph.at(_best_heuristic_node.second).backtracePath(path); - return true; - } - - return false; -} - -template -bool AStarAlgorithm::createPath( - CoordinateVector & path, int & iterations, - const float & tolerance, - std::function cancel_checker, - std::vector> * expansions_log) -{ - steady_clock::time_point start_time = steady_clock::now(); - _tolerance = tolerance; - _best_heuristic_node = {std::numeric_limits::max(), 0}; - clearQueue(); - - if (!areInputsValid()) { - return false; - } - - NodeVector coarse_check_goals, fine_check_goals; - _goal_manager.prepareGoalsForAnalyticExpansion( - coarse_check_goals, fine_check_goals, - _coarse_search_resolution); - - // 0) Add starting point to the open set - addNode(0.0, getStart()); - getStart()->setAccumulatedCost(0.0); - - // Optimization: preallocate all variables - NodePtr current_node = nullptr; - NodePtr neighbor = nullptr; - NodePtr expansion_result = nullptr; - float g_cost = 0.0; - NodeVector neighbors; - int approach_iterations = 0; - NeighborIterator neighbor_iterator; - int analytic_iterations = 0; - int closest_distance = std::numeric_limits::max(); - - // Given an index, return a node ptr reference if its collision-free and valid - const uint64_t max_index = static_cast(getSizeX()) * - static_cast(getSizeY()) * - static_cast(getSizeDim3()); - NodeGetter neighborGetter = - [&, this](const uint64_t & index, NodePtr & neighbor_rtn) -> bool - { - if (index >= max_index) { - return false; - } - - neighbor_rtn = addToGraph(index); - return true; - }; - - while (iterations < getMaxIterations() && !_queue.empty()) { - // Check for planning timeout and cancel only on every Nth iteration - if (iterations % _terminal_checking_interval == 0) { - if (cancel_checker()) { - throw nav2_core::PlannerException("Planner was cancelled"); - } - std::chrono::duration planning_duration = - std::chrono::duration_cast>(steady_clock::now() - start_time); - if (static_cast(planning_duration.count()) >= _max_planning_time) { - // In case of timeout, return the path that is closest, if within tolerance. - return getClosestPathWithinTolerance(path); - } - } - - // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue - current_node = getNextNode(); - - // Save current node coordinates for debug - if (expansions_log) { - populateExpansionsLog(current_node, expansions_log); - } - - // We allow for nodes to be queued multiple times in case - // shorter paths result in it, but we can visit only once - // Also a chance to perform last-checks necessary. - if (onVisitationCheckNode(current_node)) { - continue; - } - - iterations++; - - // 2) Mark Nbest as visited - current_node->visited(); - - // 2.1) Use an analytic expansion (if available) to generate a path - expansion_result = nullptr; - expansion_result = _expander->tryAnalyticExpansion( - current_node, coarse_check_goals, fine_check_goals, - _goal_manager.getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); - if (expansion_result != nullptr) { - current_node = expansion_result; - } - - // 3) Check if we're at the goal, backtrace if required - if (_goal_manager.isGoal(current_node)) { - return current_node->backtracePath(path); - } else if (_best_heuristic_node.first < getToleranceHeuristic()) { - // Optimization: Let us find when in tolerance and refine within reason - approach_iterations++; - if (approach_iterations >= getOnApproachMaxIterations()) { - return _graph.at(_best_heuristic_node.second).backtracePath(path); - } - } - - // 4) Expand neighbors of Nbest not visited - neighbors.clear(); - current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors); - - for (neighbor_iterator = neighbors.begin(); - neighbor_iterator != neighbors.end(); ++neighbor_iterator) - { - neighbor = *neighbor_iterator; - - // 4.1) Compute the cost to go to this node - g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor); - - // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach - if (g_cost < neighbor->getAccumulatedCost()) { - neighbor->setAccumulatedCost(g_cost); - neighbor->parent = current_node; - - // 4.3) Add to queue with heuristic cost - addNode(g_cost + getHeuristicCost(neighbor), neighbor); - } - } - } - - // If we run out of search options, return the path that is closest, if within tolerance. - return getClosestPathWithinTolerance(path); -} - -template -typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() -{ - return _start; -} - -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() -{ - NodeBasic node = _queue.top().second; - _queue.pop(); - node.processSearchNode(); - return node.graph_node_ptr; -} - -template -void AStarAlgorithm::addNode(const float & cost, NodePtr & node) -{ - NodeBasic queued_node(node->getIndex()); - queued_node.populateSearchNode(node); - _queue.emplace(cost, queued_node); -} - -template -float AStarAlgorithm::getHeuristicCost(const NodePtr & node) -{ - const Coordinates node_coords = - NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = node->getHeuristicCost(node_coords, _goal_manager.getGoalsCoordinates()); - if (heuristic < _best_heuristic_node.first) { - _best_heuristic_node = {heuristic, node->getIndex()}; - } - - return heuristic; -} - -template -bool AStarAlgorithm::onVisitationCheckNode(const NodePtr & current_node) -{ - return current_node->wasVisited(); -} - -template -void AStarAlgorithm::clearQueue() -{ - NodeQueue q; - std::swap(_queue, q); -} - -template -void AStarAlgorithm::clearGraph() -{ - Graph g; - std::swap(_graph, g); - _graph.reserve(100000); -} - -template -uint64_t AStarAlgorithm::getIndex( - const unsigned int & x, const unsigned int & y, - const unsigned int & dim_3) -{ - return NodeT::getIndex(x, y, dim_3, _shared_ctx->motion_table.size_x, - _shared_ctx->motion_table.num_angle_quantization); -} - -template -int & AStarAlgorithm::getMaxIterations() -{ - return _max_iterations; -} - -template -int & AStarAlgorithm::getOnApproachMaxIterations() -{ - return _max_on_approach_iterations; -} - -template -float & AStarAlgorithm::getToleranceHeuristic() -{ - return _tolerance; -} - -template -unsigned int & AStarAlgorithm::getSizeX() -{ - return _x_size; -} - -template -unsigned int & AStarAlgorithm::getSizeY() -{ - return _y_size; -} - -template -unsigned int & AStarAlgorithm::getSizeDim3() -{ - return _dim3_size; -} - -template -unsigned int AStarAlgorithm::getCoarseSearchResolution() -{ - return _coarse_search_resolution; -} - -template -typename AStarAlgorithm::GoalManagerT AStarAlgorithm::getGoalManager() -{ - return _goal_manager; -} - -template -typename AStarAlgorithm::NodeContext * AStarAlgorithm::getContext() -{ - return _shared_ctx.get(); -} - -// Instantiate algorithm for the supported template types -template class AStarAlgorithm; - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp deleted file mode 100644 index c8c057a1..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/analytic_expansion.cpp +++ /dev/null @@ -1,449 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#include -#include -#include - -#include "athena_smac_planner/analytic_expansion.hpp" - -namespace athena_smac_planner -{ - -template -AnalyticExpansion::AnalyticExpansion( - const MotionModel & motion_model, - const SearchInfo & search_info, - const bool & traverse_unknown, - const unsigned int & dim_3_size) -: _motion_model(motion_model), - _search_info(search_info), - _traverse_unknown(traverse_unknown), - _dim_3_size(dim_3_size), - _collision_checker(nullptr) -{ -} - -template -void AnalyticExpansion::setCollisionChecker( - GridCollisionChecker * collision_checker) -{ - _collision_checker = collision_checker; -} - -template -void AnalyticExpansion::setContext(NodeContext * ctx) -{ - _ctx = ctx; -} - -template -typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, - const NodeVector & coarse_check_goals, - const NodeVector & fine_check_goals, - const CoordinateVector & goals_coords, - const NodeGetter & getter, int & analytic_iterations, - int & closest_distance) -{ - // This must be a valid motion model for analytic expansion to be attempted - if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP || - _motion_model == MotionModel::STATE_LATTICE) - { - // See if we are closer and should be expanding more often - const Coordinates node_coords = - NodeT::getCoords( - current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); - - AnalyticExpansionNodes current_best_analytic_nodes; - NodePtr current_best_goal = nullptr; - NodePtr current_best_node = nullptr; - float current_best_score = std::numeric_limits::max(); - - closest_distance = std::min( - closest_distance, - static_cast(current_node->getHeuristicCost(node_coords, goals_coords))); - // We want to expand at a rate of d/expansion_ratio, - // but check to see if we are so close that we would be expanding every iteration - // If so, limit it to the expansion ratio (rounded up) - int desired_iterations = std::max( - static_cast(closest_distance / _search_info.analytic_expansion_ratio), - static_cast(std::ceil(_search_info.analytic_expansion_ratio))); - - // If we are closer now, we should update the target number of iterations to go - analytic_iterations = - std::min(analytic_iterations, desired_iterations); - - // Always run the expansion on the first run in case there is a - // trivial path to be found - if (analytic_iterations <= 0) { - // Reset the counter and try the analytic path expansion - analytic_iterations = desired_iterations; - bool found_valid_expansion = false; - - // First check the coarse search resolution goals - for (auto & current_goal_node : coarse_check_goals) { - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath( - current_node, current_goal_node, getter, - _ctx->motion_table.state_space); - if (!analytic_nodes.nodes.empty()) { - found_valid_expansion = true; - NodePtr node = current_node; - float score = refineAnalyticPath( - node, current_goal_node, getter, analytic_nodes); - // Update the best score if we found a better path - if (score < current_best_score) { - current_best_analytic_nodes = analytic_nodes; - current_best_goal = current_goal_node; - current_best_score = score; - current_best_node = node; - } - } - } - - // perform a final search if we found a goal - if (found_valid_expansion) { - for (auto & current_goal_node : fine_check_goals) { - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath( - current_node, current_goal_node, getter, - _ctx->motion_table.state_space); - if (!analytic_nodes.nodes.empty()) { - NodePtr node = current_node; - float score = refineAnalyticPath( - node, current_goal_node, getter, analytic_nodes); - // Update the best score if we found a better path - if (score < current_best_score) { - current_best_analytic_nodes = analytic_nodes; - current_best_goal = current_goal_node; - current_best_score = score; - current_best_node = node; - } - } - } - } - } - - if (!current_best_analytic_nodes.nodes.empty()) { - return setAnalyticPath( - current_best_node, current_best_goal, - current_best_analytic_nodes); - } - analytic_iterations--; - } - - // No valid motion model - return nullptr - return NodePtr(nullptr); -} - -template -int AnalyticExpansion::countDirectionChanges( - const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path) -{ - const double * lengths = path.length_; - int changes = 0; - int last_dir = 0; - for (int i = 0; i < 5; ++i) { - if (lengths[i] == 0.0) { - continue; - } - - int currentDirection = (lengths[i] > 0.0) ? 1 : -1; - if (last_dir != 0 && currentDirection != last_dir) { - ++changes; - } - last_dir = currentDirection; - } - - return changes; -} - -template -typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( - const NodePtr & node, - const NodePtr & goal, - const NodeGetter & node_getter, - const ompl::base::StateSpacePtr & state_space) -{ - ompl::base::ScopedState<> from(state_space), to(state_space), s(state_space); - from[0] = node->pose.x; - from[1] = node->pose.y; - from[2] = _ctx->motion_table.getAngleFromBin(node->pose.theta); - to[0] = goal->pose.x; - to[1] = goal->pose.y; - to[2] = _ctx->motion_table.getAngleFromBin(goal->pose.theta); - - float d = state_space->distance(from(), to()); - - auto rs_state_space = dynamic_cast(state_space.get()); - int direction_changes = 0; - if (rs_state_space) { - direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get())); - } - - // A move of sqrt(2) is guaranteed to be in a new cell - static const float sqrt_2 = sqrtf(2.0f); - - // If the length is too far, exit. This prevents unsafe shortcutting of paths - // into higher cost areas far out from the goal itself, let search to the work of getting - // close before the analytic expansion brings it home. This should never be smaller than - // 4-5x the minimum turning radius being used, or planning times will begin to spike. - if (d > _search_info.analytic_expansion_max_length || d < sqrt_2) { - return AnalyticExpansionNodes(); - } - - unsigned int num_intervals = static_cast(std::floor(d / sqrt_2)); - - AnalyticExpansionNodes possible_nodes; - // When "from" and "to" are zero or one cell away, - // num_intervals == 0 - possible_nodes.nodes.reserve(num_intervals); // We won't store this node or the goal - std::vector reals; - double theta; - - // Pre-allocate - NodePtr prev(node); - uint64_t index = 0; - NodePtr next(nullptr); - float angle = 0.0; - Coordinates proposed_coordinates; - bool failure = false; - std::vector node_costs; - node_costs.reserve(num_intervals); - - // Check intermediary poses (non-goal, non-start) - for (float i = 1; i <= num_intervals; i++) { - state_space->interpolate(from(), to(), i / num_intervals, s()); - reals = s.reals(); - // Make sure in range [0, 2PI) - theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; - theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; - angle = _ctx->motion_table.getAngle(theta); - - // Turn the pose into a node, and check if it is valid - index = NodeT::getIndex( - static_cast(reals[0]), - static_cast(reals[1]), - static_cast(angle), - _ctx->motion_table.size_x, - _ctx->motion_table.num_angle_quantization); - // Get the node from the graph - if (node_getter(index, next)) { - Coordinates initial_node_coords = next->pose; - proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; - next->setPose(proposed_coordinates); - if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { - // Save the node, and its previous coordinates in case we need to abort - possible_nodes.add(next, initial_node_coords, proposed_coordinates); - node_costs.emplace_back(next->getCost()); - prev = next; - } else { - // Abort - next->setPose(initial_node_coords); - failure = true; - break; - } - } else { - // Abort - failure = true; - break; - } - } - - if (!failure) { - // We found 'a' valid expansion. Now to tell if its a quality option... - const float max_cost = _search_info.analytic_expansion_max_cost; - auto max_cost_it = std::max_element(node_costs.begin(), node_costs.end()); - if (max_cost_it != node_costs.end() && *max_cost_it > max_cost) { - // If any element is above the comfortable cost limit, check edge cases: - // (1) Check if goal is in greater than max_cost space requiring - // entering it, but only entering it on final approach, not in-and-out - // (2) Checks if goal is in normal space, but enters costed space unnecessarily - // mid-way through, skirting obstacle or in non-globally confined space - bool cost_exit_high_cost_region = false; - for (auto iter = node_costs.rbegin(); iter != node_costs.rend(); ++iter) { - const float & curr_cost = *iter; - if (curr_cost <= max_cost) { - cost_exit_high_cost_region = true; - } else if (curr_cost > max_cost && cost_exit_high_cost_region) { - failure = true; - break; - } - } - - // (3) Handle exception: there may be no other option close to goal - // if max cost is set too low (optional) - if (failure) { - if (d < 2.0f * M_PI * _ctx->motion_table.min_turning_radius && - _search_info.analytic_expansion_max_cost_override) - { - failure = false; - } - } - } - } - - // Reset to initial poses to not impact future searches - for (const auto & node_pose : possible_nodes.nodes) { - const auto & n = node_pose.node; - n->setPose(node_pose.initial_coords); - } - - if (failure) { - return AnalyticExpansionNodes(); - } - - possible_nodes.setDirectionChanges(direction_changes); - return possible_nodes; -} - -template -float AnalyticExpansion::refineAnalyticPath( - NodePtr & node, - const NodePtr & goal_node, - const NodeGetter & getter, - AnalyticExpansionNodes & analytic_nodes) -{ - NodePtr test_node = node; - AnalyticExpansionNodes refined_analytic_nodes; - for (int i = 0; i < 8; i++) { - // Attempt to create better paths in 5 node increments, need to make sure - // they exist for each in order to do so (maximum of 40 points back). - if (test_node->parent && test_node->parent->parent && - test_node->parent->parent->parent && - test_node->parent->parent->parent->parent && - test_node->parent->parent->parent->parent->parent) - { - test_node = test_node->parent->parent->parent->parent->parent; - // print the goals pose - refined_analytic_nodes = - getAnalyticPath( - test_node, goal_node, getter, - _ctx->motion_table.state_space); - if (refined_analytic_nodes.nodes.empty()) { - break; - } - if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) { - // If the direction changes are worse, we don't want to use this path - continue; - } - analytic_nodes = refined_analytic_nodes; - node = test_node; - } else { - break; - } - } - - // The analytic expansion can short-cut near obstacles when closer to a goal - // So, we can attempt to refine it more by increasing the possible radius - // higher than the minimum turning radius and use the best solution based on - // a scoring function similar to that used in traversal cost estimation. - auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.nodes.size() < 2) { - return std::numeric_limits::max(); - } - - float score = 0.0; - float normalized_cost = 0.0; - // Analytic expansions are consistently spaced - const float distance = hypotf( - expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x, - expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y); - const float & weight = _ctx->motion_table.cost_penalty; - for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) { - normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function - score += distance * (1.0 + weight * normalized_cost); - } - return score; - }; - - float original_score = scoringFn(analytic_nodes); - float best_score = original_score; - float score = std::numeric_limits::max(); - float min_turn_rad = _ctx->motion_table.min_turning_radius; - const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius - while (min_turn_rad < max_min_turn_rad) { - min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps - ompl::base::StateSpacePtr state_space; - if (_ctx->motion_table.motion_model == MotionModel::DUBIN) { - state_space = std::make_shared(min_turn_rad); - } else { - state_space = std::make_shared(min_turn_rad); - } - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); - score = scoringFn(refined_analytic_nodes); - - // Normal scoring: prioritize lower cost as long as not more directional changes - if (score <= best_score && - refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes) - { - analytic_nodes = refined_analytic_nodes; - best_score = score; - continue; - } - - // Special case: If we have a better score than original (only) and less directional changes - // the path quality is still better than the original and is less operationally complex - if (score <= original_score && - refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes) - { - analytic_nodes = refined_analytic_nodes; - best_score = score; - } - } - - return best_score; -} - -template -typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( - const NodePtr & node, - const NodePtr & goal_node, - const AnalyticExpansionNodes & expanded_nodes) -{ - _detached_nodes.clear(); - // Legitimate final path - set the parent relationships, states, and poses - NodePtr prev = node; - for (const auto & node_pose : expanded_nodes.nodes) { - auto n = node_pose.node; - cleanNode(n); - if (n->getIndex() != goal_node->getIndex()) { - if (n->wasVisited()) { - _detached_nodes.push_back(std::make_unique(-1, _ctx)); - n = _detached_nodes.back().get(); - } - n->parent = prev; - n->pose = node_pose.proposed_coords; - n->visited(); - prev = n; - } - } - if (goal_node != prev) { - goal_node->parent = prev; - cleanNode(goal_node); - goal_node->visited(); - } - return goal_node; -} - -template -void AnalyticExpansion::cleanNode(const NodePtr & /*expanded_nodes*/) -{ -} - -template class AnalyticExpansion; - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp deleted file mode 100644 index 24c8baf7..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/collision_checker.cpp +++ /dev/null @@ -1,196 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#include "athena_smac_planner/collision_checker.hpp" - -namespace athena_smac_planner -{ - -GridCollisionChecker::GridCollisionChecker( - std::shared_ptr costmap_ros, - unsigned int num_quantizations, - rclcpp::Node::SharedPtr node) -: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr) -{ - if (node) { - clock_ = node->get_clock(); - logger_ = node->get_logger(); - } - - if (costmap_ros) { - costmap_ros_ = costmap_ros; - } - - // Convert number of regular bins into angles - float bin_size = 2 * M_PI / static_cast(num_quantizations); - angles_.reserve(num_quantizations); - for (unsigned int i = 0; i != num_quantizations; i++) { - angles_.push_back(bin_size * i); - } -} - -// GridCollisionChecker::GridCollisionChecker( -// nav2_costmap_2d::Costmap2D * costmap, -// std::vector & angles) -// : FootprintCollisionChecker(costmap), -// angles_(angles) -// { -// } - -void GridCollisionChecker::setFootprint( - const nav2_costmap_2d::Footprint & footprint, - const bool & radius, - const double & possible_collision_cost) -{ - possible_collision_cost_ = static_cast(possible_collision_cost); - if (possible_collision_cost_ <= 0.0f) { - RCLCPP_ERROR_THROTTLE( - logger_, *clock_, 1000, - "Inflation layer either not found or inflation is not set sufficiently for " - "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" - " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " - "github.com/ros-planning/navigation2/tree/main/athena_smac_planner#potential-fields" - " for full instructions. This will substantially impact run-time performance."); - } - - footprint_is_radius_ = radius; - - // Use radius, no caching required - if (radius) { - return; - } - - // No change, no updates required - if (footprint == unoriented_footprint_) { - return; - } - - oriented_footprints_.clear(); - oriented_footprints_.reserve(angles_.size()); - double sin_th, cos_th; - geometry_msgs::msg::Point new_pt; - const unsigned int footprint_size = footprint.size(); - - // Precompute the orientation bins for checking to use - for (unsigned int i = 0; i != angles_.size(); i++) { - sin_th = sin(angles_[i]); - cos_th = cos(angles_[i]); - nav2_costmap_2d::Footprint oriented_footprint; - oriented_footprint.reserve(footprint_size); - - for (unsigned int j = 0; j < footprint_size; j++) { - new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; - new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; - oriented_footprint.push_back(new_pt); - } - - oriented_footprints_.push_back(oriented_footprint); - } - - unoriented_footprint_ = footprint; -} - -bool GridCollisionChecker::inCollision( - const float & x, - const float & y, - const float & angle_bin, - const bool & traverse_unknown) -{ - // Check to make sure cell is inside the map - if (outsideRange(costmap_->getSizeInCellsX(), x) || - outsideRange(costmap_->getSizeInCellsY(), y)) - { - return true; - } - - // Assumes setFootprint already set - center_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5f), static_cast(y + 0.5f))); - - if (!footprint_is_radius_) { - // if footprint, then we check for the footprint's points, but first see - // if the robot is even potentially in an inscribed collision - if (center_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { - return false; - } - - // If its inscribed, in collision, or unknown in the middle, - // no need to even check the footprint, its invalid - if (center_cost_ == UNKNOWN_COST && !traverse_unknown) { - return true; - } - - if (center_cost_ == INSCRIBED_COST || center_cost_ == OCCUPIED_COST) { - return true; - } - - // if possible inscribed, need to check actual footprint pose. - // Use precomputed oriented footprints are done on initialization, - // offset by translation value to collision check - double wx, wy; - costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); - geometry_msgs::msg::Point new_pt; - const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; - nav2_costmap_2d::Footprint current_footprint; - current_footprint.reserve(oriented_footprint.size()); - for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { - new_pt.x = wx + oriented_footprint[i].x; - new_pt.y = wy + oriented_footprint[i].y; - current_footprint.push_back(new_pt); - } - - float footprint_cost = static_cast(footprintCost(current_footprint)); - - if (footprint_cost == UNKNOWN_COST && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost >= OCCUPIED_COST; - } else { - // if radius, then we can check the center of the cost assuming inflation is used - if (center_cost_ == UNKNOWN_COST && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return center_cost_ >= INSCRIBED_COST; - } -} - -bool GridCollisionChecker::inCollision( - const unsigned int & i, - const bool & traverse_unknown) -{ - center_cost_ = costmap_->getCost(i); - if (center_cost_ == UNKNOWN_COST && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return center_cost_ >= INSCRIBED_COST; -} - -float GridCollisionChecker::getCost() -{ - // Assumes inCollision called prior - return static_cast(center_cost_); -} - -bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value) -{ - return value < 0.0f || value > max; -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp deleted file mode 100644 index 4cfac234..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/costmap_downsampler.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright (c) 2020, Carlos Luis -// Copyright (c) 2020, Samsung Research America -// -// 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. Reserved. - -#include "athena_smac_planner/costmap_downsampler.hpp" - -#include -#include -#include - -namespace athena_smac_planner -{ - -CostmapDownsampler::CostmapDownsampler() -: _costmap(nullptr), - _downsampled_costmap(nullptr) -{ -} - -CostmapDownsampler::~CostmapDownsampler() -{ -} - -void CostmapDownsampler::on_configure( - nav2_costmap_2d::Costmap2D * const costmap, - const unsigned int & downsampling_factor, - const bool & use_min_cost_neighbor) -{ - _costmap = costmap; - _downsampling_factor = downsampling_factor; - _use_min_cost_neighbor = use_min_cost_neighbor; - updateCostmapSize(); - - _downsampled_costmap = std::make_unique( - _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, - _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN_COST); -} - -void CostmapDownsampler::on_cleanup() -{ - _costmap = nullptr; - _downsampled_costmap.reset(); -} - -nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( - const unsigned int & downsampling_factor) -{ - _downsampling_factor = downsampling_factor; - updateCostmapSize(); - - // Adjust costmap size if needed - if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || - _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || - _downsampled_costmap->getResolution() != _downsampled_resolution) - { - resizeCostmap(); - } - - // Assign costs - for (unsigned int i = 0; i < _downsampled_size_x; ++i) { - for (unsigned int j = 0; j < _downsampled_size_y; ++j) { - setCostOfCell(i, j); - } - } - - return _downsampled_costmap.get(); -} - -void CostmapDownsampler::updateCostmapSize() -{ - _size_x = _costmap->getSizeInCellsX(); - _size_y = _costmap->getSizeInCellsY(); - _downsampled_size_x = ceil(static_cast(_size_x) / _downsampling_factor); - _downsampled_size_y = ceil(static_cast(_size_y) / _downsampling_factor); - _downsampled_resolution = _downsampling_factor * _costmap->getResolution(); -} - -void CostmapDownsampler::resizeCostmap() -{ - _downsampled_costmap->resizeMap( - _downsampled_size_x, - _downsampled_size_y, - _downsampled_resolution, - _costmap->getOriginX(), - _costmap->getOriginY()); -} - -void CostmapDownsampler::setCostOfCell( - const unsigned int & new_mx, - const unsigned int & new_my) -{ - unsigned int mx, my; - unsigned char cost = _use_min_cost_neighbor ? 255 : 0; - unsigned int x_offset = new_mx * _downsampling_factor; - unsigned int y_offset = new_my * _downsampling_factor; - - for (unsigned int i = 0; i < _downsampling_factor; ++i) { - mx = x_offset + i; - if (mx >= _size_x) { - continue; - } - for (unsigned int j = 0; j < _downsampling_factor; ++j) { - my = y_offset + j; - if (my >= _size_y) { - continue; - } - cost = _use_min_cost_neighbor ? - std::min(cost, _costmap->getCost(mx, my)) : - std::max(cost, _costmap->getCost(mx, my)); - } - } - - _downsampled_costmap->setCost(new_mx, new_my, cost); -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp deleted file mode 100644 index 1b16307e..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/distance_heuristic.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright (c) 2026, 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. Reserved. - -#include "ompl/base/ScopedState.h" -#include "ompl/base/spaces/DubinsStateSpace.h" -#include "ompl/base/spaces/ReedsSheppStateSpace.h" -#include "athena_smac_planner/distance_heuristic.hpp" -#include "athena_smac_planner/node_hybrid.hpp" - -namespace athena_smac_planner -{ - -template<> -template -void DistanceHeuristic::precomputeDistanceHeuristic( - const float & lookup_table_dim, - const MotionModel & motion_model, - const unsigned int & dim_3_size, - const SearchInfo & search_info, - MotionTableT & motion_table) -{ - // Dubin or Reeds-Shepp shortest distances - if (motion_model == MotionModel::DUBIN) { - motion_table.state_space = std::make_shared( - search_info.minimum_turning_radius); - } else if (motion_model == MotionModel::REEDS_SHEPP) { - motion_table.state_space = std::make_shared( - search_info.minimum_turning_radius); - } else { - throw std::runtime_error( - "Node attempted to precompute distance heuristics " - "with invalid motion model!"); - } - - ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); - to[0] = 0.0; - to[1] = 0.0; - to[2] = 0.0; - size_lookup_ = lookup_table_dim; - float motion_heuristic = 0.0; - unsigned int index = 0; - int dim_3_size_int = static_cast(dim_3_size); - float angular_bin_size = 2 * M_PI / static_cast(dim_3_size); - - // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal - // to help drive the search towards admissible approaches. Deu to symmetries in the - // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror - // around the X axis any relative node lookup. This reduces memory overhead and increases - // the size of a window a platform can store in memory. - dist_heuristic_lookup_table_.resize(size_lookup_ * ceil(size_lookup_ / 2.0) * dim_3_size_int); - for (float x = ceil(-size_lookup_ / 2.0); x <= floor(size_lookup_ / 2.0); x += 1.0) { - for (float y = 0.0; y <= floor(size_lookup_ / 2.0); y += 1.0) { - for (int heading = 0; heading != dim_3_size_int; heading++) { - from[0] = x; - from[1] = y; - from[2] = heading * angular_bin_size; - motion_heuristic = motion_table.state_space->distance(from(), to()); - dist_heuristic_lookup_table_[index] = motion_heuristic; - index++; - } - } - } -} - -template -template -float DistanceHeuristic::getDistanceHeuristic( - const Coordinates & node_coords, - const Coordinates & goal_coords, - const float & obstacle_heuristic, - MotionTableT & motion_table) -{ - // rotate and translate node_coords such that goal_coords relative is (0,0,0) - // Due to the rounding involved in exact cell increments for caching, - // this is not an exact replica of a live heuristic, but has bounded error. - // (Usually less than 1 cell length) - - // This angle is negative since we are de-rotating the current node - // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th) - const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta]; - const float cos_th = trig_vals.first; - const float sin_th = -trig_vals.second; - const float dx = node_coords.x - goal_coords.x; - const float dy = node_coords.y - goal_coords.y; - - double dtheta_bin = node_coords.theta - goal_coords.theta; - if (dtheta_bin < 0) { - dtheta_bin += motion_table.num_angle_quantization; - } - if (dtheta_bin > motion_table.num_angle_quantization) { - dtheta_bin -= motion_table.num_angle_quantization; - } - - Coordinates node_coords_relative( - round(dx * cos_th - dy * sin_th), - round(dx * sin_th + dy * cos_th), - round(dtheta_bin)); - - // Check if the relative node coordinate is within the localized window around the goal - // to apply the distance heuristic. Since the lookup table is contains only the positive - // X axis, we mirror the Y and theta values across the X axis to find the heuristic values. - float motion_heuristic = 0.0; - const int floored_size = floor(size_lookup_ / 2.0); - const int ceiling_size = ceil(size_lookup_ / 2.0); - const float mirrored_relative_y = abs(node_coords_relative.y); - if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) { - // Need to mirror angle if Y coordinate was mirrored - int theta_pos; - if (node_coords_relative.y < 0.0) { - theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta; - } else { - theta_pos = node_coords_relative.theta; - } - const int x_pos = node_coords_relative.x + floored_size; - const int y_pos = static_cast(mirrored_relative_y); - const int index = - x_pos * ceiling_size * motion_table.num_angle_quantization + - y_pos * motion_table.num_angle_quantization + - theta_pos; - motion_heuristic = dist_heuristic_lookup_table_[index]; - } else if (obstacle_heuristic <= 0.0) { - ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); - to[0] = goal_coords.x; - to[1] = goal_coords.y; - from[0] = node_coords.x; - from[1] = node_coords.y; - if constexpr (std::is_same_v) { - to[2] = goal_coords.theta * motion_table.num_angle_quantization; - from[2] = node_coords.theta * motion_table.num_angle_quantization; - } else { - to[2] = motion_table.getAngleFromBin(goal_coords.theta); - from[2] = motion_table.getAngleFromBin(node_coords.theta); - } - motion_heuristic = motion_table.state_space->distance(from(), to()); - } - - return motion_heuristic; -} - -// Instantiate algorithm for the supported template types -template void DistanceHeuristic::precomputeDistanceHeuristic( - const float &, const MotionModel &, const unsigned int &, const SearchInfo &, - HybridMotionTable &); -template float DistanceHeuristic::getDistanceHeuristic( - const Coordinates &, const Coordinates &, const float &, HybridMotionTable &); -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp deleted file mode 100644 index 76c3b95b..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/global_planner_node.cpp +++ /dev/null @@ -1,159 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 - -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "athena_smac_planner/smac_planner_hybrid.hpp" -#include "msgs/msg/planner_event.hpp" - -class GlobalPlannerNode : public rclcpp::Node -{ -public: - explicit GlobalPlannerNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("global_planner", options) - { - declare_parameter("use_sim_time", false); - } - - void configure() - { - costmap_ros_ = std::make_shared( - "global_costmap", - get_namespace(), - "global_costmap"); - costmap_ros_->set_parameter(rclcpp::Parameter("use_sim_time", get_parameter("use_sim_time").as_bool())); - costmap_ros_->configure(); - costmap_ros_->activate(); - - planner_ = std::make_shared( - shared_from_this(), costmap_ros_); - - path_pub_ = create_publisher( - "/global_path", rclcpp::QoS(1).transient_local()); - event_pub_ = create_publisher("/planner_event", 10); - - robot_pose_sub_ = create_subscription( - "/robot_pose", 10, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - std::optional pending; - { - std::lock_guard lk(mutex_); - robot_pose_ = *msg; - // If a goal arrived before robot_pose was ready, plan now - if (pending_goal_.has_value()) { - pending = *pending_goal_; - pending_goal_.reset(); - } - } - if (pending.has_value()) { - RCLCPP_INFO(get_logger(), "[global_planner] robot_pose received — planning pending goal"); - plan(*pending); - } - }); - - goal_pose_sub_ = create_subscription( - "/goal_pose", 10, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { onGoal(msg); }); - - RCLCPP_INFO(get_logger(), "GlobalPlannerNode configured."); - } - - std::shared_ptr getCostmapROS() - { - return costmap_ros_; - } - -private: - void publishEvent(uint8_t event_type) - { - msgs::msg::PlannerEvent ev; - ev.event = event_type; - event_pub_->publish(ev); - } - - void onGoal(const geometry_msgs::msg::PoseStamped::SharedPtr goal) - { - publishEvent(msgs::msg::PlannerEvent::NEW_GOAL); - - { - std::lock_guard lk(mutex_); - if (!robot_pose_.has_value()) { - RCLCPP_WARN(get_logger(), - "[global_planner] goal received but no robot_pose yet — waiting"); - pending_goal_ = *goal; // cache; plan() called once robot_pose arrives - return; - } - // Overwrite any stale pending goal — the new goal supersedes it - pending_goal_.reset(); - } - - plan(*goal); - } - - void plan(const geometry_msgs::msg::PoseStamped & goal) - { - geometry_msgs::msg::PoseStamped start; - { - std::lock_guard lk(mutex_); - start = robot_pose_.value(); - } - - publishEvent(msgs::msg::PlannerEvent::PLANNING); - - try { - auto path = planner_->createPlan(start, goal, [] { return false; }); - path_pub_->publish(path); - publishEvent(msgs::msg::PlannerEvent::PLAN_SUCCEEDED); - } catch (const std::exception & ex) { - RCLCPP_ERROR(get_logger(), "Planning failed: %s", ex.what()); - publishEvent(msgs::msg::PlannerEvent::PLAN_FAILED); - } - } - - std::shared_ptr costmap_ros_; - std::shared_ptr planner_; - - rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Publisher::SharedPtr event_pub_; - rclcpp::Subscription::SharedPtr robot_pose_sub_; - rclcpp::Subscription::SharedPtr goal_pose_sub_; - - std::mutex mutex_; - std::optional robot_pose_; - std::optional pending_goal_; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - node->configure(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.add_node(node->getCostmapROS()->get_node_base_interface()); - executor.spin(); - - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp deleted file mode 100644 index ee1dc183..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_basic.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#include "athena_smac_planner/node_basic.hpp" - -namespace athena_smac_planner -{ - -template -void NodeBasic::processSearchNode() -{ -} - -template<> -void NodeBasic::processSearchNode() -{ - // We only want to override the node's pose if it has not yet been visited - // to prevent the case that a node has been queued multiple times and - // a new branch is overriding one of lower cost already visited. - if (!this->graph_node_ptr->wasVisited()) { - this->graph_node_ptr->pose = this->pose; - this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index, this->turn_dir); - } -} - -template -void NodeBasic::populateSearchNode(NodeT * & node) -{ - this->graph_node_ptr = node; -} - -template<> -void NodeBasic::populateSearchNode(NodeHybrid * & node) -{ - this->pose = node->pose; - this->graph_node_ptr = node; - this->motion_index = node->getMotionPrimitiveIndex(); - this->turn_dir = node->getTurnDirection(); -} - -template class NodeBasic; - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp deleted file mode 100644 index 0c71a622..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/node_hybrid.cpp +++ /dev/null @@ -1,540 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// Copyright (c) 2023, 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. Reserved. - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ompl/base/ScopedState.h" -#include "ompl/base/spaces/DubinsStateSpace.h" -#include "ompl/base/spaces/ReedsSheppStateSpace.h" - -#include "athena_smac_planner/node_hybrid.hpp" - -using namespace std::chrono; // NOLINT - -namespace athena_smac_planner -{ - -// Each of these tables are the projected motion models through -// time and space applied to the search on the current node in -// continuous map-coordinates (e.g. not meters but partial map cells) -// Currently, these are set to project *at minimum* into a neighboring -// cell. Though this could be later modified to project a certain -// amount of time or particular distance forward. - -// http://planning.cs.uiuc.edu/planning/node821.html -// Model for ackermann style vehicle with minimum radius restriction -void HybridMotionTable::initDubin( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, - SearchInfo & search_info) -{ - size_x = size_x_in; - change_penalty = search_info.change_penalty; - non_straight_penalty = search_info.non_straight_penalty; - cost_penalty = search_info.cost_penalty; - reverse_penalty = search_info.reverse_penalty; - travel_distance_reward = 1.0f - search_info.retrospective_penalty; - downsample_obstacle_heuristic = search_info.downsample_obstacle_heuristic; - use_quadratic_cost_penalty = search_info.use_quadratic_cost_penalty; - - // if nothing changed, no need to re-compute primitives - if (num_angle_quantization_in == num_angle_quantization && - min_turning_radius == search_info.minimum_turning_radius && - allow_primitive_interpolation == search_info.allow_primitive_interpolation && - motion_model == MotionModel::DUBIN) - { - return; - } - - num_angle_quantization = num_angle_quantization_in; - num_angle_quantization_float = static_cast(num_angle_quantization); - min_turning_radius = search_info.minimum_turning_radius; - allow_primitive_interpolation = search_info.allow_primitive_interpolation; - motion_model = MotionModel::DUBIN; - - // angle must meet 3 requirements: - // 1) be increment of quantized bin size - // 2) chord length must be greater than sqrt(2) to leave current cell - // 3) maximum curvature must be respected, represented by minimum turning angle - // Thusly: - // On circle of radius minimum turning angle, we need select motion primitives - // with chord length > sqrt(2) and be an increment of our bin size - // - // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size - // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) - float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); - // Now make sure angle is an increment of the quantized bin size - // And since its based on the minimum chord, we need to make sure its always larger - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); - float increments; - if (angle < bin_size) { - increments = 1.0f; - } else { - // Search dimensions are clean multiples of quantization - this prevents - // paths with loops in them - increments = ceil(angle / bin_size); - } - angle = increments * bin_size; - - // find deflections - // If we make a right triangle out of the chord in circle of radius - // min turning angle, we can see that delta X = R * sin (angle) - const float delta_x = min_turning_radius * sin(angle); - // Using that same right triangle, we can see that the complement - // to delta Y is R * cos (angle). If we subtract R, we get the actual value - const float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); - const float delta_dist = hypotf(delta_x, delta_y); - - projections.clear(); - projections.reserve(3); - projections.emplace_back(delta_dist, 0.0, 0.0, TurnDirection::FORWARD); // Forward - projections.emplace_back(delta_x, delta_y, increments, TurnDirection::LEFT); // Left - projections.emplace_back(delta_x, -delta_y, -increments, TurnDirection::RIGHT); // Right - - if (search_info.allow_primitive_interpolation && increments > 1.0f) { - // Create primitives that are +/- N to fill in search space to use all set angular quantizations - // Allows us to create N many primitives so that each search iteration can expand into any angle - // bin possible with the minimum turning radius constraint, not just the most extreme turns. - projections.reserve(3 + (2 * (increments - 1))); - for (unsigned int i = 1; i < static_cast(increments); i++) { - const float angle_n = static_cast(i) * bin_size; - const float turning_rad_n = delta_dist / (2.0f * sin(angle_n / 2.0f)); - const float delta_x_n = turning_rad_n * sin(angle_n); - const float delta_y_n = turning_rad_n - (turning_rad_n * cos(angle_n)); - projections.emplace_back( - delta_x_n, delta_y_n, static_cast(i), TurnDirection::LEFT); // Left - projections.emplace_back( - delta_x_n, -delta_y_n, -static_cast(i), TurnDirection::RIGHT); // Right - } - } - - // Create the correct OMPL state space - state_space = std::make_shared(min_turning_radius); - - // Precompute projection deltas - delta_xs.resize(projections.size()); - delta_ys.resize(projections.size()); - trig_values.resize(num_angle_quantization); - - for (unsigned int i = 0; i != projections.size(); i++) { - delta_xs[i].resize(num_angle_quantization); - delta_ys[i].resize(num_angle_quantization); - - for (unsigned int j = 0; j != num_angle_quantization; j++) { - double cos_theta = cos(bin_size * j); - double sin_theta = sin(bin_size * j); - if (i == 0) { - // if first iteration, cache the trig values for later - trig_values[j] = {cos_theta, sin_theta}; - } - delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; - delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; - } - } - - // Precompute travel costs for each motion primitive - travel_costs.resize(projections.size()); - for (unsigned int i = 0; i != projections.size(); i++) { - const TurnDirection turn_dir = projections[i]._turn_dir; - if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { - // Turning, so length is the arc length - const float arc_angle = projections[i]._theta * bin_size; - const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); - travel_costs[i] = turning_rad * arc_angle; - } else { - travel_costs[i] = delta_dist; - } - } -} - -// http://planning.cs.uiuc.edu/planning/node822.html -// Same as Dubin model but now reverse is valid -// See notes in Dubin for explanation -void HybridMotionTable::initReedsShepp( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, - SearchInfo & search_info) -{ - size_x = size_x_in; - change_penalty = search_info.change_penalty; - non_straight_penalty = search_info.non_straight_penalty; - cost_penalty = search_info.cost_penalty; - reverse_penalty = search_info.reverse_penalty; - travel_distance_reward = 1.0f - search_info.retrospective_penalty; - downsample_obstacle_heuristic = search_info.downsample_obstacle_heuristic; - use_quadratic_cost_penalty = search_info.use_quadratic_cost_penalty; - - // if nothing changed, no need to re-compute primitives - if (num_angle_quantization_in == num_angle_quantization && - min_turning_radius == search_info.minimum_turning_radius && - allow_primitive_interpolation == search_info.allow_primitive_interpolation && - motion_model == MotionModel::REEDS_SHEPP) - { - return; - } - - num_angle_quantization = num_angle_quantization_in; - num_angle_quantization_float = static_cast(num_angle_quantization); - min_turning_radius = search_info.minimum_turning_radius; - allow_primitive_interpolation = search_info.allow_primitive_interpolation; - motion_model = MotionModel::REEDS_SHEPP; - - float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); - float increments; - if (angle < bin_size) { - increments = 1.0f; - } else { - increments = ceil(angle / bin_size); - } - angle = increments * bin_size; - - const float delta_x = min_turning_radius * sin(angle); - const float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); - const float delta_dist = hypotf(delta_x, delta_y); - - projections.clear(); - projections.reserve(6); - projections.emplace_back(delta_dist, 0.0, 0.0, TurnDirection::FORWARD); // Forward - projections.emplace_back( - delta_x, delta_y, increments, TurnDirection::LEFT); // Forward + Left - projections.emplace_back( - delta_x, -delta_y, -increments, TurnDirection::RIGHT); // Forward + Right - projections.emplace_back(-delta_dist, 0.0, 0.0, TurnDirection::REVERSE); // Backward - projections.emplace_back( - -delta_x, delta_y, -increments, TurnDirection::REV_LEFT); // Backward + Left - projections.emplace_back( - -delta_x, -delta_y, increments, TurnDirection::REV_RIGHT); // Backward + Right - - if (search_info.allow_primitive_interpolation && increments > 1.0f) { - // Create primitives that are +/- N to fill in search space to use all set angular quantizations - // Allows us to create N many primitives so that each search iteration can expand into any angle - // bin possible with the minimum turning radius constraint, not just the most extreme turns. - projections.reserve(6 + (4 * (increments - 1))); - for (unsigned int i = 1; i < static_cast(increments); i++) { - const float angle_n = static_cast(i) * bin_size; - const float turning_rad_n = delta_dist / (2.0f * sin(angle_n / 2.0f)); - const float delta_x_n = turning_rad_n * sin(angle_n); - const float delta_y_n = turning_rad_n - (turning_rad_n * cos(angle_n)); - projections.emplace_back( - delta_x_n, delta_y_n, static_cast(i), TurnDirection::LEFT); // Forward + Left - projections.emplace_back( - delta_x_n, -delta_y_n, -static_cast(i), TurnDirection::RIGHT); // Forward + Right - projections.emplace_back( - -delta_x_n, delta_y_n, -static_cast(i), - TurnDirection::REV_LEFT); // Backward + Left - projections.emplace_back( - -delta_x_n, -delta_y_n, static_cast(i), - TurnDirection::REV_RIGHT); // Backward + Right - } - } - - // Create the correct OMPL state space - state_space = std::make_shared(min_turning_radius); - - // Precompute projection deltas - delta_xs.resize(projections.size()); - delta_ys.resize(projections.size()); - trig_values.resize(num_angle_quantization); - - for (unsigned int i = 0; i != projections.size(); i++) { - delta_xs[i].resize(num_angle_quantization); - delta_ys[i].resize(num_angle_quantization); - - for (unsigned int j = 0; j != num_angle_quantization; j++) { - double cos_theta = cos(bin_size * j); - double sin_theta = sin(bin_size * j); - if (i == 0) { - // if first iteration, cache the trig values for later - trig_values[j] = {cos_theta, sin_theta}; - } - delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; - delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; - } - } - - // Precompute travel costs for each motion primitive - travel_costs.resize(projections.size()); - for (unsigned int i = 0; i != projections.size(); i++) { - const TurnDirection turn_dir = projections[i]._turn_dir; - if (turn_dir != TurnDirection::FORWARD && turn_dir != TurnDirection::REVERSE) { - // Turning, so length is the arc length - const float arc_angle = projections[i]._theta * bin_size; - const float turning_rad = delta_dist / (2.0f * sin(arc_angle / 2.0f)); - travel_costs[i] = turning_rad * arc_angle; - } else { - travel_costs[i] = delta_dist; - } - } -} - -MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) -{ - MotionPoses projection_list; - projection_list.reserve(projections.size()); - - for (unsigned int i = 0; i != projections.size(); i++) { - const MotionPose & proj_motion_model = projections[i]; - - // normalize theta, I know its overkill, but I've been burned before... - const float & node_heading = node->pose.theta; - float new_heading = node_heading + proj_motion_model._theta; - - if (new_heading < 0.0) { - new_heading += num_angle_quantization_float; - } - - if (new_heading >= num_angle_quantization_float) { - new_heading -= num_angle_quantization_float; - } - - projection_list.emplace_back( - delta_xs[i][node_heading] + node->pose.x, - delta_ys[i][node_heading] + node->pose.y, - new_heading, proj_motion_model._turn_dir); - } - - return projection_list; -} - -unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) -{ - auto bin = static_cast(round(static_cast(theta) / bin_size)); - return bin < num_angle_quantization ? bin : 0u; -} - -float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) -{ - return bin_idx * bin_size; -} - -double HybridMotionTable::getAngle(const double & theta) -{ - return theta / bin_size; -} - -NodeHybrid::NodeHybrid(const uint64_t index, NodeContext * ctx) -: parent(nullptr), - pose(0.0f, 0.0f, 0.0f), - _cell_cost(std::numeric_limits::quiet_NaN()), - _accumulated_cost(std::numeric_limits::max()), - _index(index), - _was_visited(false), - _motion_primitive_index(std::numeric_limits::max()), - _is_node_valid(false), - _ctx(ctx) -{ -} - -NodeHybrid::~NodeHybrid() -{ - parent = nullptr; -} - -void NodeHybrid::reset() -{ - parent = nullptr; - _cell_cost = std::numeric_limits::quiet_NaN(); - _accumulated_cost = std::numeric_limits::max(); - _was_visited = false; - _motion_primitive_index = std::numeric_limits::max(); - pose.x = 0.0f; - pose.y = 0.0f; - pose.theta = 0.0f; - _is_node_valid = false; -} - -bool NodeHybrid::isNodeValid( - const bool & traverse_unknown, - GridCollisionChecker * collision_checker) -{ - // Already found, we can return the result - if (!std::isnan(_cell_cost)) { - return _is_node_valid; - } - - _is_node_valid = !collision_checker->inCollision( - this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown); - _cell_cost = collision_checker->getCost(); - return _is_node_valid; -} - -float NodeHybrid::getTraversalCost(const NodePtr & child) -{ - const float normalized_cost = child->getCost() / 252.0f; - if (std::isnan(normalized_cost)) { - throw std::runtime_error( - "Node attempted to get traversal " - "cost without a known SE2 collision cost!"); - } - - const TurnDirection & child_turn_dir = child->getTurnDirection(); - float travel_cost_raw = _ctx->motion_table.travel_costs[child->getMotionPrimitiveIndex()]; - float travel_cost = 0.0; - - if (_ctx->motion_table.use_quadratic_cost_penalty) { - travel_cost_raw *= - (_ctx->motion_table.travel_distance_reward + - (_ctx->motion_table.cost_penalty * normalized_cost * normalized_cost)); - } else { - travel_cost_raw *= - (_ctx->motion_table.travel_distance_reward + _ctx->motion_table.cost_penalty * - normalized_cost); - } - - if (child_turn_dir == TurnDirection::FORWARD || child_turn_dir == TurnDirection::REVERSE || - getMotionPrimitiveIndex() == std::numeric_limits::max()) - { - // New motion is a straight motion, no additional costs to be applied - travel_cost = travel_cost_raw; - } else { - if (getTurnDirection() == child_turn_dir) { - // Turning motion but keeps in same direction: encourages to commit to turning if starting it - travel_cost = travel_cost_raw * _ctx->motion_table.non_straight_penalty; - } else { - // Turning motion and changing direction: penalizes wiggling - travel_cost = travel_cost_raw * - (_ctx->motion_table.non_straight_penalty + _ctx->motion_table.change_penalty); - } - } - - if (child_turn_dir == TurnDirection::REV_RIGHT || - child_turn_dir == TurnDirection::REV_LEFT || - child_turn_dir == TurnDirection::REVERSE) - { - // reverse direction - travel_cost *= _ctx->motion_table.reverse_penalty; - } - - return travel_cost; -} - -float NodeHybrid::getHeuristicCost( - const Coordinates & node_coords, - const CoordinateVector & goals_coords) -{ - // obstacle heuristic does not depend on goal heading - const float obstacle_heuristic = - _ctx->obstacle_heuristic->getObstacleHeuristic(node_coords, _ctx->motion_table.cost_penalty, - _ctx->motion_table.use_quadratic_cost_penalty, - _ctx->motion_table.downsample_obstacle_heuristic); - float distance_heuristic = std::numeric_limits::max(); - for (unsigned int i = 0; i < goals_coords.size(); i++) { - distance_heuristic = std::min( - distance_heuristic, - _ctx->distance_heuristic->getDistanceHeuristic(node_coords, goals_coords[i], - obstacle_heuristic, _ctx->motion_table)); - } - return std::max(obstacle_heuristic, distance_heuristic); -} - -void NodeHybrid::initMotionModel( - NodeContext * ctx, - const MotionModel & motion_model, - unsigned int & size_x, - unsigned int & size_y, - unsigned int & num_angle_quantization, - SearchInfo & search_info) -{ - // find the motion model selected - switch (motion_model) { - case MotionModel::DUBIN: - ctx->motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); - break; - case MotionModel::REEDS_SHEPP: - ctx->motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); - break; - default: - throw std::runtime_error( - "Invalid motion model for Hybrid A*. Please select between" - " Dubin (Ackermann forward only)," - " Reeds-Shepp (Ackermann forward and back)."); - } -} - -void NodeHybrid::getNeighbors( - std::function & NeighborGetter, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown, - NodeVector & neighbors) -{ - uint64_t index = 0; - NodePtr neighbor = nullptr; - Coordinates initial_node_coords; - const MotionPoses motion_projections = _ctx->motion_table.getProjections(this); - - for (unsigned int i = 0; i != motion_projections.size(); i++) { - index = NodeHybrid::getIndex( - static_cast(motion_projections[i]._x), - static_cast(motion_projections[i]._y), - static_cast(motion_projections[i]._theta), - _ctx->motion_table.size_x, _ctx->motion_table.num_angle_quantization); - - if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { - // Cache the initial pose in case it was visited but valid - // don't want to disrupt continuous coordinate expansion - initial_node_coords = neighbor->pose; - neighbor->setPose( - Coordinates( - motion_projections[i]._x, - motion_projections[i]._y, - motion_projections[i]._theta)); - if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { - neighbor->setMotionPrimitiveIndex(i, motion_projections[i]._turn_dir); - neighbors.push_back(neighbor); - } else { - neighbor->setPose(initial_node_coords); - } - } - } -} - -bool NodeHybrid::backtracePath(CoordinateVector & path) -{ - if (!this->parent) { - return false; - } - - NodePtr current_node = this; - - while (current_node->parent) { - path.push_back(current_node->pose); - // Convert angle to radians - path.back().theta = _ctx->motion_table.getAngleFromBin(path.back().theta); - current_node = current_node->parent; - } - - // add the start pose - path.push_back(current_node->pose); - // Convert angle to radians - path.back().theta = _ctx->motion_table.getAngleFromBin(path.back().theta); - - return true; -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp deleted file mode 100644 index 74715329..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/obstacle_heuristic.cpp +++ /dev/null @@ -1,230 +0,0 @@ -// Copyright (c) 2026, 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. Reserved. - -#include "athena_smac_planner/obstacle_heuristic.hpp" - -namespace athena_smac_planner -{ - -void ObstacleHeuristic::resetObstacleHeuristic( - std::shared_ptr costmap_ros_i, - const unsigned int & start_x, const unsigned int & start_y, - const unsigned int & goal_x, const unsigned int & goal_y, - const bool downsample_obstacle_heuristic) -{ - // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up - // the planner considerably to search through 75% less cells with no detectable - // erosion of path quality after even modest smoothing. The error would be no more - // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality - costmap_ros = costmap_ros_i; - auto costmap = costmap_ros->getCostmap(); - - // Clear lookup table - unsigned int size = 0u; - unsigned int size_x = 0u; - if (downsample_obstacle_heuristic) { - size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); - size = size_x * - ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); - } else { - size_x = costmap->getSizeInCellsX(); - size = size_x * costmap->getSizeInCellsY(); - } - - if (obstacle_heuristic_lookup_table_.size() == size) { - // must reset all values - std::fill( - obstacle_heuristic_lookup_table_.begin(), - obstacle_heuristic_lookup_table_.end(), 0.0f); - } else { - unsigned int obstacle_size = obstacle_heuristic_lookup_table_.size(); - obstacle_heuristic_lookup_table_.resize(size, 0.0f); - // must reset values for non-constructed indices - std::fill_n( - obstacle_heuristic_lookup_table_.begin(), obstacle_size, 0.0f); - } - - obstacle_heuristic_queue_.clear(); - obstacle_heuristic_queue_.reserve(size); - - // Set initial goal point to queue from. Divided by 2 due to downsampled costmap. - unsigned int goal_index; - if (downsample_obstacle_heuristic) { - goal_index = floor(goal_y / 2.0f) * size_x + floor(goal_x / 2.0f); - } else { - goal_index = floor(goal_y) * size_x + floor(goal_x); - } - - obstacle_heuristic_queue_.emplace_back( - distanceHeuristic2D(goal_index, size_x, start_x, start_y), goal_index); - - // initialize goal cell with a very small value to differentiate it from 0.0 (~uninitialized) - // the negative value means the cell is in the open set - obstacle_heuristic_lookup_table_[goal_index] = -0.00001f; -} - -float ObstacleHeuristic::getObstacleHeuristic( - const Coordinates & node_coords, - const float & cost_penalty, - const bool use_quadratic_cost_penalty, - const bool downsample_obstacle_heuristic) -{ - // If already expanded, return the cost - auto costmap = costmap_ros->getCostmap(); - unsigned int size_x = 0u; - unsigned int size_y = 0u; - if (downsample_obstacle_heuristic) { - size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); - size_y = ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); - } else { - size_x = costmap->getSizeInCellsX(); - size_y = costmap->getSizeInCellsY(); - } - - // Divided by 2 due to downsampled costmap. - unsigned int start_y, start_x; - if (downsample_obstacle_heuristic) { - start_y = floor(node_coords.y / 2.0f); - start_x = floor(node_coords.x / 2.0f); - } else { - start_y = floor(node_coords.y); - start_x = floor(node_coords.x); - } - - const unsigned int start_index = start_y * size_x + start_x; - const float & requested_node_cost = obstacle_heuristic_lookup_table_[start_index]; - if (requested_node_cost > 0.0f) { - // costs are doubled due to downsampling - return downsample_obstacle_heuristic ? 2.0f * requested_node_cost : requested_node_cost; - } - - // If not, expand until it is included. This dynamic programming ensures that - // we only expand the MINIMUM spanning set of the costmap per planning request. - // Rather than naively expanding the entire (potentially massive) map for a limited - // path, we only expand to the extent required for the furthest expansion in the - // search-planning request that dynamically updates during search as needed. - - // start_x and start_y have changed since last call - // we need to recompute 2D distance heuristic and reprioritize queue - for (auto & n : obstacle_heuristic_queue_) { - n.first = -obstacle_heuristic_lookup_table_[n.second] + - distanceHeuristic2D(n.second, size_x, start_x, start_y); - } - std::make_heap( - obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), - ObstacleHeuristicComparator{}); - - const int size_x_int = static_cast(size_x); - const float sqrt2 = sqrtf(2.0f); - float c_cost, cost, travel_cost, new_cost, existing_cost; - unsigned int mx, my; - unsigned int idx, new_idx = 0; - - const std::vector neighborhood = {1, -1, // left right - size_x_int, -size_x_int, // up down - size_x_int + 1, size_x_int - 1, // upper diagonals - -size_x_int + 1, -size_x_int - 1}; // lower diagonals - - while (!obstacle_heuristic_queue_.empty()) { - idx = obstacle_heuristic_queue_.front().second; - std::pop_heap( - obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), - ObstacleHeuristicComparator{}); - obstacle_heuristic_queue_.pop_back(); - c_cost = obstacle_heuristic_lookup_table_[idx]; - if (c_cost > 0.0f) { - // cell has been processed and closed, no further cost improvements - // are mathematically possible thanks to euclidean distance heuristic consistency - continue; - } - c_cost = -c_cost; - obstacle_heuristic_lookup_table_[idx] = c_cost; // set a positive value to close the cell - - // find neighbors - for (unsigned int i = 0; i != neighborhood.size(); i++) { - new_idx = static_cast(static_cast(idx) + neighborhood[i]); - - // if neighbor path is better and non-lethal, set new cost and add to queue - if (new_idx < size_x * size_y) { - if (downsample_obstacle_heuristic) { - // Get costmap values as if downsampled - unsigned int y_offset = (new_idx / size_x) * 2; - unsigned int x_offset = (new_idx - ((new_idx / size_x) * size_x)) * 2; - cost = costmap->getCost(x_offset, y_offset); - for (unsigned int k = 0; k < 2u; ++k) { - unsigned int mxd = x_offset + k; - if (mxd >= costmap->getSizeInCellsX()) { - continue; - } - for (unsigned int j = 0; j < 2u; ++j) { - unsigned int myd = y_offset + j; - if (myd >= costmap->getSizeInCellsY()) { - continue; - } - if (k == 0 && j == 0) { - continue; - } - cost = std::min(cost, static_cast(costmap->getCost(mxd, myd))); - } - } - } else { - cost = static_cast(costmap->getCost(new_idx)); - } - - if (cost >= INSCRIBED_COST) { - continue; - } - - my = new_idx / size_x; - mx = new_idx - (my * size_x); - - if (mx >= size_x - 3 || mx <= 3) { - continue; - } - if (my >= size_y - 3 || my <= 3) { - continue; - } - - existing_cost = obstacle_heuristic_lookup_table_[new_idx]; - if (existing_cost <= 0.0f) { - if (use_quadratic_cost_penalty) { - travel_cost = - (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 63504.0f)); // 252^2 - } else { - travel_cost = - ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 252.0f)); - } - - new_cost = c_cost + travel_cost; - if (existing_cost == 0.0f || -existing_cost > new_cost) { - // the negative value means the cell is in the open set - obstacle_heuristic_lookup_table_[new_idx] = -new_cost; - obstacle_heuristic_queue_.emplace_back( - new_cost + distanceHeuristic2D(new_idx, size_x, start_x, start_y), new_idx); - std::push_heap( - obstacle_heuristic_queue_.begin(), obstacle_heuristic_queue_.end(), - ObstacleHeuristicComparator{}); - } - } - } - } - - if (idx == start_index) { - break; - } - } - return downsample_obstacle_heuristic ? 2.0f * requested_node_cost : requested_node_cost; -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp deleted file mode 100644 index bfe1879d..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/smac_planner_hybrid.cpp +++ /dev/null @@ -1,478 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2023, 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. Reserved. - -#include -#include -#include -#include -#include -#include - -#include "athena_smac_planner/smac_planner_hybrid.hpp" - -// #define BENCHMARK_TESTING - -namespace athena_smac_planner -{ - -using namespace std::chrono; // NOLINT - -SmacPlannerHybrid::SmacPlannerHybrid( - rclcpp::Node::SharedPtr node, - std::shared_ptr costmap_ros, - const std::string & name) -: _a_star(nullptr), - _collision_checker(nullptr, 1, nullptr), - _smoother(nullptr), - _costmap(nullptr), - _costmap_downsampler(nullptr) -{ - _name = name; - _logger = node->get_logger(); - _clock = node->get_clock(); - _costmap = costmap_ros->getCostmap(); - _costmap_ros = costmap_ros; - _global_frame = costmap_ros->getGlobalFrameID(); - - RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerHybrid", _name.c_str()); - - // Helper: declare param if absent then get its value - auto p = [&node, &name](const std::string & param, auto def) { - if (!node->has_parameter(name + "." + param)) { - node->declare_parameter(name + "." + param, def); - } - return node->get_parameter(name + "." + param).get_value(); - }; - - int angle_quantizations; - double analytic_expansion_max_length_m; - bool smooth_path; - - // General planner params - _downsample_costmap = p("downsample_costmap", false); - _downsampling_factor = p("downsampling_factor", 1); - - angle_quantizations = p("angle_quantization_bins", 72); - _angle_bin_size = 2.0 * M_PI / angle_quantizations; - _angle_quantizations = static_cast(angle_quantizations); - - _tolerance = static_cast(p("tolerance", 0.25)); - _allow_unknown = p("allow_unknown", true); - _max_iterations = p("max_iterations", 1000000); - _max_on_approach_iterations = p("max_on_approach_iterations", 1000); - _terminal_checking_interval = p("terminal_checking_interval", 5000); - smooth_path = p("smooth_path", true); - - _minimum_turning_radius_global_coords = p("minimum_turning_radius", 0.4); - _search_info.allow_primitive_interpolation = p("allow_primitive_interpolation", false); - _search_info.cache_obstacle_heuristic = p("cache_obstacle_heuristic", false); - _search_info.reverse_penalty = p("reverse_penalty", 2.0); - _search_info.change_penalty = p("change_penalty", 0.0); - _search_info.non_straight_penalty = p("non_straight_penalty", 1.2); - _search_info.cost_penalty = p("cost_penalty", 2.0); - _search_info.retrospective_penalty = p("retrospective_penalty", 0.015); - _search_info.analytic_expansion_ratio = p("analytic_expansion_ratio", 3.5); - _search_info.analytic_expansion_max_cost = p("analytic_expansion_max_cost", 200.0); - _search_info.analytic_expansion_max_cost_override = - p("analytic_expansion_max_cost_override", false); - _search_info.use_quadratic_cost_penalty = p("use_quadratic_cost_penalty", false); - _search_info.downsample_obstacle_heuristic = p("downsample_obstacle_heuristic", true); - - analytic_expansion_max_length_m = p("analytic_expansion_max_length", 3.0); - _search_info.analytic_expansion_max_length = - analytic_expansion_max_length_m / _costmap->getResolution(); - - _max_planning_time = p("max_planning_time", 5.0); - _lookup_table_size = p("lookup_table_size", 20.0); - _debug_visualizations = p("debug_visualizations", false); - _motion_model_for_search = p("motion_model_for_search", std::string("DUBIN")); - - std::string goal_heading_type = p("goal_heading_mode", std::string("DEFAULT")); - _goal_heading_mode = fromStringToGH(goal_heading_type); - - _coarse_search_resolution = p("coarse_search_resolution", 1); - - if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { - throw std::runtime_error( - "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION."); - } - - _motion_model = fromString(_motion_model_for_search); - - if (_motion_model == MotionModel::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get MotionModel search type. Given '%s', " - "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.", - _motion_model_for_search.c_str()); - } - - if (_max_on_approach_iterations <= 0) { - RCLCPP_WARN( - _logger, "On approach iteration selected as <= 0, " - "disabling tolerance and on approach iterations."); - _max_on_approach_iterations = std::numeric_limits::max(); - } - - if (_max_iterations <= 0) { - RCLCPP_WARN( - _logger, "maximum iteration selected as <= 0, " - "disabling maximum iterations."); - _max_iterations = std::numeric_limits::max(); - } - - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0, " - "disabling coarse iteration resolution search for goal heading"); - _coarse_search_resolution = 1; - } - - if (_angle_quantizations % _coarse_search_resolution != 0) { - throw std::runtime_error( - "coarse iteration should be an increment of the number of angular bins configured"); - } - - if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) { - RCLCPP_WARN( - _logger, "Min turning radius cannot be less than the search grid cell resolution!"); - _minimum_turning_radius_global_coords = _costmap->getResolution() * _downsampling_factor; - } - - // Convert to grid coordinates - if (!_downsample_costmap) { - _downsampling_factor = 1; - } - _search_info.minimum_turning_radius = - _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); - _lookup_table_dim = - static_cast(_lookup_table_size) / - static_cast(_costmap->getResolution() * _downsampling_factor); - - // Make sure its a whole number - _lookup_table_dim = static_cast(static_cast(_lookup_table_dim)); - - // Make sure its an odd number - if (static_cast(_lookup_table_dim) % 2 == 0) { - RCLCPP_INFO( - _logger, - "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", - _lookup_table_dim); - _lookup_table_dim += 1.0; - } - - // Initialize collision checker - _collision_checker = GridCollisionChecker( - _costmap_ros, _angle_quantizations, node); - _collision_checker.setFootprint( - costmap_ros->getRobotFootprint(), - costmap_ros->getUseRadius(), - findCircumscribedCost(_costmap_ros)); - - // Initialize A* template - _a_star = std::make_unique>(_motion_model, _search_info); - _a_star->initialize( - _allow_unknown, - _max_iterations, - _max_on_approach_iterations, - _terminal_checking_interval, - _max_planning_time, - _lookup_table_dim, - _angle_quantizations); - - // Initialize path smoother - if (smooth_path) { - SmootherParams smoother_params; - smoother_params.get(node.get(), _name); - _smoother = std::make_unique(smoother_params); - _smoother->initialize(_minimum_turning_radius_global_coords); - } - - // Initialize costmap downsampler - if (_downsample_costmap && _downsampling_factor > 1) { - _costmap_downsampler = std::make_unique(); - _costmap_downsampler->on_configure(_costmap, _downsampling_factor); - } - - // Create publishers - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); - - if (_debug_visualizations) { - _expansions_publisher = - node->create_publisher("expansions", 1); - _planned_footprints_publisher = - node->create_publisher("planned_footprints", 1); - _smoothed_footprints_publisher = - node->create_publisher("smoothed_footprints", 1); - } - - RCLCPP_INFO( - _logger, "Configured %s of type SmacPlannerHybrid with " - "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f." - "Using motion model: %s.", - _name.c_str(), _max_iterations, _max_on_approach_iterations, - _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - _tolerance, toString(_motion_model).c_str()); -} - -SmacPlannerHybrid::~SmacPlannerHybrid() -{ - RCLCPP_INFO(_logger, "Destroying %s of type SmacPlannerHybrid", _name.c_str()); - _a_star.reset(); - _smoother.reset(); - if (_costmap_downsampler) { - _costmap_downsampler->on_cleanup(); - _costmap_downsampler.reset(); - } -} - -nav_msgs::msg::Path SmacPlannerHybrid::createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal, - std::function cancel_checker) -{ - std::lock_guard lock_reinit(_mutex); - steady_clock::time_point a = steady_clock::now(); - - std::unique_lock lock(*(_costmap->getMutex())); - - // Downsample costmap, if required - nav2_costmap_2d::Costmap2D * costmap = _costmap; - if (_downsample_costmap && _downsampling_factor > 1) { - costmap = _costmap_downsampler->downsample(_downsampling_factor); - _collision_checker.setCostmap(costmap); - } - - // Set collision checker and costmap information - _collision_checker.setFootprint( - _costmap_ros->getRobotFootprint(), - _costmap_ros->getUseRadius(), - findCircumscribedCost(_costmap_ros)); - _a_star->setCollisionChecker(&_collision_checker); - - // Set starting point, in A* bin search coordinates - float mx_start, my_start, mx_goal, my_goal; - { - unsigned int umx, umy; - if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, umx, umy)) { - throw std::runtime_error( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was outside bounds"); - } - mx_start = static_cast(umx); - my_start = static_cast(umy); - } - - double start_orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size); - while (start_orientation_bin < 0.0) { - start_orientation_bin += static_cast(_angle_quantizations); - } - // This is needed to handle precision issues - if (start_orientation_bin >= static_cast(_angle_quantizations)) { - start_orientation_bin -= static_cast(_angle_quantizations); - } - unsigned int start_orientation_bin_int = - static_cast(start_orientation_bin); - _a_star->setStart(mx_start, my_start, start_orientation_bin_int); - - // Set goal point, in A* bin search coordinates - { - unsigned int umx, umy; - if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, umx, umy)) { - throw std::runtime_error( - "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + - std::to_string(goal.pose.position.y) + ") was outside bounds"); - } - mx_goal = static_cast(umx); - my_goal = static_cast(umy); - } - double goal_orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); - while (goal_orientation_bin < 0.0) { - goal_orientation_bin += static_cast(_angle_quantizations); - } - // This is needed to handle precision issues - if (goal_orientation_bin >= static_cast(_angle_quantizations)) { - goal_orientation_bin -= static_cast(_angle_quantizations); - } - unsigned int goal_orientation_bin_int = - static_cast(goal_orientation_bin); - _a_star->setGoal( - mx_goal, my_goal, static_cast(goal_orientation_bin_int), - _goal_heading_mode, _coarse_search_resolution); - - // Setup message - nav_msgs::msg::Path plan; - plan.header.stamp = _clock->now(); - plan.header.frame_id = _global_frame; - geometry_msgs::msg::PoseStamped pose; - pose.header = plan.header; - pose.pose.position.z = 0.0; - pose.pose.orientation.x = 0.0; - pose.pose.orientation.y = 0.0; - pose.pose.orientation.z = 0.0; - pose.pose.orientation.w = 1.0; - - // Corner case of start and goal being on the same cell - if (std::floor(mx_start) == std::floor(mx_goal) && - std::floor(my_start) == std::floor(my_goal) && - start_orientation_bin_int == goal_orientation_bin_int) - { - pose.pose = start.pose; - pose.pose.orientation = goal.pose.orientation; - plan.poses.push_back(pose); - - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - auto msg = std::make_unique(plan); - _raw_plan_publisher->publish(std::move(msg)); - } - - return plan; - } - - // Compute plan - NodeHybrid::CoordinateVector path; - int num_iterations = 0; - std::string error; - std::unique_ptr>> expansions = nullptr; - if (_debug_visualizations) { - expansions = std::make_unique>>(); - } - - if (!_a_star->createPath( - path, num_iterations, - _tolerance / static_cast(costmap->getResolution()), cancel_checker, expansions.get())) - { - if (_debug_visualizations) { - auto msg = std::make_unique(); - geometry_msgs::msg::Pose msg_pose; - msg->header.stamp = _clock->now(); - msg->header.frame_id = _global_frame; - for (auto & e : *expansions) { - msg_pose.position.x = std::get<0>(e); - msg_pose.position.y = std::get<1>(e); - msg_pose.orientation = getWorldOrientation(std::get<2>(e)); - msg->poses.push_back(msg_pose); - } - _expansions_publisher->publish(std::move(msg)); - } - - if (num_iterations == 1) { - throw std::runtime_error("Start occupied"); - } - - if (num_iterations < _a_star->getMaxIterations()) { - throw std::runtime_error("No valid path could be found"); - } else { - throw std::runtime_error("Exceeded maximum iterations"); - } - } - - // Convert to world coordinates - plan.poses.reserve(path.size()); - for (int i = path.size() - 1; i >= 0; --i) { - pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); - pose.pose.orientation = getWorldOrientation(path[i].theta); - plan.poses.push_back(pose); - } - - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - auto msg = std::make_unique(plan); - _raw_plan_publisher->publish(std::move(msg)); - } - - if (_debug_visualizations) { - // Publish expansions for debug - auto now = _clock->now(); - auto msg = std::make_unique(); - geometry_msgs::msg::Pose msg_pose; - msg->header.stamp = now; - msg->header.frame_id = _global_frame; - for (auto & e : *expansions) { - msg_pose.position.x = std::get<0>(e); - msg_pose.position.y = std::get<1>(e); - msg_pose.orientation = getWorldOrientation(std::get<2>(e)); - msg->poses.push_back(msg_pose); - } - _expansions_publisher->publish(std::move(msg)); - - if (_planned_footprints_publisher->get_subscription_count() > 0) { - // Clear all markers first - auto marker_array = std::make_unique(); - visualization_msgs::msg::Marker clear_all_marker; - clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array->markers.push_back(clear_all_marker); - _planned_footprints_publisher->publish(std::move(marker_array)); - - // Publish planned footprints for debug - marker_array = std::make_unique(); - for (size_t i = 0; i < plan.poses.size(); i++) { - const std::vector edge = - transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); - marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); - } - _planned_footprints_publisher->publish(std::move(marker_array)); - } - } - - // Find how much time we have left to do smoothing - steady_clock::time_point b = steady_clock::now(); - duration time_span = duration_cast>(b - a); - double time_remaining = _max_planning_time - static_cast(time_span.count()); - -#ifdef BENCHMARK_TESTING - std::cout << "It took " << time_span.count() * 1000 << - " milliseconds with " << num_iterations << " iterations." << std::endl; -#endif - - // Smooth plan - if (_smoother && num_iterations > 1) { - _smoother->smooth(plan, costmap, time_remaining); - } - -#ifdef BENCHMARK_TESTING - steady_clock::time_point c = steady_clock::now(); - duration time_span2 = duration_cast>(c - b); - std::cout << "It took " << time_span2.count() * 1000 << - " milliseconds to smooth path." << std::endl; -#endif - - if (_debug_visualizations) { - if (_smoothed_footprints_publisher->get_subscription_count() > 0) { - // Clear all markers first - auto marker_array = std::make_unique(); - visualization_msgs::msg::Marker clear_all_marker; - clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array->markers.push_back(clear_all_marker); - _smoothed_footprints_publisher->publish(std::move(marker_array)); - - // Publish smoothed footprints for debug - marker_array = std::make_unique(); - auto now = _clock->now(); - for (size_t i = 0; i < plan.poses.size(); i++) { - const std::vector edge = - transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); - marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); - } - _smoothed_footprints_publisher->publish(std::move(marker_array)); - } - } - - return plan; -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp b/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp deleted file mode 100644 index 7438c3d8..00000000 --- a/src/subsystems/minimal_navigation/athena_smac_planner/src/smoother.cpp +++ /dev/null @@ -1,436 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// 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. Reserved. - -#include -#include - -#include -#include -#include - -#include "angles/angles.h" - -#include "tf2/utils.hpp" - -#include "athena_smac_planner/smoother.hpp" -#include "nav2_smoother/smoother_utils.hpp" - -namespace athena_smac_planner -{ -using namespace nav2_util::geometry_utils; // NOLINT -using namespace std::chrono; // NOLINT -using smoother_utils::PathSegment; - -Smoother::Smoother(const SmootherParams & params) -{ - tolerance_ = params.tolerance_; - max_its_ = params.max_its_; - data_w_ = params.w_data_; - smooth_w_ = params.w_smooth_; - is_holonomic_ = params.holonomic_; - do_refinement_ = params.do_refinement_; - refinement_num_ = params.refinement_num_; -} - -void Smoother::initialize(const double & min_turning_radius) -{ - min_turning_rad_ = min_turning_radius; - state_space_ = std::make_unique(min_turning_rad_); -} - -bool Smoother::smooth( - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time) -{ - // by-pass path orientations approximation when skipping smac smoother - if (max_its_ == 0) { - return false; - } - - steady_clock::time_point start = steady_clock::now(); - double time_remaining = max_time; - bool success = true, reversing_segment; - nav_msgs::msg::Path curr_path_segment; - curr_path_segment.header = path.header; - std::vector path_segments = smoother_utils::findDirectionalPathSegments(path); - - for (unsigned int i = 0; i != path_segments.size(); i++) { - if (path_segments[i].end - path_segments[i].start > 10) { - // Populate path segment - curr_path_segment.poses.clear(); - std::copy( - path.poses.begin() + path_segments[i].start, - path.poses.begin() + path_segments[i].end + 1, - std::back_inserter(curr_path_segment.poses)); - - // Make sure we're still able to smooth with time remaining - steady_clock::time_point now = steady_clock::now(); - time_remaining = max_time - duration_cast>(now - start).count(); - refinement_ctr_ = 0; - - // Smooth path segment naively - const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose; - const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back().pose; - bool local_success = - smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining); - success = success && local_success; - - // Enforce boundary conditions - if (!is_holonomic_ && local_success) { - enforceStartBoundaryConditions(start_pose, curr_path_segment, costmap, reversing_segment); - enforceEndBoundaryConditions(goal_pose, curr_path_segment, costmap, reversing_segment); - } - - // Assemble the path changes to the main path - std::copy( - curr_path_segment.poses.begin(), - curr_path_segment.poses.end(), - path.poses.begin() + path_segments[i].start); - } - } - - return success; -} - -bool Smoother::smoothImpl( - nav_msgs::msg::Path & path, - bool & reversing_segment, - const nav2_costmap_2d::Costmap2D * costmap, - const double & max_time) -{ - steady_clock::time_point a = steady_clock::now(); - rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time); - - int its = 0; - double change = tolerance_; - const unsigned int & path_size = path.poses.size(); - double x_i, y_i, y_m1, y_ip1, y_i_org; - unsigned int mx, my; - - nav_msgs::msg::Path new_path = path; - nav_msgs::msg::Path last_path = path; - - while (change >= tolerance_) { - its += 1; - change = 0.0; - - // Make sure the smoothing function will converge - if (its >= max_its_) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Number of iterations has exceeded limit of %i.", max_its_); - path = last_path; - smoother_utils::updateApproximatePathOrientations(path, reversing_segment); - return false; - } - - // Make sure still have time left to process - steady_clock::time_point b = steady_clock::now(); - rclcpp::Duration timespan(duration_cast>(b - a)); - if (timespan > max_dur) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Smoothing time exceeded allowed duration of %0.2f.", max_time); - path = last_path; - smoother_utils::updateApproximatePathOrientations(path, reversing_segment); - return false; - } - - for (unsigned int i = 1; i != path_size - 1; i++) { - for (unsigned int j = 0; j != 2; j++) { - x_i = getFieldByDim(path.poses[i], j); - y_i = getFieldByDim(new_path.poses[i], j); - y_m1 = getFieldByDim(new_path.poses[i - 1], j); - y_ip1 = getFieldByDim(new_path.poses[i + 1], j); - y_i_org = y_i; - - // Smooth based on local 3 point neighborhood and original data locations - y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i)); - setFieldByDim(new_path.poses[i], j, y_i); - change += abs(y_i - y_i_org); - } - - // validate update is admissible, only checks cost if a valid costmap pointer is provided - float cost = 0.0; - if (costmap) { - costmap->worldToMap( - getFieldByDim(new_path.poses[i], 0), - getFieldByDim(new_path.poses[i], 1), - mx, my); - cost = static_cast(costmap->getCost(mx, my)); - } - - if (cost > MAX_NON_OBSTACLE_COST && cost != UNKNOWN_COST) { - RCLCPP_DEBUG( - rclcpp::get_logger("SmacPlannerSmoother"), - "Smoothing process resulted in an infeasible collision. " - "Returning the last path before the infeasibility was introduced."); - path = last_path; - smoother_utils::updateApproximatePathOrientations(path, reversing_segment); - return false; - } - } - - last_path = new_path; - } - - // Let's do additional refinement, it shouldn't take more than a couple milliseconds - // but really puts the path quality over the top. - if (do_refinement_ && refinement_ctr_ < refinement_num_) { - refinement_ctr_++; - smoothImpl(new_path, reversing_segment, costmap, max_time); - } - - smoother_utils::updateApproximatePathOrientations(new_path, reversing_segment); - path = new_path; - return true; -} - -double Smoother::getFieldByDim( - const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim) -{ - if (dim == 0) { - return msg.pose.position.x; - } else if (dim == 1) { - return msg.pose.position.y; - } else { - return msg.pose.position.z; - } -} - -void Smoother::setFieldByDim( - geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, - const double & value) -{ - if (dim == 0) { - msg.pose.position.x = value; - } else if (dim == 1) { - msg.pose.position.y = value; - } else { - msg.pose.position.z = value; - } -} - -unsigned int Smoother::findShortestBoundaryExpansionIdx( - const BoundaryExpansions & boundary_expansions) -{ - // Check which is valid with the minimum integrated length such that - // shorter end-points away that are infeasible to achieve without - // a loop-de-loop are punished - double min_length = 1e9; - int shortest_boundary_expansion_idx = 1e9; - for (unsigned int idx = 0; idx != boundary_expansions.size(); idx++) { - if (boundary_expansions[idx].expansion_path_length0.0 && - boundary_expansions[idx].expansion_path_length > 0.0) - { - min_length = boundary_expansions[idx].expansion_path_length; - shortest_boundary_expansion_idx = idx; - } - } - - return shortest_boundary_expansion_idx; -} - -void Smoother::findBoundaryExpansion( - const geometry_msgs::msg::Pose & start, - const geometry_msgs::msg::Pose & end, - BoundaryExpansion & expansion, - const nav2_costmap_2d::Costmap2D * costmap) -{ - ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_); - - from[0] = start.position.x; - from[1] = start.position.y; - from[2] = tf2::getYaw(start.orientation); - to[0] = end.position.x; - to[1] = end.position.y; - to[2] = tf2::getYaw(end.orientation); - - double d = state_space_->distance(from(), to()); - // If this path is too long compared to the original, then this is probably - // a loop-de-loop, treat as invalid as to not deviate too far from the original path. - // 2.0 selected from prinicipled choice of boundary test points - // r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be - // approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI. - // For all but the last backup test point, a loop would be approximately - // 2x greater than any of the selections. - if (d > 2.0 * expansion.original_path_length) { - return; - } - - std::vector reals; - double theta(0.0), x(0.0), y(0.0); - double x_m = start.position.x; - double y_m = start.position.y; - - // Get intermediary poses - for (double i = 0; i <= expansion.path_end_idx; i++) { - state_space_->interpolate(from(), to(), i / expansion.path_end_idx, s()); - reals = s.reals(); - // Make sure in range [0, 2PI) - theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; - theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; - x = reals[0]; - y = reals[1]; - - // Check for collision - unsigned int mx, my; - costmap->worldToMap(x, y, mx, my); - if (static_cast(costmap->getCost(mx, my)) >= INSCRIBED_COST) { - expansion.in_collision = true; - } - - // Integrate path length - expansion.expansion_path_length += hypot(x - x_m, y - y_m); - x_m = x; - y_m = y; - - // Store point - expansion.pts.emplace_back(x, y, theta); - } -} - -template -BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, IteratorT end) -{ - std::vector distances = { - min_turning_rad_, // Radius - 2.0 * min_turning_rad_, // Diameter - M_PI * min_turning_rad_, // 50% Circumference - 2.0 * M_PI * min_turning_rad_ // Circumference - }; - - BoundaryExpansions boundary_expansions; - boundary_expansions.resize(distances.size()); - double curr_dist = 0.0; - double x_last = start->pose.position.x; - double y_last = start->pose.position.y; - geometry_msgs::msg::Point pt; - unsigned int curr_dist_idx = 0; - - for (IteratorT iter = start; iter != end; iter++) { - pt = iter->pose.position; - curr_dist += hypot(pt.x - x_last, pt.y - y_last); - x_last = pt.x; - y_last = pt.y; - - if (curr_dist >= distances[curr_dist_idx]) { - boundary_expansions[curr_dist_idx].path_end_idx = iter - start; - boundary_expansions[curr_dist_idx].original_path_length = curr_dist; - curr_dist_idx++; - } - - if (curr_dist_idx == boundary_expansions.size()) { - break; - } - } - - return boundary_expansions; -} - -void Smoother::enforceStartBoundaryConditions( - const geometry_msgs::msg::Pose & start_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment) -{ - // Find range of points for testing - BoundaryExpansions boundary_expansions = - generateBoundaryExpansionPoints(path.poses.begin(), path.poses.end()); - - // Generate the motion model and metadata from start -> test points - for (unsigned int i = 0; i != boundary_expansions.size(); i++) { - BoundaryExpansion & expansion = boundary_expansions[i]; - if (expansion.path_end_idx == 0.0) { - continue; - } - - if (!reversing_segment) { - findBoundaryExpansion( - start_pose, path.poses[expansion.path_end_idx].pose, expansion, - costmap); - } else { - findBoundaryExpansion( - path.poses[expansion.path_end_idx].pose, start_pose, expansion, - costmap); - } - } - - // Find the shortest kinematically feasible boundary expansion - unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); - if (best_expansion_idx > boundary_expansions.size()) { - return; - } - - // Override values to match curve - BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; - if (reversing_segment) { - std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); - } - for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { - path.poses[i].pose.position.x = best_expansion.pts[i].x; - path.poses[i].pose.position.y = best_expansion.pts[i].y; - path.poses[i].pose.orientation = orientationAroundZAxis(best_expansion.pts[i].theta); - } -} - -void Smoother::enforceEndBoundaryConditions( - const geometry_msgs::msg::Pose & end_pose, - nav_msgs::msg::Path & path, - const nav2_costmap_2d::Costmap2D * costmap, - const bool & reversing_segment) -{ - // Find range of points for testing - BoundaryExpansions boundary_expansions = - generateBoundaryExpansionPoints(path.poses.rbegin(), path.poses.rend()); - - // Generate the motion model and metadata from start -> test points - unsigned int expansion_starting_idx; - for (unsigned int i = 0; i != boundary_expansions.size(); i++) { - BoundaryExpansion & expansion = boundary_expansions[i]; - if (expansion.path_end_idx == 0.0) { - continue; - } - expansion_starting_idx = path.poses.size() - expansion.path_end_idx - 1; - if (!reversing_segment) { - findBoundaryExpansion(path.poses[expansion_starting_idx].pose, end_pose, expansion, costmap); - } else { - findBoundaryExpansion(end_pose, path.poses[expansion_starting_idx].pose, expansion, costmap); - } - } - - // Find the shortest kinematically feasible boundary expansion - unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); - if (best_expansion_idx > boundary_expansions.size()) { - return; - } - - // Override values to match curve - BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; - if (reversing_segment) { - std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); - } - expansion_starting_idx = path.poses.size() - best_expansion.path_end_idx - 1; - for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { - path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x; - path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y; - path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis( - best_expansion.pts[i].theta); - } -} - -} // namespace athena_smac_planner diff --git a/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt b/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt deleted file mode 100644 index 8b0d98b2..00000000 --- a/src/subsystems/minimal_navigation/dem_costmap/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(dem_costmap) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(OpenCV REQUIRED) - -set(dependencies - rclcpp - nav_msgs - sensor_msgs - cv_bridge - OpenCV -) - -add_executable(map_node src/map_node.cpp) -target_include_directories(map_node PUBLIC - $ - $ - ${OpenCV_INCLUDE_DIRS}) - -target_compile_features(map_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies( - map_node SYSTEM - ${dependencies} -) - -target_link_libraries(map_node ${OpenCV_LIBRARIES}) - -install(TARGETS map_node - DESTINATION lib/${PROJECT_NAME}) - -# Install maps directory -install(DIRECTORY maps/ - DESTINATION share/${PROJECT_NAME}/maps) - -# Install launch directory -install(DIRECTORY launch/ - DESTINATION share/${PROJECT_NAME}/launch) - -# Install config directory -install(DIRECTORY config/ - DESTINATION share/${PROJECT_NAME}/config) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/subsystems/minimal_navigation/dem_costmap/LICENSE b/src/subsystems/minimal_navigation/dem_costmap/LICENSE deleted file mode 100644 index d6456956..00000000 --- a/src/subsystems/minimal_navigation/dem_costmap/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml b/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml deleted file mode 100644 index 60bafdb3..00000000 --- a/src/subsystems/minimal_navigation/dem_costmap/config/dem_costmap.yaml +++ /dev/null @@ -1,8 +0,0 @@ -dem_costmap_converter: - ros__parameters: - dem_file_path: "" - map_resolution: 1.0 - max_passable_slope_degrees: 15.0 - output_frame: map - origin_x: -400.0 - origin_y: -400.0 \ No newline at end of file diff --git a/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py b/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py deleted file mode 100644 index 8b03a15d..00000000 --- a/src/subsystems/minimal_navigation/dem_costmap/launch/dem_costmap.launch.py +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env python3 - -import os -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - pkg_dir = get_package_share_directory('dem_costmap') - config_file = os.path.join(pkg_dir, 'config', 'dem_costmap.yaml') - dem_file = os.path.join(pkg_dir, 'maps', 'north_site800m.tif') - - dem_node = Node( - package='dem_costmap', - executable='map_node', - name='dem_costmap_converter', - output='screen', - parameters=[ - config_file, - { - 'dem_file_path': dem_file - } - ] - ) - - return LaunchDescription([ - dem_node - ]) diff --git a/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif b/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif deleted file mode 100644 index ec4e9fca..00000000 --- a/src/subsystems/minimal_navigation/dem_costmap/maps/north_site800m.tif +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:122777571140e3b8484bc334cdbdf3247eacf3a9c06a6624ec7cdc340476a3a3 -size 2559156 diff --git a/src/subsystems/minimal_navigation/dem_costmap/package.xml b/src/subsystems/minimal_navigation/dem_costmap/package.xml deleted file mode 100644 index 00fe8e66..00000000 --- a/src/subsystems/minimal_navigation/dem_costmap/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - dem_costmap - 0.0.0 - DEM to Costmap converter for terrain-based navigation - mdurrani - Apache-2.0 - - ament_cmake - rclcpp - nav_msgs - sensor_msgs - cv_bridge - libopencv-dev - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp b/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp deleted file mode 100644 index 637faf71..00000000 --- a/src/subsystems/minimal_navigation/dem_costmap/src/map_node.cpp +++ /dev/null @@ -1,236 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -class DEMCostmapConverter : public rclcpp::Node -{ -public: - DEMCostmapConverter() : Node("dem_costmap_converter") - { - // Declare parameters - this->declare_parameter("dem_file_path", ""); - this->declare_parameter("map_resolution", 1.0); // meters per pixel - this->declare_parameter("max_passable_slope_degrees", 15.0); - this->declare_parameter("output_frame", "map"); - this->declare_parameter("origin_x", 0.0); - this->declare_parameter("origin_y", 0.0); - - // Get parameters - std::string dem_file_path = this->get_parameter("dem_file_path").as_string(); - map_resolution_ = this->get_parameter("map_resolution").as_double(); - max_passable_slope_degrees_ = this->get_parameter("max_passable_slope_degrees").as_double(); - output_frame_ = this->get_parameter("output_frame").as_string(); - origin_x_ = this->get_parameter("origin_x").as_double(); - origin_y_ = this->get_parameter("origin_y").as_double(); - - // Create publisher with transient_local QoS for static maps - costmap_publisher_ = this->create_publisher( - "map", rclcpp::QoS(1).transient_local()); - - RCLCPP_INFO(this->get_logger(), "DEM Costmap Converter initialized"); - RCLCPP_INFO(this->get_logger(), "DEM file: %s", dem_file_path.c_str()); - RCLCPP_INFO(this->get_logger(), "Resolution: %.2f m/pixel", map_resolution_); - RCLCPP_INFO(this->get_logger(), "Max passable slope: %.1f degrees", max_passable_slope_degrees_); - - // Load and process DEM - costmap_ready_ = false; - if (!dem_file_path.empty()) { - loadAndProcessDEM(dem_file_path); - } else { - RCLCPP_ERROR(this->get_logger(), "No DEM file path provided. Use param 'dem_file_path'"); - RCLCPP_ERROR(this->get_logger(), "Node will not publish costmap without valid DEM file"); - } - - // Publish at 1Hz - timer_ = this->create_wall_timer( - std::chrono::seconds(1), - std::bind(&DEMCostmapConverter::publishCostmap, this)); - } - -private: - void loadAndProcessDEM(const std::string& file_path) - { - RCLCPP_INFO(this->get_logger(), "Loading DEM from: %s", file_path.c_str()); - - // Check if file exists - if (!std::filesystem::exists(file_path)) { - RCLCPP_ERROR(this->get_logger(), "DEM file does not exist: %s", file_path.c_str()); - return; - } - - // Load DEM using OpenCV (supports TIFF) - cv::Mat dem = cv::imread(file_path, cv::IMREAD_ANYDEPTH | cv::IMREAD_GRAYSCALE); - - if (dem.empty()) { - RCLCPP_ERROR(this->get_logger(), "Failed to load DEM file: %s", file_path.c_str()); - return; - } - - RCLCPP_INFO(this->get_logger(), "DEM loaded successfully: %dx%d pixels", dem.cols, dem.rows); - - // Convert to float for calculations - cv::Mat dem_float; - dem.convertTo(dem_float, CV_32F); - - // Calculate slope map - cv::Mat slope_map = calculateSlope(dem_float); - - // Convert slope to costmap - costmap_msg_ = createCostmapFromSlope(slope_map, dem.cols, dem.rows); - costmap_ready_ = true; - - RCLCPP_INFO(this->get_logger(), "Costmap generated successfully"); - } - - cv::Mat calculateSlope(const cv::Mat& dem) - { - RCLCPP_INFO(this->get_logger(), "Calculating slope map..."); - - cv::Mat grad_x, grad_y; - cv::Mat slope_radians = cv::Mat::zeros(dem.size(), CV_32F); - - // Calculate gradients using Sobel operator - cv::Sobel(dem, grad_x, CV_32F, 1, 0, 3); - cv::Sobel(dem, grad_y, CV_32F, 0, 1, 3); - - // Scale gradients by resolution to get proper slope - grad_x = grad_x / (8.0 * map_resolution_); // Sobel scale factor is 8 - grad_y = grad_y / (8.0 * map_resolution_); - - // Calculate slope magnitude - for (int i = 0; i < dem.rows; ++i) { - for (int j = 0; j < dem.cols; ++j) { - float dx = grad_x.at(i, j); - float dy = grad_y.at(i, j); - float slope_rad = std::atan(std::sqrt(dx*dx + dy*dy)); - slope_radians.at(i, j) = slope_rad; - } - } - - // Convert to degrees - cv::Mat slope_degrees; - slope_radians *= (180.0 / M_PI); - slope_degrees = slope_radians; - - return slope_degrees; - } - - nav_msgs::msg::OccupancyGrid createCostmapFromSlope(const cv::Mat& slope_map, int width, int height) - { - RCLCPP_INFO(this->get_logger(), "Converting slope to costmap..."); - - nav_msgs::msg::OccupancyGrid costmap; - costmap.header.frame_id = output_frame_; - costmap.header.stamp = this->get_clock()->now(); - - // Set map info - costmap.info.resolution = map_resolution_; - costmap.info.width = width; - costmap.info.height = height; - costmap.info.origin.position.x = origin_x_; - costmap.info.origin.position.y = origin_y_; - costmap.info.origin.position.z = 0.0; - costmap.info.origin.orientation.w = 1.0; - - // Calculate and log costmap bounds - double costmap_max_x = origin_x_ + (width * map_resolution_); - double costmap_max_y = origin_y_ + (height * map_resolution_); - - RCLCPP_INFO(this->get_logger(), "Costmap bounds:"); - RCLCPP_INFO(this->get_logger(), " Origin: (%.2f, %.2f)", origin_x_, origin_y_); - RCLCPP_INFO(this->get_logger(), " Size: %dx%d pixels", width, height); - RCLCPP_INFO(this->get_logger(), " Resolution: %.2f m/pixel", map_resolution_); - RCLCPP_INFO(this->get_logger(), " Bounds: X[%.2f, %.2f], Y[%.2f, %.2f]", - origin_x_, costmap_max_x, origin_y_, costmap_max_y); - RCLCPP_INFO(this->get_logger(), " Total coverage: %.2f x %.2f meters", - width * map_resolution_, height * map_resolution_); - - // Resize data array - costmap.data.resize(width * height); - - // Convert slope to cost values following nav2 costmap2d architecture - int lethal_count = 0; - for (int i = 0; i < height; ++i) { - for (int j = 0; j < width; ++j) { - float slope_deg = slope_map.at(i, j); - int cost = slopeToCost(slope_deg); - - // OpenCV uses (row, col) but OccupancyGrid uses (x, y) - // Need to flip Y coordinate for ROS convention - int ros_i = height - 1 - i; - costmap.data[ros_i * width + j] = cost; - - if (cost >= 99) lethal_count++; - } - } - - RCLCPP_INFO(this->get_logger(), - "Costmap conversion complete. Lethal/high-cost cells: %d/%d (%.1f%%)", - lethal_count, width * height, - 100.0 * lethal_count / (width * height)); - - return costmap; - } - - int slopeToCost(float slope_degrees) - { - // Nav2 costmap2d cost values: - // 0: Free space - // 1-252: Scaled costs - // 253: Possibly circumscribed - // 254: Lethal obstacle (impassable) - // 255: No information / unknown - - if (slope_degrees >= max_passable_slope_degrees_) { - return 254; // Lethal - impassable - } else if (slope_degrees >= 10.0) { - // High cost: linear scale from 150-252 for 10-15 degrees - float ratio = (slope_degrees - 10.0) / (max_passable_slope_degrees_ - 10.0); - return static_cast(150 + ratio * 102); - } else if (slope_degrees >= 5.0) { - // Medium cost: linear scale from 50-149 for 5-10 degrees - float ratio = (slope_degrees - 5.0) / 5.0; - return static_cast(50 + ratio * 99); - } else { - // Low cost: linear scale from 0-49 for 0-5 degrees - float ratio = slope_degrees / 5.0; - return static_cast(ratio * 49); - } - } - - - void publishCostmap() - { - if (costmap_ready_) { - costmap_msg_.header.stamp = this->get_clock()->now(); - costmap_publisher_->publish(costmap_msg_); - } else { - RCLCPP_DEBUG(this->get_logger(), "Costmap not ready, skipping publish"); - } - } - - // Member variables - rclcpp::Publisher::SharedPtr costmap_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - nav_msgs::msg::OccupancyGrid costmap_msg_; - bool costmap_ready_; - - double map_resolution_; - double max_passable_slope_degrees_; - std::string output_frame_; - double origin_x_; - double origin_y_; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt b/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt deleted file mode 100644 index 209d45e6..00000000 --- a/src/subsystems/minimal_navigation/gps_pose_publisher/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(gps_pose_publisher) - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2 REQUIRED) -find_package(msgs REQUIRED) -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") -find_package(GeographicLib REQUIRED) - -add_executable(gps_pose_publisher_node src/gps_pose_publisher_node.cpp) -target_include_directories(gps_pose_publisher_node PRIVATE ${GeographicLib_INCLUDE_DIRS}) -target_link_libraries(gps_pose_publisher_node - rclcpp::rclcpp - ${geometry_msgs_TARGETS} - ${sensor_msgs_TARGETS} - ${std_msgs_TARGETS} - tf2_ros::tf2_ros - tf2::tf2 - ${msgs_TARGETS} - ${GeographicLib_LIBRARIES} -) - -install(TARGETS gps_pose_publisher_node - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml b/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml deleted file mode 100644 index 9442c5f1..00000000 --- a/src/subsystems/minimal_navigation/gps_pose_publisher/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - gps_pose_publisher - 0.1.0 - Converts GPS fix + heading into a map-frame PoseStamped and TF broadcast - UMD Loop - Apache-2.0 - - ament_cmake - - rclcpp - geometry_msgs - sensor_msgs - std_msgs - tf2_ros - tf2 - msgs - geographiclib - - - ament_cmake - - diff --git a/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp b/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp deleted file mode 100644 index 10d0291c..00000000 --- a/src/subsystems/minimal_navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 - -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "std_msgs/msg/string.hpp" -#include "msgs/msg/heading.hpp" -#include "tf2_ros/transform_broadcaster.hpp" -#include "msgs/srv/lat_lon_to_enu.hpp" - -#include - -class GpsPosePublisher : public rclcpp::Node -{ -public: - explicit GpsPosePublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("gps_pose_publisher", options) - { - declare_parameter("heading_topic", std::string("/gps/heading")); - declare_parameter("use_start_gate_ref", false); - declare_parameter("origin_lat", std::numeric_limits::quiet_NaN()); - declare_parameter("origin_lon", std::numeric_limits::quiet_NaN()); - declare_parameter("origin_alt", 0.0); - - tf_broadcaster_ = std::make_shared(*this); - - pose_pub_ = create_publisher("/robot_pose", 10); - status_pub_ = create_publisher("/gps_status", 10); - - fix_sub_ = create_subscription( - "/gps/fix", rclcpp::SensorDataQoS(), - [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { onFix(msg); }); - - std::string heading_topic; - get_parameter("heading_topic", heading_topic); - heading_sub_ = create_subscription( - heading_topic, rclcpp::SensorDataQoS(), - [this](const msgs::msg::Heading::SharedPtr msg) { - heading_enu_rad_ = (90.0 - msg->compass_bearing) * M_PI / 180.0; - has_heading_ = true; - }); - - // Pre-set origin from params if provided (non-NaN overrides first-fix logic) - const double param_lat = get_parameter("origin_lat").as_double(); - const double param_lon = get_parameter("origin_lon").as_double(); - const double param_alt = get_parameter("origin_alt").as_double(); - if (!std::isnan(param_lat) && !std::isnan(param_lon)) { - setOrigin(param_lat, param_lon, param_alt); - } - - bool use_start_gate_ref; - get_parameter("use_start_gate_ref", use_start_gate_ref); - if (use_start_gate_ref) { - start_gate_sub_ = create_subscription( - "/start_gate_ref", 1, - [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - if (!origin_set_) { - setOrigin(msg->latitude, msg->longitude, msg->altitude); - } - }); - } - - latlon_srv_ = create_service( - "~/latlon_to_enu", - [this]( - const msgs::srv::LatLonToENU::Request::SharedPtr req, - msgs::srv::LatLonToENU::Response::SharedPtr res) - { - if (!origin_set_) { - RCLCPP_ERROR(get_logger(), "latlon_to_enu called before origin is set"); - res->x = 0.0; res->y = 0.0; res->z = 0.0; - return; - } - double x, y, z; - projection_.Forward(req->lat, req->lon, 0.0, x, y, z); - res->x = x; res->y = y; res->z = z; - }); - } - -private: - void setOrigin(double lat, double lon, double alt) - { - projection_.Reset(lat, lon, alt); - origin_set_ = true; - RCLCPP_INFO(get_logger(), "GPS origin set: lat=%.8f lon=%.8f alt=%.3f", lat, lon, alt); - } - - void onFix(const sensor_msgs::msg::NavSatFix::SharedPtr msg) - { - if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { - std_msgs::msg::String s; - s.data = "NO_FIX"; - status_pub_->publish(s); - return; - } - - if (!origin_set_) { - bool use_start_gate_ref; - get_parameter("use_start_gate_ref", use_start_gate_ref); - if (!use_start_gate_ref) { - setOrigin(msg->latitude, msg->longitude, msg->altitude); - } else { - return; - } - } - - double x, y, z; - projection_.Forward(msg->latitude, msg->longitude, msg->altitude, x, y, z); - - // Publish fix quality + heading source - std::string quality; - switch (msg->status.status) { - case sensor_msgs::msg::NavSatStatus::STATUS_FIX: quality = "GPS"; break; - case sensor_msgs::msg::NavSatStatus::STATUS_SBAS_FIX: quality = "SBAS"; break; - case sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX: quality = "GBAS"; break; - default: quality = "FIX"; break; - } - std_msgs::msg::String s; - s.data = "FIX quality=" + quality + " heading=" + (has_heading_ ? "compass" : "none"); - status_pub_->publish(s); - - if (!has_heading_) { - return; - } - - geometry_msgs::msg::PoseStamped pose; - pose.header.stamp = msg->header.stamp; - pose.header.frame_id = "map"; - pose.pose.position.x = x; - pose.pose.position.y = y; - pose.pose.position.z = z; - - const double half = heading_enu_rad_ * 0.5; - pose.pose.orientation.x = 0.0; - pose.pose.orientation.y = 0.0; - pose.pose.orientation.z = std::sin(half); - pose.pose.orientation.w = std::cos(half); - - pose_pub_->publish(pose); - - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = msg->header.stamp; - tf.header.frame_id = "map"; - tf.child_frame_id = "base_link"; - tf.transform.translation.x = x; - tf.transform.translation.y = y; - tf.transform.translation.z = z; - tf.transform.rotation = pose.pose.orientation; - tf_broadcaster_->sendTransform(tf); - } - - std::shared_ptr tf_broadcaster_; - rclcpp::Publisher::SharedPtr pose_pub_; - rclcpp::Publisher::SharedPtr status_pub_; - rclcpp::Subscription::SharedPtr fix_sub_; - rclcpp::Subscription::SharedPtr heading_sub_; - rclcpp::Subscription::SharedPtr start_gate_sub_; - rclcpp::Service::SharedPtr latlon_srv_; - - GeographicLib::LocalCartesian projection_; - bool origin_set_{false}; - bool has_heading_{false}; - double heading_enu_rad_{0.0}; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt b/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt deleted file mode 100644 index 57406adb..00000000 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(minimal_nav_bringup) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY launch config - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml b/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml deleted file mode 100644 index 0227280c..00000000 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/config/nav_params.yaml +++ /dev/null @@ -1,92 +0,0 @@ -# nav_params.yaml — single parameter file passed to all navigation nodes. -# See PLAN.md §7 for full documentation. - -# ── Global ────────────────────────────────────────────────────────────────── -use_sim_time: false - -# ── gps_pose_publisher ─────────────────────────────────────────────────────── -gps_pose_publisher: - ros__parameters: - use_sim_time: false - heading_topic: "/gps/heading" - origin_lat: .nan # auto-set from first fix when NaN - origin_lon: .nan - origin_alt: 0.0 - use_start_gate_ref: true # wait for /start_gate_ref NavSatFix to set origin - -# ── mission_executive ──────────────────────────────────────────────────────── -mission_executive: - ros__parameters: - use_sim_time: false - velocity_zero_threshold: 0.05 # m/s — applied to /odom twist (ZED SDK) - arrival_hold_time: 1.0 # seconds of continuous zero velocity - replan_distance_m: 3.0 # cross-track error threshold to trigger replan - latlon_to_enu_service: "/gps_pose_publisher/latlon_to_enu" - -# ── global_planner (athena_smac_planner) ───────────────────────────────────── -global_planner: - ros__parameters: - use_sim_time: false - geotiff_path: "" - grid_size_m: 500.0 - grid_resolution_m: 1.0 - obstacle_threshold: 50 - min_turning_r: 1.2 # must match mppi_runner - allow_reverse: false - -global_costmap: - ros__parameters: - use_sim_time: false - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 1.0 - publish_frequency: 0.5 - transform_tolerance: 1.0 # generous — GPS-only localization has jitter - rolling_window: false - plugins: ["static_layer", "inflation_layer"] - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_topic: /map - subscribe_to_updates: true - map_subscribe_transient_local: true - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 1.0 # >= robot_radius (0.45 m) - -# ── mppi_runner (ackermann_mppi) ───────────────────────────────────────────── -mppi_runner: - ros__parameters: - use_sim_time: false - controller_frequency: 20.0 - motion_model: "Ackermann" - min_turning_r: 1.2 - vx_max: 3.0 - vx_min: 0.0 - wz_max: 1.0 - time_steps: 56 - batch_size: 1000 - model_dt: 0.05 - critics: - - ConstraintCritic - - GoalCritic - - GoalAngleCritic - - PathFollowCritic - - PathAlignCritic - - PreferForwardCritic - # Local costmap — owned by mppi_runner as a child Costmap2DROS node - local_costmap: - ros__parameters: - use_sim_time: false - global_frame: map - robot_base_frame: base_link - robot_radius: 0.45 - update_frequency: 5.0 - publish_frequency: 2.0 - transform_tolerance: 1.0 - rolling_window: true - width: 20 - height: 20 - resolution: 0.1 - plugins: [] # Phase 2a: add ObstacleLayer + InflationLayer diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py b/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py deleted file mode 100644 index 1edbd225..00000000 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/launch/nav.launch.py +++ /dev/null @@ -1,147 +0,0 @@ -#!/usr/bin/env python3 -# Copyright (c) 2025 UMD Loop -# -# 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. - -"""nav.launch.py — Full Athena navigation stack (GPS-only, Nav2-free). - -Launch order / node summary -─────────────────────────── - athena_gps : Pixhawk GPS + heading bridge (from athena_gps/gps_launch.py) - gps_pose_publisher : WGS84→ENU, /robot_pose, map→base_link TF - map_node : DEM GeoTIFF → nav_msgs/OccupancyGrid /map - global_planner : Hybrid-A* planner, /goal_pose → /global_path - ackermann_mppi : MPPI local controller, /global_path → /cmd_vel_nav - mission_executive : State machine, action/service operator interface - cmd_vel_stamper : TwistStamped bridge → /rear_ackermann_controller/reference -""" - -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - # ── Arguments ──────────────────────────────────────────────────────────── - sim_arg = DeclareLaunchArgument( - 'sim', - default_value='false', - choices=['true', 'false'], - description='Use simulation GPS bridge instead of real hardware', - ) - - sim = LaunchConfiguration('sim') - - # ── Config ─────────────────────────────────────────────────────────────── - nav_bringup_dir = get_package_share_directory('minimal_nav_bringup') - nav_params_file = os.path.join(nav_bringup_dir, 'config', 'nav_params.yaml') - - athena_map_dir = get_package_share_directory('dem_costmap') - dem_file = os.path.join(athena_map_dir, 'maps', 'north_site800m.tif') - - # ── GPS hardware / sim bridge ───────────────────────────────────────────── - gps_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory('athena_gps'), - 'launch', - 'gps_launch.py', - ) - ), - launch_arguments={'sim': sim}.items(), - ) - - # ── gps_pose_publisher ──────────────────────────────────────────────────── - gps_pose_publisher_node = Node( - package='gps_pose_publisher', - executable='gps_pose_publisher_node', - name='gps_pose_publisher', - output='screen', - parameters=[nav_params_file], - ) - - # ── map_node (DEM → OccupancyGrid) ──────────────────────────────────────── - map_node = Node( - package='dem_costmap', - executable='map_node', - name='map_node', - output='screen', - parameters=[ - nav_params_file, - {'dem_file_path': dem_file}, - ], - ) - - # ── global_planner (Hybrid-A* via athena_smac_planner) ─────────────────── - global_planner_node = Node( - package='athena_smac_planner', - executable='global_planner_node', - name='global_planner', - output='screen', - parameters=[nav_params_file], - ) - - # ── ackermann_mppi (MPPI local controller) ──────────────────────────────── - # /odom remapped to /odom/ground_truth (ZED SDK visual-inertial odometry) - ackermann_mppi_node = Node( - package='ackermann_mppi', - executable='ackermann_mppi_node', - name='mppi_runner', - output='screen', - parameters=[nav_params_file], - remappings=[ - ('/odom', '/odom/ground_truth'), - ], - ) - - # ── mission_executive (state machine) ──────────────────────────────────── - # /odom remapped to /odom/ground_truth for stop detection - mission_executive_node = Node( - package='mission_executive', - executable='mission_executive_node', - name='mission_executive', - output='screen', - parameters=[nav_params_file], - remappings=[ - ('/odom', '/odom/ground_truth'), - ], - ) - - # ── twist_stamper: /cmd_vel_nav → /rear_ackermann_controller/reference ──── - twist_stamper_node = Node( - package='twist_stamper', - executable='twist_stamper', - name='cmd_vel_stamper', - output='screen', - parameters=[{'use_sim_time': sim}], - remappings=[ - ('cmd_vel_in', '/cmd_vel_nav'), - ('cmd_vel_out', '/rear_ackermann_controller/reference'), - ], - ) - - return LaunchDescription([ - sim_arg, - gps_launch, - gps_pose_publisher_node, - map_node, - global_planner_node, - ackermann_mppi_node, - mission_executive_node, - twist_stamper_node, - ]) diff --git a/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml b/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml deleted file mode 100644 index 71d3ccc1..00000000 --- a/src/subsystems/minimal_navigation/minimal_nav_bringup/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - minimal_nav_bringup - 0.1.0 - Launch and configuration files for the full Athena navigation stack - UMD Loop - Apache-2.0 - - ament_cmake - - gps_pose_publisher - dem_costmap - athena_smac_planner - ackermann_mppi - mission_executive - athena_gps - twist_stamper - - - ament_cmake - - diff --git a/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt b/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt deleted file mode 100644 index d3f37c8b..00000000 --- a/src/subsystems/minimal_navigation/mission_executive/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(mission_executive) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(msgs REQUIRED) - -add_executable(mission_executive_node src/mission_executive_node.cpp) -target_compile_features(mission_executive_node PUBLIC cxx_std_17) -target_link_libraries(mission_executive_node - rclcpp::rclcpp - rclcpp_action::rclcpp_action - ${geometry_msgs_TARGETS} - ${nav_msgs_TARGETS} - ${std_msgs_TARGETS} - ${std_srvs_TARGETS} - ${msgs_TARGETS} -) - -install(TARGETS mission_executive_node - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/src/subsystems/minimal_navigation/mission_executive/package.xml b/src/subsystems/minimal_navigation/mission_executive/package.xml deleted file mode 100644 index 49fb99be..00000000 --- a/src/subsystems/minimal_navigation/mission_executive/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - mission_executive - 0.1.0 - State-machine node that manages target sequencing, abort/return, teleop mode, and arrival detection for the Athena navigation stack - UMD Loop - Apache-2.0 - - ament_cmake - - rclcpp - rclcpp_action - geometry_msgs - nav_msgs - std_msgs - std_srvs - msgs - - - ament_cmake - - diff --git a/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp b/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp deleted file mode 100644 index 97c8e998..00000000 --- a/src/subsystems/minimal_navigation/mission_executive/src/mission_executive_node.cpp +++ /dev/null @@ -1,784 +0,0 @@ -// Copyright (c) 2025 UMD Loop -// -// 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 -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "nav_msgs/msg/path.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/string.hpp" -#include "std_srvs/srv/set_bool.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include "msgs/action/navigate_to_target.hpp" -#include "msgs/msg/active_target.hpp" -#include "msgs/msg/nav_status.hpp" -#include "msgs/msg/planner_event.hpp" -#include "msgs/srv/lat_lon_to_enu.hpp" -#include "msgs/srv/set_target.hpp" - -using namespace std::chrono_literals; - -// ── State ────────────────────────────────────────────────────────────────── - -enum class State -{ - IDLE, - NAVIGATING, - ARRIVING, - STOPPED_AT_TARGET, - ABORTING, - RETURNING, - STOPPED_AT_RETURN, - TELEOP -}; - -static std::string stateToStr(State s) -{ - switch (s) { - case State::IDLE: return "IDLE"; - case State::NAVIGATING: return "NAVIGATING"; - case State::ARRIVING: return "ARRIVING"; - case State::STOPPED_AT_TARGET: return "STOPPED_AT_TARGET"; - case State::ABORTING: return "ABORTING"; - case State::RETURNING: return "RETURNING"; - case State::STOPPED_AT_RETURN: return "STOPPED_AT_RETURN"; - case State::TELEOP: return "TELEOP"; - default: return "UNKNOWN"; - } -} - -// ── Node ─────────────────────────────────────────────────────────────────── - -class MissionExecutive : public rclcpp::Node -{ -public: - using NavAction = msgs::action::NavigateToTarget; - using GoalHandle = rclcpp_action::ServerGoalHandle; - - // Target entry stored in the registry and as the active target - struct TargetEntry - { - std::string id; - double x_m{0.0}; - double y_m{0.0}; - uint8_t target_type{0}; - uint8_t goal_source{0}; // GPS=0, METER=1 - double tolerance_m{3.0}; - bool visited{false}; - geometry_msgs::msg::PoseStamped goal_enu; - }; - - explicit MissionExecutive(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("mission_executive", options) - { - declare_parameter("velocity_zero_threshold", 0.05); - declare_parameter("arrival_hold_time", 1.0); - declare_parameter("replan_distance_m", 3.0); - declare_parameter("latlon_to_enu_service", - std::string("/gps_pose_publisher/latlon_to_enu")); - // §8: launch file should remap to /odom/ground_truth when using ZED ground-truth odometry - declare_parameter("odom_topic", std::string("/odom")); - - velocity_zero_threshold_ = get_parameter("velocity_zero_threshold").as_double(); - arrival_hold_time_ = get_parameter("arrival_hold_time").as_double(); - replan_distance_m_ = get_parameter("replan_distance_m").as_double(); - const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); - const auto odom_topic = get_parameter("odom_topic").as_string(); - - // Reentrant group — allows the action server / service client to block - // inside handleAccepted() while other callbacks still run on other threads. - reentrant_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - // ── Publishers ────────────────────────────────────────────────────────── - goal_pub_ = create_publisher("/goal_pose", 10); - nav_enabled_pub_ = create_publisher( - "/nav_enabled", rclcpp::QoS(1).reliable()); - nav_mode_pub_ = create_publisher("/nav_mode", 10); - active_target_pub_ = create_publisher("/active_target", 10); - nav_status_pub_ = create_publisher( - "/nav_status", rclcpp::QoS(1).reliable()); - - // ── Service client ────────────────────────────────────────────────────── - latlon_client_ = create_client( - latlon_svc, - rmw_qos_profile_services_default, - reentrant_group_); - - // ── Action server ─────────────────────────────────────────────────────── - action_server_ = rclcpp_action::create_server( - this, - "~/navigate_to_target", - [this](const rclcpp_action::GoalUUID &, std::shared_ptr goal) { - return handleGoal(goal); - }, - [this](std::shared_ptr gh) { - return handleCancel(gh); - }, - [this](std::shared_ptr gh) { - handleAccepted(gh); - }, - rcl_action_server_get_default_options(), - reentrant_group_); - - // ── Services ──────────────────────────────────────────────────────────── - abort_srv_ = create_service( - "~/abort", - [this]( - const std_srvs::srv::Trigger::Request::SharedPtr, - std_srvs::srv::Trigger::Response::SharedPtr res) - { - std::lock_guard lk(mutex_); - if (state_ == State::IDLE || state_ == State::TELEOP || - state_ == State::ABORTING || - state_ == State::STOPPED_AT_TARGET || state_ == State::STOPPED_AT_RETURN) - { - res->success = false; - res->message = "Nothing to abort in state " + stateToStr(state_); - return; - } - transition(State::ABORTING, "~/abort service called"); - res->success = true; - res->message = "Aborting"; - }); - - set_target_srv_ = create_service( - "~/set_target", - [this]( - const msgs::srv::SetTarget::Request::SharedPtr req, - msgs::srv::SetTarget::Response::SharedPtr res) - { - onSetTarget(req, res); - }, - rmw_qos_profile_services_default, - reentrant_group_); - - teleop_srv_ = create_service( - "~/teleop", - [this]( - const std_srvs::srv::SetBool::Request::SharedPtr req, - std_srvs::srv::SetBool::Response::SharedPtr res) - { - std::lock_guard lk(mutex_); - if (req->data) { - // TELEOP_ON — valid from any state - transition(State::TELEOP, "teleop enabled"); - res->success = true; - res->message = "Teleop ON"; - } else { - // TELEOP_OFF — only from TELEOP - if (state_ == State::TELEOP) { - transition(State::IDLE, "teleop disabled"); - res->success = true; - res->message = "Teleop OFF"; - } else { - res->success = false; - res->message = "Not in TELEOP state"; - } - } - }); - - // ── Subscriptions ──────────────────────────────────────────────────────── - robot_pose_sub_ = create_subscription( - "/robot_pose", 10, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - std::lock_guard lk(mutex_); - robot_pose_ = *msg; - // Arrival check first: once we transition to ARRIVING, - // checkCrossTrackError's state guard prevents a spurious replan. - checkArrival(); - checkCrossTrackError(); - }); - - odom_sub_ = create_subscription( - odom_topic, 10, - [this](const nav_msgs::msg::Odometry::SharedPtr msg) { - std::lock_guard lk(mutex_); - const auto & lin = msg->twist.twist.linear; - robot_speed_ = std::hypot(lin.x, lin.y); - checkStopDetection(); - }); - - planner_event_sub_ = create_subscription( - "/planner_event", 10, - [this](const msgs::msg::PlannerEvent::SharedPtr msg) { - std::lock_guard lk(mutex_); - last_planner_event_ = msg->event; - if (msg->event == msgs::msg::PlannerEvent::PLAN_FAILED) { - onPlanFailed(); - } - }); - - auto transient_qos = rclcpp::QoS(1).transient_local(); - global_path_sub_ = create_subscription( - "/global_path", transient_qos, - [this](const nav_msgs::msg::Path::SharedPtr msg) { - std::lock_guard lk(mutex_); - global_path_ = *msg; - }); - - // ── 2 Hz status timer ──────────────────────────────────────────────────── - status_timer_ = create_wall_timer( - 500ms, - [this]() { - std::lock_guard lk(mutex_); - publishStatus(); - checkActionResult(); - }); - - // Publish initial safe state - publishNavEnabled(false); - publishNavMode(); - - RCLCPP_INFO(get_logger(), "MissionExecutive ready — state: IDLE"); - } - -private: - // ── State transitions ───────────────────────────────────────────────────── - - // Must be called with mutex_ held. - void transition(State new_state, const std::string & reason) - { - RCLCPP_INFO(get_logger(), "[mission_executive] %s → %s: %s", - stateToStr(state_).c_str(), stateToStr(new_state).c_str(), reason.c_str()); - state_ = new_state; - - const bool nav_active = - state_ == State::NAVIGATING || - state_ == State::ARRIVING || - state_ == State::RETURNING; - publishNavEnabled(nav_active); - publishNavMode(); - publishStatus(); - - if (state_ == State::TELEOP || !nav_active) { - // Clear stop-detection tracking on any non-navigating transition - low_speed_tracking_ = false; - } - } - - // ── Internal publish helpers (no lock needed — caller holds mutex) ───────── - - void publishNavEnabled(bool enabled) - { - std_msgs::msg::Bool msg; - msg.data = enabled; - nav_enabled_pub_->publish(msg); - } - - void publishNavMode() - { - std_msgs::msg::String msg; - switch (state_) { - case State::TELEOP: - msg.data = "teleop"; break; - case State::NAVIGATING: - case State::ARRIVING: - case State::RETURNING: - msg.data = "autonomous"; break; - default: - msg.data = "stopped"; break; - } - nav_mode_pub_->publish(msg); - } - - void publishActiveTarget() - { - if (!active_target_.has_value()) return; - msgs::msg::ActiveTarget at; - at.target_id = active_target_->id; - at.target_type = active_target_->target_type; - at.tolerance_m = active_target_->tolerance_m; - at.goal_enu = active_target_->goal_enu; - at.goal_source = active_target_->goal_source; - at.status = stateToStr(state_); - active_target_pub_->publish(at); - } - - void publishStatus() - { - msgs::msg::NavStatus s; - s.state = stateToStr(state_); - if (active_target_.has_value()) { - s.active_target_id = active_target_->id; - s.active_target_type = active_target_->target_type; - s.goal_source = active_target_->goal_source; - } - s.distance_to_goal_m = distToGoal(); - s.cross_track_error_m = computeCrossTrackError(); - s.heading_error_rad = computeHeadingError(); - s.robot_speed_mps = robot_speed_; - s.is_return = is_return_; - s.last_planner_event = last_planner_event_; - nav_status_pub_->publish(s); - } - - // ── Geometry helpers ────────────────────────────────────────────────────── - - // Both return -1.0 if data is unavailable. Called with mutex_ held. - - static double quaternionToYaw(const geometry_msgs::msg::Quaternion & q) - { - const double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); - const double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); - return std::atan2(siny_cosp, cosy_cosp); - } - - double computeHeadingError() const - { - if (!robot_pose_.has_value() || !active_target_.has_value()) return 0.0; - const double yaw = quaternionToYaw(robot_pose_->pose.orientation); - const double dx = active_target_->goal_enu.pose.position.x - robot_pose_->pose.position.x; - const double dy = active_target_->goal_enu.pose.position.y - robot_pose_->pose.position.y; - const double bearing = std::atan2(dy, dx); - double err = bearing - yaw; - while (err > M_PI) err -= 2.0 * M_PI; - while (err < -M_PI) err += 2.0 * M_PI; - return err; - } - - double distToGoal() const - { - if (!robot_pose_.has_value() || !active_target_.has_value()) return -1.0; - const double dx = - robot_pose_->pose.position.x - active_target_->goal_enu.pose.position.x; - const double dy = - robot_pose_->pose.position.y - active_target_->goal_enu.pose.position.y; - return std::hypot(dx, dy); - } - - double computeCrossTrackError() const - { - if (!robot_pose_.has_value() || !global_path_.has_value()) return -1.0; - const auto & poses = global_path_->poses; - if (poses.size() < 2) return -1.0; - - const double rx = robot_pose_->pose.position.x; - const double ry = robot_pose_->pose.position.y; - double min_dist = std::numeric_limits::max(); - - for (size_t i = 0; i < poses.size() - 1; ++i) { - const double ax = poses[i].pose.position.x, ay = poses[i].pose.position.y; - const double bx = poses[i+1].pose.position.x, by = poses[i+1].pose.position.y; - const double dx = bx - ax, dy = by - ay; - const double len2 = dx*dx + dy*dy; - double dist; - if (len2 < 1e-10) { - dist = std::hypot(rx - ax, ry - ay); - } else { - const double t = std::clamp(((rx-ax)*dx + (ry-ay)*dy) / len2, 0.0, 1.0); - dist = std::hypot(rx - (ax + t*dx), ry - (ay + t*dy)); - } - min_dist = std::min(min_dist, dist); - } - return min_dist; - } - - // ── Subscription callbacks (mutex_ already held by caller) ──────────────── - - void checkCrossTrackError() - { - if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; - if (!active_target_.has_value()) return; - - const double xte = computeCrossTrackError(); - if (xte < 0.0 || xte <= replan_distance_m_) return; - - RCLCPP_INFO(get_logger(), - "[mission_executive] replan triggered: xtrack=%.2fm", xte); - goal_pub_->publish(active_target_->goal_enu); - } - - void checkArrival() - { - // Transition NAVIGATING/RETURNING → ARRIVING when within tolerance - if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; - if (!active_target_.has_value()) return; - - const double d = distToGoal(); - if (d < 0.0 || d >= active_target_->tolerance_m) return; - - transition(State::ARRIVING, - "within tolerance (d=" + std::to_string(d) + "m)"); - } - - void checkStopDetection() - { - // Transition ARRIVING → STOPPED_AT_TARGET / STOPPED_AT_RETURN - if (state_ != State::ARRIVING) return; - - if (robot_speed_ < velocity_zero_threshold_) { - if (!low_speed_tracking_) { - low_speed_start_ = now(); - low_speed_tracking_ = true; - } else { - const double held = (now() - low_speed_start_).seconds(); - if (held >= arrival_hold_time_) { - low_speed_tracking_ = false; - if (is_return_) { - transition(State::STOPPED_AT_RETURN, "velocity held < threshold"); - } else { - // Mark target as visited so RETURN is permitted later - if (active_target_.has_value()) { - auto it = target_registry_.find(active_target_->id); - if (it != target_registry_.end()) { - it->second.visited = true; - } - } - transition(State::STOPPED_AT_TARGET, "velocity held < threshold"); - } - } - } - } else { - low_speed_tracking_ = false; - } - } - - void onPlanFailed() - { - // Called with mutex_ held - if (state_ == State::NAVIGATING || state_ == State::RETURNING) { - RCLCPP_WARN(get_logger(), - "[mission_executive] PLAN_FAILED — transitioning to ABORTING"); - transition(State::ABORTING, "PLAN_FAILED received"); - } - } - - // ── Action result dispatch (called from the 2 Hz status timer) ──────────── - // Must be called with mutex_ held. - - void checkActionResult() - { - if (!active_goal_handle_) return; - - auto result = std::make_shared(); - - // Cancel requested externally — result already sent, go straight to IDLE. - // (ABORTING is only for the case where the result hasn't been sent yet.) - if (active_goal_handle_->is_canceling()) { - result->success = false; - result->message = "Cancelled"; - active_goal_handle_->canceled(result); - active_goal_handle_ = nullptr; - transition(State::IDLE, "action cancelled"); - return; - } - - switch (state_) { - case State::STOPPED_AT_TARGET: - case State::STOPPED_AT_RETURN: - result->success = true; - result->message = "Arrived at target"; - active_goal_handle_->succeed(result); - active_goal_handle_ = nullptr; - return; - - case State::ABORTING: - result->success = false; - result->message = "Aborted"; - active_goal_handle_->abort(result); - active_goal_handle_ = nullptr; - if (queued_goal_handle_) { - // Promote queued goal — go directly to NAVIGATING/RETURNING (§3.2.2) - active_target_ = queued_entry_; - is_return_ = queued_is_return_; - active_goal_handle_ = queued_goal_handle_; - queued_goal_handle_ = nullptr; - transition(is_return_ ? State::RETURNING : State::NAVIGATING, - "queued goal dequeued after abort"); - goal_pub_->publish(active_target_->goal_enu); - publishActiveTarget(); - } else { - transition(State::IDLE, "abort complete"); - } - return; - - default: - break; - } - - // Still executing — push feedback - auto fb = std::make_shared(); - fb->distance_to_goal_m = distToGoal(); - fb->cross_track_error_m = computeCrossTrackError(); - fb->state = stateToStr(state_); - active_goal_handle_->publish_feedback(fb); - } - - // ── Action server callbacks ─────────────────────────────────────────────── - - rclcpp_action::GoalResponse handleGoal( - std::shared_ptr /*goal*/) - { - // Accept all goals; teleop/state guards are applied in handleAccepted. - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - rclcpp_action::CancelResponse handleCancel(std::shared_ptr /*gh*/) - { - return rclcpp_action::CancelResponse::ACCEPT; - } - - // Runs on a thread from the MultiThreadedExecutor (reentrant group). - // May block briefly for the latlon_to_enu service call. - void handleAccepted(std::shared_ptr goal_handle) - { - const auto goal = goal_handle->get_goal(); - - // ── Resolve ENU coordinates ──────────────────────────────────────────── - TargetEntry entry; - const bool is_ret = goal->is_return; - - if (!goal->target_id.empty()) { - // Look up pre-registered target - std::lock_guard lk(mutex_); - auto it = target_registry_.find(goal->target_id); - if (it == target_registry_.end()) { - auto res = std::make_shared(); - res->success = false; - res->message = "Unknown target_id: " + goal->target_id; - goal_handle->abort(res); - return; - } - if (is_ret && !it->second.visited) { - auto res = std::make_shared(); - res->success = false; - res->message = "Target not yet visited — cannot RETURN"; - goal_handle->abort(res); - return; - } - entry = it->second; - } else { - // Inline goal — no target_id - entry.id = ""; - entry.target_type = goal->target_type; - entry.tolerance_m = goal->tolerance_m; - entry.goal_source = goal->goal_type; - - if (goal->goal_type == NavAction::Goal::GPS) { - if (!latlon_client_->wait_for_service(3s)) { - auto res = std::make_shared(); - res->success = false; - res->message = "latlon_to_enu service not available"; - goal_handle->abort(res); - return; - } - auto req = std::make_shared(); - req->lat = goal->lat; - req->lon = goal->lon; - auto future = latlon_client_->async_send_request(req); - // Blocking wait is safe here: MultiThreadedExecutor + reentrant group - // means other callbacks continue on other threads. - if (future.wait_for(5s) != std::future_status::ready) { - auto res = std::make_shared(); - res->success = false; - res->message = "latlon_to_enu service timeout"; - goal_handle->abort(res); - return; - } - auto resp = future.get(); - entry.x_m = resp->x; - entry.y_m = resp->y; - } else { - entry.x_m = goal->x_m; - entry.y_m = goal->y_m; - } - - entry.goal_enu.header.frame_id = "map"; - entry.goal_enu.header.stamp = now(); - entry.goal_enu.pose.position.x = entry.x_m; - entry.goal_enu.pose.position.y = entry.y_m; - entry.goal_enu.pose.orientation.w = 1.0; - } - - // ── Apply state transition ───────────────────────────────────────────── - { - std::lock_guard lk(mutex_); - - if (state_ == State::TELEOP) { - auto res = std::make_shared(); - res->success = false; - res->message = "Cannot navigate — currently in TELEOP mode"; - goal_handle->abort(res); - return; - } - - // §3.2.2: During ABORTING, queue the goal rather than preempting the abort - if (state_ == State::ABORTING) { - // Preempt any previously queued goal - if (queued_goal_handle_ && queued_goal_handle_->is_active()) { - auto old_res = std::make_shared(); - old_res->success = false; - old_res->message = "Preempted by newer queued goal"; - queued_goal_handle_->abort(old_res); - } - queued_goal_handle_ = goal_handle; - queued_entry_ = entry; - queued_is_return_ = is_ret; - RCLCPP_INFO(get_logger(), - "[mission_executive] goal queued — currently ABORTING"); - return; - } - - // Preempt any existing active goal (guard: abort() throws if not active) - if (active_goal_handle_ && active_goal_handle_->is_active()) { - auto old_res = std::make_shared(); - old_res->success = false; - old_res->message = "Preempted by new goal"; - active_goal_handle_->abort(old_res); - active_goal_handle_ = nullptr; - } - - active_target_ = entry; - is_return_ = is_ret; - active_goal_handle_ = goal_handle; - - transition(is_ret ? State::RETURNING : State::NAVIGATING, - is_ret ? "RETURN action accepted" : "GO_TO action accepted"); - - // Trigger global planner - goal_pub_->publish(active_target_->goal_enu); - publishActiveTarget(); - } - } - - // ── SetTarget service ───────────────────────────────────────────────────── - - void onSetTarget( - const msgs::srv::SetTarget::Request::SharedPtr req, - msgs::srv::SetTarget::Response::SharedPtr res) - { - TargetEntry entry; - entry.id = req->target_id; - entry.target_type = req->target_type; - entry.tolerance_m = req->tolerance_m; - entry.goal_source = req->goal_type; - - if (req->goal_type == msgs::srv::SetTarget::Request::GPS) { - if (!latlon_client_->wait_for_service(3s)) { - res->success = false; - res->message = "latlon_to_enu service not available"; - return; - } - auto conv = std::make_shared(); - conv->lat = req->lat; - conv->lon = req->lon; - auto future = latlon_client_->async_send_request(conv); - if (future.wait_for(5s) != std::future_status::ready) { - res->success = false; - res->message = "latlon_to_enu service timeout"; - return; - } - auto resp = future.get(); - entry.x_m = resp->x; - entry.y_m = resp->y; - } else { - entry.x_m = req->x_m; - entry.y_m = req->y_m; - } - - entry.goal_enu.header.frame_id = "map"; - entry.goal_enu.header.stamp = now(); - entry.goal_enu.pose.position.x = entry.x_m; - entry.goal_enu.pose.position.y = entry.y_m; - entry.goal_enu.pose.orientation.w = 1.0; - - { - std::lock_guard lk(mutex_); - target_registry_[entry.id] = entry; - } - - res->success = true; - res->message = "Target '" + entry.id + "' registered"; - RCLCPP_INFO(get_logger(), - "Target '%s' registered: (%.2f, %.2f) tol=%.2fm", - entry.id.c_str(), entry.x_m, entry.y_m, entry.tolerance_m); - } - - // ── Member data ─────────────────────────────────────────────────────────── - - std::mutex mutex_; - State state_{State::IDLE}; - - std::unordered_map target_registry_; - std::optional active_target_; - bool is_return_{false}; - - // Goal queued while ABORTING (§3.2.2 — GO_TO/RETURN during ABORTING are "queued") - std::shared_ptr queued_goal_handle_; - TargetEntry queued_entry_; - bool queued_is_return_{false}; - - // Sensor cache - std::optional robot_pose_; - std::optional global_path_; - double robot_speed_{0.0}; - rclcpp::Time low_speed_start_{0, 0, RCL_ROS_TIME}; - bool low_speed_tracking_{false}; - uint8_t last_planner_event_{0}; - - // Parameters - double velocity_zero_threshold_{0.05}; - double arrival_hold_time_{1.0}; - double replan_distance_m_{3.0}; - - // ROS interfaces - rclcpp::CallbackGroup::SharedPtr reentrant_group_; - - rclcpp::Publisher::SharedPtr goal_pub_; - rclcpp::Publisher::SharedPtr nav_enabled_pub_; - rclcpp::Publisher::SharedPtr nav_mode_pub_; - rclcpp::Publisher::SharedPtr active_target_pub_; - rclcpp::Publisher::SharedPtr nav_status_pub_; - - rclcpp_action::Server::SharedPtr action_server_; - std::shared_ptr active_goal_handle_; - - rclcpp::Service::SharedPtr abort_srv_; - rclcpp::Service::SharedPtr set_target_srv_; - rclcpp::Service::SharedPtr teleop_srv_; - - rclcpp::Subscription::SharedPtr robot_pose_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr planner_event_sub_; - rclcpp::Subscription::SharedPtr global_path_sub_; - - rclcpp::Client::SharedPtr latlon_client_; - - rclcpp::TimerBase::SharedPtr status_timer_; -}; - -// ── main ────────────────────────────────────────────────────────────────── - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/minimal_navigation/todo.md b/src/subsystems/minimal_navigation/todo.md deleted file mode 100644 index 284b0975..00000000 --- a/src/subsystems/minimal_navigation/todo.md +++ /dev/null @@ -1,69 +0,0 @@ -# Navigation Stack — Implementation Progress - -## Legend -- [x] Done -- [ ] Not started - ---- - -## Plan 1 — `gps_pose_publisher` ✅ -- [x] New package: `gps_pose_publisher/` -- [x] WGS84 → ENU via `GeographicLib::LocalCartesian` -- [x] Publishes `/robot_pose` (PoseStamped, frame: map) -- [x] Broadcasts `map → base_link` TF -- [x] Publishes `/gps_status` (String) -- [x] Service `~/latlon_to_enu` (msgs/srv/LatLonToENU) -- [x] `use_start_gate_ref` param — waits for `/start_gate_ref` NavSatFix if true -- [x] Heading conversion: compass deg → ENU rad (`(90 - deg) * π/180`) -- [x] Only publishes once both GPS fix AND heading are available - -## Plan 2 — `ackermann_mppi` modifications ✅ -- [x] Input topic renamed: `/plan` → `/global_path` (transient_local QoS) -- [x] `nav_enabled_topic` param (default `/nav_enabled`) -- [x] Subscribe `std_msgs/Bool` on nav_enabled topic -- [x] `controlLoop()` short-circuits (zero cmd_vel, DEBUG log) when nav disabled -- [x] `std::atomic nav_enabled_` — thread-safe with MultiThreadedExecutor - -## Plan 3 — `athena_smac_planner` + `global_planner` ✅ -- [x] Package rename: `nav2_smac_planner` → `athena_smac_planner` (package.xml + CMakeLists.txt) -- [x] Library rename: `nav2_smac_planner` → `athena_smac_planner` -- [x] Removed nonexistent plugin XML exports from package.xml -- [x] `global_planner_node.cpp` — subscribes `/goal_pose`, `/robot_pose` -- [x] Owns `Costmap2DROS` under `"global_costmap"` namespace (StaticLayer from `/map`) -- [x] Uses `SmacPlannerHybrid::createPlan()` directly (no lifecycle) -- [x] Publishes `/global_path` (transient_local) and `/planner_event` -- [x] Emits `PLAN_FAILED` event on exception; `PLAN_SUCCEEDED` on success -- [x] WARN if `/goal_pose` arrives before `/robot_pose` -- [x] MultiThreadedExecutor with node + costmap node - ---- - -## Plan 4 — `mission_executive` ✅ -- [x] Package: `mission_executive/` (package.xml, CMakeLists.txt) -- [x] Action server `~/navigate_to_target` (NavigateToTarget) -- [x] Services: `~/abort` (Trigger), `~/set_target` (SetTarget), `~/teleop` (SetBool) -- [x] Publishes: `/goal_pose`, `/nav_enabled`, `/nav_mode`, `/active_target`, `/nav_status` -- [x] Subscribes: `/robot_pose`, `/odom`, `/planner_event`, `/global_path` -- [x] States: IDLE, NAVIGATING, ARRIVING, STOPPED_AT_TARGET, ABORTING, RETURNING, STOPPED_AT_RETURN, TELEOP -- [x] Full transition table from PLAN.md §3.2.2 -- [x] Calls `gps_pose_publisher/latlon_to_enu` for coordinate conversion -- [x] MultiThreadedExecutor + reentrant callback group (deadlock-safe) -- [x] Cross-track error replanning: replan when xtrack > `replan_distance_m` -- [x] Stop detection: `|twist.linear| < velocity_zero_threshold` held for `arrival_hold_time` seconds -- [x] Target registry via `~/set_target`; RETURN requires visited target -- [x] Preemption: new GO_TO aborts current active goal - -## Plan 5 — `nav.launch.py` + unified build ✅ -- [x] Package: `nav_bringup/` (package.xml, CMakeLists.txt) -- [x] `config/nav_params.yaml` — single param file for all nodes (from PLAN.md §7) -- [x] `launch/nav.launch.py` — includes `athena_gps/launch/gps_launch.py` -- [x] `/odom` → `/odom/ground_truth` remapped for `mission_executive` + `ackermann_mppi` -- [x] twist_stamper node: `cmd_vel_nav` → `/rear_ackermann_controller/reference` -- [x] Launches: `gps_pose_publisher`, `map_node`, `global_planner`, `ackermann_mppi`, `mission_executive` -- [x] Single `nav_params.yaml` passed to all nodes - ---- - -## Notes -- `msgs` package (at `src/msgs/`) already contains all required message/service/action definitions -- Phase 2 extensions (obstacle layers, ArUco, object detection) are deferred per PLAN.md §5 From 406e827063c8eb36f1c5fe523be8199a4dd9dfb6 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sat, 11 Apr 2026 16:27:57 -0400 Subject: [PATCH 17/21] fix cmake --- src/athena_sensors/launch/sensors.launch.py | 2 +- src/msgs/CMakeLists.txt | 4 ---- .../nav_bringup/config/nav_params_real.yaml | 2 +- .../navigation/nav_bringup/launch/emmn.launch.py | 12 ------------ 4 files changed, 2 insertions(+), 18 deletions(-) diff --git a/src/athena_sensors/launch/sensors.launch.py b/src/athena_sensors/launch/sensors.launch.py index 34dc12be..39958a5a 100644 --- a/src/athena_sensors/launch/sensors.launch.py +++ b/src/athena_sensors/launch/sensors.launch.py @@ -61,7 +61,7 @@ def generate_launch_description(): 'publish_tf': publish_odom, # this is really the odom tf, idk why its just called tf 'publish_map_tf': publish_map, 'publish_urdf': 'false', # we take care of this elsewhere - 'enable_gnss': 'true', + 'enable_gnss': enable_gnss, }.items(), condition=UnlessCondition(sim), ) diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 6d117f96..8d6c9814 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -23,10 +23,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ActiveTarget.msg" "msg/NavStatus.msg" "msg/Heading.msg" - "msg/PlannerEvent.msg" - "msg/ActiveTarget.msg" - "msg/NavStatus.msg" - "msg/Heading.msg" "srv/SetController.srv" "srv/LatLonToENU.srv" "srv/SetTarget.srv" diff --git a/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml index 976c33b8..d141cafd 100644 --- a/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml @@ -3,7 +3,7 @@ gps_pose_publisher: ros__parameters: use_sim_time: false - heading_topic: "/gps/heading" + heading_topic: "/heading" heading_offset_deg: 0.0 # set to 180 if sensor is mounted backwards use_start_gate_ref: false # if true, waits for /start_gate_ref NavSatFix to set origin origin_lat: .nan # set to a fixed lat/lon to skip first-fix origin logic diff --git a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py index 27c930da..490c9015 100644 --- a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py +++ b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py @@ -97,17 +97,6 @@ def generate_launch_description(): }.items(), ) - gps_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory('athena_gps'), - 'launch', - 'gps_launch.py', - ) - ), - launch_arguments={'sim': sim}.items(), - ) - gps_pose_publisher_node = Node( package='gps_pose_publisher', executable='gps_pose_publisher_node', @@ -172,7 +161,6 @@ def generate_launch_description(): sim_arg, robot_state_publisher_node, sensors_launch, - gps_launch, gps_pose_publisher_node, dem_costmap_converter_node, global_planner_node, From 8acbdb0a096b598164125d7f488f1b81727ef053 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sat, 11 Apr 2026 18:40:51 -0400 Subject: [PATCH 18/21] more fixes for real life testing --- src/simulation/scripts/heading_publisher.py | 53 +++--------------- .../metadata.yaml | 21 +++++++ .../rosbag2_1969_12_31-19_14_31_0.db3 | Bin 0 -> 24576 bytes .../metadata.yaml | 21 +++++++ .../rosbag2_1969_12_31-19_15_26_0.db3 | Bin 0 -> 24576 bytes 5 files changed, 49 insertions(+), 46 deletions(-) create mode 100644 umdloop athena-code on_rover rosbag2_1969_12_31-19_14_31/metadata.yaml create mode 100644 umdloop athena-code on_rover rosbag2_1969_12_31-19_14_31/rosbag2_1969_12_31-19_14_31_0.db3 create mode 100644 umdloop athena-code on_rover rosbag2_1969_12_31-19_15_26/metadata.yaml create mode 100644 umdloop athena-code on_rover rosbag2_1969_12_31-19_15_26/rosbag2_1969_12_31-19_15_26_0.db3 diff --git a/src/simulation/scripts/heading_publisher.py b/src/simulation/scripts/heading_publisher.py index 60d813a1..ac2f5763 100755 --- a/src/simulation/scripts/heading_publisher.py +++ b/src/simulation/scripts/heading_publisher.py @@ -4,11 +4,7 @@ import rclpy from rclpy.node import Node -<<<<<<< HEAD -from rclpy.qos import QoSPresetProfiles -======= from rclpy.qos import SensorDataQoS ->>>>>>> f33dc56 (building) from sensor_msgs.msg import MagneticField from msgs.msg import Heading @@ -26,69 +22,34 @@ def __init__(self): self.heading_acc_ = self.get_parameter('heading_acc').get_parameter_value().double_value self.pub_ = self.create_publisher(Heading, heading_topic, 10) -<<<<<<< HEAD - - # Always cache the first heading to start at 0 degrees - self.initial_heading_enu = None -======= ->>>>>>> f33dc56 (building) self.sub_ = self.create_subscription( MagneticField, mag_topic, self.on_mag, -<<<<<<< HEAD - QoSPresetProfiles.SENSOR_DATA.value, - ) - - self.get_logger().info( - f'Heading publisher: {mag_topic} -> {heading_topic} (Force Initial Offset: Enabled)' -======= SensorDataQoS(), ) self.get_logger().info( f'Heading publisher: {mag_topic} -> {heading_topic}' ->>>>>>> f33dc56 (building) ) def on_mag(self, msg: MagneticField): bx = msg.magnetic_field.x by = msg.magnetic_field.y -<<<<<<< HEAD - # In Gazebo's magnetometer the field is reported in the robot body frame - # with the simulated Earth field pointing North (+Y world). For a robot - # with x=forward, y=left this gives: - # bx = H * sin(yaw_enu) (North projected onto forward axis) - # by = H * cos(yaw_enu) (North projected onto left axis) - # So atan2(bx, by) = atan2(sin θ, cos θ) = θ, the correct CCW-from-East - # ENU heading. Using atan2(by, bx) would give a CW compass bearing - # instead, inverting the sign of every turn. - heading_enu_raw = math.degrees(math.atan2(bx, by)) - - if self.initial_heading_enu is None: - self.initial_heading_enu = heading_enu_raw - self.get_logger().info(f'Initial raw heading cached: {self.initial_heading_enu:.2f} deg. Starting at 0.00 deg.') - - # Apply offset so we start at 0 (East) - # ENU heading: degrees CCW from East - heading_enu = (heading_enu_raw - self.initial_heading_enu + 360.0) % 360.0 + # Gazebo magnetometer reports field in robot body frame (x=forward, y=left). + # Earth's horizontal field points North, so: + # bx = H*sin(yaw_enu), by = H*cos(yaw_enu) + # atan2(bx, by) = atan2(sin θ, cos θ) = θ = ENU heading directly. + heading_enu_deg = math.degrees(math.atan2(bx, by)) # Compass bearing: degrees clockwise from North - # North is 90 deg ENU. - compass_bearing = (90.0 - heading_enu + 360.0) % 360.0 -======= - # compass bearing: degrees clockwise from north - compass_bearing = (math.degrees(math.atan2(-by, bx)) + 360.0) % 360.0 - - # ENU heading: degrees CCW from east - heading_enu = (90.0 - compass_bearing + 360.0) % 360.0 ->>>>>>> f33dc56 (building) + compass_bearing = (90.0 - heading_enu_deg + 360.0) % 360.0 out = Heading() out.header = msg.header - out.heading = heading_enu + out.heading = heading_enu_deg # degrees — gps_pose_publisher applies * pi/180 out.heading_acc = self.heading_acc_ out.compass_bearing = compass_bearing self.pub_.publish(out) diff --git a/umdloop athena-code on_rover rosbag2_1969_12_31-19_14_31/metadata.yaml b/umdloop athena-code on_rover rosbag2_1969_12_31-19_14_31/metadata.yaml new file mode 100644 index 00000000..d907c56f --- /dev/null +++ b/umdloop athena-code on_rover rosbag2_1969_12_31-19_14_31/metadata.yaml @@ -0,0 +1,21 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 0 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + message_count: 0 + topics_with_message_count: + [] + compression_format: "" + compression_mode: "" + relative_file_paths: + - rosbag2_1969_12_31-19_14_31_0.db3 + files: + - path: rosbag2_1969_12_31-19_14_31_0.db3 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + duration: + nanoseconds: 0 + message_count: 0 \ No newline at end of file diff --git a/umdloop athena-code on_rover rosbag2_1969_12_31-19_14_31/rosbag2_1969_12_31-19_14_31_0.db3 b/umdloop athena-code on_rover rosbag2_1969_12_31-19_14_31/rosbag2_1969_12_31-19_14_31_0.db3 new file mode 100644 index 0000000000000000000000000000000000000000..8d37ede6e5f7bc6ba5f0198482595947f8cbb001 GIT binary patch literal 24576 zcmeI&Z;#S29KiAI_;b+&G4TQ60ZE?;$!Sb{=o8^w%;^N>X6{H#NWB%W31hfTbNCG3 zgKx=q!lEuW!1B{DYmI* z@@iJ!6!xw9p=#ZFdhw=VdTWiqYW`|AAAD;3HdPV=2q1s}0tg_000IagP;P;XTD|qS zW3TkWS2u|cRGi3QIo1A+smAh)itDFir|&u9Z1~E#6onyTG!*$T(cN{4{`qNd{iu4} z>U8Y&Dv_V(Dj$}YwQN?eKX9GhBwg*Bbv);_GZyd1XYcysiFoTwPLgn`uS`(8&f&-t z!;h|e@^8`nxm}m=Wg^AE9Sua@)>}WT9JktS`{#>oRNHyuG_MvSO4|;_vyg!jo^xq} z*afm$?l;9M(sHhU$V7*WX?CA2J{8VpDpLORYZy+SW~rJFDD2)5vVu z*KrbsdrNKZ=63UsWnLr%5I_I{1Q0*~0R#|0009ILD3idwO2ukle+$f4MVtG-`O%+* z00IagfB*srAb20zz2fJpcdz literal 0 HcmV?d00001 diff --git a/umdloop athena-code on_rover rosbag2_1969_12_31-19_15_26/metadata.yaml b/umdloop athena-code on_rover rosbag2_1969_12_31-19_15_26/metadata.yaml new file mode 100644 index 00000000..acd5fb7b --- /dev/null +++ b/umdloop athena-code on_rover rosbag2_1969_12_31-19_15_26/metadata.yaml @@ -0,0 +1,21 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 0 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + message_count: 0 + topics_with_message_count: + [] + compression_format: "" + compression_mode: "" + relative_file_paths: + - rosbag2_1969_12_31-19_15_26_0.db3 + files: + - path: rosbag2_1969_12_31-19_15_26_0.db3 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + duration: + nanoseconds: 0 + message_count: 0 \ No newline at end of file diff --git a/umdloop athena-code on_rover rosbag2_1969_12_31-19_15_26/rosbag2_1969_12_31-19_15_26_0.db3 b/umdloop athena-code on_rover rosbag2_1969_12_31-19_15_26/rosbag2_1969_12_31-19_15_26_0.db3 new file mode 100644 index 0000000000000000000000000000000000000000..8d37ede6e5f7bc6ba5f0198482595947f8cbb001 GIT binary patch literal 24576 zcmeI&Z;#S29KiAI_;b+&G4TQ60ZE?;$!Sb{=o8^w%;^N>X6{H#NWB%W31hfTbNCG3 zgKx=q!lEuW!1B{DYmI* z@@iJ!6!xw9p=#ZFdhw=VdTWiqYW`|AAAD;3HdPV=2q1s}0tg_000IagP;P;XTD|qS zW3TkWS2u|cRGi3QIo1A+smAh)itDFir|&u9Z1~E#6onyTG!*$T(cN{4{`qNd{iu4} z>U8Y&Dv_V(Dj$}YwQN?eKX9GhBwg*Bbv);_GZyd1XYcysiFoTwPLgn`uS`(8&f&-t z!;h|e@^8`nxm}m=Wg^AE9Sua@)>}WT9JktS`{#>oRNHyuG_MvSO4|;_vyg!jo^xq} z*afm$?l;9M(sHhU$V7*WX?CA2J{8VpDpLORYZy+SW~rJFDD2)5vVu z*KrbsdrNKZ=63UsWnLr%5I_I{1Q0*~0R#|0009ILD3idwO2ukle+$f4MVtG-`O%+* z00IagfB*srAb20zz2fJpcdz literal 0 HcmV?d00001 From 688a0570a46e79f22917f61a9f27089eb924dcba Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sat, 11 Apr 2026 18:44:05 -0400 Subject: [PATCH 19/21] removed heading --- .../athena_gps/config/pixhawk_params.yaml | 1 - .../navigation/athena_gps/src/PixhawkNode.cpp | 15 --------------- .../nav_bringup/config/nav_params_real.yaml | 2 +- 3 files changed, 1 insertion(+), 17 deletions(-) diff --git a/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml b/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml index de013f9e..ad9c2471 100644 --- a/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml +++ b/src/subsystems/navigation/athena_gps/config/pixhawk_params.yaml @@ -11,7 +11,6 @@ athena_gps: imu_topic: "imu/data" gps_topic: "gps/fix" - heading_topic: "heading" mavsdk_system_id: 10 mavsdk_component_id: 1 diff --git a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp index 545129b5..c7d3d1b7 100644 --- a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp +++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp @@ -7,7 +7,6 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "msgs/msg/heading.hpp" #include #include @@ -21,7 +20,6 @@ class PixhawkNode : public rclcpp::Node : Node("athena_gps"), last_gps_info_{}, last_attitude_quaternion_{}, - last_heading_deg_{}, is_initialized_(false), connection_added_(false) { @@ -33,7 +31,6 @@ class PixhawkNode : public rclcpp::Node this->declare_parameter("gps_frame_id", "gps_link"); this->declare_parameter("imu_topic", "imu/data"); this->declare_parameter("gps_topic", "gps/fix"); - this->declare_parameter("heading_topic", "heading"); this->declare_parameter("mavsdk_system_id", 10); this->declare_parameter("mavsdk_component_id", 1); this->declare_parameter("mavsdk_always_send_heartbeats", false); @@ -46,7 +43,6 @@ class PixhawkNode : public rclcpp::Node gps_frame_id_ = this->get_parameter("gps_frame_id").as_string(); std::string imu_topic = this->get_parameter("imu_topic").as_string(); std::string gps_topic = this->get_parameter("gps_topic").as_string(); - std::string heading_topic = this->get_parameter("heading_topic").as_string(); int mavsdk_system_id = this->get_parameter("mavsdk_system_id").as_int(); int mavsdk_component_id = this->get_parameter("mavsdk_component_id").as_int(); bool mavsdk_always_send_heartbeats = this->get_parameter("mavsdk_always_send_heartbeats").as_bool(); @@ -54,7 +50,6 @@ class PixhawkNode : public rclcpp::Node // Create publishers unconditionally so they're always valid imu_publisher_ = this->create_publisher(imu_topic, 10); gps_publisher_ = this->create_publisher(gps_topic, 10); - heading_publisher_ = this->create_publisher(heading_topic, 10); // Create MAVSDK instance mavsdk_ = std::make_unique(Mavsdk::Configuration{ @@ -78,12 +73,10 @@ class PixhawkNode : public rclcpp::Node rclcpp::Publisher::SharedPtr imu_publisher_; rclcpp::Publisher::SharedPtr gps_publisher_; - rclcpp::Publisher::SharedPtr heading_publisher_; rclcpp::TimerBase::SharedPtr init_timer_; Telemetry::GpsInfo last_gps_info_; Telemetry::Quaternion last_attitude_quaternion_; - double last_heading_deg_; std::string imu_frame_id_; std::string gps_frame_id_; @@ -272,14 +265,6 @@ class PixhawkNode : public rclcpp::Node } else { gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } - auto heading_msg = msgs::msg::Heading(); - heading_msg.header.stamp = this->now(); - heading_msg.compass_bearing = raw_gps.yaw_deg; - // Convert from compass bearing (0=magnetic North, CW, degrees) to ROS convention (0=East, CCW, radians) - heading_msg.heading = (90.0 - raw_gps.yaw_deg) * M_PI / 180.0; - heading_msg.heading_acc = raw_gps.heading_uncertainty_deg * M_PI / 180.0; - - heading_publisher_->publish(heading_msg); gps_publisher_->publish(gps_msg); } }; diff --git a/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml index d141cafd..f71445f9 100644 --- a/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml @@ -73,7 +73,7 @@ vector_field_planner: use_sim_time: false map_frame: map base_frame: base_link - cmd_vel_topic: "/front_ackermann_controller/reference" + cmd_vel_topic: "/rear_ackermann_controller/reference" tf_timeout_s: 0.2 # slightly longer to handle wireless / CPU spikes max_speed_mps: 0.5 # LOWERED FOR REAL LIFE SAFETY - adjust up to 1.0 once tested max_steering_angle_rad: 0.5 # radians — clamp on angular.z output From f9f9b943876ed66000fe8bbaf352a5896f4cb7a9 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani <46766905+mdurrani808@users.noreply.github.com> Date: Mon, 13 Apr 2026 19:13:45 -0400 Subject: [PATCH 20/21] Delete .claude/settings.local.json --- .claude/settings.local.json | 11 ----------- 1 file changed, 11 deletions(-) delete mode 100644 .claude/settings.local.json diff --git a/.claude/settings.local.json b/.claude/settings.local.json deleted file mode 100644 index 4fe02daa..00000000 --- a/.claude/settings.local.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "permissions": { - "allow": [ - "Bash(find:*)", - "Bash(git rm:*)", - "Bash(git checkout:*)", - "Bash(git -C /Users/mdurrani/Development/umdloop/ros2-devenv/main/athena-code diff main...HEAD)", - "Bash(xargs sed:*)" - ] - } -} From 27f4b058f1f3b742c6bc782b87e18fbbc75bb67f Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Fri, 24 Apr 2026 19:38:07 -0400 Subject: [PATCH 21/21] update vector field planner and mission executive --- .../mission_executive/CMakeLists.txt | 5 + .../navigation/mission_executive/package.xml | 3 + .../mission_executive/scripts/mission_cli.py | 570 ++++++++++++++++++ .../nav_bringup/config/nav_params_real.yaml | 6 +- .../nav_bringup/config/nav_params_sim.yaml | 6 +- .../nav_bringup/launch/emmn.launch.py | 13 +- .../vector_field_planner_algo.hpp | 15 + .../src/vector_field_planner_algo.cpp | 273 ++++++++- .../src/vector_field_planner_node.cpp | 13 +- .../test/test_vector_field_planner_algo.cpp | 108 ++++ 10 files changed, 978 insertions(+), 34 deletions(-) create mode 100644 src/subsystems/navigation/mission_executive/scripts/mission_cli.py diff --git a/src/subsystems/navigation/mission_executive/CMakeLists.txt b/src/subsystems/navigation/mission_executive/CMakeLists.txt index 913ef8a3..9cac422a 100644 --- a/src/subsystems/navigation/mission_executive/CMakeLists.txt +++ b/src/subsystems/navigation/mission_executive/CMakeLists.txt @@ -38,4 +38,9 @@ install(TARGETS mission_executive_node DESTINATION lib/${PROJECT_NAME} ) +install(PROGRAMS scripts/mission_cli.py + DESTINATION lib/${PROJECT_NAME} + RENAME mission_cli +) + ament_package() diff --git a/src/subsystems/navigation/mission_executive/package.xml b/src/subsystems/navigation/mission_executive/package.xml index c115b8e0..cd918460 100644 --- a/src/subsystems/navigation/mission_executive/package.xml +++ b/src/subsystems/navigation/mission_executive/package.xml @@ -21,6 +21,9 @@ vision_msgs msgs + rclpy + action_msgs + ament_cmake diff --git a/src/subsystems/navigation/mission_executive/scripts/mission_cli.py b/src/subsystems/navigation/mission_executive/scripts/mission_cli.py new file mode 100644 index 00000000..111e4260 --- /dev/null +++ b/src/subsystems/navigation/mission_executive/scripts/mission_cli.py @@ -0,0 +1,570 @@ +#!/usr/bin/env python3 +""" +mission_cli — Interactive CLI for the MissionExecutive node + +Usage: + ros2 run mission_executive mission_cli [command] [args...] + +Commands: + status Print current nav status once + watch Stream nav status (Ctrl-C to stop) + abort Abort the current mission + teleop on|off Enable / disable teleop mode + + nav gps [tol] Navigate to GPS coords (GNSS_ONLY target) + nav meter [tol] Navigate to ENU metre coords + nav aruco [tol] Navigate + spiral for ArUco post + nav object [tol] Navigate + spiral for OBJECT detection + + set-target gps [type] [tol] Register GPS target + set-target meter [type] [tol] Register ENU target + nav-by-id [is_return] Navigate to a pre-registered target id + + menu Interactive menu (default if no args) + +target_type: 0=GNSS_ONLY 1=ARUCO_POST 2=OBJECT 3=LOCAL +tol: arrival radius in metres (default 3.0) +""" + +import signal +import sys +import threading +import time +from typing import Optional + +import rclpy +from rclpy.action import ActionClient +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from action_msgs.msg import GoalStatus +from msgs.action import NavigateToTarget +from msgs.msg import NavStatus +from msgs.srv import SetTarget +from std_srvs.srv import SetBool, Trigger + +# ── Constants ───────────────────────────────────────────────────────────────── + +NODE_NS = "/mission_executive" +STATUS_TOPIC = "/nav_status" + +RED = "\033[0;31m" +GRN = "\033[0;32m" +YLW = "\033[1;33m" +CYN = "\033[0;36m" +BLD = "\033[1m" +DIM = "\033[2m" +RST = "\033[0m" + +TARGET_TYPE_NAMES = {0: "GNSS_ONLY", 1: "ARUCO_POST", 2: "OBJECT", 3: "LOCAL"} + +GOAL_STATUS_NAMES = { + GoalStatus.STATUS_UNKNOWN: "UNKNOWN", + GoalStatus.STATUS_ACCEPTED: "ACCEPTED", + GoalStatus.STATUS_EXECUTING: "EXECUTING", + GoalStatus.STATUS_CANCELING: "CANCELING", + GoalStatus.STATUS_SUCCEEDED: "SUCCEEDED", + GoalStatus.STATUS_CANCELED: "CANCELED", + GoalStatus.STATUS_ABORTED: "ABORTED", +} + +# ── Helpers ─────────────────────────────────────────────────────────────────── + +def info(msg): print(f"{GRN}[✓]{RST} {msg}") +def warn(msg): print(f"{YLW}[!]{RST} {msg}") +def error(msg): print(f"{RED}[✗]{RST} {msg}", file=sys.stderr) +def title(msg): print(f"\n{BLD}{msg}{RST}") + +def target_type_name(t: int) -> str: + return TARGET_TYPE_NAMES.get(t, f"UNKNOWN({t})") + +# ── MissionCLI ──────────────────────────────────────────────────────────────── + +class MissionCLI: + """ + Thin wrapper around rclpy primitives. A single Node is created once; a + background executor thread keeps it spinning so every async future and + subscription callback is dispatched automatically. + """ + + def __init__(self): + rclpy.init() + self._node = rclpy.create_node("mission_cli") + + # Background executor — all ROS 2 I/O happens here + self._executor = SingleThreadedExecutor() + self._executor.add_node(self._node) + self._spin_thread = threading.Thread( + target=self._executor.spin, daemon=True, name="ros2_spin" + ) + self._spin_thread.start() + + # Service clients (created once, reused for every call) + self._abort_client = self._node.create_client( + Trigger, f"{NODE_NS}/abort" + ) + self._teleop_client = self._node.create_client( + SetBool, f"{NODE_NS}/teleop" + ) + self._set_target_client = self._node.create_client( + SetTarget, f"{NODE_NS}/set_target" + ) + + # Action client + self._action_client = ActionClient( + self._node, NavigateToTarget, f"{NODE_NS}/navigate_to_target" + ) + + def destroy(self): + self._executor.shutdown(timeout_sec=2.0) + self._node.destroy_node() + rclpy.shutdown() + + # ── Low-level helpers ────────────────────────────────────────────────── + + def _wait_for_service(self, client, timeout: float = 5.0) -> bool: + """Poll service readiness without calling spin (executor already running).""" + deadline = time.monotonic() + timeout + while not client.service_is_ready(): + if time.monotonic() > deadline: + return False + time.sleep(0.05) + return True + + def _wait_for_action_server(self, timeout: float = 5.0) -> bool: + deadline = time.monotonic() + timeout + while not self._action_client.server_is_ready(): + if time.monotonic() > deadline: + return False + time.sleep(0.05) + return True + + def _call_service(self, client, request, timeout: float = 5.0): + """Send a service request and block until the response arrives.""" + if not self._wait_for_service(client, timeout): + error(f"Service {client.srv_name} not available") + return None + future = client.call_async(request) + deadline = time.monotonic() + timeout + while not future.done(): + if time.monotonic() > deadline: + error("Service call timed out") + return None + time.sleep(0.05) + return future.result() + + def _get_status_once(self, timeout: float = 2.0) -> Optional[NavStatus]: + """Subscribe to /nav_status, return the first message, then unsubscribe.""" + event = threading.Event() + holder: list[Optional[NavStatus]] = [None] + + def _cb(msg: NavStatus): + holder[0] = msg + event.set() + + sub = self._node.create_subscription(NavStatus, STATUS_TOPIC, _cb, 10) + try: + event.wait(timeout=timeout) + finally: + self._node.destroy_subscription(sub) + return holder[0] + + # ── cmd_status / cmd_watch ───────────────────────────────────────────── + + def cmd_status(self): + title("Nav Status") + msg = self._get_status_once() + if msg is None: + warn("No message received — is mission_executive running?") + return + print(f" {'State:':<22} {BLD}{msg.state}{RST}") + print(f" {'Active target:':<22} {msg.active_target_id or '(none)'}") + print(f" {'Target type:':<22} {target_type_name(msg.active_target_type)}") + print(f" {'Dist to goal:':<22} {msg.distance_to_goal_m:.3f} m") + print(f" {'Cross-track err:':<22} {msg.cross_track_error_m:.3f} m") + print(f" {'Robot speed:':<22} {msg.robot_speed_mps:.3f} m/s") + print(f" {'Is return:':<22} {msg.is_return}") + + def cmd_watch(self): + title(f"Watching {STATUS_TOPIC} (Ctrl-C to stop)") + done = threading.Event() + + def _cb(msg: NavStatus): + print( + f"state={msg.state:<20} " + f"tgt={msg.active_target_id:<12} " + f"dist={msg.distance_to_goal_m:>7.2f} m " + f"xte={msg.cross_track_error_m:>7.2f} m" + ) + + sub = self._node.create_subscription(NavStatus, STATUS_TOPIC, _cb, 10) + try: + done.wait() # blocks until KeyboardInterrupt below + except KeyboardInterrupt: + pass + finally: + self._node.destroy_subscription(sub) + + # ── cmd_abort / cmd_teleop ───────────────────────────────────────────── + + def cmd_abort(self): + title("Abort Mission") + resp = self._call_service(self._abort_client, Trigger.Request()) + if resp: + print(f" success : {resp.success}") + print(f" message : {resp.message}") + + def cmd_teleop(self, mode: str): + mode = mode.lower() + if mode in ("on", "1", "true"): + title("Teleop ON") + req = SetBool.Request() + req.data = True + elif mode in ("off", "0", "false"): + title("Teleop OFF") + req = SetBool.Request() + req.data = False + else: + error("Usage: teleop on|off") + sys.exit(1) + resp = self._call_service(self._teleop_client, req) + if resp: + print(f" success : {resp.success}") + print(f" message : {resp.message}") + + # ── Live-status display while an action runs ─────────────────────────── + + def _run_nav_with_status(self, goal: NavigateToTarget.Goal): + target_type = goal.target_type + if target_type in (1, 2): + print(f" {CYN}Note: After arrival the node will enter SPIRAL_COVERAGE{RST}") + print(f" {CYN} and complete when a detection is received or it times out.{RST}") + print() + + if not self._wait_for_action_server(timeout=5.0): + error("navigate_to_target action server not available") + return + + prev_state: list[str] = [""] + start_t = time.monotonic() + interrupted = False + goal_handle_holder: list = [None] + goal_accepted_event = threading.Event() + result_event = threading.Event() + result_holder: list = [None] + + def _feedback_cb(feedback_msg): + fb = feedback_msg.feedback + elapsed = int(time.monotonic() - start_t) + if fb.state != prev_state[0]: + print(f"\n {YLW}→ State: {BLD}{fb.state}{RST}") + prev_state[0] = fb.state + print( + f"\r [{elapsed:3d}s] " + f"dist={fb.distance_to_goal_m:>8.2f} m " + f"xte={fb.cross_track_error_m:>8.2f} m ", + end="", flush=True, + ) + + def _on_result(future): + result_holder[0] = future.result() + result_event.set() + + def _on_goal_response(future): + gh = future.result() + goal_handle_holder[0] = gh + if gh and gh.accepted: + gh.get_result_async().add_done_callback(_on_result) + goal_accepted_event.set() + + send_future = self._action_client.send_goal_async( + goal, feedback_callback=_feedback_cb + ) + send_future.add_done_callback(_on_goal_response) + + def _on_sigint(sig, frame): + nonlocal interrupted + interrupted = True + print() + warn("Interrupted — calling abort...") + self._abort_client.call_async(Trigger.Request()) + gh = goal_handle_holder[0] + if gh: + gh.cancel_goal_async() + result_event.set() + + old_handler = signal.signal(signal.SIGINT, _on_sigint) + print(f" {DIM}Waiting for goal acceptance...{RST}") + + goal_accepted_event.wait(timeout=10.0) + gh = goal_handle_holder[0] + if not gh or not gh.accepted: + error("Goal was rejected by the action server") + signal.signal(signal.SIGINT, old_handler) + return + + print(f" {DIM}Goal accepted — navigating...{RST}") + result_event.wait() + + signal.signal(signal.SIGINT, old_handler) + print("\n") + + if not interrupted: + result_wrapper = result_holder[0] + if result_wrapper: + print(f"{BLD}── Action Result ──────────────────────────────{RST}") + print(f" success : {result_wrapper.result.success}") + print(f" message : {result_wrapper.result.message}") + status = GOAL_STATUS_NAMES.get(result_wrapper.status, + str(result_wrapper.status)) + print(f" status : {status}") + print(f"{BLD}───────────────────────────────────────────────{RST}") + + # ── _build_nav_goal / cmd_nav ────────────────────────────────────────── + + def _build_nav_goal( + self, + goal_type: int, + target_id: str, + a: str, + b: str, + target_type: int = 0, + tolerance: float = 3.0, + is_return: bool = False, + ) -> NavigateToTarget.Goal: + goal = NavigateToTarget.Goal() + goal.target_id = target_id + goal.goal_type = goal_type + goal.target_type = target_type + goal.tolerance_m = tolerance + goal.is_return = is_return + if goal_type == NavigateToTarget.Goal.GPS: + goal.lat = float(a) + goal.lon = float(b) + else: + goal.x_m = float(a) + goal.y_m = float(b) + + title("navigate_to_target") + print(f" target_id : {target_id or '(inline)'}") + print(f" goal_type : {'GPS' if goal_type == 0 else 'METER'}") + if goal_type == NavigateToTarget.Goal.GPS: + print(f" lat / lon : {goal.lat} / {goal.lon}") + else: + print(f" x_m / y_m : {goal.x_m} / {goal.y_m}") + print(f" target_type : {target_type_name(target_type)}") + print(f" tolerance_m : {tolerance}") + print(f" is_return : {is_return}") + print() + return goal + + def _nav_usage(self): + error("Usage:") + error(" nav gps [tol] — GNSS_ONLY target") + error(" nav meter [tol] — ENU metre target") + error(" nav aruco [tol] — navigate + spiral (ArUco)") + error(" nav object [tol] — navigate + spiral (YOLO object)") + sys.exit(1) + + def cmd_nav(self, args: list): + if not args: + self._nav_usage() + coord = args[0].lower() + tol = float(args[3]) if len(args) > 3 else 3.0 + + if coord == "gps": + if len(args) < 3: self._nav_usage() + goal = self._build_nav_goal(0, "", args[1], args[2], 0, tol) + elif coord in ("meter", "m"): + if len(args) < 3: self._nav_usage() + goal = self._build_nav_goal(1, "", args[1], args[2], 0, tol) + elif coord == "aruco": + if len(args) < 3: self._nav_usage() + goal = self._build_nav_goal(0, "", args[1], args[2], 1, tol) + elif coord in ("object", "obj"): + if len(args) < 3: self._nav_usage() + goal = self._build_nav_goal(0, "", args[1], args[2], 2, tol) + else: + self._nav_usage() + return + self._run_nav_with_status(goal) + + def cmd_nav_by_id(self, target_id: str, is_return: bool = False): + title(f"navigate_to_target (id={target_id} is_return={is_return})") + goal = NavigateToTarget.Goal() + goal.target_id = target_id + goal.goal_type = NavigateToTarget.Goal.GPS + goal.target_type = NavigateToTarget.Goal.GNSS_ONLY + goal.tolerance_m = 3.0 + goal.is_return = is_return + self._run_nav_with_status(goal) + + # ── cmd_set_target ───────────────────────────────────────────────────── + + def _set_target_usage(self): + error("Usage: set-target gps [type 0-3] [tol]") + error(" set-target meter [type 0-3] [tol]") + sys.exit(1) + + def cmd_set_target(self, args: list): + if not args: + self._set_target_usage() + coord = args[0].lower() + if coord == "gps": + if len(args) < 4: self._set_target_usage() + self._call_set_target(0, args[1], args[2], args[3], + int(args[4]) if len(args) > 4 else 0, + float(args[5]) if len(args) > 5 else 3.0) + elif coord in ("meter", "m"): + if len(args) < 4: self._set_target_usage() + self._call_set_target(1, args[1], args[2], args[3], + int(args[4]) if len(args) > 4 else 0, + float(args[5]) if len(args) > 5 else 3.0) + else: + self._set_target_usage() + + def _call_set_target(self, goal_type: int, target_id: str, a: str, b: str, + target_type: int = 0, tolerance: float = 3.0): + req = SetTarget.Request() + req.target_id = target_id + req.goal_type = goal_type + req.target_type = target_type + req.tolerance_m = tolerance + if goal_type == SetTarget.Request.GPS: + req.lat = float(a) + req.lon = float(b) + else: + req.x_m = float(a) + req.y_m = float(b) + + title(f"set_target (id={target_id} type={target_type_name(target_type)})") + resp = self._call_service(self._set_target_client, req) + if resp: + print(f" success : {resp.success}") + print(f" message : {resp.message}") + + # ── Interactive menu ─────────────────────────────────────────────────── + + def _menu_nav_goal(self): + print() + print("Navigation type:") + print(" 1) GPS point (GNSS_ONLY)") + print(" 2) GPS point (ArUco post — will spiral after arrival)") + print(" 3) GPS point (YOLO object — will spiral after arrival)") + print(" 4) ENU metres (GNSS_ONLY)") + print(" 5) By registered target ID") + ct = input("Choice: ").strip() + + if ct == "5": + tid = input("Target ID: ").strip() + ret = input("Is return? [y/N]: ").strip().lower() + self.cmd_nav_by_id(tid, ret in ("y", "yes")) + return + + goal_type, target_type = 0, 0 + if ct == "2": target_type = 1 + elif ct == "3": target_type = 2 + elif ct == "4": goal_type = 1 + + if goal_type == 0: + a = input("Latitude: ").strip() + b = input("Longitude: ").strip() + else: + a = input("x_m: ").strip() + b = input("y_m: ").strip() + + tol_s = input("Arrival tolerance in metres [3.0]: ").strip() + goal = self._build_nav_goal(goal_type, "", a, b, target_type, + float(tol_s) if tol_s else 3.0, False) + self._run_nav_with_status(goal) + + def _menu_set_target(self): + print() + print("Coordinate type:") + print(" 1) GPS (lat/lon) 2) ENU metres (x/y)") + ct = input("Choice: ").strip() + tid = input("Target ID: ").strip() + if ct == "1": + a, b, goal_type = input("Latitude: ").strip(), input("Longitude: ").strip(), 0 + else: + a, b, goal_type = input("x_m: ").strip(), input("y_m: ").strip(), 1 + print("Target type: 0=GNSS_ONLY 1=ARUCO_POST 2=OBJECT 3=LOCAL") + ttype_s = input("Target type [0]: ").strip() + tol_s = input("Arrival tolerance [3.0]: ").strip() + self._call_set_target(goal_type, tid, a, b, + int(ttype_s) if ttype_s else 0, + float(tol_s) if tol_s else 3.0) + + def cmd_menu(self): + while True: + print() + print(f"{BLD}╔══ Mission Executive CLI ════════════════════╗{RST}") + print(f"{BLD}║{RST} 1) Status (snapshot) {BLD}║{RST}") + print(f"{BLD}║{RST} 2) Watch status (stream) {BLD}║{RST}") + print(f"{BLD}║{RST} 3) Navigate (GPS / meter / aruco / object) {BLD}║{RST}") + print(f"{BLD}║{RST} 4) Register target (set_target service) {BLD}║{RST}") + print(f"{BLD}║{RST} 5) Abort mission {BLD}║{RST}") + print(f"{BLD}║{RST} 6) Teleop ON {BLD}║{RST}") + print(f"{BLD}║{RST} 7) Teleop OFF {BLD}║{RST}") + print(f"{BLD}║{RST} q) Quit {BLD}║{RST}") + print(f"{BLD}╚═════════════════════════════════════════════╝{RST}") + try: + choice = input("Choice: ").strip() + except (KeyboardInterrupt, EOFError): + info("Bye.") + return + + if choice == "1": self.cmd_status() + elif choice == "2": self.cmd_watch() + elif choice == "3": self._menu_nav_goal() + elif choice == "4": self._menu_set_target() + elif choice == "5": self.cmd_abort() + elif choice == "6": self.cmd_teleop("on") + elif choice == "7": self.cmd_teleop("off") + elif choice in ("q", "Q"): + info("Bye.") + return + else: + warn(f"Unknown option '{choice}'") + +# ── Entry point ─────────────────────────────────────────────────────────────── + +def main(): + args = sys.argv[1:] + cmd = args[0] if args else "menu" + rest = args[1:] + + cli = MissionCLI() + try: + if cmd in ("-h", "--help", "help"): + print(__doc__) + elif cmd == "status": + cli.cmd_status() + elif cmd == "watch": + cli.cmd_watch() + elif cmd == "abort": + cli.cmd_abort() + elif cmd == "teleop": + if not rest: + error("Usage: teleop on|off"); sys.exit(1) + cli.cmd_teleop(rest[0]) + elif cmd == "nav": + cli.cmd_nav(rest) + elif cmd == "nav-by-id": + if not rest: + error("nav-by-id requires a target_id"); sys.exit(1) + is_return = len(rest) > 1 and rest[1].lower() in ("true", "1", "yes") + cli.cmd_nav_by_id(rest[0], is_return) + elif cmd == "set-target": + cli.cmd_set_target(rest) + elif cmd == "menu": + cli.cmd_menu() + else: + error(f"Unknown command: {cmd}") + print("Run with 'help' for usage.") + sys.exit(1) + finally: + cli.destroy() + + +if __name__ == "__main__": + main() diff --git a/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml index f71445f9..3818cc29 100644 --- a/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml @@ -45,12 +45,12 @@ mission_executive: active_target_topic: "/active_target" nav_status_topic: "/nav_status" -reframe_pointcloud: +point_cloud_filterer: ros__parameters: use_sim_time: false input_topic: "/zed/zed_node/point_cloud/cloud_registered" output_topic: "/zed/cloud_base_link" - target_frame: "base_link" + frame_override: "base_link" pointcloud_to_laserscan: ros__parameters: @@ -76,7 +76,7 @@ vector_field_planner: cmd_vel_topic: "/rear_ackermann_controller/reference" tf_timeout_s: 0.2 # slightly longer to handle wireless / CPU spikes max_speed_mps: 0.5 # LOWERED FOR REAL LIFE SAFETY - adjust up to 1.0 once tested - max_steering_angle_rad: 0.5 # radians — clamp on angular.z output + max_steering_angle_rad: 0.785 # radians — clamp on angular.z output (45 deg physical limit) lookahead_dist_m: 3.0 # pure-pursuit lookahead distance k_p_steering: 1.5 # proportional gain: angular.z = k_p * heading_error # ── Obstacle avoidance ────────────────────────────────────────────────── diff --git a/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml b/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml index 21091350..0f78357d 100644 --- a/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml @@ -45,12 +45,12 @@ mission_executive: active_target_topic: "/active_target" nav_status_topic: "/nav_status" -reframe_pointcloud: +point_cloud_filterer: ros__parameters: use_sim_time: true input_topic: "/zed/zed_node/point_cloud/cloud_registered" output_topic: "/zed/cloud_base_link" - target_frame: "base_link" + frame_override: "base_link" pointcloud_to_laserscan: ros__parameters: @@ -76,7 +76,7 @@ vector_field_planner: cmd_vel_topic: "/front_ackermann_controller/reference" tf_timeout_s: 0.1 max_speed_mps: 0.8 - max_steering_angle_rad: 0.5 # radians — clamp on angular.z output + max_steering_angle_rad: 0.785 # radians — clamp on angular.z output (45 deg physical limit) lookahead_dist_m: 3.0 # pure-pursuit lookahead distance k_p_steering: 1.5 # proportional gain: angular.z = k_p * heading_error # ── Obstacle avoidance ────────────────────────────────────────────────── diff --git a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py index 490c9015..86f7b926 100644 --- a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py +++ b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py @@ -41,7 +41,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, IfElseSubstitution, Command, FindExecutable, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, Command, FindExecutable, PathJoinSubstitution from launch.conditions import IfCondition from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -145,6 +145,14 @@ def generate_launch_description(): ) + point_cloud_filterer_node = Node( + package='point_cloud_filterer', + executable='point_cloud_filtered', + name='point_cloud_filterer', + output='screen', + parameters=[nav_params_file], + ) + pointcloud_to_laserscan_node = Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', @@ -152,7 +160,7 @@ def generate_launch_description(): output='screen', parameters=[nav_params_file], remappings=[ - ('cloud_in', IfElseSubstitution(sim, if_value='/zed/cloud_base_link', else_value='/zed/zed_node/point_cloud/cloud_registered')), + ('cloud_in', '/zed/cloud_base_link'), ('scan', '/scan'), ], ) @@ -164,6 +172,7 @@ def generate_launch_description(): gps_pose_publisher_node, dem_costmap_converter_node, global_planner_node, + point_cloud_filterer_node, pointcloud_to_laserscan_node, vector_field_planner_node, mission_executive_node, diff --git a/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp b/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp index c5740da7..110e238f 100644 --- a/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp +++ b/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp @@ -41,6 +41,7 @@ struct PlannerParams { bool obstacle_avoidance_enabled = false; double repulsion_gain = 0.5; double repulsion_cutoff_m = 3.0; + double vfh_threshold = 0.5; // Approach velocity scaling: ramp speed down to min_approach_linear_velocity // as the robot closes within lookahead_dist_m of the goal. The velocity-scaled @@ -69,6 +70,10 @@ struct PlannerResult { int active_points = 0; double closest_r = -1.0; + // VFH* debug info + int vfh_k_best = -1; + int vfh_candidates_n = 0; + // Debug info for new features double effective_lookahead_dist = 0.0; // actual lookahead used this tick bool lookahead_interpolated = false; // true when carrot was interpolated between waypoints @@ -142,9 +147,19 @@ class VectorFieldPlannerAlgo { PlannerParams params_; std::vector path_; std::vector obstacles_; + int prev_k_{0}; double effectiveLookaheadDist(double current_speed) const; double approachVelocityScale(double dist_to_goal) const; + + // VFH* internals + std::vector buildHistogram(const std::vector& obs, + double rx, double ry) const; + std::vector smoothHistogram(const std::vector& h) const; + std::vector findCandidates(const std::vector& h, + int k_target, int k_prev) const; + int runAstar(double rx, double ry, double yaw, + int k_target, int k_prev) const; }; } // namespace vector_field_planner diff --git a/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp index b9282ad6..24741cea 100644 --- a/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp +++ b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp @@ -17,9 +17,24 @@ #include #include #include +#include namespace vector_field_planner { +// VFH* Constants (§9.2) +constexpr double kAlphaDeg = 5.0; +constexpr double kAlphaRad = kAlphaDeg * M_PI / 180.0; +constexpr int kNumBins = static_cast(360.0 / kAlphaDeg); +constexpr int kSmoothL = 3; // 7-bin smoothing window +constexpr int kSMax = 18; // wide valley threshold (bins) +constexpr int kNg = 5; // look-ahead depth +constexpr double kDs = 1.0; // step distance (meters, ~robot diameter) +constexpr double kLambda = 0.85; + +// Cost weights +constexpr double kMu1 = 6.0, kMu2 = 3.0, kMu3 = 2.0; // depth 0 +constexpr double kMu1p = 5.0, kMu2p = 2.0, kMu3p = 1.0; // depth > 0 + // --------------------------------------------------------------------------- // Helpers // --------------------------------------------------------------------------- @@ -83,6 +98,214 @@ VectorFieldPlannerAlgo::LookaheadResult VectorFieldPlannerAlgo::findLookahead( return {path_.back().x, path_.back().y, false}; } +// --------------------------------------------------------------------------- +// VFH* Internals +// --------------------------------------------------------------------------- + +std::vector VectorFieldPlannerAlgo::buildHistogram( + const std::vector& obs, double rx, double ry) const +{ + std::vector h(kNumBins, 0.0); + const double d_max = params_.repulsion_cutoff_m; + + for (const auto& p : obs) { + const double dx = p.x - rx; + const double dy = p.y - ry; + const double dist = std::hypot(dx, dy); + + if (dist >= d_max || dist < 0.1) continue; + + // Magnitude calculation (VFH+) + // m = c^2 * (a - b*d) + // Simplified: weight decreases with distance + const double magnitude = (d_max - dist) / d_max; + + double angle = std::atan2(dy, dx); + if (angle < 0) angle += 2.0 * M_PI; + + int bin = static_cast(angle / kAlphaRad) % kNumBins; + h[bin] += magnitude; + } + return h; +} + +std::vector VectorFieldPlannerAlgo::smoothHistogram(const std::vector& h) const +{ + std::vector h_smooth(kNumBins, 0.0); + for (int i = 0; i < kNumBins; ++i) { + double sum = 0.0; + for (int l = -kSmoothL; l <= kSmoothL; ++l) { + int idx = (i + l + kNumBins) % kNumBins; + // Linear weighting 4, 3, 2, 1 for center to edge + sum += h[idx] * (kSmoothL + 1 - std::abs(l)); + } + h_smooth[i] = sum / (kSmoothL + 1); + } + return h_smooth; +} + +std::vector VectorFieldPlannerAlgo::findCandidates( + const std::vector& h, int k_target, int k_prev) const +{ + std::vector candidates; + + // Find valleys (consecutive bins below threshold) + std::vector> valleys; + int start = -1; + for (int i = 0; i < kNumBins; ++i) { + if (h[i] < params_.vfh_threshold) { + if (start == -1) start = i; + } else { + if (start != -1) { + valleys.push_back({start, i - 1}); + start = -1; + } + } + } + if (start != -1) { + // Handle wrap around valley + if (!valleys.empty() && valleys[0].first == 0) { + valleys[0].first = start; + } else { + valleys.push_back({start, kNumBins - 1}); + } + } + + for (auto& v : valleys) { + int size = (v.second - v.first + kNumBins) % kNumBins + 1; + if (size >= kSMax) { + // Wide valley: candidates are edges and goal/prev if inside + candidates.push_back((v.first + kSMax/2) % kNumBins); + candidates.push_back((v.second - kSMax/2 + kNumBins) % kNumBins); + + // Check if target or prev is in valley + auto in_valley = [&](int k) { + if (v.first <= v.second) return k >= v.first && k <= v.second; + return k >= v.first || k <= v.second; + }; + if (in_valley(k_target)) candidates.push_back(k_target); + if (in_valley(k_prev)) candidates.push_back(k_prev); + } else { + // Narrow valley: candidate is the middle + int mid = (v.first + size / 2) % kNumBins; + candidates.push_back(mid); + } + } + + // Deduplicate and filter candidates that are too far from target? + // No, A* will handle cost. Just deduplicate. + std::sort(candidates.begin(), candidates.end()); + candidates.erase(std::unique(candidates.begin(), candidates.end()), candidates.end()); + + return candidates; +} + +static int angular_diff(int k1, int k2) { + int diff = std::abs(k1 - k2); + if (diff > kNumBins / 2) diff = kNumBins - diff; + return diff; +} + +struct VfhNode { + double rx, ry, yaw; + int k_prev; + int root_k; + int depth; + double g; + double f; + + bool operator>(const VfhNode& other) const { return f > other.f; } +}; + +int VectorFieldPlannerAlgo::runAstar(double rx, double ry, double yaw, + int k_target, int k_prev) const +{ + std::priority_queue, std::greater> open; + + auto h0 = buildHistogram(obstacles_, rx, ry); + auto h0s = smoothHistogram(h0); + auto candidates0 = findCandidates(h0s, k_target, k_prev); + + if (candidates0.empty()) return k_target; + if (candidates0.size() == 1) return candidates0[0]; + + int current_k = static_cast(std::fmod(yaw + 2.0*M_PI, 2.0*M_PI) / kAlphaRad); + + for (int ck : candidates0) { + // cost_primary (depth 0) + // g = mu1*delta(ck, kt) + mu2*delta(orient, kt) + mu3*delta(ck, kp) + double g = kMu1 * angular_diff(ck, k_target) + + kMu2 * angular_diff(current_k, k_target) + + kMu3 * angular_diff(ck, k_prev); + + // Simple admissible heuristic (Dijkstra if h=0, but we'll use a small goal-bias) + double h = kMu1p * std::pow(kLambda, 1) * angular_diff(ck, k_target); + + open.push({rx, ry, yaw, ck, ck, 0, g, g + h}); + } + + if (open.empty()) return candidates0[0]; + + while (!open.empty()) { + VfhNode n = open.top(); + open.pop(); + + if (n.depth >= kNg) return n.root_k; + + // Project next position assuming we moved in direction ck + double move_yaw = n.k_prev * kAlphaRad; + double next_rx = n.rx + kDs * std::cos(move_yaw); + double next_ry = n.ry + kDs * std::sin(move_yaw); + double next_yaw = move_yaw; // Simple model: orientation becomes direction of motion + + auto hi = buildHistogram(obstacles_, next_rx, next_ry); + auto his = smoothHistogram(hi); + auto candidatesi = findCandidates(his, k_target, n.k_prev); + + // Branching reduction: pick only the best candidate per side of target (plus target itself) + int best_left = -1, best_right = -1; + double min_g_left = std::numeric_limits::infinity(); + double min_g_right = std::numeric_limits::infinity(); + + for (int ck : candidatesi) { + int next_depth = n.depth + 1; + double cost_ck = std::pow(kLambda, next_depth) * ( + kMu1p * angular_diff(ck, k_target) + + kMu2p * angular_diff(static_cast(next_yaw/kAlphaRad), k_target) + + kMu3p * angular_diff(ck, n.k_prev) + ); + + // Simple branching reduction logic + int diff = ck - k_target; + if (diff > kNumBins/2) diff -= kNumBins; + if (diff < -kNumBins/2) diff += kNumBins; + + if (diff < 0 && cost_ck < min_g_left) { + best_left = ck; min_g_left = cost_ck; + } else if (diff > 0 && cost_ck < min_g_right) { + best_right = ck; min_g_right = cost_ck; + } else if (diff == 0) { + // Target itself is always expanded if found + double h_new = kMu1p * std::pow(kLambda, next_depth + 1) * angular_diff(ck, k_target); + open.push({next_rx, next_ry, next_yaw, ck, n.root_k, next_depth, n.g + cost_ck, n.g + cost_ck + h_new}); + } + } + + if (best_left != -1) { + double h_new = kMu1p * std::pow(kLambda, n.depth + 2) * angular_diff(best_left, k_target); + open.push({next_rx, next_ry, next_yaw, best_left, n.root_k, n.depth + 1, n.g + min_g_left, n.g + min_g_left + h_new}); + } + if (best_right != -1) { + double h_new = kMu1p * std::pow(kLambda, n.depth + 2) * angular_diff(best_right, k_target); + open.push({next_rx, next_ry, next_yaw, best_right, n.root_k, n.depth + 1, n.g + min_g_right, n.g + min_g_right + h_new}); + } + + if (open.size() > 200) break; + } + + return k_target; +} + // --------------------------------------------------------------------------- // Closest index // --------------------------------------------------------------------------- @@ -144,34 +367,44 @@ PlannerResult VectorFieldPlannerAlgo::compute(double rx, double ry, double yaw, while (res.heading_err > M_PI) res.heading_err -= 2.0 * M_PI; while (res.heading_err < -M_PI) res.heading_err += 2.0 * M_PI; - // Obstacle repulsion - if (params_.obstacle_avoidance_enabled && params_.repulsion_gain > 0.0) { + // Obstacle avoidance (VFH*) + // Skip VFH* when lookahead is behind: the kinematic constraint prevents large + // heading corrections and the robot needs a hard turn, not micro-adjustments. + if (params_.obstacle_avoidance_enabled && !res.lookahead_behind) { + double target_heading = std::atan2(lk.y - ry, lk.x - rx); + if (target_heading < 0) target_heading += 2.0 * M_PI; + int k_target = static_cast(target_heading / kAlphaRad) % kNumBins; + + // Initialize prev_k_ to current yaw on first call + static bool first_run = true; + if (first_run) { + prev_k_ = static_cast(std::fmod(yaw + 2.0*M_PI, 2.0*M_PI) / kAlphaRad); + first_run = false; + } + + int k_best = runAstar(rx, ry, yaw, k_target, prev_k_); + + double desired_heading = k_best * kAlphaRad; + res.heading_err = desired_heading - yaw; + while (res.heading_err > M_PI) res.heading_err -= 2.0 * M_PI; + while (res.heading_err < -M_PI) res.heading_err += 2.0 * M_PI; + + res.vfh_k_best = k_best; + prev_k_ = k_best; + + // Track active points and closest_r for diagnostics for (const auto& p : obstacles_) { const double dx = rx - p.x; const double dy = ry - p.y; const double dist = std::hypot(dx, dy); - - if (dist >= params_.repulsion_cutoff_m || dist < 1e-6) continue; - - res.closest_r = std::min(res.closest_r, dist); - - const double weight = (params_.repulsion_cutoff_m - dist) / params_.repulsion_cutoff_m; - // Project unit repulsion vector onto robot's left lateral axis - const double lateral = (dx / dist) * (-std::sin(yaw)) + (dy / dist) * std::cos(yaw); - res.lateral_sum += lateral * weight; - - ++res.active_points; - - if (lateral > 0.0) { - res.lateral_left += lateral * weight; - } else { - res.lateral_right += lateral * weight; + if (dist < params_.repulsion_cutoff_m) { + res.closest_r = std::min(res.closest_r, dist); + res.active_points++; } } - res.repulsion_steering = params_.repulsion_gain * res.lateral_sum; } - res.steering_unclamped = params_.k_p_steering * res.heading_err + res.repulsion_steering; + res.steering_unclamped = params_.k_p_steering * res.heading_err; res.angular_vel = std::clamp(res.steering_unclamped, -params_.max_steering_angle_rad, params_.max_steering_angle_rad); diff --git a/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp index 56a70ff8..43db6f96 100644 --- a/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp +++ b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp @@ -58,6 +58,7 @@ class VectorFieldPlanner : public rclcpp::Node declare_parameter("k_p_steering", 1.5); declare_parameter("repulsion_gain", 0.5); declare_parameter("repulsion_cutoff_m", 3.0); + declare_parameter("vfh_threshold", 0.5); declare_parameter("obstacle_memory_time_s", 3.0); declare_parameter("obstacle_max_points", 2000); declare_parameter("obstacle_avoidance_enabled", false); @@ -79,6 +80,7 @@ class VectorFieldPlanner : public rclcpp::Node p.k_p_steering = get_parameter("k_p_steering").as_double(); p.repulsion_gain = get_parameter("repulsion_gain").as_double(); p.repulsion_cutoff_m = get_parameter("repulsion_cutoff_m").as_double(); + p.vfh_threshold = get_parameter("vfh_threshold").as_double(); p.obstacle_avoidance_enabled = get_parameter("obstacle_avoidance_enabled").as_bool(); p.goal_tolerance_m = get_parameter("goal_tolerance_m").as_double(); p.min_approach_linear_velocity = get_parameter("min_approach_linear_velocity").as_double(); @@ -374,19 +376,18 @@ class VectorFieldPlanner : public rclcpp::Node stuck_ticks_ = 0; } - if (p.obstacle_avoidance_enabled && p.repulsion_gain > 0.0 && latest_scan_) { + if (p.obstacle_avoidance_enabled && latest_scan_) { const double scan_age = (now_time - latest_scan_->header.stamp).seconds(); RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "[AVOIDANCE age=%.3fs] buffered=%zu in_range=%d closest=%.2fm " - "lateral_sum=%.3f left=%.3f right=%.3f rep_steer=%.4f", + "vfh_k_best=%d heading_err=%.1fd", scan_age, obstacle_map_.size(), res.active_points, res.closest_r, - res.lateral_sum, res.lateral_left, res.lateral_right, res.repulsion_steering); + res.vfh_k_best, res.heading_err * 180.0 / M_PI); if (res.active_points > 0) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 250, - "[AVOIDANCE ACTIVE] %d pts within %.1fm left=%.3f right=%.3f rep_steer=%.4f path_steer=%.4f", - res.active_points, p.repulsion_cutoff_m, res.lateral_left, res.lateral_right, - res.repulsion_steering, p.k_p_steering * res.heading_err); + "[AVOIDANCE ACTIVE] %d pts within %.1fm k_best=%d steer=%.4f", + res.active_points, p.repulsion_cutoff_m, res.vfh_k_best, res.angular_vel); } } diff --git a/src/subsystems/navigation/vector_field_planner/test/test_vector_field_planner_algo.cpp b/src/subsystems/navigation/vector_field_planner/test/test_vector_field_planner_algo.cpp index 2646e9ee..51c84fad 100644 --- a/src/subsystems/navigation/vector_field_planner/test/test_vector_field_planner_algo.cpp +++ b/src/subsystems/navigation/vector_field_planner/test/test_vector_field_planner_algo.cpp @@ -262,3 +262,111 @@ TEST(Regression, StraightAheadZeroSteering) EXPECT_NEAR(res.angular_vel, 0.0, 1e-6); EXPECT_NEAR(res.linear_vel, p.max_speed_mps, 1e-6); } + +// --------------------------------------------------------------------------- +// VFH* Obstacle Avoidance +// --------------------------------------------------------------------------- + +TEST(VfhStar, AvoidsWallInFront) +{ + PlannerParams p; + p.obstacle_avoidance_enabled = true; + p.vfh_threshold = 0.5; + p.repulsion_cutoff_m = 3.0; + p.lookahead_dist_m = 5.0; + p.max_speed_mps = 1.0; + p.k_p_steering = 1.0; + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + // Place a dense wall at x=2, from y=-2 to y=2 + std::vector obs; + for (double y = -2.0; y <= 2.0; y += 0.1) { + obs.push_back({2.0, y}); + } + algo.updateObstacles(obs); + + // Robot at origin facing +X. Target is at x=5 (lookahead). + // Wall is in between. VFH* should steer away. + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + + EXPECT_GT(res.active_points, 0); + EXPECT_NE(res.vfh_k_best, 0); // 0 corresponds to +X (roughly) + EXPECT_GT(std::abs(res.angular_vel), 0.1); +} + +TEST(VfhStar, TargetHeadingWhenClear) +{ + PlannerParams p; + p.obstacle_avoidance_enabled = true; + p.vfh_threshold = 0.5; + p.repulsion_cutoff_m = 3.0; + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + // No obstacles + algo.updateObstacles({}); + + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_NEAR(res.heading_err, 0.0, 0.1); + EXPECT_EQ(res.active_points, 0); +} + +TEST(VfhStar, ThresholdFilter) +{ + PlannerParams p; + p.obstacle_avoidance_enabled = true; + p.vfh_threshold = 10.0; // Extremely high threshold - everything should be clear + p.repulsion_cutoff_m = 3.0; + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + // Place a single obstacle at x=1 + algo.updateObstacles({{1.0, 0.0}}); + + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + // High threshold means it should ignore the obstacle + EXPECT_NEAR(res.heading_err, 0.0, 0.1); +} + +TEST(VfhStar, TrapScenario) +{ + PlannerParams p; + p.obstacle_avoidance_enabled = true; + p.vfh_threshold = 0.5; + p.repulsion_cutoff_m = 4.0; + p.lookahead_dist_m = 3.0; + p.max_speed_mps = 1.0; + p.k_p_steering = 1.0; + p.max_steering_angle_rad = 1.0; // Allow sharp turns + + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + // Create a U-shaped trap blocking +X + // Front wall at x=2 + std::vector obs; + for (double y = -1.5; y <= 1.5; y += 0.1) { + obs.push_back({2.0, y}); + } + // Side walls + for (double x = 2.0; x <= 5.0; x += 0.2) { + obs.push_back({x, 1.5}); + obs.push_back({x, -1.5}); + } + algo.updateObstacles(obs); + + // Robot at origin facing +X. + // A local planner (VFH+) might steer slightly but stay in the U. + // VFH* should see that all paths through the front wall are blocked + // and steer much more aggressively to the side to go around the trap. + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + + EXPECT_GT(res.active_points, 0); + // Heading error should be significant (steering away from the U) + // 0 rad is straight into the trap. + EXPECT_GT(std::abs(res.heading_err), 0.5); +}