diff --git a/.gitignore b/.gitignore index 2bf691f1..58606b66 100644 --- a/.gitignore +++ b/.gitignore @@ -208,4 +208,7 @@ cython_debug/ *.app .DS_Store -*.mcap \ No newline at end of file +*.mcap + +# Local-only navigation sim test harness (not tracked) +src/subsystems/navigation/nav_sim_harness/ \ No newline at end of file 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/bringup/package.xml b/src/bringup/package.xml index 90138541..9a8faca3 100644 --- a/src/bringup/package.xml +++ b/src/bringup/package.xml @@ -19,6 +19,9 @@ science_bringup description athena_planner + led_indicator + aruco_bt + yolo_ros_bt robot_state_publisher xacro diff --git a/src/description/models/aruco_post/materials/textures/aruco_4x4_50_id4.png b/src/description/models/aruco_post/materials/textures/aruco_4x4_50_id4.png new file mode 100644 index 00000000..bed452c1 Binary files /dev/null and b/src/description/models/aruco_post/materials/textures/aruco_4x4_50_id4.png differ diff --git a/src/description/models/aruco_post/model.config b/src/description/models/aruco_post/model.config new file mode 100644 index 00000000..edf039a5 --- /dev/null +++ b/src/description/models/aruco_post/model.config @@ -0,0 +1,21 @@ + + + + aruco_post + 1.0 + model.sdf + + + UMD Loop Navigation + + + + URC-style ArUco post: a 3-sided (triangular-prism) marker carrying three + identical DICT_4X4_50 tag faces, one per side, mounted on a support post. + Faces are 20x20 cm (15 cm marker + 2.5 cm white quiet zone). The tag texture + is pre-generated once with nav_sim_harness/scripts/gen_aruco_texture.py and + baked into materials/textures/ — nothing generates it at spawn time. + Default tag id baked in = 4. Used by the nav_sim_harness Gazebo test harness. + + + diff --git a/src/description/models/aruco_post/model.sdf b/src/description/models/aruco_post/model.sdf new file mode 100644 index 00000000..e28445e7 --- /dev/null +++ b/src/description/models/aruco_post/model.sdf @@ -0,0 +1,127 @@ + + + + + true + + + + + 0 0 0.45 0 0 0 + + + 0.03 + 0.9 + + + + 0.5 0.5 0.5 1 + 0.6 0.6 0.6 1 + 0.1 0.1 0.1 1 + + + + 0 0 0.45 0 0 0 + + + 0.03 + 0.9 + + + + + + + 0.057735 0 1.0 0 0 0 + + + 0.005 0.20 0.20 + + + + 1 1 1 1 + 1 1 1 1 + + + materials/textures/aruco_4x4_50_id4.png + 0.0 + 1.0 + + + + + + + + -0.0288675 0.05 1.0 0 0 2.0943951 + + + 0.005 0.20 0.20 + + + + 1 1 1 1 + 1 1 1 1 + + + materials/textures/aruco_4x4_50_id4.png + 0.0 + 1.0 + + + + + + + + -0.0288675 -0.05 1.0 0 0 4.1887902 + + + 0.005 0.20 0.20 + + + + 1 1 1 1 + 1 1 1 1 + + + materials/textures/aruco_4x4_50_id4.png + 0.0 + 1.0 + + + + + + + + 0 0 1.0 0 0 0 + + + 0.063 + 0.20 + + + + + + + diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 294ff484..afc94329 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -15,6 +15,7 @@ + @@ -55,6 +56,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/description/urdf/zed_camera.xacro b/src/description/urdf/zed_camera.xacro index 3b715c14..b8d44419 100644 --- a/src/description/urdf/zed_camera.xacro +++ b/src/description/urdf/zed_camera.xacro @@ -33,8 +33,8 @@ 1.8151 - 672 - 376 + 320 + 180 R_FLOAT32 @@ -52,15 +52,15 @@ 1 - 15 + 30 true ${camera_name}/zed_node/left/image_rect_color ${camera_name}_left_camera_frame_optical 1.8151 - 672 - 376 + 1280 + 720 RGB_INT8 diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 443530f5..119e81a7 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -19,17 +19,24 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Currentdraw.msg" "msg/JointStatus.msg" "msg/SystemInfo.msg" + "msg/PlannerEvent.msg" + "msg/ActiveTarget.msg" + "msg/NavStatus.msg" "msg/Heading.msg" "msg/WaypointEntry.msg" "msg/WaypointManagerState.msg" + "msg/LocalPlannerStuck.msg" "srv/SetController.srv" "srv/AddWaypoint.srv" "srv/ClearWaypoints.srv" "msg/LedStatus.msg" "srv/StatusReq.srv" "srv/MaintenanceReq.srv" + "srv/LatLonToENU.srv" + "srv/SetTarget.srv" "action/NavigateToGPS.action" "action/NavigateToWaypoint.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..6dfb68de --- /dev/null +++ b/src/msgs/action/NavigateToTarget.action @@ -0,0 +1,24 @@ +uint8 GPS=0 +uint8 METER=1 +uint8 GNSS_ONLY=0 +uint8 ARUCO_POST_1=1 +uint8 ARUCO_POST_2=2 +uint8 OBJECT=3 +uint8 LOCAL=4 + +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/LocalPlannerStuck.msg b/src/msgs/msg/LocalPlannerStuck.msg new file mode 100644 index 00000000..112914dc --- /dev/null +++ b/src/msgs/msg/LocalPlannerStuck.msg @@ -0,0 +1,25 @@ +# Published by the local planner (vector_field_planner) when its tentacle FSM +# enters the REPLAN state — i.e. forward arcs are blocked and reverse arcs +# cannot open a new vantage on their own. +# +# Future consumers (mission_executive, waypoint manager, web GUI) use this to +# insert a detour waypoint, mark the original waypoint as failed after N +# attempts, or surface "rover is stuck" to the operator. +# +# Lifecycle: +# - On REPLAN entry: publish once with stuck=true. +# - While stuck: heartbeat at ~1 Hz with stuck=true (so a late +# subscriber catches up after a restart). +# - On REPLAN exit: publish once with stuck=false. +# +# The "suggested_detour" is the local planner's best guess at a pose that +# would escape the current obstacle pocket — derived from the highest- +# clearance tentacle (forward OR reverse, whichever was clearer). Consumers +# may use it directly or override with their own policy. + +std_msgs/Header header # stamp at publish time; frame_id = map frame +bool stuck # true while in REPLAN, false on exit +geometry_msgs/PoseStamped stuck_pose # rover pose when REPLAN entered +geometry_msgs/PoseStamped suggested_detour # local planner's escape suggestion +float64 best_forward_clearance_m # clearance along best forward tentacle (m); 0 if none clear +float64 best_reverse_clearance_m # clearance along best reverse tentacle (m); 0 if none clear 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..1dfb58d8 --- /dev/null +++ b/src/msgs/srv/SetTarget.srv @@ -0,0 +1,18 @@ +uint8 GPS=0 +uint8 METER=1 +uint8 GNSS_ONLY=0 +uint8 ARUCO_POST_1=1 +uint8 ARUCO_POST_2=2 +uint8 OBJECT=3 +uint8 LOCAL=4 +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..8be25f96 100644 --- a/src/simulation/launch/bringup.launch.py +++ b/src/simulation/launch/bringup.launch.py @@ -41,11 +41,23 @@ 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' + ), + DeclareLaunchArgument( + 'headless', + default_value='false', + choices=['true', 'false'], + description='Run Gazebo server-only (no GUI). Sensors still render.' + ), ] def generate_launch_description(): @@ -66,6 +78,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource([gazebo_launch]), launch_arguments=[ ('world', LaunchConfiguration('world')), # World file taken from description/worlds/ + ('headless', LaunchConfiguration('headless')), ] ) @@ -92,11 +105,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/launch/gz_sim.launch.py b/src/simulation/launch/gz_sim.launch.py index 575a22a3..653be0a4 100644 --- a/src/simulation/launch/gz_sim.launch.py +++ b/src/simulation/launch/gz_sim.launch.py @@ -4,11 +4,14 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression ARGUMENTS = [ DeclareLaunchArgument('world', default_value='empty.sdf', description='Gazebo world file'), + DeclareLaunchArgument('headless', default_value='false', + choices=['true', 'false'], + description='Run Gazebo server-only (no GUI). Sensors still render.'), ] def generate_launch_description(): @@ -29,12 +32,18 @@ def generate_launch_description(): world_path = PathJoinSubstitution( [pkg_description, 'worlds', LaunchConfiguration('world')]) + # '-s' makes Gazebo run server-only (no GUI). Sensors still render so the + # ZED/navsat plugins keep publishing — exactly what the headless harness needs. + headless_flag = PythonExpression( + ["' -s' if '", LaunchConfiguration('headless'), "' == 'true' else ''"]) + gazebo_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource([gz_sim_launch]), launch_arguments=[ ('gz_args', [world_path, ' -r', - ' -v 4'] # Verbosity level, 0=errors only, 4=debug + ' -v 4', # Verbosity level, 0=errors only, 4=debug + headless_flag] ) ] ) diff --git a/src/simulation/scripts/heading_publisher.py b/src/simulation/scripts/heading_publisher.py new file mode 100755 index 00000000..7dfa337a --- /dev/null +++ b/src/simulation/scripts/heading_publisher.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 + +import math + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +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, + qos_profile_sensor_data, + ) + + 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 + + # 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 + compass_bearing = (90.0 - heading_enu_deg + 360.0) % 360.0 + + out = Heading() + out.header = msg.header + 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) + + +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/drive/drive_controllers/src/rear_ackermann_controller.cpp b/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp index 2af5cfda..49325d8b 100644 --- a/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp +++ b/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp @@ -215,13 +215,14 @@ controller_interface::return_type RearAckermannController::update( const double rad_s_to_rpm = 60.0 / (2.0 * M_PI); if (DEBUG_MODE == 1) { 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; diff --git a/src/subsystems/navigation/aruco_bt/aruco_bt/aruco_node.py b/src/subsystems/navigation/aruco_bt/aruco_bt/aruco_node.py index 9bc75c94..40436f20 100644 --- a/src/subsystems/navigation/aruco_bt/aruco_bt/aruco_node.py +++ b/src/subsystems/navigation/aruco_bt/aruco_bt/aruco_node.py @@ -39,7 +39,7 @@ def __init__(self): aruco_params = cv2.aruco.DetectorParameters() # Tightening up parameters to reduce false positives - aruco_params.minMarkerPerimeterRate = 0.04 # rejects markers that are too small, default 0.03 + aruco_params.minMarkerPerimeterRate = 0.01 # accept smaller/farther markers (lower = detect at range) aruco_params.maxMarkerPerimeterRate = 2.0 # rejects markers that are too large, default 4.0 aruco_params.polygonalApproxAccuracyRate = 0.02 # controls how loosely the detected contour is approximated to a quadrilateral, default 0.03 aruco_params.errorCorrectionRate = 0.4 # controls how much error correction is applied, default 0.6 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_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/navigation/global_planner/CMakeLists.txt b/src/subsystems/navigation/global_planner/CMakeLists.txt new file mode 100644 index 00000000..63ec1fe8 --- /dev/null +++ b/src/subsystems/navigation/global_planner/CMakeLists.txt @@ -0,0 +1,46 @@ +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_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} + tf2_ros::tf2_ros + tf2::tf2 +) + +install(TARGETS global_planner_algo global_planner_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + + + +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..a810c1f3 --- /dev/null +++ b/src/subsystems/navigation/global_planner/include/global_planner/global_planner_algo.hpp @@ -0,0 +1,103 @@ +// 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; + + /* + * 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: + PlannerParams params_; + std::optional costmap_; +}; + +} // namespace global_planner diff --git a/src/subsystems/navigation/global_planner/package.xml b/src/subsystems/navigation/global_planner/package.xml new file mode 100644 index 00000000..5cbb7242 --- /dev/null +++ b/src/subsystems/navigation/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/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/navigation/gps_pose_publisher/CMakeLists.txt b/src/subsystems/navigation/gps_pose_publisher/CMakeLists.txt new file mode 100644 index 00000000..209d45e6 --- /dev/null +++ b/src/subsystems/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/navigation/gps_pose_publisher/package.xml b/src/subsystems/navigation/gps_pose_publisher/package.xml new file mode 100644 index 00000000..9442c5f1 --- /dev/null +++ b/src/subsystems/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/navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp b/src/subsystems/navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp new file mode 100644 index 00000000..d89c7918 --- /dev/null +++ b/src/subsystems/navigation/gps_pose_publisher/src/gps_pose_publisher_node.cpp @@ -0,0 +1,204 @@ +// 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); + // 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); + + 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); + 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_ = (msg->heading + heading_offset_deg_) * 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(using 0)"); + status_pub_->publish(s); + + // 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; + 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"; + // Broadcast to base_footprint (the URDF root), NOT base_link. base_link has + // a static parent (base_footprint->base_link) from the URDF; publishing + // map->base_link here would give base_link two parents, detaching + // base_footprint and every sensor hanging off it (IMU/GNSS/ZED) into a + // separate TF tree. map->base_footprint keeps the whole tree connected. + tf.child_frame_id = "base_footprint"; + 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}; + double heading_offset_deg_{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/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 diff --git a/src/subsystems/navigation/mission_executive/CMakeLists.txt b/src/subsystems/navigation/mission_executive/CMakeLists.txt new file mode 100644 index 00000000..7efc5cad --- /dev/null +++ b/src/subsystems/navigation/mission_executive/CMakeLists.txt @@ -0,0 +1,50 @@ +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(tf2_geometry_msgs REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(msgs REQUIRED) +find_package(nlohmann_json 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 + tf2_geometry_msgs::tf2_geometry_msgs + ${vision_msgs_TARGETS} + ${msgs_TARGETS} + nlohmann_json::nlohmann_json +) + +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 new file mode 100644 index 00000000..e6a0bdb0 --- /dev/null +++ b/src/subsystems/navigation/mission_executive/package.xml @@ -0,0 +1,32 @@ + + + + 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 + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_ros + tf2_geometry_msgs + vision_msgs + msgs + nlohmann-json-dev + + 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..e1ce9f62 --- /dev/null +++ b/src/subsystems/navigation/mission_executive/scripts/mission_cli.py @@ -0,0 +1,600 @@ +#!/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) + nav meter [tol] Navigate to ENU metre coords (GNSS_ONLY) + nav post1 [tol] ArUco Post 1 (GPS 5-10m from post, tol default 13m) + nav post2 [tol] ArUco Post 2 (GPS 10-20m from post, tol default 23m) + nav object [tol] YOLO object detection + spiral + + 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_1 2=ARUCO_POST_2 3=OBJECT 4=LOCAL +tol: arrival radius in metres (GPS vicinity — aruco posts use camera for final 2m) +""" + +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_1", 2: "ARUCO_POST_2", 3: "OBJECT", 4: "LOCAL"} + +# Default GPS vicinity tolerances for each post type. +# The rover drives to within tol of the GPS fix, then uses the camera for the final approach. +POST1_DEFAULT_TOL = 13.0 # GPS 5-10m from post + 3m margin +POST2_DEFAULT_TOL = 23.0 # GPS 10-20m from post + 3m margin + +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: ArUco post — rover drives TO the given GPS point, then spirals{RST}") + print(f" {CYN} outward from it (SPIRAL_COVERAGE) to search for the tag.{RST}") + print(f" {CYN} If the tag is confirmed for aruco_confirm_frames consecutive{RST}") + print(f" {CYN} frames at any time, it switches to ARUCO_APPROACH and stops{RST}") + print(f" {CYN} when camera depth < aruco_stop_dist_m.{RST}") + print() + elif target_type == 3: + print(f" {CYN}Note: OBJECT — after GPS arrival the node enters SPIRAL_COVERAGE{RST}") + print(f" {CYN} and completes when YOLO detects the target or spiral 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(f" nav post1 [tol] — ArUco Post 1 (default tol={POST1_DEFAULT_TOL}m)") + error(f" nav post2 [tol] — ArUco Post 2 (default tol={POST2_DEFAULT_TOL}m)") + error(" nav object [tol] — YOLO object detection + spiral") + sys.exit(1) + + def cmd_nav(self, args: list): + if not args: + self._nav_usage() + coord = args[0].lower() + + if coord == "gps": + if len(args) < 3: self._nav_usage() + tol = float(args[3]) if len(args) > 3 else 3.0 + goal = self._build_nav_goal(0, "", args[1], args[2], 0, tol) + elif coord in ("meter", "m"): + if len(args) < 3: self._nav_usage() + tol = float(args[3]) if len(args) > 3 else 3.0 + goal = self._build_nav_goal(1, "", args[1], args[2], 0, tol) + elif coord == "post1": + if len(args) < 3: self._nav_usage() + tol = float(args[3]) if len(args) > 3 else POST1_DEFAULT_TOL + goal = self._build_nav_goal(0, "", args[1], args[2], 1, tol) + elif coord == "post2": + if len(args) < 3: self._nav_usage() + tol = float(args[3]) if len(args) > 3 else POST2_DEFAULT_TOL + goal = self._build_nav_goal(0, "", args[1], args[2], 2, tol) + elif coord in ("object", "obj"): + if len(args) < 3: self._nav_usage() + tol = float(args[3]) if len(args) > 3 else 3.0 + goal = self._build_nav_goal(0, "", args[1], args[2], 3, 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(f" 2) ArUco Post 1 (GPS 5-10m from post, default tol={POST1_DEFAULT_TOL}m)") + print(f" 3) ArUco Post 2 (GPS 10-20m from post, default tol={POST2_DEFAULT_TOL}m)") + print(" 4) GPS point (YOLO object — spiral after arrival)") + print(" 5) ENU metres (GNSS_ONLY)") + print(" 6) By registered target ID") + ct = input("Choice: ").strip() + + if ct == "6": + 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 + default_tol = 3.0 + if ct == "2": + target_type = 1 + default_tol = POST1_DEFAULT_TOL + elif ct == "3": + target_type = 2 + default_tol = POST2_DEFAULT_TOL + elif ct == "4": + target_type = 3 + elif ct == "5": + 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(f"GPS vicinity tolerance in metres [{default_tol}]: ").strip() + goal = self._build_nav_goal(goal_type, "", a, b, target_type, + float(tol_s) if tol_s else default_tol, 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_1 2=ARUCO_POST_2 3=OBJECT 4=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/mission_executive/scripts/test_services.sh b/src/subsystems/navigation/mission_executive/scripts/test_services.sh new file mode 100755 index 00000000..959e9be2 --- /dev/null +++ b/src/subsystems/navigation/mission_executive/scripts/test_services.sh @@ -0,0 +1,201 @@ +#!/usr/bin/env bash +# Smoke test for the mission_executive queue/services rework. +# Run inside the devcontainer with the workspace sourced. +set -u + +PASS=0 +FAIL=0 +ok() { echo " [PASS] $*"; PASS=$((PASS+1)); } +bad() { echo " [FAIL] $*"; FAIL=$((FAIL+1)); } +step() { echo; echo "=== $* ==="; } + +CACHE="${HOME}/.athena/test_waypoints.json" +rm -f "$CACHE" + +# Wait until the cache file has settled to a state that satisfies the given +# python predicate (receives the parsed JSON as `q`). Polling avoids races +# between service-call return and the persist+publish that follows. +wait_cache() { + local pred=$1 deadline=$(( SECONDS + 5 )) + while (( SECONDS < deadline )); do + if [[ -f "$CACHE" ]] && python3 -c " +import json, sys +try: + q = json.load(open('$CACHE')) +except Exception: + sys.exit(1) +sys.exit(0 if ($pred) else 1) +" 2>/dev/null; then + return 0 + fi + sleep 0.1 + done + return 1 +} + +dump_cache() { python3 -c "import json; print(json.dumps(json.load(open('$CACHE')), indent=2))"; } + +cleanup() { + [[ -n "${ECHO_PID:-}" ]] && kill "$ECHO_PID" 2>/dev/null || true + # `ros2 run` exec's the node so the pgid kill is more reliable than the bg pid + pkill -9 -x mission_executive_node 2>/dev/null || true + pkill -9 -x static_transform_publisher 2>/dev/null || true + wait 2>/dev/null || true +} +trap cleanup EXIT + +# Pre-clean: don't race with leftover nodes from a previous failed run +pkill -9 -x mission_executive_node 2>/dev/null || true +pkill -9 -x static_transform_publisher 2>/dev/null || true +sleep 0.3 + +start_node() { + ros2 run mission_executive mission_executive_node --ros-args \ + -p waypoint_cache_path:="$CACHE" \ + -p replan_throttle_s:=10.0 \ + >/tmp/me.log 2>&1 & + NODE_PID=$! + for _ in $(seq 1 50); do + ros2 service list 2>/dev/null | grep -q '/mission_executive/advance' && return 0 + sleep 0.2 + done + return 1 +} + +step "Start static map->base_link TF + mission_executive" +ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map base_link \ + >/tmp/tf.log 2>&1 & +TF_PID=$! + +start_node \ + && ok "node up, services registered" \ + || { bad "node never came up"; cat /tmp/me.log; exit 1; } + +step "Add three waypoints via ~/set_target (insertion order: far, near, mid)" +add_target() { + local id=$1 x=$2 y=$3 + ros2 service call /mission_executive/set_target msgs/srv/SetTarget \ + "{target_id: '$id', goal_type: 1, target_type: 0, x_m: $x, y_m: $y, tolerance_m: 2.0}" \ + >/dev/null +} +add_target wp_far 50.0 0.0 +add_target wp_near 5.0 0.0 +add_target wp_mid 20.0 0.0 + +if wait_cache "len(q)==3 and all(e['visited']==False and e['skipped']==False for e in q)"; then + python3 -c " +import json +q = json.load(open('$CACHE')) +ids = [e['id'] for e in q] +assert ids == ['wp_near','wp_mid','wp_far'], f'expected near,mid,far got {ids}' +print(' cache order:', ids) +" && ok "queue auto-sorted by distance from robot (0,0)" || bad "queue order wrong: $(dump_cache)" +else + bad "cache never reached 3-pending state"; dump_cache +fi + +step "~/advance: pops wp_near, marks visited=false, becomes active" +ros2 service call /mission_executive/advance std_srvs/srv/Trigger >/dev/null +# Active target isn't reflected in the cache's visited/skipped fields, so check +# the latched /waypoint_queue topic by parsing the status array. Use python to +# decode the YAML-quoted JSON robustly. +read_queue_topic() { + timeout 3 ros2 topic echo --once /waypoint_queue std_msgs/msg/String 2>/dev/null \ + | python3 -c " +import sys, yaml +for doc in yaml.safe_load_all(sys.stdin.read()): + if isinstance(doc, dict) and 'data' in doc: + print(doc['data']); break +" +} + +check_active() { + local want=$1 + for _ in $(seq 1 20); do + if read_queue_topic | WANT="$want" python3 -c " +import json, os, sys +data = sys.stdin.read().strip() +if not data: sys.exit(2) +q = json.loads(data) +active = [e for e in q if e['status']=='ACTIVE'] +sys.exit(0 if (len(active)==1 and active[0]['id']==os.environ['WANT']) else 1) +"; then return 0; fi + sleep 0.2 + done + return 1 +} + +check_active wp_near && ok "wp_near is ACTIVE in /waypoint_queue" || bad "wp_near not ACTIVE: $(read_queue_topic)" + +step "~/skip: wp_near becomes SKIPPED, no ACTIVE in queue" +ros2 service call /mission_executive/skip std_srvs/srv/Trigger >/dev/null +if wait_cache "next(e for e in q if e['id']=='wp_near')['skipped']==True"; then + ok "wp_near skipped=true in cache" +else + bad "wp_near not marked skipped: $(dump_cache)" +fi + +step "~/advance again: should pop wp_mid (next pending by distance)" +echo " cache BEFORE 2nd advance:"; python3 -c "import json; print(' ', [(e['id'],e['visited'],e['skipped']) for e in json.load(open('$CACHE'))])" +ros2 service call /mission_executive/advance std_srvs/srv/Trigger >/dev/null +sleep 0.5 +echo " cache AFTER 2nd advance:"; python3 -c "import json; print(' ', [(e['id'],e['visited'],e['skipped']) for e in json.load(open('$CACHE'))])" +check_active wp_mid && ok "wp_mid is ACTIVE" || bad "wp_mid not ACTIVE: $(read_queue_topic)" + +step "Replan on /local_planner/stuck rising edge (throttle=10s)" +# Echo goal_pose stamps; count one line per emitted message via the '---' separator. +( ros2 topic echo /goal_pose --field header.stamp.sec >/tmp/goals.log 2>&1 ) & +ECHO_PID=$! +sleep 1.0 +count_goals() { grep -c '^---' /tmp/goals.log || true; } +BEFORE=$(count_goals) + +publish_stuck() { + local v=$1 + ros2 topic pub --once /local_planner/stuck msgs/msg/LocalPlannerStuck \ + "{header: {frame_id: 'map'}, stuck: $v, best_forward_clearance_m: 0.0, best_reverse_clearance_m: 0.0}" \ + >/dev/null 2>&1 +} + +publish_stuck false; sleep 0.3 +publish_stuck true; sleep 0.7 # rising edge -> 1 republish +RISE1=$(count_goals) +publish_stuck true; sleep 0.7 # still stuck, not an edge +NOEDGE=$(count_goals) +publish_stuck false; sleep 0.3 +publish_stuck true; sleep 0.7 # rising edge but within 10s throttle +THROTTLED=$(count_goals) + +kill $ECHO_PID 2>/dev/null || true +ECHO_PID= + +echo " goal counts: before=$BEFORE rise1=$RISE1 noedge=$NOEDGE throttled=$THROTTLED" +echo " --- /tmp/goals.log (last 40) ---"; tail -n 40 /tmp/goals.log; echo " ---" +(( RISE1 > BEFORE )) && ok "rising edge republished goal" || bad "rising edge did not republish" +(( NOEDGE == RISE1 )) && ok "non-edge stuck=true did not republish" || bad "non-edge republished" +(( THROTTLED == RISE1 )) && ok "throttled rising edge suppressed (10s window)" || bad "throttle not honored" + +step "Persistence across restart" +pkill -9 -x mission_executive_node 2>/dev/null || true +wait 2>/dev/null || true +NODE_PID= +sleep 0.5 +start_node || { bad "node restart failed"; cat /tmp/me.log; exit 1; } + +if wait_cache "len(q)==3"; then + python3 -c " +import json +q = json.load(open('$CACHE')) +by = {e['id']: (e['visited'], e['skipped']) for e in q} +assert by['wp_near']==(False, True), by +assert by['wp_far'] ==(False, False), by +print(' restored:', by) +" && ok "queue restored from cache (wp_near skipped, wp_far pending)" \ + || bad "restart state wrong: $(dump_cache)" +else + bad "cache disappeared on restart" +fi + +step "Result" +echo " PASS=$PASS FAIL=$FAIL" +[[ $FAIL -eq 0 ]] 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..7dce977d --- /dev/null +++ b/src/subsystems/navigation/mission_executive/src/mission_executive_node.cpp @@ -0,0 +1,1633 @@ +// 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 +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "tf2/exceptions.h" +#include "tf2/time.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.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 "vision_msgs/msg/detection2_d.hpp" + +#include + +#include "msgs/action/navigate_to_target.hpp" +#include "msgs/msg/active_target.hpp" +#include "msgs/msg/led_status.hpp" +#include "msgs/msg/local_planner_stuck.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; + +#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, + ARUCO_CONFIRM, + ARUCO_APPROACH, + 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::ARUCO_CONFIRM: return "ARUCO_CONFIRM"; + case State::ARUCO_APPROACH: return "ARUCO_APPROACH"; + 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}; + bool skipped{false}; +}; + +// ── Waypoint persistence (single JSON file, atomic save) ──────────────────── + +static const char* targetTypeStr(uint8_t t) { + switch (t) { + case 0: return "GNSS"; + case 1: return "ARUCO_1"; + case 2: return "ARUCO_2"; + case 3: return "OBJ_DETECT"; + default: return "UNKNOWN"; + } +} + +static nlohmann::json entryToJson(const TargetEntry& e) { + return nlohmann::json{ + {"id", e.id}, + {"x", e.x_m}, + {"y", e.y_m}, + {"type", targetTypeStr(e.target_type)}, + {"type_code", e.target_type}, + {"goal_source", e.goal_source}, + {"tolerance", e.tolerance_m}, + {"visited", e.visited}, + {"skipped", e.skipped}, + }; +} + +static std::optional jsonToEntry(const nlohmann::json& j) { + try { + TargetEntry e; + e.id = j.at("id").get(); + e.x_m = j.at("x").get(); + e.y_m = j.at("y").get(); + e.target_type = j.at("type_code").get(); + e.goal_source = j.at("goal_source").get(); + e.tolerance_m = j.at("tolerance").get(); + e.visited = j.value("visited", false); + e.skipped = j.value("skipped", false); + return e; + } catch (const std::exception&) { + return std::nullopt; + } +} + +static std::vector loadQueueFromFile(const std::string& path) { + std::vector out; + std::ifstream in(path); + if (!in.is_open()) return out; + try { + nlohmann::json j; + in >> j; + if (!j.is_array()) return out; + for (const auto& je : j) { + if (auto e = jsonToEntry(je)) out.push_back(*e); + } + } catch (const std::exception&) {} + return out; +} + +static bool saveQueueToFile(const std::string& path, + const std::vector& q) { + try { + std::filesystem::path p(path); + if (p.has_parent_path()) { + std::filesystem::create_directories(p.parent_path()); + } + const std::string tmp = path + ".tmp"; + { + std::ofstream out(tmp, std::ios::trunc); + if (!out.is_open()) return false; + nlohmann::json arr = nlohmann::json::array(); + for (const auto& e : q) arr.push_back(entryToJson(e)); + out << arr.dump(2); + } + std::filesystem::rename(tmp, path); + return true; + } catch (const std::exception&) { + return false; + } +} + +static std::string expandTilde(const std::string& p) { + if (p.empty() || p[0] != '~') return p; + const char* home = std::getenv("HOME"); + if (!home) return p; + return std::string(home) + p.substr(1); +} + +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}; + int aruco_confirm_frames{5}; + double aruco_max_age_s{0.5}; + double aruco_lost_timeout_s{1.0}; + // Visual-servo approach (image-based; no pose/solvePnP/TF). + double aruco_servo_kp{0.8}; // angular gain on normalized bbox-center error + double aruco_servo_linear_mps{0.4}; // forward speed while homing + double aruco_servo_max_angular{0.6}; // clamp on commanded yaw rate (rad/s) + double aruco_bbox_stop_frac{0.45}; // stop when bbox height / image height exceeds this +}; + +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: + using NavAction = msgs::action::NavigateToTarget; + using GoalHandle = rclcpp_action::ServerGoalHandle; + + explicit MissionExecutive(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("mission_executive", options) + { + 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")); + 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); + declare_parameter("aruco_confirm_frames", 5); + declare_parameter("aruco_max_age_s", 0.5); + declare_parameter("aruco_lost_timeout_s", 1.0); + declare_parameter("aruco_servo_kp", 0.8); + declare_parameter("aruco_servo_linear_mps", 0.4); + declare_parameter("aruco_servo_max_angular", 0.6); + declare_parameter("aruco_bbox_stop_frac", 0.45); + declare_parameter("aruco_detection_topic", std::string("/aruco_detection")); + declare_parameter("camera_info_topic", std::string("/zed/zed_node/left/camera_info")); + declare_parameter("cmd_vel_topic", std::string("/front_ackermann_controller/reference")); + declare_parameter("yolo_detection_topic", std::string("/yolo_detection")); + 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")); + declare_parameter("goal_pose_topic", std::string("/goal_pose")); + declare_parameter("nav_enabled_topic", std::string("/nav_enabled")); + declare_parameter("obstacle_avoidance_topic", std::string("/obstacle_avoidance_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")); + declare_parameter("led_status_topic", std::string("/led_status")); + declare_parameter("local_planner_stuck_topic", std::string("/local_planner/stuck")); + declare_parameter("waypoint_queue_topic", std::string("/waypoint_queue")); + declare_parameter("waypoint_cache_path", std::string("~/.athena/waypoints.json")); + declare_parameter("replan_throttle_s", 3.0); + + 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(); + params_.aruco_confirm_frames = get_parameter("aruco_confirm_frames").as_int(); + params_.aruco_max_age_s = get_parameter("aruco_max_age_s").as_double(); + params_.aruco_lost_timeout_s = get_parameter("aruco_lost_timeout_s").as_double(); + params_.aruco_servo_kp = get_parameter("aruco_servo_kp").as_double(); + params_.aruco_servo_linear_mps = get_parameter("aruco_servo_linear_mps").as_double(); + params_.aruco_servo_max_angular = get_parameter("aruco_servo_max_angular").as_double(); + params_.aruco_bbox_stop_frac = get_parameter("aruco_bbox_stop_frac").as_double(); + + const auto latlon_svc = get_parameter("latlon_to_enu_service").as_string(); + aruco_detection_topic_ = get_parameter("aruco_detection_topic").as_string(); + const auto& aruco_detection_topic = aruco_detection_topic_; + const auto camera_info_topic = get_parameter("camera_info_topic").as_string(); + const auto cmd_vel_topic = get_parameter("cmd_vel_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 avoidance_topic = get_parameter("obstacle_avoidance_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 led_status_topic = get_parameter("led_status_topic").as_string(); + const auto stuck_topic = get_parameter("local_planner_stuck_topic").as_string(); + const auto waypoint_queue_topic = get_parameter("waypoint_queue_topic").as_string(); + waypoint_cache_path_ = expandTilde(get_parameter("waypoint_cache_path").as_string()); + replan_throttle_s_ = get_parameter("replan_throttle_s").as_double(); + + queue_ = loadQueueFromFile(waypoint_cache_path_); + RCLCPP_INFO(get_logger(), "Loaded %zu waypoints from %s", + queue_.size(), waypoint_cache_path_.c_str()); + + 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); + // Direct velocity command for the ArUco visual-servo approach. Published to + // the same controller topic the local planner uses; the planner is held + // silent (nav_enabled=false) during the servo so they never both drive. + cmd_vel_pub_ = create_publisher(cmd_vel_topic, 10); + nav_enabled_pub_ = create_publisher( + nav_enabled_topic, rclcpp::QoS(1).reliable()); + nav_mode_pub_ = create_publisher(nav_mode_topic, 10); + // Latched so the planner (a possibly-late/volatile subscriber) always gets + // the current intent on connect; transient_local pub is compatible with the + // planner's volatile sub for live updates. + obstacle_avoidance_pub_ = create_publisher( + avoidance_topic, rclcpp::QoS(1).reliable().transient_local()); + active_target_pub_ = create_publisher(active_target_topic, 10); + nav_status_pub_ = create_publisher( + nav_status_topic, rclcpp::QoS(1).reliable()); + led_status_pub_ = create_publisher( + led_status_topic, rclcpp::QoS(1).reliable()); + waypoint_queue_pub_ = create_publisher( + waypoint_queue_topic, rclcpp::QoS(1).reliable().transient_local()); + + 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) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [this](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [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 = abort(); + res->success = cmd.success; + res->message = cmd.message; + if (cmd.success) { publishNavEnabled(); publishNavMode(); publishLedStatus(); 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_); + + advance_srv_ = create_service( + "~/advance", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr res) + { + std::lock_guard lk(mutex_); + auto cmd = onAdvance(); + res->success = cmd.success; + res->message = cmd.message; + if (cmd.success) { publishNavEnabled(); publishNavMode(); publishLedStatus(); publishStatus(); } + }); + + skip_srv_ = create_service( + "~/skip", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr res) + { + std::lock_guard lk(mutex_); + auto cmd = onSkip(); + res->success = cmd.success; + res->message = cmd.message; + if (cmd.success) { publishNavEnabled(); publishNavMode(); publishLedStatus(); publishStatus(); } + }); + + 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 = setTeleop(req->data); + res->success = cmd.success; + res->message = cmd.message; + if (cmd.success) { publishNavEnabled(); publishNavMode(); publishLedStatus(); 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; + 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_); + last_planner_event_ = msg->event; + if (msg->event == msgs::msg::PlannerEvent::PLAN_FAILED) { + onPlanFailed(); + publishNavEnabled(); + publishNavMode(); + publishLedStatus(); + publishStatus(); + } + }); + + global_path_sub_ = create_subscription( + global_path_topic, rclcpp::QoS(1).transient_local(), + [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)}); + } + global_path_ = std::move(path); + }); + + aruco_sub_ = create_subscription( + aruco_detection_topic, 10, + [this](const vision_msgs::msg::Detection2D::SharedPtr msg) { + std::lock_guard lk(mutex_); + onArucoDetection(msg); + }); + + // Camera intrinsics — only the image centre/size are needed for the + // image-based servo. Latched, read once. + camera_info_sub_ = create_subscription( + camera_info_topic, rclcpp::SensorDataQoS(), + [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + std::lock_guard lk(mutex_); + cam_width_ = static_cast(msg->width); + cam_height_ = static_cast(msg->height); + cam_cx_ = (msg->k[2] > 0.0) ? msg->k[2] : cam_width_ * 0.5; + cam_info_ready_ = true; + }); + + yolo_sub_ = create_subscription( + yolo_detection_topic, 10, + [this](const std_msgs::msg::Bool::SharedPtr msg) { + std::lock_guard lk(mutex_); + if (msg->data) onDetection(); + }); + + stuck_sub_ = create_subscription( + stuck_topic, rclcpp::QoS(1).reliable().transient_local(), + [this](const msgs::msg::LocalPlannerStuck::SharedPtr msg) { + std::lock_guard lk(mutex_); + onPlannerStuck(*msg); + }); + + status_timer_ = create_wall_timer( + 500ms, + [this]() { + std::lock_guard lk(mutex_); + refreshPoseFromTF(); + auto res = tick(now().seconds()); + + if (res.publish_goal) publishGoal(res.goal_to_publish); + checkActionResult(res); + publishNavEnabled(); + publishNavMode(); + publishLedStatus(); + publishAvoidanceState(); + publishStatus(); + }); + + publishNavEnabled(); + publishNavMode(); + publishLedStatus(); + publishAvoidanceState(); + publishWaypointQueue(); + RCLCPP_INFO(get_logger(), "MissionExecutive ready — state: IDLE"); + } + +private: + // ── State machine ───────────────────────────────────────────────────────── + + bool isNavEnabled() const { + // ARUCO_CONFIRM and ARUCO_APPROACH are deliberately excluded: dropping + // /nav_enabled holds the local planner silent so it never fights the + // visual-servo velocity commands (CONFIRM = stopped; APPROACH = servo drives). + 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::ARUCO_CONFIRM: + case State::ARUCO_APPROACH: + case State::ARRIVING: + case State::RETURNING: + case State::SPIRAL_COVERAGE: return "autonomous"; + default: return "stopped"; + } + } + + bool isArucoTarget() const { + return active_target_.has_value() && + (active_target_->target_type == 1 || active_target_->target_type == 2); + } + + bool arucoTagVisible(double current_time_s) const { + return aruco_last_detection_s_ > 0.0 && + (current_time_s - aruco_last_detection_s_) < params_.aruco_max_age_s; + } + + void publishCmd(double linear, double angular) { + geometry_msgs::msg::TwistStamped cmd; + cmd.header.stamp = now(); + cmd.header.frame_id = "base_link"; + cmd.twist.linear.x = linear; + cmd.twist.angular.z = angular; + cmd_vel_pub_->publish(cmd); + } + + 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()) { + if (auto* e = findEntry(active_target_->id)) { + e->visited = true; + persistAndPublishQueue(); + } + } + 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 && !isArucoTarget()) { + detection_received_ = true; + transition(State::SPIRAL_DONE); + } + } + + // Image-based ArUco homing. We use ONLY the 2D bounding box (centre pixel + + // height) — no pose, no solvePnP, no TF. Centre the tag in frame and drive + // forward; the tick() loop ends the approach when the box fills the frame. + void onArucoDetection(const vision_msgs::msg::Detection2D::SharedPtr& msg) { + if (!isArucoTarget()) return; + if (msg->bbox.size_x <= 0.0 || msg->bbox.size_y <= 0.0) return; // no usable box + + const double now_s = now().seconds(); + + // Reset confirmation count if the tag was lost between frames. + const bool gap_reset = (now_s - aruco_last_detection_s_ > params_.aruco_max_age_s); + if (gap_reset) aruco_candidate_frames_ = 0; + aruco_last_detection_s_ = now_s; + aruco_candidate_frames_++; + + // Cache the latest box for the servo / heartbeat. + aruco_bbox_cx_ = msg->bbox.center.position.x; + aruco_bbox_height_ = msg->bbox.size_y; + + // Stop-and-confirm: the instant we first glimpse the tag while transiting or + // searching, halt in place (ARUCO_CONFIRM drops /nav_enabled) and accumulate + // the remaining confirmation frames so a single false positive can't trigger + // an approach. If it's lost mid-confirm, tick() resumes the prior phase. + if (state_ == State::NAVIGATING || state_ == State::SPIRAL_COVERAGE) { + pre_confirm_state_ = state_; + transition(State::ARUCO_CONFIRM); + publishNavEnabled(); // halt the planner immediately, don't wait for the timer + publishCmd(0.0, 0.0); // and actively brake so the tag stays in the FOV to confirm + } + + if (aruco_candidate_frames_ < params_.aruco_confirm_frames) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, + "[ARUCO] tag seen — holding to confirm %d/%d frames%s", + aruco_candidate_frames_, params_.aruco_confirm_frames, + gap_reset ? " (restarted after gap)" : ""); + return; + } + + // Confirmed — lock on and start the visual-servo approach. + if (state_ == State::ARUCO_CONFIRM) { + transition(State::ARUCO_APPROACH); + publishNavEnabled(); // keep the planner silent; the servo owns cmd_vel now + RCLCPP_INFO(get_logger(), "[ARUCO] confirmed — starting visual-servo approach"); + } + + // Drive the servo at camera rate while approaching. + if (state_ == State::ARUCO_APPROACH) driveVisualServo(); + } + + // Centre the tag (bbox centre → image centre) and drive forward. Sets the + // arrival flag once the box fills enough of the frame; tick() finalizes. + void driveVisualServo() { + if (!cam_info_ready_ || cam_cx_ <= 0.0 || cam_height_ <= 0.0) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "[ARUCO] no camera_info yet — cannot visual-servo"); + publishCmd(0.0, 0.0); + return; + } + + // Arrival: the box is large enough → we are right in front of the post. + const double height_frac = aruco_bbox_height_ / cam_height_; + if (height_frac >= params_.aruco_bbox_stop_frac) { + publishCmd(0.0, 0.0); + aruco_arrived_ = true; // tick() turns this into STOPPED_AT_TARGET + return; + } + + // Horizontal error, normalized to [-1, 1]. Image +x is to the right; turning + // right is a negative yaw rate, so angular = -kp * err. + const double err = (aruco_bbox_cx_ - cam_cx_) / cam_cx_; + double angular = -params_.aruco_servo_kp * err; + angular = std::clamp(angular, + -params_.aruco_servo_max_angular, + params_.aruco_servo_max_angular); + // Slow forward speed when the tag is off-centre so we straighten before + // closing distance (and the tag stays in frame). + const double linear = params_.aruco_servo_linear_mps * + std::clamp(1.0 - std::fabs(err), 0.2, 1.0); + publishCmd(linear, angular); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, + "[ARUCO] servo: err=%+.2f height=%.0f%% lin=%.2f ang=%+.2f", + err, height_frac * 100.0, linear, angular); + } + + TargetEntry* findEntry(const std::string& id) { + for (auto& e : queue_) if (e.id == id) return &e; + return nullptr; + } + + // Re-sort the not-yet-acted-on entries (excluding the currently active one) + // by distance from current robot pose. Visited/skipped entries keep their + // slot positions so the GUI sees a stable history list. + void sortPending() { + if (!robot_pose_.has_value()) return; + const double rx = robot_pose_->x; + const double ry = robot_pose_->y; + std::vector idx; + idx.reserve(queue_.size()); + for (size_t i = 0; i < queue_.size(); ++i) { + const auto& e = queue_[i]; + if (e.visited || e.skipped) continue; + if (active_target_.has_value() && active_target_->id == e.id) continue; + idx.push_back(i); + } + if (idx.size() < 2) return; + std::vector entries; + entries.reserve(idx.size()); + for (auto i : idx) entries.push_back(queue_[i]); + std::sort(entries.begin(), entries.end(), + [rx, ry](const TargetEntry& a, const TargetEntry& b) { + return std::hypot(a.x_m - rx, a.y_m - ry) < + std::hypot(b.x_m - rx, b.y_m - ry); + }); + for (size_t k = 0; k < idx.size(); ++k) queue_[idx[k]] = entries[k]; + } + + std::optional popNextPending() { + for (const auto& e : queue_) { + if (!e.visited && !e.skipped) { + if (active_target_.has_value() && active_target_->id == e.id) continue; + return e; + } + } + return std::nullopt; + } + + std::optional lastVisitedBefore(const std::string& current_id) { + std::optional last; + for (const auto& e : queue_) { + if (e.id == current_id) break; + if (e.visited) last = e; + } + return last; + } + + void persistAndPublishQueue() { + if (!waypoint_cache_path_.empty()) { + if (!saveQueueToFile(waypoint_cache_path_, queue_)) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "Failed to persist waypoint cache to '%s'", waypoint_cache_path_.c_str()); + } + } + publishWaypointQueue(); + } + + CommandResult setTarget(const TargetEntry& entry) { + if (auto* existing = findEntry(entry.id)) { + const bool was_terminal = existing->visited || existing->skipped; + *existing = entry; + // Update of an in-progress entry keeps its slot; only pending re-sort. + if (!was_terminal) sortPending(); + } else { + queue_.push_back(entry); + sortPending(); + } + persistAndPublishQueue(); + 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* found = findEntry(target_id); + if (!found) { + return {false, "Unknown target_id: " + target_id}; + } + if (is_return && !found->visited) { + return {false, "Target not yet visited — cannot RETURN"}; + } + entry = *found; + } 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; + aruco_candidate_frames_ = 0; + aruco_last_detection_s_ = 0.0; + aruco_arrived_ = false; + aruco_bbox_height_ = 0.0; + 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 onAdvance() { + const bool advanceable = (state_ == State::IDLE || + state_ == State::STOPPED_AT_TARGET || + state_ == State::STOPPED_AT_RETURN || + state_ == State::SPIRAL_DONE); + if (!advanceable) { + return {false, "Cannot advance from state " + stateToStr(state_)}; + } + auto next = popNextPending(); + if (!next.has_value()) { + return {false, "No pending waypoints"}; + } + auto nav = startNav(next->id, std::nullopt, /*is_return=*/false); + if (!nav.accepted) return {false, nav.message}; + if (nav.publish_goal) { + publishGoal(nav.goal_to_publish); + publishActiveTarget(); + } + persistAndPublishQueue(); + return {true, "Advanced to '" + next->id + "'"}; + } + + CommandResult onSkip() { + if (!active_target_.has_value()) { + return {false, "No active waypoint to skip"}; + } + if (auto* e = findEntry(active_target_->id)) { + e->skipped = true; + } + active_target_.reset(); + transition(State::IDLE); + persistAndPublishQueue(); + return {true, "Skipped — call ~/advance for next waypoint"}; + } + + void onPlannerStuck(const msgs::msg::LocalPlannerStuck& msg) { + const bool rising = msg.stuck && !prev_stuck_; + prev_stuck_ = msg.stuck; + if (!rising) return; + if (!active_target_.has_value()) return; + if (state_ != State::NAVIGATING && state_ != State::RETURNING) return; + const double now_s = now().seconds(); + if (now_s - last_replan_pub_s_ < replan_throttle_s_) return; + last_replan_pub_s_ = now_s; + publishGoal({active_target_->x_m, active_target_->y_m, 0.0}); + RCLCPP_WARN(get_logger(), + "[REPLAN] /local_planner/stuck rising edge — re-publishing goal for '%s'", + active_target_->id.c_str()); + } + + 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& center) { + std::vector waypoints; + const double a = params_.spiral_spacing_m / (2.0 * M_PI); + if (a <= 0.0) return waypoints; + // Drive to the centre first — the point where the tag/object is expected — + // then spiral outward from it. + waypoints.push_back({center.x, center.y, 0.0}); + const double max_angle = params_.spiral_radius_m / a; + const double min_start_r = 1.5; + const double yaw0 = center.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({ + center.x + r * std::cos(yaw0 + theta + M_PI_2), + center.y + r * std::sin(yaw0 + theta + M_PI_2), + 0.0 + }); + } + return waypoints; + } + + void startSpiralCoverage(double current_time_s) { + // Centre the spiral on the TARGET point (where the tag/object should be), + // not on wherever the rover stopped within the GPS arrival tolerance. The + // first spiral waypoint is the target point itself, so the rover drives + // there first and then expands outward. (If a tag is spotted at any point, + // tick() step 1a preempts the spiral and switches to ARUCO_APPROACH.) + Pose2D center; + if (active_target_.has_value()) { + center.x = active_target_->x_m; + center.y = active_target_->y_m; + center.yaw = robot_pose_.has_value() ? robot_pose_->yaw : 0.0; + } else if (robot_pose_.has_value()) { + center = robot_pose_.value(); + } else { + transition(State::SPIRAL_DONE); + return; + } + spiral_waypoints_ = generateSpiralWaypoints(center); + spiral_waypoint_idx_ = 0; + spiral_start_time_s_ = current_time_s; + detection_received_ = false; + if (spiral_waypoints_.empty()) transition(State::SPIRAL_DONE); + } + + // Advance spiral_waypoint_idx_ past every waypoint the rover is already within + // tolerance of, then return the next not-yet-reached waypoint as the goal. + // Collapsing the cluster in one step (instead of one index per tick) is what + // keeps the published goal genuinely ahead of the rover: spiral waypoints are + // spaced far closer than spiral_waypoint_tolerance_m, so advancing one-at-a- + // time republishes a goal that circles the centre and the rover never commits + // to a heading. Returns false when the spiral is exhausted. + bool nextSpiralGoal(Pose2D& goal_out) { + if (spiral_waypoints_.empty()) return false; + if (robot_pose_.has_value()) { + while (spiral_waypoint_idx_ < spiral_waypoints_.size()) { + 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) { + break; + } + ++spiral_waypoint_idx_; + } + } + if (spiral_waypoint_idx_ >= spiral_waypoints_.size()) return false; + goal_out = spiral_waypoints_[spiral_waypoint_idx_]; + return true; + } + + TickResult tick(double current_time_s) { + TickResult res; + + // ArUco debug heartbeat — one line at 1 Hz whenever an ArUco post is the + // active target: state, confirm progress, whether the tag is currently in + // view, the normalized horizontal error, and how much of the frame it fills. + if (isArucoTarget()) { + const double det_age = (aruco_last_detection_s_ > 0.0) + ? current_time_s - aruco_last_detection_s_ : -1.0; + const double err = (cam_info_ready_ && cam_cx_ > 0.0) + ? (aruco_bbox_cx_ - cam_cx_) / cam_cx_ : 0.0; + const double hfrac = (cam_height_ > 0.0) ? aruco_bbox_height_ / cam_height_ : 0.0; + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, + "[ARUCO] state=%s id=%s frames=%d/%d visible=%d last_det=%.1fs err=%+.2f height=%.0f%%", + stateToStr(state_).c_str(), active_target_->id.c_str(), + aruco_candidate_frames_, params_.aruco_confirm_frames, + static_cast(arucoTagVisible(current_time_s)), det_age, err, hfrac * 100.0); + } + + // 1a. ArUco visual-servo approach. The servo loop (centre + drive) runs in + // onArucoDetection at camera rate; here we only finalize arrival and + // handle a lost tag. The rover stops (nav_enabled is dropped in this + // state, and we publish zero) whenever the tag is not currently in view. + if (state_ == State::ARUCO_APPROACH) { + if (aruco_arrived_) { + publishCmd(0.0, 0.0); + transition(State::STOPPED_AT_TARGET); + res.action_finished = true; + res.action_success = true; + res.action_message = "Reached ArUco post (tag filled frame)"; + return res; + } + if (!arucoTagVisible(current_time_s)) { + publishCmd(0.0, 0.0); // do not drive blind between frames + if (current_time_s - aruco_last_detection_s_ >= params_.aruco_lost_timeout_s) { + const double hfrac = (cam_height_ > 0.0) ? aruco_bbox_height_ / cam_height_ : 0.0; + if (hfrac >= 0.5 * params_.aruco_bbox_stop_frac) { + // Already close when it left the FOV → count it as reached. + transition(State::STOPPED_AT_TARGET); + res.action_finished = true; + res.action_success = true; + res.action_message = "Reached ArUco post (tag left view at close range)"; + return res; + } + // Lost while still far — resume the spiral search. + RCLCPP_WARN(get_logger(), + "[ARUCO] tag lost mid-approach while still far (height=%.0f%%) — resuming spiral", + hfrac * 100.0); + transition(State::SPIRAL_COVERAGE); + startSpiralCoverage(current_time_s); + Pose2D spiral_goal; + if (state_ == State::SPIRAL_COVERAGE) { + if (nextSpiralGoal(spiral_goal)) { + res.publish_goal = true; + res.goal_to_publish = spiral_goal; + } else { + transition(State::SPIRAL_DONE); + } + } + } + } + return res; // tag visible → the servo callback is driving + } + + // 1c. Stop-and-confirm hold. Actively brake every tick (don't rely on the + // planner's single stop) so the rover holds still and the tag stays in + // the FOV long enough to accumulate confirmation frames. onArucoDetection + // promotes us to ARUCO_APPROACH once confirmed; here we keep zero velocity + // and detect loss to resume the prior search/transit phase. + if (state_ == State::ARUCO_CONFIRM) { + publishCmd(0.0, 0.0); + if (current_time_s - aruco_last_detection_s_ > params_.aruco_max_age_s) { + RCLCPP_INFO(get_logger(), + "[ARUCO] tag lost before confirming (%d/%d) — resuming %s", + aruco_candidate_frames_, params_.aruco_confirm_frames, + stateToStr(pre_confirm_state_).c_str()); + aruco_candidate_frames_ = 0; + transition(pre_confirm_state_); + if (pre_confirm_state_ == State::SPIRAL_COVERAGE && + spiral_waypoint_idx_ < spiral_waypoints_.size()) { + res.publish_goal = true; + res.goal_to_publish = spiral_waypoints_[spiral_waypoint_idx_]; + } else if (active_target_.has_value()) { + res.publish_goal = true; + res.goal_to_publish = {active_target_->x_m, active_target_->y_m, 0.0}; + } + } + return res; // otherwise hold position and keep accumulating frames + } + + // 1b. GPS arrival check + if ((state_ == State::NAVIGATING || state_ == State::RETURNING) && + active_target_.has_value()) { + const double d = getDistToGoal(); + if (d >= 0.0) { + if (isArucoTarget()) { + // Drive all the way TO the given GPS point, then spiral outward from + // it to search for the tag. The spiral is the deterministic search + // phase — NOT a fallback triggered by "arrived without tag lock". A + // confirmed tag at any point preempts to ARUCO_APPROACH via step 1a + // above, regardless of state. + if (d < params_.spiral_waypoint_tolerance_m) { + transition(State::SPIRAL_COVERAGE); + startSpiralCoverage(current_time_s); + Pose2D spiral_goal; + if (state_ == State::SPIRAL_COVERAGE) { + if (nextSpiralGoal(spiral_goal)) { + res.publish_goal = true; + res.goal_to_publish = spiral_goal; + } else { + transition(State::SPIRAL_DONE); + } + } + } + } else if (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 size_t prev_idx = spiral_waypoint_idx_; + Pose2D goal; + if (!nextSpiralGoal(goal)) { + transition(State::SPIRAL_DONE); + } else if (spiral_waypoint_idx_ != prev_idx) { + // Only republish (and trigger a global replan) when the target + // waypoint actually changed. + res.publish_goal = true; + res.goal_to_publish = goal; + } + } + } + + // 4. Action result resolution + if (state_ == State::STOPPED_AT_TARGET) { + // type 3 = OBJECT (needs spiral+YOLO); ArUco posts (1,2) finish via aruco_stop above + const bool needs_spiral = active_target_.has_value() && + active_target_->target_type == 3; + if (needs_spiral) { + transition(State::SPIRAL_COVERAGE); + startSpiralCoverage(current_time_s); + Pose2D spiral_goal; + if (state_ == State::SPIRAL_COVERAGE) { + if (nextSpiralGoal(spiral_goal)) { + res.publish_goal = true; + res.goal_to_publish = spiral_goal; + } else { + transition(State::SPIRAL_DONE); + } + } + } 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; + if (isArucoTarget()) { + // A confirmed tag preempts to ARUCO_APPROACH (step 1a) and finishes at + // STOPPED_AT_TARGET, so reaching SPIRAL_DONE means the tag was never + // found within the search radius/timeout — that is a failure. + res.action_success = false; + res.action_message = "ArUco tag not found during spiral search"; + } else { + 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 = isNavEnabled(); + nav_enabled_pub_->publish(msg); + } + + void publishNavMode() { + std_msgs::msg::String msg; + msg.data = getNavMode(); + nav_mode_pub_->publish(msg); + } + + // Obstacle avoidance should be ON for normal navigation and object-detection + // (type 3) spirals, but OFF while searching for / homing on an ArUco post: + // the post reads as an obstacle in the laserscan, so avoidance would steer the + // rover away from the very tag it must drive up to. + bool desiredAvoidanceEnabled() const { + if (isArucoTarget() && + (state_ == State::SPIRAL_COVERAGE || + state_ == State::ARUCO_CONFIRM || + state_ == State::ARUCO_APPROACH)) { + return false; + } + return true; + } + + void publishAvoidanceState() { + const bool want = desiredAvoidanceEnabled(); + if (last_avoidance_enabled_.has_value() && last_avoidance_enabled_.value() == want) { + return; // publish only on change + } + std_msgs::msg::Bool msg; + msg.data = want; + obstacle_avoidance_pub_->publish(msg); + last_avoidance_enabled_ = want; + RCLCPP_INFO(get_logger(), "[AVOIDANCE] obstacle avoidance %s%s", + want ? "ENABLED" : "DISABLED", + want ? "" : " — ArUco approach (post would read as an obstacle)"); + } + + void publishWaypointQueue() { + if (!waypoint_queue_pub_) return; + nlohmann::json arr = nlohmann::json::array(); + for (const auto& e : queue_) { + auto je = entryToJson(e); + const bool is_active = active_target_.has_value() && + active_target_->id == e.id && + !e.visited && !e.skipped; + std::string status = "PENDING"; + if (e.skipped) status = "SKIPPED"; + else if (e.visited) status = "VISITED"; + else if (is_active) status = "ACTIVE"; + je["status"] = status; + arr.push_back(je); + } + std_msgs::msg::String s; + s.data = arr.dump(); + waypoint_queue_pub_->publish(s); + } + + void publishLedStatus() { + msgs::msg::LedStatus led; + switch (state_) { + case State::NAVIGATING: + case State::ARUCO_CONFIRM: + case State::ARUCO_APPROACH: + case State::ARRIVING: + case State::RETURNING: + case State::SPIRAL_COVERAGE: + case State::ABORTING: + // Red — autonomous operation + led.cmd = msgs::msg::LedStatus::CMD_SOLID; + led.r = 255; led.g = 0; led.b = 0; + break; + case State::TELEOP: + // Blue — teleoperation + led.cmd = msgs::msg::LedStatus::CMD_SOLID; + led.r = 0; led.g = 0; led.b = 255; + break; + case State::STOPPED_AT_TARGET: + case State::STOPPED_AT_RETURN: + case State::SPIRAL_DONE: + // Flashing green — successful arrival + led.cmd = msgs::msg::LedStatus::CMD_FLASH; + led.r = 0; led.g = 255; led.b = 0; + led.param = 20; // 2 Hz + break; + default: + led.cmd = msgs::msg::LedStatus::CMD_OFF; + break; + } + led_status_pub_->publish(led); + } + + 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.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 = 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 = 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); + } + + void checkActionResult(const 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; + cancelNav(); + return; + } + + if (res.action_finished) { + // Guard against a non-active handle (e.g. already terminal, or shutting + // down) — succeed()/abort() on such a handle throws. + if (active_goal_handle_->is_active()) { + 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 = getDistToGoal(); + fb->cross_track_error_m = getCrossTrackError(); + fb->state = stateToStr(state_); + active_goal_handle_->publish_feedback(fb); + } + + if (res.start_queued_goal) { + active_goal_handle_ = queued_goal_handle_; + queued_goal_handle_ = nullptr; + publishActiveTarget(); + } + } + + void handleAccepted(std::shared_ptr goal_handle) { + const auto goal = goal_handle->get_goal(); + + std::optional inline_target; + if (goal->target_id.empty()) { + 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; + } + const 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 = startNav(goal->target_id, inline_target, goal->is_return); + + if (!cmd.accepted) { + auto res = std::make_shared(); + res->success = false; + res->message = cmd.message; + goal_handle->abort(res); + return; + } + + if (queued_active_ && state_ == State::ABORTING) { + if (queued_goal_handle_ && queued_goal_handle_->is_active()) { + 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.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.publish_goal) { + publishGoal(cmd.goal_to_publish); + publishActiveTarget(); + } + publishNavEnabled(); + publishNavMode(); + publishLedStatus(); + publishStatus(); + } + } + + 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; + } + const 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 = setTarget(entry); + res->success = cmd.success; + res->message = cmd.message; + } + + // ── State ───────────────────────────────────────────────────────────────── + + std::mutex mutex_; + MissionParams params_; + State state_{State::IDLE}; + + std::vector queue_; + std::string waypoint_cache_path_; + 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}; + + // ArUco visual-servo approach state (image-based; no pose/TF/solvePnP) + std::string aruco_detection_topic_; + State pre_confirm_state_{State::SPIRAL_COVERAGE}; + int aruco_candidate_frames_{0}; + double aruco_last_detection_s_{0.0}; + double aruco_bbox_cx_{0.0}; // latest bbox centre x (pixels) + double aruco_bbox_height_{0.0}; // latest bbox height (pixels) + bool aruco_arrived_{false}; // bbox filled the frame → at the post + // Camera intrinsics (image centre / size only) + bool cam_info_ready_{false}; + double cam_cx_{0.0}; + double cam_width_{0.0}; + double cam_height_{0.0}; + + // ── 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 cmd_vel_pub_; + rclcpp::Publisher::SharedPtr nav_enabled_pub_; + rclcpp::Publisher::SharedPtr nav_mode_pub_; + rclcpp::Publisher::SharedPtr obstacle_avoidance_pub_; + std::optional last_avoidance_enabled_; + rclcpp::Publisher::SharedPtr active_target_pub_; + rclcpp::Publisher::SharedPtr nav_status_pub_; + rclcpp::Publisher::SharedPtr led_status_pub_; + rclcpp::Publisher::SharedPtr waypoint_queue_pub_; + + rclcpp_action::Server::SharedPtr action_server_; + + rclcpp::Service::SharedPtr abort_srv_; + rclcpp::Service::SharedPtr advance_srv_; + rclcpp::Service::SharedPtr skip_srv_; + rclcpp::Service::SharedPtr set_target_srv_; + rclcpp::Service::SharedPtr teleop_srv_; + + 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 camera_info_sub_; + rclcpp::Subscription::SharedPtr yolo_sub_; + rclcpp::Subscription::SharedPtr stuck_sub_; + + // Replan-on-stuck state + bool prev_stuck_{false}; + double last_replan_pub_s_{0.0}; + double replan_throttle_s_{3.0}; + + rclcpp::Client::SharedPtr latlon_client_; + + std::shared_ptr active_goal_handle_; + std::shared_ptr queued_goal_handle_; + + 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/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..511ee1b3 --- /dev/null +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_real.yaml @@ -0,0 +1,109 @@ +# nav_params_real.yaml + +gps_pose_publisher: + ros__parameters: + use_sim_time: false + 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 + 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" + yolo_detection_topic: "/yolo/target_found" + # ArUco approach + aruco_detection_topic: "/aruco_detection" # Detection2D (bbox) from aruco_bt + camera_info_topic: "/zed/zed_node/left/camera_info" # image centre/size for the servo + cmd_vel_topic: "/rear_ackermann_controller/reference" # servo drives this during approach + aruco_confirm_frames: 5 # consecutive frames before trusting a sighting + aruco_max_age_s: 0.5 # tag considered lost if no detection for this long + aruco_lost_timeout_s: 1.0 # after this long lost mid-approach, finalize/resume + # Visual-servo approach (image-based: centre the bbox, drive forward) + aruco_servo_kp: 0.8 # yaw gain on normalized bbox-centre error + aruco_servo_linear_mps: 0.4 # forward speed while homing + aruco_servo_max_angular: 0.6 # clamp on commanded yaw rate (rad/s) + aruco_bbox_stop_frac: 0.45 # stop when bbox height exceeds this fraction of the image + # 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" + +point_cloud_filterer: + ros__parameters: + use_sim_time: false + input_topic: "/zed/zed_node/point_cloud/cloud_registered" + output_topic: "/zed/cloud_base_link" + frame_override: "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: "/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.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 ────────────────────────────────────────────────── + 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 + # ── 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 new file mode 100644 index 00000000..5c80eb9a --- /dev/null +++ b/src/subsystems/navigation/nav_bringup/config/nav_params_sim.yaml @@ -0,0 +1,109 @@ +# nav_params_sim.yaml + +gps_pose_publisher: + ros__parameters: + use_sim_time: true + heading_topic: "/gps/heading" + heading_offset_deg: 180.0 # sim /gps/heading reports ~180° off from body x-axis + 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: 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: + 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: + 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" + yolo_detection_topic: "/yolo/target_found" + # ArUco approach + aruco_detection_topic: "/aruco_detection" # Detection2D (bbox) from aruco_bt + camera_info_topic: "/zed/zed_node/left/camera_info" # image centre/size for the servo + cmd_vel_topic: "/front_ackermann_controller/reference" # servo drives this during approach + aruco_confirm_frames: 5 # consecutive frames before trusting a sighting + aruco_max_age_s: 0.5 # tag considered lost if no detection for this long + aruco_lost_timeout_s: 1.0 # after this long lost mid-approach, finalize/resume + # Visual-servo approach (image-based: centre the bbox, drive forward) + aruco_servo_kp: 0.8 # yaw gain on normalized bbox-centre error + aruco_servo_linear_mps: 0.4 # forward speed while homing + aruco_servo_max_angular: 0.6 # clamp on commanded yaw rate (rad/s) + aruco_bbox_stop_frac: 0.45 # stop when bbox height exceeds this fraction of the image + # 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" + +point_cloud_filterer: + ros__parameters: + use_sim_time: true + input_topic: "/zed/zed_node/point_cloud/cloud_registered" + output_topic: "/zed/cloud_base_link" + frame_override: "base_link" + +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: + 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.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 ────────────────────────────────────────────────── + 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.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 + # ── 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 new file mode 100644 index 00000000..be0d4d56 --- /dev/null +++ b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py @@ -0,0 +1,242 @@ +#!/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. + +"""emmn.launch.py — Athena EMMN navigation stack (GPS-only, Nav2-free). + +Deployment modes +──────────────── + jetson : all sensor/compute nodes (run on the rover Jetson) + base_station : mission_executive only (operator UI) + standalone : everything on one machine (default, for local dev/sim) + +Node summary +──────────── + Jetson: + robot_state_publisher : URDF → TF static transforms + athena_sensors : ZED, IMU, etc. + mag_heading : magnetometer heading + gps_pose_publisher : WGS84→ENU, /robot_pose, map→base_link TF + dem_costmap_converter : DEM GeoTIFF → /map + global_planner : /goal_pose → /global_path + point_cloud_filterer : ZED cloud preprocessing + pointcloud_to_laserscan : cloud → /scan for obstacle avoidance + vector_field_planner : /global_path → /cmd_vel + yolo_ros_bt : YOLO object detection + aruco_bt : ArUco marker detection + led_indicator : physical LED status indicator + + Base station: + mission_executive : state machine, action/service operator interface +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, Command, FindExecutable, PathJoinSubstitution, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + 'mode', + default_value='standalone', + choices=['standalone', 'jetson', 'base_station'], + description=( + "Deployment mode. " + "'standalone' (default) starts all nodes on a single machine. " + "'jetson' starts only the sensor/compute nodes (run on the rover). " + "'base_station' starts only the mission_executive (operator UI)." + ), + ), + DeclareLaunchArgument( + 'sim', + default_value='false', + choices=['true', 'false'], + description='Use simulation GPS bridge instead of real hardware', + ), + ] + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + + +def launch_setup(context, *args, **kwargs): + mode = LaunchConfiguration('mode').perform(context) + sim = LaunchConfiguration('sim') + + nav_bringup_dir = get_package_share_directory('nav_bringup') + + 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'), "'" + ]) + + 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')]), + ' ', + PathJoinSubstitution([ + FindPackageShare('description'), 'urdf', 'athena_drive.urdf.xacro' + ]), + ]) + + # ── Jetson nodes ────────────────────────────────────────────────────────── + + 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_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('athena_sensors'), 'launch', 'sensors.launch.py') + ), + launch_arguments={ + 'sim': sim, + 'enable_gnss': 'false', + }.items(), + ) + + mag_heading_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('mag_heading'), 'launch', 'mag_heading.launch.py') + ), + ) + + gps_pose_publisher_node = Node( + package='gps_pose_publisher', + executable='gps_pose_publisher_node', + name='gps_pose_publisher', + output='screen', + parameters=[nav_params_file], + ) + + dem_costmap_converter_node = Node( + package='athena_map', + executable='map_node', + name='dem_costmap_converter', + output='screen', + parameters=[ + nav_params_file, + {'dem_file_path': dem_file}, + ], + ) + + global_planner_node = Node( + package='global_planner', + executable='global_planner_node', + name='global_planner', + output='screen', + parameters=[nav_params_file], + ) + + 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', + name='pointcloud_to_laserscan', + output='screen', + parameters=[nav_params_file], + remappings=[ + ('cloud_in', '/zed/cloud_base_link'), + ('scan', '/scan'), + ], + ) + + vector_field_planner_node = Node( + package='vector_field_planner', + executable='vector_field_planner_node', + name='vector_field_planner', + output='screen', + parameters=[nav_params_file], + remappings=[ + ('/odom', '/odom/ground_truth'), + ], + ) + + yolo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('yolo_ros_bt'), 'launch', 'yolo.ros.launch.py') + ), + ) + + aruco_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('aruco_bt'), 'launch', 'aruco.launch.py') + ), + ) + + led_indicator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('led_indicator'), 'launch', 'led_indicator.launch.py') + ), + ) + + # ── Base station nodes ──────────────────────────────────────────────────── + + mission_executive_node = Node( + package='mission_executive', + executable='mission_executive_node', + name='mission_executive', + output='screen', + parameters=[nav_params_file], + ) + + # ── Mode selection ──────────────────────────────────────────────────────── + + jetson_actions = [ + robot_state_publisher_node, + sensors_launch, + mag_heading_launch, + gps_pose_publisher_node, + dem_costmap_converter_node, + global_planner_node, + point_cloud_filterer_node, + pointcloud_to_laserscan_node, + vector_field_planner_node, + yolo_launch, + aruco_launch, + led_indicator_launch, + ] + + base_station_actions = [ + mission_executive_node, + ] + + if mode == 'jetson': + return jetson_actions + elif mode == 'base_station': + return base_station_actions + else: + return jetson_actions + base_station_actions diff --git a/src/subsystems/navigation/nav_bringup/package.xml b/src/subsystems/navigation/nav_bringup/package.xml index 9a8331ae..155d426f 100644 --- a/src/subsystems/navigation/nav_bringup/package.xml +++ b/src/subsystems/navigation/nav_bringup/package.xml @@ -12,10 +12,19 @@ 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 + mag_heading + yolo_ros_bt + aruco_bt + led_indicator + point_cloud_filterer ament_cmake diff --git a/src/subsystems/navigation/vector_field_planner/CMakeLists.txt b/src/subsystems/navigation/vector_field_planner/CMakeLists.txt new file mode 100644 index 00000000..840859dc --- /dev/null +++ b/src/subsystems/navigation/vector_field_planner/CMakeLists.txt @@ -0,0 +1,63 @@ +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(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(msgs 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} + ${sensor_msgs_TARGETS} + ${std_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${msgs_TARGETS} + tf2_ros::tf2_ros + tf2::tf2 +) + +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_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 new file mode 100644 index 00000000..1f376fcc --- /dev/null +++ b/src/subsystems/navigation/vector_field_planner/include/vector_field_planner/vector_field_planner_algo.hpp @@ -0,0 +1,245 @@ +// 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 + +namespace vector_field_planner { + +struct Pose2D { + double x; + double y; +}; + +struct ObstaclePoint { + double x; // map frame + double y; // map frame +}; + +enum class NavState : std::uint8_t { + NAVIGATE = 0, + SLOW = 1, + PROBE = 2, + ESCAPE = 3, + REPLAN = 4, +}; + +struct PlannerParams { + // --- path following --- + double lookahead_dist_m = 3.0; + double k_p_steering = 1.5; // pure-pursuit fallback (avoidance off) + double max_steering_angle_rad = 0.5; + double max_speed_mps = 1.5; + double goal_tolerance_m = 1.5; + double min_approach_linear_velocity = 0.3; + + // --- avoidance master switch --- + bool obstacle_avoidance_enabled = false; + + // --- robot footprint / kinematics --- + double robot_radius_m = 0.35; + double min_turn_radius_m = 1.0; // tightest physically reachable arc + + // --- local grid --- + double local_grid_size_m = 10.0; // square, base-frame, centered on robot + double local_grid_resolution_m = 0.1; + double inflate_margin_m = 0.15; // added to robot_radius for dilation + double scan_buffer_max_dist_m = 5.0; // node-side scan filter; surfaced here for parity + + // --- tentacles --- + double tentacle_length_forward_m = 3.0; + double tentacle_length_reverse_m = 1.5; + double tentacle_sample_step_m = 0.1; + int num_tentacles_per_side = 6; // total per direction = 2*N + 1 (incl. straight) + + // --- safety / FSM thresholds --- + double r_stop_hard_m = 0.55; // hard zero-vel floor + double r_stop_m = 0.70; // NAVIGATE/SLOW → PROBE + double r_slow_m = 1.35; // SLOW band upper edge + double v_escape_mps = 0.4; // reverse speed magnitude in ESCAPE + double t_escape_max_s = 4.0; + int max_escape_attempts = 3; + double tick_period_s = 0.05; // matches node timer + + // --- tentacle scoring weights --- + double w_clear = 1.0; + double w_goal = 0.6; + double w_smooth = 0.15; + double w_reverse = 0.3; +}; + +struct PlannerResult { + double linear_vel = 0.0; + double angular_vel = 0.0; + bool goal_reached = false; + + // Pure-pursuit / carrot diagnostics (unchanged). + double lookahead_x = 0.0; + double lookahead_y = 0.0; + bool lookahead_interpolated = false; + bool lookahead_behind = false; + std::size_t closest_idx = 0; + double heading_err = 0.0; + double steering_unclamped = 0.0; + bool clamped = false; + double effective_lookahead_dist = 0.0; + double approach_velocity_scale = 1.0; + + // Avoidance diagnostics. + double closest_r = -1.0; // nearest inflated obstacle, m; -1 if none + int active_points = 0; // obstacles within local grid + + // Tentacle / FSM diagnostics. + NavState nav_state = NavState::NAVIGATE; + int chosen_tentacle_idx = -1; + double chosen_curvature = 0.0; + double chosen_direction = 1.0; // +1 forward, -1 reverse + double chosen_clearance = 0.0; + double best_forward_clearance = 0.0; + double best_reverse_clearance = 0.0; + // Indices into VectorFieldPlannerAlgo::tentacles() of the best-scoring + // forward/reverse arcs this tick. Always populated (even in PROBE/REPLAN + // when no command was applied), so consumers can derive an escape + // direction without re-running the scoring pass. + int best_forward_idx = -1; + int best_reverse_idx = -1; + bool request_replan = false; // raised in REPLAN until a new path arrives +}; + +// --------------------------------------------------------------------------- +// Internal types — exposed for testability / debug consumers. +// --------------------------------------------------------------------------- + +struct Tentacle { + double curvature; // signed, 1/m (body-frame) + double direction; // +1 forward, -1 reverse + double length; // m + std::vector samples; // base-frame, evenly spaced; samples[0] = origin +}; + +struct TentacleScore { + int idx = -1; + double clearance = 0.0; + double goal_align = 0.0; + double smoothness = 0.0; + double total = -std::numeric_limits::infinity(); + bool collides = true; // true if tentacle hits within r_stop_hard_m +}; + +struct FsmContext { + NavState state = NavState::NAVIGATE; + int ticks_in_state = 0; + double best_clearance_seen_in_state = 0.0; + int escape_attempts = 0; + double prev_curvature = 0.0; +}; + +class VectorFieldPlannerAlgo { + public: + VectorFieldPlannerAlgo() = default; + + /* + * Sets planner parameters. Rebuilds the tentacle library on the next compute(). + */ + void setParams(const PlannerParams& params) { + params_ = params; + tentacles_dirty_ = true; + } + + const PlannerParams& getParams() const { return params_; } + + /* + * Sets the path to follow. Resets the FSM (and escape counter) — a new path + * is treated as a fresh start, including after a REPLAN cycle. + */ + void setPath(const std::vector& path) { + path_ = path; + fsm_ = FsmContext{}; + } + + /* + * Updates the local obstacle set (map-frame points, already age-filtered by node). + */ + void updateObstacles(const std::vector& obstacles) { + obstacles_ = obstacles; + } + + /* + * Compute velocity + steering commands. + * rx, ry, yaw — robot pose in map frame + * current_speed — last commanded linear speed (m/s), used for velocity-scaled lookahead + */ + PlannerResult compute(double rx, double ry, double yaw, double current_speed); + + // -- exposed for tests -- + std::size_t findClosestIndex(double rx, double ry) const; + + struct LookaheadResult { + double x; + double y; + bool interpolated; + }; + LookaheadResult findLookahead(double rx, double ry, std::size_t closest_idx, + double lookahead_dist) const; + + const std::vector& tentacles() const { return tentacles_; } + const FsmContext& fsm() const { return fsm_; } + + private: + PlannerParams params_; + std::vector path_; + std::vector obstacles_; + FsmContext fsm_; + + // Tentacle library (rebuilt on param change). + std::vector tentacles_; + bool tentacles_dirty_ = true; + + // Local grid (rebuilt every compute()). + std::vector grid_; // 0=free, 1=blocked; row-major + int grid_n_ = 0; + double grid_res_ = 0.1; + // Disk kernel cell offsets used for obstacle dilation (recomputed with params). + std::vector> disk_offsets_; + + // ----- helpers ----- + double effectiveLookaheadDist(double current_speed) const; + double approachVelocityScale(double dist_to_goal) const; + + void ensureTentaclesBuilt(); + void buildTentacles(); + void buildDiskKernel(); + + void buildLocalGrid(double rx, double ry, double yaw); + bool gridAt(double x, double y) const; + void paintDisk(int cx, int cy); + + TentacleScore scoreTentacle(const Tentacle& t, int idx, double carrot_bearing_base) const; + + void stepFsm(const TentacleScore& best_fwd, + const TentacleScore& best_rev, + double approach_vel, + PlannerResult& res); + + void applyTentacleCommand(const TentacleScore& sc, double linear_vel, + PlannerResult& res); + + double nearestObstacleDistBase(double rx, double ry, double yaw) const; +}; + +} // namespace vector_field_planner diff --git a/src/subsystems/navigation/vector_field_planner/package.xml b/src/subsystems/navigation/vector_field_planner/package.xml new file mode 100644 index 00000000..0cc0bd28 --- /dev/null +++ b/src/subsystems/navigation/vector_field_planner/package.xml @@ -0,0 +1,30 @@ + + + + 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 + sensor_msgs + std_msgs + visualization_msgs + tf2_ros + tf2 + msgs + + ament_cmake_gtest + + + ament_cmake + + 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..31c44d90 --- /dev/null +++ b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_algo.cpp @@ -0,0 +1,492 @@ +// 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 { + +namespace { + +inline double wrapAngle(double a) { + while (a > M_PI) a -= 2.0 * M_PI; + while (a < -M_PI) a += 2.0 * M_PI; + return a; +} + +} // namespace + +// --------------------------------------------------------------------------- +// Carrot / lookahead (unchanged behaviour, kept for path following). +// --------------------------------------------------------------------------- + +double VectorFieldPlannerAlgo::effectiveLookaheadDist(double current_speed) const { + 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 { + return std::clamp(dist_to_goal / params_.lookahead_dist_m, 0.0, 1.0); +} + +VectorFieldPlannerAlgo::LookaheadResult VectorFieldPlannerAlgo::findLookahead( + double rx, double ry, std::size_t closest_idx, double lookahead_dist) const +{ + if (path_.empty()) return {rx, ry, false}; + + const double ld2 = lookahead_dist * lookahead_dist; + for (std::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; + + 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; + + 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}; + } + } + return {path_.back().x, path_.back().y, false}; +} + +std::size_t VectorFieldPlannerAlgo::findClosestIndex(double rx, double ry) const { + std::size_t best = 0; + double best_d2 = std::numeric_limits::infinity(); + for (std::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_d2) { best_d2 = d2; best = i; } + } + return best; +} + +// --------------------------------------------------------------------------- +// Tentacle library +// --------------------------------------------------------------------------- + +void VectorFieldPlannerAlgo::ensureTentaclesBuilt() { + if (!tentacles_dirty_) return; + buildTentacles(); + buildDiskKernel(); + tentacles_dirty_ = false; +} + +void VectorFieldPlannerAlgo::buildTentacles() { + tentacles_.clear(); + + const int n_side = std::max(0, params_.num_tentacles_per_side); + const double k_max = + (params_.min_turn_radius_m > 1e-6) ? (1.0 / params_.min_turn_radius_m) : 0.0; + const double step = std::max(1e-3, params_.tentacle_sample_step_m); + + auto build_one = [&](double curvature, double direction, double length) { + Tentacle t; + t.curvature = curvature; + t.direction = direction; + t.length = length; + + double x = 0.0, y = 0.0, theta = 0.0; + t.samples.push_back({x, y}); + const int n_steps = static_cast(std::floor(length / step)); + for (int i = 0; i < n_steps; ++i) { + x += direction * step * std::cos(theta); + y += direction * step * std::sin(theta); + theta += direction * step * curvature; + t.samples.push_back({x, y}); + } + tentacles_.push_back(std::move(t)); + }; + + // Forward family: 2*n_side + 1 curvatures evenly spaced in [-k_max, +k_max]. + for (int i = -n_side; i <= n_side; ++i) { + const double k = (n_side > 0) ? (k_max * static_cast(i) / n_side) : 0.0; + build_one(k, +1.0, params_.tentacle_length_forward_m); + } + // Reverse family. + for (int i = -n_side; i <= n_side; ++i) { + const double k = (n_side > 0) ? (k_max * static_cast(i) / n_side) : 0.0; + build_one(k, -1.0, params_.tentacle_length_reverse_m); + } +} + +void VectorFieldPlannerAlgo::buildDiskKernel() { + disk_offsets_.clear(); + const double inflate_r = params_.robot_radius_m + params_.inflate_margin_m; + const double res = std::max(1e-3, params_.local_grid_resolution_m); + const int r_cells = static_cast(std::ceil(inflate_r / res)); + const double r2 = (inflate_r / res) * (inflate_r / res); + for (int dy = -r_cells; dy <= r_cells; ++dy) { + for (int dx = -r_cells; dx <= r_cells; ++dx) { + if (static_cast(dx * dx + dy * dy) <= r2) { + disk_offsets_.emplace_back(dx, dy); + } + } + } +} + +// --------------------------------------------------------------------------- +// Local grid (base frame, centered on robot) +// --------------------------------------------------------------------------- + +void VectorFieldPlannerAlgo::buildLocalGrid(double rx, double ry, double yaw) { + grid_res_ = std::max(1e-3, params_.local_grid_resolution_m); + grid_n_ = std::max(2, static_cast(std::round(params_.local_grid_size_m / grid_res_))); + // Ensure even count so the robot sits cleanly at cell-center origin. + if (grid_n_ % 2 != 0) ++grid_n_; + + grid_.assign(static_cast(grid_n_) * grid_n_, 0); + + const double c = std::cos(yaw), s = std::sin(yaw); + const int half = grid_n_ / 2; + + for (const auto& p : obstacles_) { + const double dx = p.x - rx; + const double dy = p.y - ry; + // Map → base: rotate by -yaw. + const double bx = c * dx + s * dy; + const double by = -s * dx + c * dy; + + const int cx = static_cast(std::floor(bx / grid_res_)) + half; + const int cy = static_cast(std::floor(by / grid_res_)) + half; + if (cx < -half || cx >= grid_n_ + half) continue; // beyond any reachable inflation + paintDisk(cx, cy); + } +} + +void VectorFieldPlannerAlgo::paintDisk(int cx, int cy) { + for (const auto& off : disk_offsets_) { + const int x = cx + off.first; + const int y = cy + off.second; + if (x < 0 || x >= grid_n_ || y < 0 || y >= grid_n_) continue; + grid_[static_cast(y) * grid_n_ + x] = 1; + } +} + +bool VectorFieldPlannerAlgo::gridAt(double x, double y) const { + const int half = grid_n_ / 2; + const int gx = static_cast(std::floor(x / grid_res_)) + half; + const int gy = static_cast(std::floor(y / grid_res_)) + half; + if (gx < 0 || gx >= grid_n_ || gy < 0 || gy >= grid_n_) return false; + return grid_[static_cast(gy) * grid_n_ + gx] != 0; +} + +double VectorFieldPlannerAlgo::nearestObstacleDistBase(double rx, double ry, double yaw) const { + double best = std::numeric_limits::infinity(); + const double c = std::cos(yaw), s = std::sin(yaw); + for (const auto& p : obstacles_) { + const double dx = p.x - rx; + const double dy = p.y - ry; + const double bx = c * dx + s * dy; + const double by = -s * dx + c * dy; + const double d = std::hypot(bx, by); + if (d < best) best = d; + } + return best; +} + +// --------------------------------------------------------------------------- +// Tentacle scoring +// --------------------------------------------------------------------------- + +TentacleScore VectorFieldPlannerAlgo::scoreTentacle(const Tentacle& t, int idx, + double carrot_bearing_base) const +{ + TentacleScore sc; + sc.idx = idx; + + const double step = std::max(1e-3, params_.tentacle_sample_step_m); + + // Walk samples (skip the origin) until the first blocked cell. + double clearance = t.length; + for (std::size_t i = 1; i < t.samples.size(); ++i) { + if (gridAt(t.samples[i].x, t.samples[i].y)) { + clearance = static_cast(i - 1) * step; + break; + } + } + sc.clearance = clearance; + sc.collides = (clearance < params_.r_stop_hard_m); + + // Goal alignment: bearing from origin to tentacle endpoint vs. carrot bearing. + // Endpoint bearing is well-defined as long as the tentacle has at least one step. + const auto& end = t.samples.back(); + const double end_bearing = std::atan2(end.y, end.x); + sc.goal_align = std::cos(wrapAngle(end_bearing - carrot_bearing_base)); + + // Smoothness: penalize curvature change from previous chosen tentacle. + const double k_max = (params_.min_turn_radius_m > 1e-6) + ? (1.0 / params_.min_turn_radius_m) : 1.0; + const double dk_norm = std::clamp(std::fabs(t.curvature - fsm_.prev_curvature) / (2.0 * k_max), + 0.0, 1.0); + sc.smoothness = 1.0 - dk_norm; + + const double clear_norm = (t.length > 1e-6) ? (clearance / t.length) : 0.0; + const double reverse_pen = (t.direction < 0.0) ? params_.w_reverse : 0.0; + + sc.total = params_.w_clear * clear_norm + + params_.w_goal * sc.goal_align + + params_.w_smooth * sc.smoothness + - reverse_pen; + + // Colliding tentacles get a very low score regardless of other terms, so they + // never "win" a pool if any non-colliding option exists. + if (sc.collides) { + sc.total -= 100.0; + } + return sc; +} + +// --------------------------------------------------------------------------- +// FSM +// --------------------------------------------------------------------------- + +void VectorFieldPlannerAlgo::stepFsm(const TentacleScore& best_fwd, + const TentacleScore& best_rev, + double approach_vel, + PlannerResult& res) +{ + const double fwd_clear = best_fwd.collides ? 0.0 : best_fwd.clearance; + const double rev_clear = best_rev.collides ? 0.0 : best_rev.clearance; + const double any_clear = std::max(fwd_clear, rev_clear); + + res.best_forward_clearance = fwd_clear; + res.best_reverse_clearance = rev_clear; + res.best_forward_idx = best_fwd.idx; + res.best_reverse_idx = best_rev.idx; + + const NavState prev_state = fsm_.state; + + // ---- transitions ---- + NavState next = fsm_.state; + switch (fsm_.state) { + case NavState::NAVIGATE: + if (fwd_clear >= params_.r_slow_m) next = NavState::NAVIGATE; + else if (fwd_clear >= params_.r_stop_m) next = NavState::SLOW; + else next = NavState::PROBE; + break; + + case NavState::SLOW: + if (fwd_clear >= params_.r_slow_m) next = NavState::NAVIGATE; + else if (fwd_clear < params_.r_stop_m) next = NavState::PROBE; + else next = NavState::SLOW; + break; + + case NavState::PROBE: + if (fwd_clear >= params_.r_stop_m) { + next = (fwd_clear >= params_.r_slow_m) ? NavState::NAVIGATE : NavState::SLOW; + } else if (rev_clear >= params_.r_stop_m + && fsm_.escape_attempts < params_.max_escape_attempts) { + next = NavState::ESCAPE; + } else { + next = NavState::REPLAN; + } + break; + + case NavState::ESCAPE: { + // Re-enter NAVIGATE as soon as a clean forward arc opens up. + if (fwd_clear >= params_.r_slow_m) { + next = NavState::NAVIGATE; + } else { + const double elapsed = fsm_.ticks_in_state * params_.tick_period_s; + const bool improving = + (any_clear > fsm_.best_clearance_seen_in_state + 1e-3); + if (elapsed > params_.t_escape_max_s && !improving) { + next = NavState::REPLAN; + } else if (rev_clear < params_.r_stop_m) { + // Lost our reverse option mid-escape — re-triage. + next = NavState::PROBE; + } else { + next = NavState::ESCAPE; + } + } + break; + } + + case NavState::REPLAN: + // Sticky until setPath() resets the FSM. + next = NavState::REPLAN; + break; + } + + // ---- state entry bookkeeping ---- + if (next != prev_state) { + fsm_.state = next; + fsm_.ticks_in_state = 0; + fsm_.best_clearance_seen_in_state = any_clear; + if (next == NavState::ESCAPE) ++fsm_.escape_attempts; + } else { + ++fsm_.ticks_in_state; + if (any_clear > fsm_.best_clearance_seen_in_state) { + fsm_.best_clearance_seen_in_state = any_clear; + } + } + + // ---- actions ---- + switch (fsm_.state) { + case NavState::NAVIGATE: { + const double v = std::min(params_.max_speed_mps, approach_vel); + applyTentacleCommand(best_fwd, v, res); + break; + } + case NavState::SLOW: { + const double band = std::max(1e-6, params_.r_slow_m - params_.r_stop_m); + const double scale = std::clamp((fwd_clear - params_.r_stop_m) / band, 0.0, 1.0); + const double v = std::min(params_.max_speed_mps, approach_vel) * scale; + applyTentacleCommand(best_fwd, v, res); + break; + } + case NavState::PROBE: { + res.linear_vel = 0.0; + res.angular_vel = 0.0; + break; + } + case NavState::ESCAPE: { + // Negative linear vel; ω = κ * v handles Ackermann reverse correctly. + applyTentacleCommand(best_rev, -std::fabs(params_.v_escape_mps), res); + break; + } + case NavState::REPLAN: { + res.linear_vel = 0.0; + res.angular_vel = 0.0; + res.request_replan = true; + break; + } + } + + res.nav_state = fsm_.state; +} + +void VectorFieldPlannerAlgo::applyTentacleCommand(const TentacleScore& sc, double linear_vel, + PlannerResult& res) +{ + if (sc.idx < 0 || sc.idx >= static_cast(tentacles_.size())) { + res.linear_vel = 0.0; + res.angular_vel = 0.0; + return; + } + const auto& t = tentacles_[sc.idx]; + res.linear_vel = linear_vel; + res.angular_vel = linear_vel * t.curvature; // ω = v * κ (Ackermann, signed v) + res.chosen_tentacle_idx = sc.idx; + res.chosen_curvature = t.curvature; + res.chosen_direction = t.direction; + res.chosen_clearance = sc.clearance; + fsm_.prev_curvature = t.curvature; +} + +// --------------------------------------------------------------------------- +// Main compute +// --------------------------------------------------------------------------- + +PlannerResult VectorFieldPlannerAlgo::compute(double rx, double ry, double yaw, + double current_speed) +{ + PlannerResult res; + res.closest_r = -1.0; + + 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; + return res; + } + + // Path-following intermediates. + res.closest_idx = findClosestIndex(rx, ry); + res.effective_lookahead_dist = effectiveLookaheadDist(current_speed); + + 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 cyaw = std::cos(yaw), syaw = std::sin(yaw); + const double carrot_dx = lk.x - rx; + const double carrot_dy = lk.y - ry; + res.lookahead_behind = (carrot_dx * cyaw + carrot_dy * syaw < 0.0); + res.heading_err = wrapAngle(std::atan2(carrot_dy, carrot_dx) - yaw); + + // Approach velocity (used by both pure-pursuit and tentacle paths). + 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); + + // ------------------------------------------------------------------------- + // Avoidance OFF → pure-pursuit fallback (legacy behaviour preserved). + // ------------------------------------------------------------------------- + if (!params_.obstacle_avoidance_enabled) { + 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); + res.clamped = std::fabs(res.steering_unclamped) > params_.max_steering_angle_rad; + res.linear_vel = std::min(params_.max_speed_mps, approach_vel); + res.nav_state = NavState::NAVIGATE; + return res; + } + + // ------------------------------------------------------------------------- + // Tentacle planner + // ------------------------------------------------------------------------- + ensureTentaclesBuilt(); + buildLocalGrid(rx, ry, yaw); + + // Carrot bearing in base frame. + const double cb_x = cyaw * carrot_dx + syaw * carrot_dy; + const double cb_y = -syaw * carrot_dx + cyaw * carrot_dy; + const double carrot_bearing_base = std::atan2(cb_y, cb_x); + + TentacleScore best_fwd, best_rev; + for (std::size_t i = 0; i < tentacles_.size(); ++i) { + const auto sc = scoreTentacle(tentacles_[i], static_cast(i), carrot_bearing_base); + if (tentacles_[i].direction > 0.0) { + if (sc.total > best_fwd.total) best_fwd = sc; + } else { + if (sc.total > best_rev.total) best_rev = sc; + } + } + + // Diagnostics (raw obstacle distance — independent of grid dilation). + const double nearest = nearestObstacleDistBase(rx, ry, yaw); + res.closest_r = std::isinf(nearest) ? -1.0 : nearest; + res.active_points = static_cast(obstacles_.size()); + + // FSM picks state + command. + stepFsm(best_fwd, best_rev, approach_vel, res); + + // Hard safety stop — independent of FSM, last word on forward motion. + if (res.closest_r > 0.0 && res.closest_r < params_.r_stop_hard_m && res.linear_vel > 0.0) { + res.linear_vel = 0.0; + res.angular_vel = 0.0; + } + + return res; +} + +} // 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 new file mode 100644 index 00000000..9ceff685 --- /dev/null +++ b/src/subsystems/navigation/vector_field_planner/src/vector_field_planner_node.cpp @@ -0,0 +1,695 @@ +// 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 with optional obstacle avoidance. + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.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 "std_msgs/msg/header.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "msgs/msg/local_planner_stuck.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/exceptions.h" + +#include "vector_field_planner/vector_field_planner_algo.hpp" + +struct StampedObstaclePoint +{ + double x, y; // map frame + rclcpp::Time stamp; +}; + +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); + + // Operator-facing knobs only. Algorithm internals (grid resolution, + // tentacle count, scoring weights, FSM thresholds) live as defaults + // in PlannerParams / constants in the algo. + declare_parameter("max_speed_mps", 1.5); + declare_parameter("max_steering_angle_rad", 0.5); // pure-pursuit fallback + declare_parameter("min_turn_radius_m", 1.0); // tentacle kinematics + declare_parameter("lookahead_dist_m", 3.0); + declare_parameter("k_p_steering", 1.5); // pure-pursuit fallback + declare_parameter("goal_tolerance_m", 1.5); + declare_parameter("min_approach_linear_velocity", 0.3); + declare_parameter("robot_radius_m", 0.35); + + declare_parameter("obstacle_avoidance_enabled", false); + declare_parameter("scan_topic", std::string("/scan")); + declare_parameter("scan_max_age_s", 0.5); + declare_parameter("obstacle_memory_time_s", 3.0); + declare_parameter("obstacle_max_points", 2000); + 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(); + + vector_field_planner::PlannerParams p; // start from defaults + 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.min_turn_radius_m = get_parameter("min_turn_radius_m").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.goal_tolerance_m = get_parameter("goal_tolerance_m").as_double(); + p.min_approach_linear_velocity = get_parameter("min_approach_linear_velocity").as_double(); + p.robot_radius_m = get_parameter("robot_radius_m").as_double(); + p.obstacle_avoidance_enabled = get_parameter("obstacle_avoidance_enabled").as_bool(); + + // Derive safety bands from robot_radius so they scale with the rover. + p.r_stop_hard_m = p.robot_radius_m + 0.20; + p.r_stop_m = p.robot_radius_m + 0.35; + p.r_slow_m = p.robot_radius_m + 1.00; + // Tentacle horizon tracks lookahead. + p.tentacle_length_forward_m = p.lookahead_dist_m; + p.tentacle_length_reverse_m = 0.5 * p.lookahead_dist_m; + // Scan buffer covers the local grid plus a small margin. + p.scan_buffer_max_dist_m = 0.5 * p.local_grid_size_m; + + 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(); + scan_topic_ = get_parameter("scan_topic").as_string(); + scan_max_age_s_ = get_parameter("scan_max_age_s").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); + + // Stuck/replan signal — consumed by mission_executive (and, in the future, + // the new waypoint manager / web GUI). Reliable QoS + transient_local so + // a late subscriber (operator dashboard, restarted mission_executive) + // receives the last value on connect. See LocalPlannerStuck.msg for the + // protocol; this node is the sole publisher. + stuck_pub_ = create_publisher( + "/local_planner/stuck", + rclcpp::QoS(1).reliable().transient_local()); + + 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; + obstacle_map_.clear(); + // setPath() inside the algo resets the FSM; mirror that here so the + // edge detector emits a clean "stuck=false" if we were mid-REPLAN. + prev_nav_state_ = vector_field_planner::NavState::NAVIGATE; + ticks_since_last_stuck_pub_ = 0; + + std::vector algo_path; + algo_path.reserve(msg->poses.size()); + + double total_len = 0.0; + double max_gap = 0.0; + 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.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 — consider reducing path_resolution_m " + "or increasing 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; + last_cmd_linear_vel_ = 0.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) { + 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]", + p.obstacle_avoidance_enabled ? "ENABLED" : "DISABLED"); + } + }); + + scan_sub_ = create_subscription( + scan_topic_, rclcpp::SensorDataQoS(), + [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) { + ++scan_msg_count_; + 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; + 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(p.scan_buffer_max_dist_m)); + } + latest_scan_ = msg; + + if (!p.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) >= p.scan_buffer_max_dist_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}); + } + + 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); + } + }); + + timer_ = create_wall_timer( + std::chrono::milliseconds(50), + [this]() { controlLoop(); }); + + const auto current_params = algo_.getParams(); + RCLCPP_INFO(get_logger(), + "VectorFieldPlanner ready " + "speed=%.2f steer_max=%.3f lookahead=%.1fm kp=%.2f goal_tol=%.1fm", + 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' scan_buffer=%.2fm" + " r_stop_hard=%.2fm r_stop=%.2fm r_slow=%.2fm robot_r=%.2fm" + " memory_time=%.1fs max_points=%d — publish true/false to /obstacle_avoidance_enabled to toggle", + static_cast(current_params.obstacle_avoidance_enabled), + scan_topic_.c_str(), + current_params.scan_buffer_max_dist_m, + current_params.r_stop_hard_m, current_params.r_stop_m, current_params.r_slow_m, + current_params.robot_radius_m, + obstacle_memory_time_s_, obstacle_max_points_); + } + +private: + void controlLoop() + { + if (!nav_enabled_ || !path_ || path_->poses.empty()) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, + "[NAV_IDLE] waiting — nav_enabled=%d has_path=%d", + static_cast(nav_enabled_), static_cast(path_ && !path_->poses.empty())); + return; + } + + ++tick_count_; + + 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; + + 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)); + + const rclcpp::Time now_time = now(); + obstacle_map_.erase( + std::remove_if(obstacle_map_.begin(), obstacle_map_.end(), + [&](const StampedObstaclePoint & p) { + return (now_time - p.stamp).seconds() > obstacle_memory_time_s_; + }), + obstacle_map_.end()); + + const auto p = algo_.getParams(); + + if (p.obstacle_avoidance_enabled) { + if (!latest_scan_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "[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", scan_age, scan_max_age_s_); + } + } + } else { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 10000, + "[AVOIDANCE OFF] publish 'true' to /obstacle_avoidance_enabled to activate"); + } + + 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, last_cmd_linear_vel_); + + 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_THROTTLE(get_logger(), *get_clock(), 2000, + "[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_THROTTLE(get_logger(), *get_clock(), 2000, + "[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 && latest_scan_) { + const double scan_age = (now_time - latest_scan_->header.stamp).seconds(); + // Only worth a line when there is actually something to avoid — otherwise + // the 1 Hz [NAV] heartbeat already covers the nominal case. + if (res.active_points > 0) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, + "[AVOIDANCE %s] closest=%.2fm in_range=%d fwd_clear=%.2fm rev_clear=%.2fm " + "tent=%d kappa=%.2f dir=%+.0f (buffered=%zu age=%.2fs)", + navStateStr(res.nav_state), res.closest_r, res.active_points, + res.best_forward_clearance, res.best_reverse_clearance, + res.chosen_tentacle_idx, res.chosen_curvature, res.chosen_direction, + obstacle_map_.size(), scan_age); + } + + if (res.request_replan) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "[REPLAN_REQUEST] local planner stuck — fwd_clear=%.2fm rev_clear=%.2fm " + "(mission_executive integration pending)", + res.best_forward_clearance, res.best_reverse_clearance); + } + } + + if (res.clamped) { + ++consecutive_clamped_; + if (consecutive_clamped_ == 40) { + RCLCPP_WARN(get_logger(), + "[STEER_SAT] pure-pursuit steering saturated for %u consecutive ticks (%.1f s). " + "err=%.1fd unclamped=%.3f max=%.3f.", + consecutive_clamped_, consecutive_clamped_ / 20.0, + res.heading_err * 180.0 / M_PI, + res.steering_unclamped, p.max_steering_angle_rad); + } + } else { + consecutive_clamped_ = 0; + } + + // Heartbeat telemetry — one line at 1 Hz. Leads with the FSM state and the + // numbers an operator watches in the field (progress to goal, heading error, + // and the actual command being sent). Raw lookahead coordinates and other + // internals stay out; flags surface only when something is off-nominal. + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, + "[NAV t=%u %s] goal=%.2fm idx=%zu/%zu err=%+.0fd lin=%.2f steer=%+.3f" + " pos=(%.2f,%.2f) yaw=%.0fd%s%s%s", + tick_count_, navStateStr(res.nav_state), + dist_to_goal, res.closest_idx, path_->poses.size() - 1, + res.heading_err * 180.0 / M_PI, + res.linear_vel, res.angular_vel, + rx, ry, yaw * 180.0 / M_PI, + res.clamped ? " CLAMPED" : "", + res.lookahead_behind ? " LK_BEHIND" : "", + p.obstacle_avoidance_enabled ? " AVOID_ON" : ""); + + geometry_msgs::msg::TwistStamped cmd; + cmd.header.stamp = now(); + cmd.header.frame_id = base_frame_; + 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; + + // Drive the stuck/replan topic. Lifecycle: + // - rising edge into REPLAN → publish stuck=true once + snapshot detour + // - while in REPLAN → republish stuck=true at ~1 Hz heartbeat + // - falling edge out of REPLAN → publish stuck=false once + publishStuckIfNeeded(rx, ry, yaw, res); + + if (publish_debug_markers_) { + publishDebugMarkers(rx, ry, res.lookahead_x, res.lookahead_y); + } + } + + // ----------------------------------------------------------------------- + // Stuck publisher + // + // Future waypoint manager / mission executive will subscribe to + // `/local_planner/stuck` and react by inserting a detour goal, marking the + // current waypoint as failed after N attempts, etc. See LocalPlannerStuck.msg + // for the contract. This node is the sole publisher and should treat the + // topic as write-only. + // ----------------------------------------------------------------------- + void publishStuckIfNeeded(double rx, double ry, double yaw, + const vector_field_planner::PlannerResult& res) + { + using ::vector_field_planner::NavState; + const bool now_stuck = (res.nav_state == NavState::REPLAN); + const bool was_stuck = (prev_nav_state_ == NavState::REPLAN); + const bool rising_edge = now_stuck && !was_stuck; + const bool falling_edge = !now_stuck && was_stuck; + + // 1 Hz heartbeat while stuck; derive tick count from the algo's tick + // period so a control-loop rate change doesn't silently break cadence. + const auto p = algo_.getParams(); + const unsigned int heartbeat_ticks = std::max( + 1u, static_cast(std::round(1.0 / std::max(1e-3, p.tick_period_s)))); + ++ticks_since_last_stuck_pub_; + + const bool heartbeat_due = now_stuck && (ticks_since_last_stuck_pub_ >= heartbeat_ticks); + + if (!(rising_edge || falling_edge || heartbeat_due)) { + prev_nav_state_ = res.nav_state; + return; + } + + msgs::msg::LocalPlannerStuck msg; + msg.header.stamp = now(); + msg.header.frame_id = map_frame_; + msg.stuck = now_stuck; + + msg.stuck_pose.header = msg.header; + if (rising_edge) { + // Freeze the entry pose for the duration of this REPLAN cycle. + stuck_entry_rx_ = rx; + stuck_entry_ry_ = ry; + } + msg.stuck_pose.pose.position.x = stuck_entry_rx_; + msg.stuck_pose.pose.position.y = stuck_entry_ry_; + msg.stuck_pose.pose.orientation.w = 1.0; + + msg.best_forward_clearance_m = res.best_forward_clearance; + msg.best_reverse_clearance_m = res.best_reverse_clearance; + + // Derive suggested detour from the better of (forward, reverse) tentacles. + // Endpoint of the chosen tentacle is in base frame; transform to map. + const auto& tents = algo_.tentacles(); + const bool prefer_fwd = res.best_forward_clearance >= res.best_reverse_clearance; + const int idx = prefer_fwd ? res.best_forward_idx : res.best_reverse_idx; + + geometry_msgs::msg::PoseStamped detour; + detour.header = msg.header; + detour.pose.orientation.w = 1.0; + + if (idx >= 0 && idx < static_cast(tents.size()) && !tents[idx].samples.empty()) { + const auto& end_b = tents[idx].samples.back(); + const double mag = std::hypot(end_b.x, end_b.y); + if (mag > 1e-6) { + // Clamp suggested distance to the measured clearance — never suggest a + // detour past terrain we couldn't actually see along that arc. + const double clearance = prefer_fwd ? res.best_forward_clearance + : res.best_reverse_clearance; + const double detour_max = 0.4 * p.local_grid_size_m; + const double detour_dist = std::min(detour_max, clearance); + const double ux_b = end_b.x / mag; + const double uy_b = end_b.y / mag; + const double dx_b = ux_b * detour_dist; + const double dy_b = uy_b * detour_dist; + const double c = std::cos(yaw), s = std::sin(yaw); + detour.pose.position.x = rx + c * dx_b - s * dy_b; + detour.pose.position.y = ry + s * dx_b + c * dy_b; + } else { + detour.pose.position.x = rx; + detour.pose.position.y = ry; + } + } else { + // No tentacle had a usable direction — fall back to current pose so + // consumers can still parse the message but treat the suggestion as + // advisory (best_*_clearance_m will both be ~0). + detour.pose.position.x = rx; + detour.pose.position.y = ry; + } + msg.suggested_detour = detour; + + stuck_pub_->publish(msg); + ticks_since_last_stuck_pub_ = 0; + + if (rising_edge) { + RCLCPP_WARN(get_logger(), + "[STUCK_PUB] entered REPLAN at (%.2f,%.2f) fwd_clear=%.2fm rev_clear=%.2fm " + "suggested_detour=(%.2f,%.2f)", + rx, ry, res.best_forward_clearance, res.best_reverse_clearance, + detour.pose.position.x, detour.pose.position.y); + } else if (falling_edge) { + RCLCPP_INFO(get_logger(), + "[STUCK_PUB] cleared REPLAN at (%.2f,%.2f) — consumers may resume original goal", + rx, ry); + } + + prev_nav_state_ = res.nav_state; + } + + static const char* navStateStr(::vector_field_planner::NavState s) + { + using ::vector_field_planner::NavState; + switch (s) { + case NavState::NAVIGATE: return "NAVIGATE"; + case NavState::SLOW: return "SLOW"; + case NavState::PROBE: return "PROBE"; + case NavState::ESCAPE: return "ESCAPE"; + case NavState::REPLAN: return "REPLAN"; + } + return "?"; + } + + 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); + } + + void publishDebugMarkers(double rx, double ry, double lx, double ly) + { + visualization_msgs::msg::MarkerArray arr; + const auto stamp = now(); + + 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); + + 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); + } + + std::string map_frame_; + std::string base_frame_; + std::string cmd_vel_topic_; + double tf_timeout_s_; + double obstacle_memory_time_s_; + int obstacle_max_points_; + std::string scan_topic_; + double scan_max_age_s_; + bool publish_debug_markers_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Publisher::SharedPtr cmd_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Publisher::SharedPtr stuck_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_; + + 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}; + double last_cmd_linear_vel_{0.0}; + std::vector obstacle_map_; + vector_field_planner::VectorFieldPlannerAlgo algo_; + + // Stuck publisher state. prev_nav_state_ drives edge detection; + // ticks_since_last_stuck_pub_ throttles the heartbeat to ~1 Hz. + vector_field_planner::NavState prev_nav_state_{vector_field_planner::NavState::NAVIGATE}; + unsigned int ticks_since_last_stuck_pub_{0}; + double stuck_entry_rx_{0.0}; + double stuck_entry_ry_{0.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/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..dbcc8a4a --- /dev/null +++ b/src/subsystems/navigation/vector_field_planner/test/test_vector_field_planner_algo.cpp @@ -0,0 +1,354 @@ +// 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; +} + +// Dense rectangular wall (1 cm spacing) in map frame. +static std::vector wall(double x, double y_min, double y_max, double step = 0.05) +{ + std::vector obs; + for (double y = y_min; y <= y_max + 1e-9; y += step) obs.push_back({x, y}); + return obs; +} + +// Default avoidance-enabled params with sensible defaults for tests. +static PlannerParams avoidanceParams() +{ + PlannerParams p; + p.obstacle_avoidance_enabled = true; + p.max_speed_mps = 1.0; + p.lookahead_dist_m = 3.0; + p.goal_tolerance_m = 0.5; + p.min_approach_linear_velocity = 0.3; + p.robot_radius_m = 0.30; + p.min_turn_radius_m = 1.0; + p.local_grid_size_m = 8.0; + p.local_grid_resolution_m = 0.1; + p.inflate_margin_m = 0.10; + p.tentacle_length_forward_m = 3.0; + p.tentacle_length_reverse_m = 1.5; + p.tentacle_sample_step_m = 0.1; + p.num_tentacles_per_side = 6; + p.r_stop_hard_m = 0.45; + p.r_stop_m = 0.65; + p.r_slow_m = 1.30; + p.v_escape_mps = 0.4; + p.t_escape_max_s = 1.0; // short for tests + p.max_escape_attempts = 3; + p.tick_period_s = 0.05; + return p; +} + +// --------------------------------------------------------------------------- +// Pure-pursuit fallback (avoidance disabled) — regression coverage +// --------------------------------------------------------------------------- + +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)); + + 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; + p.min_approach_linear_velocity = 0.5; + p.goal_tolerance_m = 0.2; + + auto algo = makeAlgo(p); + 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); + EXPECT_NEAR(res.linear_vel, 1.25, 1e-6); +} + +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)); + + 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(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); + 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); + const double actual_dist = std::hypot(res.lookahead_x, res.lookahead_y); + EXPECT_NEAR(actual_dist, res.effective_lookahead_dist, 1e-6); +} + +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, StraightAheadZeroSteeringPureP) +{ + 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)); + + 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); +} + +// --------------------------------------------------------------------------- +// Tentacle library shape +// --------------------------------------------------------------------------- + +TEST(Tentacles, BuiltWithBothDirections) +{ + auto algo = makeAlgo(avoidanceParams()); + algo.setPath(straightPath(20.0)); + algo.updateObstacles({}); + + // Trigger build via compute(). + (void)algo.compute(0.0, 0.0, 0.0, 0.0); + + const auto& ts = algo.tentacles(); + const auto p = algo.getParams(); + const int expected = 2 * (2 * p.num_tentacles_per_side + 1); + ASSERT_EQ(static_cast(ts.size()), expected); + + int fwd = 0, rev = 0; + for (const auto& t : ts) { + if (t.direction > 0) ++fwd; else ++rev; + ASSERT_FALSE(t.samples.empty()); + EXPECT_NEAR(t.samples.front().x, 0.0, 1e-9); + EXPECT_NEAR(t.samples.front().y, 0.0, 1e-9); + } + EXPECT_EQ(fwd, expected / 2); + EXPECT_EQ(rev, expected / 2); +} + +TEST(Tentacles, ForwardSampleAtExpectedKinematicEnd) +{ + // A straight forward tentacle (curvature 0) must end at (L, 0). + auto algo = makeAlgo(avoidanceParams()); + algo.setPath(straightPath(20.0)); + algo.updateObstacles({}); + (void)algo.compute(0.0, 0.0, 0.0, 0.0); + + const auto& ts = algo.tentacles(); + const auto p = algo.getParams(); + + // Find the straight forward tentacle. + const Tentacle* straight = nullptr; + for (const auto& t : ts) { + if (t.direction > 0 && std::fabs(t.curvature) < 1e-9) { straight = &t; break; } + } + ASSERT_NE(straight, nullptr); + EXPECT_NEAR(straight->samples.back().x, p.tentacle_length_forward_m, 0.15); + EXPECT_NEAR(straight->samples.back().y, 0.0, 1e-6); +} + +// --------------------------------------------------------------------------- +// Tentacle behaviour: clear, wall, near-stop +// --------------------------------------------------------------------------- + +TEST(Avoidance, PicksStraightTentacleWhenClear) +{ + auto algo = makeAlgo(avoidanceParams()); + algo.setPath(straightPath(20.0)); + algo.updateObstacles({}); + + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(res.nav_state, NavState::NAVIGATE); + EXPECT_NEAR(res.chosen_curvature, 0.0, 1e-9); + EXPECT_GT(res.linear_vel, 0.0); + EXPECT_NEAR(res.angular_vel, 0.0, 1e-9); +} + +TEST(Avoidance, SteersAroundWallInFront) +{ + auto algo = makeAlgo(avoidanceParams()); + algo.setPath(straightPath(20.0)); + // Wall at x=2.0 spanning y ∈ [-2, 2] — blocks the straight tentacle but + // leaves the side arcs clear. + algo.updateObstacles(wall(2.0, -2.0, 2.0)); + + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + // We expect a curved tentacle (non-zero curvature) and forward motion. + EXPECT_GT(res.linear_vel, 0.0); + EXPECT_GT(std::fabs(res.chosen_curvature), 0.05); + EXPECT_EQ(res.chosen_direction, +1.0); +} + +TEST(Avoidance, HardSafetyStopWhenObstacleVeryClose) +{ + auto algo = makeAlgo(avoidanceParams()); + algo.setPath(straightPath(20.0)); + // Obstacle 0.30 m in front — closer than r_stop_hard (=0.45 m). + algo.updateObstacles({{0.30, 0.0}}); + + auto res = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_NEAR(res.linear_vel, 0.0, 1e-9); + EXPECT_NEAR(res.angular_vel, 0.0, 1e-9); + EXPECT_GT(res.closest_r, 0.0); + EXPECT_LT(res.closest_r, avoidanceParams().r_stop_hard_m); +} + +// --------------------------------------------------------------------------- +// FSM: NAVIGATE → PROBE → ESCAPE with reverse availability +// --------------------------------------------------------------------------- + +TEST(Fsm, EntersEscapeWhenForwardBlockedAndReverseClear) +{ + auto algo = makeAlgo(avoidanceParams()); + algo.setPath(straightPath(20.0)); + // Tight half-ring of obstacles in front and to both sides at ~1 m, but + // nothing behind — reverse should remain a viable arc. + std::vector obs; + for (double a = -M_PI / 2.0; a <= M_PI / 2.0 + 1e-6; a += 0.05) { + obs.push_back({1.0 * std::cos(a), 1.0 * std::sin(a)}); + } + algo.updateObstacles(obs); + + // Tick 1: NAVIGATE sees forward blocked → transitions into PROBE. + auto r1 = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(r1.nav_state, NavState::PROBE); + EXPECT_NEAR(r1.linear_vel, 0.0, 1e-9); + + // Tick 2: PROBE sees reverse clear → enters ESCAPE, commands reverse motion. + auto r2 = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(r2.nav_state, NavState::ESCAPE); + EXPECT_LT(r2.linear_vel, 0.0); + EXPECT_EQ(r2.chosen_direction, -1.0); +} + +TEST(Fsm, EscapeTimeoutEntersReplan) +{ + auto p = avoidanceParams(); + p.t_escape_max_s = 0.1; // 2 ticks at 50 ms + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + // Box the rover in on all sides ~0.9 m away (inside r_slow but outside + // r_stop_hard) so PROBE picks ESCAPE but reverse never opens up forward. + std::vector obs; + for (double a = 0.0; a < 2.0 * M_PI; a += 0.04) { + obs.push_back({0.9 * std::cos(a), 0.9 * std::sin(a)}); + } + algo.updateObstacles(obs); + + // Drive ticks until REPLAN. Should happen well within ~20 ticks. + bool saw_replan = false; + for (int i = 0; i < 30; ++i) { + auto r = algo.compute(0.0, 0.0, 0.0, 0.0); + if (r.nav_state == NavState::REPLAN) { + EXPECT_TRUE(r.request_replan); + EXPECT_NEAR(r.linear_vel, 0.0, 1e-9); + saw_replan = true; + break; + } + } + EXPECT_TRUE(saw_replan); +} + +TEST(Fsm, NewPathResetsFsm) +{ + auto p = avoidanceParams(); + p.t_escape_max_s = 0.1; + auto algo = makeAlgo(p); + algo.setPath(straightPath(20.0)); + + std::vector obs; + for (double a = 0.0; a < 2.0 * M_PI; a += 0.04) { + obs.push_back({0.9 * std::cos(a), 0.9 * std::sin(a)}); + } + algo.updateObstacles(obs); + + // Drive into REPLAN. + for (int i = 0; i < 30; ++i) algo.compute(0.0, 0.0, 0.0, 0.0); + ASSERT_EQ(algo.fsm().state, NavState::REPLAN); + + // New path + clear surroundings → FSM should reset, NAVIGATE again. + algo.setPath(straightPath(20.0)); + algo.updateObstacles({}); + auto r = algo.compute(0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(r.nav_state, NavState::NAVIGATE); + EXPECT_FALSE(r.request_replan); + EXPECT_GT(r.linear_vel, 0.0); +} diff --git a/src/subsystems/navigation/yolo_ros_bt/setup.py b/src/subsystems/navigation/yolo_ros_bt/setup.py index 1c9e288b..20e5a857 100644 --- a/src/subsystems/navigation/yolo_ros_bt/setup.py +++ b/src/subsystems/navigation/yolo_ros_bt/setup.py @@ -35,7 +35,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'yolo_node = yolo_ros_bt.yolo_ros_node:main', + 'yolo_ros_node = yolo_ros_bt.yolo_ros_node:main', ], }, ) \ No newline at end of file