diff --git a/CMakeLists.txt b/CMakeLists.txt index 724d594..1e2d66d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,7 +73,7 @@ endif() add_definitions(${PCL_DEFINITIONS}) # add executables -add_executable(mighty src/mighty/mighty_node.cpp src/mighty/mighty.cpp src/hgp/hgp_manager.cpp src/hgp/utils.cpp src/hgp/hgp_planner.cpp src/hgp/graph_search.cpp src/mighty/utils.cpp src/mighty/lbfgs_solver.cpp src/mighty/lbfgs_solver_utils.cpp src/mighty/frontier_detector.cpp src/mighty/frontier_manager.cpp) +add_executable(mighty src/mighty/mighty_node.cpp src/mighty/mighty.cpp src/hgp/hgp_manager.cpp src/hgp/utils.cpp src/hgp/hgp_planner.cpp src/hgp/graph_search.cpp src/hgp/perception_planner.cpp src/mighty/utils.cpp src/mighty/lbfgs_solver.cpp src/mighty/lbfgs_solver_utils.cpp src/mighty/frontier_detector.cpp src/mighty/frontier_manager.cpp) target_link_libraries(mighty ${PCL_LIBRARIES}) ament_target_dependencies(mighty rclcpp example_interfaces dynus_interfaces visualization_msgs Eigen3 decomp_ros_msgs sensor_msgs std_msgs nav_msgs decomp_rviz_plugins decomp_test_node tf2_geometry_msgs tf2 tf2_ros tf2_eigen std_srvs pcl_ros) @@ -171,6 +171,8 @@ install(PROGRAMS scripts/frame_align_publisher.py scripts/formation_viz_node.py scripts/wait_for_tf.py + scripts/diag_grid_test.py + scripts/mid360_fov_viz.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/config/mighty_ground_robot.yaml b/config/mighty_ground_robot.yaml index 33784e5..21e109a 100644 --- a/config/mighty_ground_robot.yaml +++ b/config/mighty_ground_robot.yaml @@ -269,6 +269,7 @@ mighty_node: # --- 2D Ground Robot Planning (not used for UAV) --- use_2d_planning: true # [-] Enable 2D terrain-aware planning for ground robots + perception_aware_planning: false # [-] Route HGP global path through the perception-aware lattice A* (sensor-coverage invariant; see hgp/perception_planner.hpp). Ground-robot 2D only. robot_height: 1.0 # [m] Robot height for obstacle column detection obstacle_min_height: 0.3 # [m] Min column height span to classify as obstacle use_column_any_occupied: true # [-] Any occupied voxel in column → 2D occupied diff --git a/include/hgp/hgp_planner.hpp b/include/hgp/hgp_planner.hpp index 5e31359..c5b9300 100644 --- a/include/hgp/hgp_planner.hpp +++ b/include/hgp/hgp_planner.hpp @@ -14,9 +14,12 @@ #include +#include + #include #include #include +#include // hgp::PerceptionParams, SensorModel, Mid360FOV /** @brief Global path planner using A* and JPS on a 3D voxel map. * @@ -261,6 +264,50 @@ class HGPPlanner { esdf_d_safe_astar_ = d_safe; } + // --- Perception-aware planning (ground robot 2D only) ------------------------ + /** @brief Enable/disable the perception-aware lattice A* and supply the tristate + * belief it plans over. When enabled AND in 2D mode AND a belief is set, plan() + * produces the global path via the sensor-coverage lattice search (see + * hgp/perception_planner.hpp) instead of the plain grid A*. The Mid-360 sensor + * model is constructed lazily on first enable. Pass a null belief (or enabled + * = false) to fall back to the existing grid search. */ + void configurePerceptionAware(bool enabled, std::shared_ptr belief, + const hgp::PerceptionParams& params = hgp::PerceptionParams()) { + perception_aware_ = enabled; + perception_belief_ = std::move(belief); + perception_params_ = params; + if (perception_aware_ && !perception_sensor_) { + perception_sensor_ = std::make_shared(); + } + } + + bool perceptionAwareActive() const { + return perception_aware_ && is_2d_mode_ && perception_belief_ != nullptr; + } + + bool perception_aware_{false}; + std::shared_ptr perception_belief_; + std::shared_ptr perception_sensor_; + hgp::PerceptionParams perception_params_; + + // Telemetry from the last perception-aware plan() that returned true. A behavior + // layer can poll these to distinguish a full solution from a receding-horizon + // partial and to detect "stuck creeping" (repeated small-residual partials). + bool perception_last_partial_{false}; ///< true if the accepted path stopped short of the goal + double perception_last_residual_m_{0.0}; ///< distance [m] from the path end to the goal (0 if reached) + + /** @return true if the last successful perception-aware plan was a partial + * (goal-not-reached) receding-horizon path rather than a full solution. */ + bool perceptionLastPartial() const { return perception_last_partial_; } + /** @return residual distance [m] from the last perception-aware path's end to the + * goal (0 when the goal was reached). Meaningful only after plan() returned true. */ + double perceptionLastResidualToGoal() const { return perception_last_residual_m_; } + + /** @brief Run the perception-aware lattice A* and fill path_/raw_path_. + * @return true on success (a coverage-feasible path to the goal was found). */ + bool planPerceptionAware(const Vecf<3>& start, const Vecf<3>& start_vel, const Vecf<3>& goal, + double& final_g); + /** @brief Configure corridor-center corner snap post-processing (ground robot only). * * At each sharp corner waypoint in the processed path, performs ESDF gradient diff --git a/include/hgp/perception_planner.hpp b/include/hgp/perception_planner.hpp new file mode 100644 index 0000000..b8df494 --- /dev/null +++ b/include/hgp/perception_planner.hpp @@ -0,0 +1,204 @@ +// /* ---------------------------------------------------------------------------- +// * Perception-aware lattice A* (C++ port of the Python prototype). +// * +// * State = (ix, iy, ith, moved) on a 2D grid with n_headings discrete headings. +// * `moved` = 1 iff the state was reached by a forward primitive (so the +// * pose ~L behind along the incoming heading was really occupied by the +// * robot). `moved` = 0 after an in-place turn or at the start. +// * +// * Belief = tristate grid (OccGrid2D): FREE / OCC / UNK. Planning is optimistic: +// * UNK is traversable iff the observation-coverage invariant holds. +// * +// * Sensor = bearing-dependent range window (Mid360FOV for the tilted rover, or a +// * simple annulus+FOV), occluded by known-OCC cells (UNK is optimistically +// * transparent). +// * +// * Hard invariant (per forward primitive p -> p'): every swept cell is known FREE, +// * or UNK and predicted-visible from the primitive start pose p, or UNK and +// * predicted-visible from a virtual previous pose p (-) k*step*dir(theta) (only if +// * moved == 1). Any other case makes the edge INFEASIBLE (not merely expensive). +// * +// * This is a faithful transfer of the prototype; the belief source is the existing +// * tristate OccGrid2D rather than a bespoke BeliefMap. Kept ROS-free and Eigen-free +// * so it is buildable/testable in isolation; the HGP stack converts its pose output +// * to vec_Vecf<3> at the wiring boundary. +// * -------------------------------------------------------------------------- */ +#pragma once + +#include +#include +#include +#include +#include + +#include "mighty/occ_grid_2d.hpp" + +namespace hgp { + +// --------------------------------------------------------------------------- +// Sensor models +// --------------------------------------------------------------------------- + +/** @brief Bearing-dependent range window [r_lo, r_hi] about the heading. */ +class SensorModel { + public: + virtual ~SensorModel() = default; + /** @return true and sets (r_lo, r_hi) if `rel_bearing` (pose-relative, rad) is + * inside the FOV; false otherwise. */ + virtual bool rangeBounds(double rel_bearing, double& r_lo, double& r_hi) const = 0; + virtual double rMax() const = 0; + virtual double fovDeg() const = 0; +}; + +/** @brief Simple annulus [r_min, r_max] x azimuth wedge (prototype SensorModel). */ +class AnnulusSensor : public SensorModel { + public: + AnnulusSensor(double r_min = 1.0, double r_max = 5.0, double fov_deg = 360.0) + : r_min_(r_min), r_max_(r_max), fov_deg_(fov_deg) {} + bool rangeBounds(double rel_bearing, double& r_lo, double& r_hi) const override; + double rMax() const override { return r_max_; } + double fovDeg() const override { return fov_deg_; } + + private: + double r_min_, r_max_, fov_deg_; +}; + +/** @brief Tilted Livox Mid-360 ground-rover FOV (prototype Mid360FOV). + * + * Near boundary = ground-start horn of the pitched elevation band; far boundary + * = elliptical trust envelope ((x-xc)/(F-xc))^2 + (y/W)^2 = 1 in the pose frame. + * Occlusion is handled separately by Visibility. Constants default to the values + * in the prototype and can be overridden from config. */ +class Mid360FOV : public SensorModel { + public: + Mid360FOV(double tilt_deg = 20.0, double e_lo = -7.0, double e_hi = 52.0, + double ground_start_fwd = 1.10, double F = 2.30, double W = 0.315, + double xc = 0.40, double bin_deg = 0.25); + bool rangeBounds(double rel_bearing, double& r_lo, double& r_hi) const override; + double rMax() const override { return r_max_; } + double fovDeg() const override { return fov_deg_; } + + private: + double bin_deg_; + int n_ = 0; + std::vector r_near_; // +inf where the ground horn is absent + std::vector r_far_; + double r_max_ = 0.0; + double fov_deg_ = 0.0; +}; + +// --------------------------------------------------------------------------- +// Parameters +// --------------------------------------------------------------------------- + +struct PerceptionParams { + double res = 0.125; ///< grid resolution [m] (should match belief res) + int n_headings = 12; ///< heading bins (12 -> 30 deg) + double prim_len = 1.25; ///< forward primitive length [m] (>= sensor r_min!) + double robot_radius = 0.25; ///< disc footprint for collision inflation [m] + double turn_cost = 0.35; ///< cost of one in-place heading step [m-equiv] + double w_unknown = 0.20; ///< soft cost per metre of unknown traversed + double goal_tol = 0.55; ///< goal position tolerance [m] + bool use_coverage_rule = true; ///< false -> naive plan-through-unknown baseline + int back_projection_steps = 5; ///< ladder depth for the virtual-previous-pose credit + double back_projection_step = 0.25; ///< ladder spacing [m] + // Resource guards (mirror MIGHTY's grid A*): on hitting either limit, or on the + // open set emptying before the goal, the search returns the partial path to the + // best (closest-to-goal) node reached rather than failing outright. + int max_expand = 10000; ///< max A* expansions; <= 0 means unlimited + int timeout_ms = 1000; ///< wall-clock planning budget [ms]; <= 0 means none +}; + +// --------------------------------------------------------------------------- +// Motion primitives on the heading lattice +// --------------------------------------------------------------------------- + +struct Primitive { + enum Kind { FWD, ARC, TURN }; + Kind kind = FWD; + int dth = 0; ///< heading change in bins + int end_dx = 0, end_dy = 0; ///< integer end cell offset + std::vector> sweep; ///< centerline swept cell offsets (dx, dy) + double cost = 0.0; + double end_dx_m = 0.0, end_dy_m = 0.0, dtheta = 0.0; ///< exact end pose (viz) +}; + +/** @return per start-heading list of primitives (straight, gentle arcs, turns). */ +std::vector> buildPrimitives(const PerceptionParams& P); + +// --------------------------------------------------------------------------- +// Visibility (predicted observation) with memoized ray casting +// --------------------------------------------------------------------------- + +/** @brief Full-width (four 32-bit ints) ray-cache key -- no bit-packing, so + * negative/out-of-bounds coordinates and indices > 65535 can never alias. */ +struct RayKey { + int ix0, iy0, ix1, iy1; + bool operator==(const RayKey& o) const { + return ix0 == o.ix0 && iy0 == o.iy0 && ix1 == o.ix1 && iy1 == o.iy1; + } +}; +struct RayKeyHash { + std::size_t operator()(const RayKey& k) const { + std::size_t h = 1469598103934665603ull; // FNV-1a + auto mix = [&h](int v) { + h = (h ^ static_cast(static_cast(v))) * 1099511628211ull; + }; + mix(k.ix0); + mix(k.iy0); + mix(k.ix1); + mix(k.iy1); + return h; + } +}; + +class Visibility { + public: + Visibility(const OccGrid2D& belief, const SensorModel& sensor) + : m_(belief), s_(sensor) {} + /** Predicted-visible test for cell (cix,ciy) from metric pose (px,py,pth). */ + bool visible(double px, double py, double pth, int cix, int ciy); + + private: + bool rayClear(int ix0, int iy0, int ix1, int iy1); + const OccGrid2D& m_; + const SensorModel& s_; + std::unordered_map ray_cache_; // 0/1 = clear cached false/true +}; + +// --------------------------------------------------------------------------- +// Planner entry +// --------------------------------------------------------------------------- + +/** @brief Why the search stopped -- lets the caller log/monitor (e.g. distinguish + * "hit the expansion cap" from "genuinely unreachable"). */ +enum class StopReason { GOAL, MAX_EXPAND, TIMEOUT, EXHAUSTED, NO_PROGRESS }; +const char* stopReasonStr(StopReason r); + +struct PerceptionPlanResult { + bool ok = false; + bool partial = false; ///< true if `states` is a best-node partial path (goal not reached) + StopReason stop_reason = StopReason::EXHAUSTED; + std::vector> states; ///< dense pose path (x, y, theta) + double cost = 0.0; + int expanded = 0; + int blind_unknown_entries = 0; ///< unknown centerline cells the coverage audit + ///< could not credit (ladder-based; diagnostic only) + std::vector> blind_cells; ///< their world coords (audit) +}; + +/** @brief Perception-aware lattice A*. + * + * @param belief tristate belief (FREE/OCC/UNK). + * @param sensor sensor model used for the coverage invariant. + * @param P planner parameters. + * @param start_x/y/theta start pose (world metres / rad). + * @param goal_x/y goal position (world metres). + * @param audit_sensor optional sensor used only to audit realised coverage of + * the returned path (defaults to `sensor`). */ +PerceptionPlanResult planPerceptionAware(const OccGrid2D& belief, const SensorModel& sensor, + const PerceptionParams& P, double start_x, double start_y, + double start_theta, double goal_x, double goal_y, + const SensorModel* audit_sensor = nullptr); + +} // namespace hgp diff --git a/include/mighty/mighty_type.hpp b/include/mighty/mighty_type.hpp index 00e7388..fcf04fa 100644 --- a/include/mighty/mighty_type.hpp +++ b/include/mighty/mighty_type.hpp @@ -266,6 +266,12 @@ struct parameters { // 2D ground robot planning parameters bool use_2d_planning{false}; // Master toggle for 2D ground planning + // Perception-aware frontier planning (ground robot 2D only). When enabled, the + // HGP global path is produced by the lattice A* with the sensor-coverage + // invariant (see hgp/perception_planner.hpp) instead of the plain grid A*, so + // the path only enters unknown space it could actually have observed. The + // lattice/sensor constants use the tuned defaults in PerceptionParams/Mid360FOV. + bool perception_aware_planning{false}; double robot_height{0.5}; // [m] Robot height for obstacle column detection double obstacle_min_height{0.3}; // [m] Min height span in column to classify as obstacle bool use_column_any_occupied{true}; // [-] Any occupied voxel in column → 2D occupied diff --git a/launch/diag_test.launch.py b/launch/diag_test.launch.py new file mode 100644 index 0000000..40f1d60 --- /dev/null +++ b/launch/diag_test.launch.py @@ -0,0 +1,100 @@ +# /* ---------------------------------------------------------------------------- +# * Standalone diagonal-grid test for the HGP anti-corner-cutting logic. +# * +# * Brings up ONLY the MIGHTY planner (ground-robot / 2D mode), RViz, and the +# * diag_grid_test publisher -- no Gazebo, no fake_sim, no mapper -- so the +# * planned path reacts purely to the synthetic diagonal wall. The robot does not +# * drive (nothing integrates the trajectory); we only watch the planned +# * `hgp_path_marker` update as it routes through / around the wall. +# * +# * Usage: +# * ros2 launch mighty diag_test.launch.py +# * +# * Then in RViz set Fixed Frame = map, add the OccupancyGrid (/NX01/occ_2d_topic) +# * and the MarkerArray (/NX01/hgp_path_marker). Rebuild first so the C++ fix is +# * compiled in (colcon build --packages-select mighty). Compare branch `main` +# * (path cuts through the wall) vs `mad_demo` (path goes around). +# * -------------------------------------------------------------------------- */ +import os +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def launch_setup(context, *args, **kwargs): + namespace = LaunchConfiguration("namespace").perform(context) + use_rviz = LaunchConfiguration("use_rviz").perform(context).lower() in ("true", "1") + + share = get_package_share_directory("mighty") + + # Load the ground-robot planner config and force the 2D / ground-robot path. + cfg_path = os.path.join(share, "config", "mighty_ground_robot.yaml") + with open(cfg_path) as f: + params = yaml.safe_load(f)["mighty_node"]["ros__parameters"] + params["vehicle_type"] = "ground_robot" # zDim_ == 1 -> diagonal check active + params["map_frame_id"] = "map" # single fixed frame for the test + params["sim_env"] = "fake_sim" # avoid gazebo/hardware code paths + + mighty_node = Node( + package="mighty", + executable="mighty", + name="mighty_node", + namespace=namespace, + output="screen", + emulate_tty=True, + parameters=[params], + # We don't feed a point cloud; point the sensor input at an unused topic + # so the ground-robot 2D map comes solely from our occ_2d_topic. + remappings=[("lidar_cloud_in", "unused_lidar_cloud")], + ) + + diag_pub = Node( + package="mighty", + executable="diag_grid_test.py", + name="diag_grid_test", + namespace=namespace, + output="screen", + parameters=[{ + "frame_id": "map", + "base_frame_id": "base_link", # -> /base_link + "publish_state": True, # no fake_sim here, so we own /state + }], + ) + + rviz_cfg = os.path.join(share, "rviz", "mighty_sim_ground_robot.rviz") + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=(["-d", rviz_cfg] if os.path.exists(rviz_cfg) else []), + output="screen", + condition=None, + ) + + # Mid-360 coverage overlay: draws the elliptical far envelope + near/blind ring + # at the robot pose (follows /state). Publishes MarkerArray on /mid360_fov. + fov_viz = Node( + package="mighty", + executable="mid360_fov_viz.py", + name="mid360_fov_viz", + namespace=namespace, + output="screen", + parameters=[{"frame_id": "map", "use_state": True}], + ) + + nodes = [mighty_node, diag_pub, fov_viz] + if use_rviz: + nodes.append(rviz_node) + return nodes + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument("namespace", default_value="NX01"), + DeclareLaunchArgument("use_rviz", default_value="true"), + OpaqueFunction(function=launch_setup), + ]) diff --git a/rviz/mighty_sim_ground_robot.rviz b/rviz/mighty_sim_ground_robot.rviz index 57e6185..42eddf7 100644 --- a/rviz/mighty_sim_ground_robot.rviz +++ b/rviz/mighty_sim_ground_robot.rviz @@ -205,6 +205,18 @@ Visualization Manager: Reliability Policy: Best Effort Value: /NX01/hgp_path_marker Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: NX01 Mid360 FOV + Namespaces: + mid360_fov: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /NX01/mid360_fov + Value: true - Alpha: 0.009999999776482582 BoundColor: 38; 162; 105 Class: decomp_rviz_plugins/PolyhedronArray diff --git a/scripts/diag_grid_test.py b/scripts/diag_grid_test.py new file mode 100755 index 0000000..cf10ceb --- /dev/null +++ b/scripts/diag_grid_test.py @@ -0,0 +1,219 @@ +#!/usr/bin/env python3 +# /* ---------------------------------------------------------------------------- +# * Diagonal-grid test harness for MIGHTY's HGP anti-corner-cutting logic +# * (src/hgp/graph_search.cpp, getSucc()). +# * -------------------------------------------------------------------------- */ +""" +Publishes, in the `map` frame, everything the ground-robot 2D planner needs to +compute a path WITHOUT the full sim (no Gazebo, no fake_sim, no real mapper), so +the diagonal corner-cutting behavior can be seen in isolation in RViz: + + occ_2d_topic nav_msgs/OccupancyGrid A diagonal STAIRCASE wall of occupied + cells. In ground-robot mode MIGHTY + prefers this source (buildMap2DFromOcc2D + -- binary, NO inflation), so the wall + stays exactly one cell wide and the + diagonal "pinholes" between consecutive + occupied cells are preserved. Those + pinholes are what the new check must + refuse to cross. + state dynus_interfaces/State Fixed start pose on one side of the wall + (published only if publish_state:=true; + set false if fake_sim already owns it). + term_goal geometry_msgs/PoseStamped Goal on the far side, so the straight + path wants to cut across the wall. + TF map->base_frame Static transform at the start pose. + +Topics are RELATIVE, so launch this node INTO the planner's namespace (e.g. +NX01) and they resolve to /NX01/occ_2d_topic, /NX01/state, /NX01/term_goal. + +What to watch in RViz (display the OccupancyGrid + the `hgp_path_marker` +MarkerArray, fixed frame = map): + * WITHOUT the fix (branch `main`): the path zig-zags diagonally THROUGH the + staircase corners -- i.e. straight across the wall. + * WITH the fix (branch `mad_demo`): each diagonal whose two orthogonally- + adjacent side cells are occupied is rejected, so the path detours around an + END of the wall. + +Occupancy encoding matches the mapper's occ_2d_topic: 0 = free, 100 = occupied, +-1 = unknown (MIGHTY treats unknown as traversable). +""" +import math + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy + +from nav_msgs.msg import OccupancyGrid +from geometry_msgs.msg import PoseStamped, TransformStamped +from tf2_ros import StaticTransformBroadcaster + +try: + from dynus_interfaces.msg import State + HAVE_STATE = True +except Exception: # pragma: no cover - only when dynus_interfaces isn't sourced + HAVE_STATE = False + + +class DiagGridTest(Node): + def __init__(self): + super().__init__("diag_grid_test") + + # --- parameters (override from the launch or CLI) --- + self.declare_parameter("frame_id", "map") + self.declare_parameter("base_frame_id", "base_link") # relative -> /base_link + self.declare_parameter("resolution", 0.15) # match mighty_map_res + self.declare_parameter("width_cells", 100) + self.declare_parameter("height_cells", 100) + self.declare_parameter("origin_x", -7.5) + self.declare_parameter("origin_y", -7.5) + # Diagonal wall drawn as a 1-cell-wide line between these two world points. + self.declare_parameter("wall_x0", -3.0) + self.declare_parameter("wall_y0", -3.0) + self.declare_parameter("wall_x1", 3.0) + self.declare_parameter("wall_y1", 3.0) + # Start / goal placed on opposite sides of the diagonal so the straight + # path crosses it (default: below-right start, above-left goal). + self.declare_parameter("start_x", 2.5) + self.declare_parameter("start_y", -2.5) + self.declare_parameter("goal_x", -2.5) + self.declare_parameter("goal_y", 2.5) + self.declare_parameter("goal_z", 0.2) + self.declare_parameter("publish_state", True) + + gp = self.get_parameter + self.frame_id = gp("frame_id").value + self.base_frame_id = gp("base_frame_id").value + self.res = float(gp("resolution").value) + self.w = int(gp("width_cells").value) + self.h = int(gp("height_cells").value) + self.ox = float(gp("origin_x").value) + self.oy = float(gp("origin_y").value) + self.start = (float(gp("start_x").value), float(gp("start_y").value)) + self.goal = (float(gp("goal_x").value), float(gp("goal_y").value), float(gp("goal_z").value)) + self.publish_state = bool(gp("publish_state").value) + + self.grid_msg = self._build_grid( + float(gp("wall_x0").value), float(gp("wall_y0").value), + float(gp("wall_x1").value), float(gp("wall_y1").value), + ) + + # Latched QoS (RELIABLE + TRANSIENT_LOCAL) is compatible with any + # subscriber and delivers the map to late joiners; we also republish. + latched = QoSProfile(depth=1) + latched.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL + latched.reliability = QoSReliabilityPolicy.RELIABLE + + self.pub_occ = self.create_publisher(OccupancyGrid, "occ_2d_topic", latched) + self.pub_goal = self.create_publisher(PoseStamped, "term_goal", latched) + if self.publish_state: + if not HAVE_STATE: + raise RuntimeError( + "publish_state:=true but dynus_interfaces/State is not importable -- " + "source your mighty_ws install first." + ) + self.pub_state = self.create_publisher(State, "state", 10) + + self.tf_static = StaticTransformBroadcaster(self) + self._publish_static_tf() + + # Publish once immediately, then keep republishing (occ/goal latched, but + # re-sending is cheap and covers any subscriber that starts later). + self._publish_all() + self.create_timer(0.5, self._publish_all) # occ + goal @ 2 Hz + if self.publish_state: + self.create_timer(0.02, self._publish_state) # state @ 50 Hz + + occ = sum(1 for v in self.grid_msg.data if v == 100) + self.get_logger().info( + f"diag_grid_test: {self.w}x{self.h} @ {self.res}m, {occ} occupied (diagonal wall), " + f"start={self.start} goal={self.goal[:2]} frame={self.frame_id} " + f"publish_state={self.publish_state}" + ) + + def _world_to_cell(self, wx, wy): + cx = int(math.floor((wx - self.ox) / self.res)) + cy = int(math.floor((wy - self.oy) / self.res)) + return cx, cy + + def _build_grid(self, x0, y0, x1, y1): + msg = OccupancyGrid() + msg.header.frame_id = self.frame_id + msg.info.resolution = self.res + msg.info.width = self.w + msg.info.height = self.h + msg.info.origin.position.x = self.ox + msg.info.origin.position.y = self.oy + msg.info.origin.position.z = 0.0 + msg.info.origin.orientation.w = 1.0 + data = [0] * (self.w * self.h) # 0 = free + + # Rasterize the diagonal as a 1-cell-wide line of occupied cells. Sampling + # finely and marking the containing cell yields the classic staircase, so + # consecutive occupied cells touch only at their corners -> the diagonal + # pinholes the corner-cut check is meant to close. + c0 = self._world_to_cell(x0, y0) + c1 = self._world_to_cell(x1, y1) + steps = max(abs(c1[0] - c0[0]), abs(c1[1] - c0[1])) * 4 + 1 + for i in range(steps + 1): + t = i / steps + wx = x0 + t * (x1 - x0) + wy = y0 + t * (y1 - y0) + cx, cy = self._world_to_cell(wx, wy) + if 0 <= cx < self.w and 0 <= cy < self.h: + data[cy * self.w + cx] = 100 # occupied + msg.data = data + return msg + + def _stamp(self): + return self.get_clock().now().to_msg() + + def _publish_static_tf(self): + tf = TransformStamped() + tf.header.stamp = self._stamp() + tf.header.frame_id = self.frame_id + tf.child_frame_id = self.base_frame_id + tf.transform.translation.x = self.start[0] + tf.transform.translation.y = self.start[1] + tf.transform.translation.z = 0.0 + tf.transform.rotation.w = 1.0 + self.tf_static.sendTransform(tf) + + def _publish_all(self): + self.grid_msg.header.stamp = self._stamp() + self.pub_occ.publish(self.grid_msg) + + goal = PoseStamped() + goal.header.stamp = self._stamp() + goal.header.frame_id = self.frame_id + goal.pose.position.x = self.goal[0] + goal.pose.position.y = self.goal[1] + goal.pose.position.z = self.goal[2] + goal.pose.orientation.w = 1.0 + self.pub_goal.publish(goal) + + def _publish_state(self): + st = State() + st.header.stamp = self._stamp() + st.header.frame_id = self.frame_id + st.pos.x = self.start[0] + st.pos.y = self.start[1] + st.pos.z = 0.0 + st.quat.w = 1.0 + self.pub_state.publish(st) + + +def main(): + rclpy.init() + node = DiagGridTest() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/scripts/mid360_fov_viz.py b/scripts/mid360_fov_viz.py new file mode 100644 index 0000000..ec1cf90 --- /dev/null +++ b/scripts/mid360_fov_viz.py @@ -0,0 +1,193 @@ +#!/usr/bin/env python3 +# /* ---------------------------------------------------------------------------- +# * RViz visualization of the tilted Livox Mid-360 ground-coverage envelope used +# * by the perception-aware planner (hgp/perception_planner.hpp Mid360FOV). +# * +# * Draws, at the robot's current pose (from dynus_interfaces/State, else a static +# * pose), a MarkerArray on `mid360_fov`: +# * - FAR envelope : green ring = elliptical trust boundary (~2.30 m). +# * - NEAR/BLIND ring : red ring = ground-start horn (~1.1 m); inside it is the +# * sensor blind spot. +# * - covered sector : translucent green fill between near and far (what the +# * robot can actually observe this instant). +# * - blind sector : translucent red fill from the robot out to the near ring. +# * +# * Reuses the exact prototype Mid360FOV precompute (verified to match the C++: +# * r_max=2.30 m, fov=29.75 deg). Standalone (no dependency on the C++ planner), +# * so it works both in diag_test.launch.py and in the live ground-robot sim. +# * -------------------------------------------------------------------------- */ +import math + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy + +from geometry_msgs.msg import Point +from std_msgs.msg import ColorRGBA +from visualization_msgs.msg import Marker, MarkerArray + +try: + from dynus_interfaces.msg import State + HAVE_STATE = True +except Exception: # pragma: no cover + HAVE_STATE = False + + +def compute_mid360_envelope(tilt_deg=20.0, e_lo=-7.0, e_hi=52.0, ground_start_fwd=1.10, + F=2.30, W=0.315, xc=0.40, bin_deg=0.25): + """Return (bearings_rad, r_near, r_far) over the azimuth bins where both the + ground horn and the ellipse are valid (the FOV wedge). Faithful to the + prototype / C++ Mid360FOV.""" + tau = math.radians(tilt_deg) + h = ground_start_fwd * math.tan(math.radians(-e_lo) + tau) + phi = np.radians(np.arange(-180.0, 180.0, 0.1)) + ele = np.radians(np.arange(e_lo, e_hi + 1e-9, 0.2)) + PHI, ELE = np.meshgrid(phi, ele) + dxw = np.cos(ELE) * np.cos(PHI) * math.cos(tau) + np.sin(ELE) * math.sin(tau) + dyw = np.cos(ELE) * np.sin(PHI) + dzw = -np.cos(ELE) * np.cos(PHI) * math.sin(tau) + np.sin(ELE) * math.cos(tau) + hit = dzw < -1e-9 + t = np.where(hit, h / np.maximum(-dzw, 1e-12), np.nan) + gx, gy = (t * dxw)[hit], (t * dyw)[hit] + brg = np.degrees(np.arctan2(gy, gx)) + r = np.hypot(gx, gy) + edges = np.arange(-180.0, 180.0 + bin_deg, bin_deg) + idx = np.clip(np.digitize(brg, edges) - 1, 0, len(edges) - 2) + rn = np.full(len(edges) - 1, np.inf) + np.minimum.at(rn, idx, r) + bc = np.radians(0.5 * (edges[:-1] + edges[1:])) + a, b = F - xc, W + A, B, C, D = 1 / a**2, 1 / b**2, -2 * xc / a**2, xc**2 / a**2 - 1 + qa = A * np.cos(bc) ** 2 + B * np.sin(bc) ** 2 + qb = C * np.cos(bc) + rf = (-qb + np.sqrt(qb * qb - 4 * qa * D)) / (2 * qa) + okb = np.isfinite(rn) & (rf > rn) + order = np.argsort(bc[okb]) + return bc[okb][order], rn[okb][order], rf[okb][order] + + +class Mid360FovViz(Node): + def __init__(self): + super().__init__("mid360_fov_viz") + self.declare_parameter("frame_id", "map") + self.declare_parameter("z", 0.05) # lay the fans just above ground + self.declare_parameter("use_state", True) # follow the robot via /state + self.declare_parameter("static_x", 0.0) + self.declare_parameter("static_y", 0.0) + self.declare_parameter("static_theta", 0.0) + # Mid-360 constants (defaults match the C++/prototype). + self.declare_parameter("tilt_deg", 20.0) + self.declare_parameter("e_lo", -7.0) + self.declare_parameter("e_hi", 52.0) + self.declare_parameter("ground_start_fwd", 1.10) + self.declare_parameter("F", 2.30) + self.declare_parameter("W", 0.315) + self.declare_parameter("xc", 0.40) + + gp = self.get_parameter + self.frame_id = gp("frame_id").value + self.z = float(gp("z").value) + self.use_state = bool(gp("use_state").value) and HAVE_STATE + self.pose = (float(gp("static_x").value), float(gp("static_y").value), + float(gp("static_theta").value)) + + self.bc, self.rn, self.rf = compute_mid360_envelope( + float(gp("tilt_deg").value), float(gp("e_lo").value), float(gp("e_hi").value), + float(gp("ground_start_fwd").value), float(gp("F").value), float(gp("W").value), + float(gp("xc").value)) + self.get_logger().info( + f"Mid360 FOV: {len(self.bc)} bins, r_near~{self.rn.min():.2f}m, " + f"r_far(max)={self.rf.max():.2f}m, fov={2*math.degrees(abs(self.bc).max()):.1f}deg") + + latched = QoSProfile(depth=1) + latched.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL + latched.reliability = QoSReliabilityPolicy.RELIABLE + self.pub = self.create_publisher(MarkerArray, "mid360_fov", latched) + + if self.use_state: + self.create_subscription(State, "state", self._on_state, 10) + self.create_timer(0.1, self._publish) # 10 Hz + + def _on_state(self, msg): + yaw = 2.0 * math.atan2(msg.quat.z, msg.quat.w) # planar yaw from quaternion + self.pose = (msg.pos.x, msg.pos.y, yaw) + + def _pt(self, x, y, px, py, cth, sth): + p = Point() + p.x = px + x * cth - y * sth + p.y = py + x * sth + y * cth + p.z = self.z + return p + + def _publish(self): + px, py, th = self.pose + cth, sth = math.cos(th), math.sin(th) + near = [(self.rn[i] * math.cos(self.bc[i]), self.rn[i] * math.sin(self.bc[i])) + for i in range(len(self.bc))] + far = [(self.rf[i] * math.cos(self.bc[i]), self.rf[i] * math.sin(self.bc[i])) + for i in range(len(self.bc))] + + arr = MarkerArray() + stamp = self.get_clock().now().to_msg() + + def base(mid, mtype, r, g, b, a, scale): + m = Marker() + m.header.frame_id = self.frame_id + m.header.stamp = stamp + m.ns = "mid360_fov" + m.id = mid + m.type = mtype + m.action = Marker.ADD + m.pose.orientation.w = 1.0 + m.scale.x = m.scale.y = m.scale.z = scale + m.color = ColorRGBA(r=r, g=g, b=b, a=a) + return m + + # covered sector (green fill) between near and far arcs + cov = base(0, Marker.TRIANGLE_LIST, 0.1, 0.8, 0.2, 0.25, 1.0) + for i in range(len(self.bc) - 1): + n0, n1, f0, f1 = near[i], near[i + 1], far[i], far[i + 1] + for (ax, ay), (bx, by), (cx, cy) in ((n0, f0, f1), (n0, f1, n1)): + cov.points += [self._pt(ax, ay, px, py, cth, sth), + self._pt(bx, by, px, py, cth, sth), + self._pt(cx, cy, px, py, cth, sth)] + arr.markers.append(cov) + + # blind sector (red fill) from robot origin out to the near arc + blind = base(1, Marker.TRIANGLE_LIST, 0.9, 0.1, 0.1, 0.30, 1.0) + for i in range(len(self.bc) - 1): + n0, n1 = near[i], near[i + 1] + for (ax, ay), (bx, by), (cx, cy) in (((0.0, 0.0), n0, n1),): + blind.points += [self._pt(ax, ay, px, py, cth, sth), + self._pt(bx, by, px, py, cth, sth), + self._pt(cx, cy, px, py, cth, sth)] + arr.markers.append(blind) + + # far envelope ring (solid green line) + far_line = base(2, Marker.LINE_STRIP, 0.1, 0.9, 0.2, 0.9, 0.04) + far_line.points = [self._pt(x, y, px, py, cth, sth) for (x, y) in far] + arr.markers.append(far_line) + + # near / blind boundary ring (solid red line) + near_line = base(3, Marker.LINE_STRIP, 0.95, 0.15, 0.15, 0.95, 0.04) + near_line.points = [self._pt(x, y, px, py, cth, sth) for (x, y) in near] + arr.markers.append(near_line) + + self.pub.publish(arr) + + +def main(): + rclpy.init() + node = Mid360FovViz() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/hgp/hgp_manager.cpp b/src/hgp/hgp_manager.cpp index 0e003f8..dc71496 100644 --- a/src/hgp/hgp_manager.cpp +++ b/src/hgp/hgp_manager.cpp @@ -286,6 +286,22 @@ bool HGPManager::solveHGP(const Vec3f& start_sent, const Vec3f& start_vel, const // Set collision checking function planner_ptr_->setMapUtil(map_util_for_planning_); + // Perception-aware planning: hand the planner the current tristate belief and + // the enable flag. It only takes effect in 2D ground-robot mode with a belief + // present (perceptionAwareActive()); otherwise this is a no-op and the standard + // grid search runs. The belief is the same occ_2d source the 2D map is built + // from, but tristate (FREE/OCC/UNK) as the coverage invariant needs. + { + const bool pa_enable = par_.perception_aware_planning && is_ground_robot_ && par_.use_2d_planning; + hgp::PerceptionParams pparams; + pparams.res = res_; // = par_.res (planPerceptionAware re-aligns to the belief's own res) + if (par_.drone_bbox.size() >= 2) { + const double bx = par_.drone_bbox[0], by = par_.drone_bbox[1]; + pparams.robot_radius = 0.5 * (bx > by ? bx : by); + } + planner_ptr_->configurePerceptionAware(pa_enable, occ_grid_2d_, pparams); + } + // HGP Plan bool result = false; diff --git a/src/hgp/hgp_planner.cpp b/src/hgp/hgp_planner.cpp index 86f924a..747bc70 100644 --- a/src/hgp/hgp_planner.cpp +++ b/src/hgp/hgp_planner.cpp @@ -346,6 +346,51 @@ bool HGPPlanner::plan(const Vecf<3>& start, const Vecf<3>& start_vel, const Vecf raw_path_.clear(); status_ = 0; + // Perception-aware routing. Once perception-aware planning is ENABLED for this + // (ground, 2D) robot, this path is intentionally fail-STOP and NEVER falls back to + // the plain grid A*/JPS below -- falling back would drive through unobserved space + // and defeat the whole point. So we gate on the *intent* flag (perception_aware_), + // not on readiness: if the coverage search cannot even run (not in 2D mode, or no + // tristate belief published yet) we reject the replan and let the robot hold, and + // if it runs but finds no feasible path we also reject (handled in + // planPerceptionAware). The robot never plans blindly through unknown. + if (perception_aware_) { + if (!is_2d_mode_) { + printf(ANSI_COLOR_RED + "perception-aware enabled but planner is NOT in 2D mode; rejecting replan " + "(fail-stop, NO grid fallback)\n" ANSI_COLOR_RESET); + path_.clear(); + raw_path_.clear(); + status_ = 0; + return false; + } + if (perception_belief_ == nullptr) { + printf(ANSI_COLOR_RED + "perception-aware enabled but no tristate belief available yet; rejecting " + "replan (fail-stop, NO grid fallback). Robot holds until the belief is " + "published.\n" ANSI_COLOR_RESET); + path_.clear(); + raw_path_.clear(); + status_ = 0; + return false; + } + + if (planPerceptionAware(start, start_vel, goal, final_g)) { + return true; + } + + if (planner_verbose_) { + printf(ANSI_COLOR_YELLOW + "perception-aware planning failed; rejecting this replan (no grid fallback by design)\n" + ANSI_COLOR_RESET); + } + + path_.clear(); + raw_path_.clear(); + status_ = 0; + return false; + } + Veci<3> start_int = map_util_->floatToInt(start); // In 2D mode, validate against 2D map instead of 3D map (ground points would block start/goal) @@ -694,3 +739,123 @@ double HGPPlanner::getCheckPathTime() { return hgp_check_path_time_; } double HGPPlanner::getDynamicAstarTime() { return hgp_dynamic_astar_time_; } double HGPPlanner::getRecoverPathTime() { return hgp_recover_path_time_; } + +// --------------------------------------------------------------------------- +// Perception-aware lattice A* routing (ground robot 2D). See +// hgp/perception_planner.hpp for the coverage invariant. Bypasses the LoS/ +// smoothing pipeline: the coverage-feasible lattice path is used directly. +// --------------------------------------------------------------------------- +bool HGPPlanner::planPerceptionAware(const Vecf<3>& start, const Vecf<3>& start_vel, + const Vecf<3>& goal, double& final_g) { + if (!perception_belief_ || !perception_sensor_) return false; + + // Start heading: prefer the velocity direction; else aim from start toward goal. + double heading; + const double vx = start_vel(0), vy = start_vel(1); + if (std::hypot(vx, vy) > 1e-6) { + heading = std::atan2(vy, vx); + } else { + heading = std::atan2(goal(1) - start(1), goal(0) - start(0)); + } + + // Keep the lattice resolution aligned with the belief grid so cell math matches, + // and hand the perception search the same resource guards as the grid A* (so it + // times out / caps expansions and recovers a best-node partial path too). + hgp::PerceptionParams P = perception_params_; + P.res = perception_belief_->resolution(); + P.max_expand = max_expand_; + P.timeout_ms = hgp_timeout_duration_ms_; + + // Reset the partial-plan telemetry each call (queryable via the getters below so a + // behavior layer can detect "stuck creeping" and escalate). + perception_last_partial_ = false; + perception_last_residual_m_ = 0.0; + + hgp::PerceptionPlanResult r = hgp::planPerceptionAware( + *perception_belief_, *perception_sensor_, P, start(0), start(1), heading, goal(0), goal(1)); + + if (!r.ok || r.states.empty()) { + // Always surface WHY the coverage search produced no usable path (stop reason + + // work done), so a rover-side stall is diagnosable without planner_verbose_. + // NO_PROGRESS/EXHAUSTED = no coverage-feasible move existed; MAX_EXPAND/TIMEOUT = + // resource limit hit before any progress (raise perception max_expand/timeout). + printf(ANSI_COLOR_RED + "perception-aware A*: no usable path (stop=%s, expanded=%d, max_expand=%d, " + "timeout=%dms). Replan rejected (fail-stop, no grid fallback by design).\n" + ANSI_COLOR_RESET, + hgp::stopReasonStr(r.stop_reason), r.expanded, P.max_expand, P.timeout_ms); + return false; + } + + // Progress toward the goal (start distance minus the returned path's residual). + const double start_res = std::hypot(goal(0) - start(0), goal(1) - start(1)); + const auto& last_state = r.states.back(); + const double res_to_goal = std::hypot(goal(0) - last_state[0], goal(1) - last_state[1]); + const double progress = start_res - res_to_goal; + + // Progress gate: a best-node PARTIAL path that barely advances (< ~1 cell net) is a + // stall, not a solution -- reject it (fail-stop) so the robot holds/escalates rather + // than inching in place. Full (goal-reached) paths always pass this gate. + if (r.partial && progress < P.res) { + printf(ANSI_COLOR_RED + "perception-aware A*: partial path makes only %.3f m net progress (< %.3f m); " + "treating as stall, rejecting replan (fail-stop).\n" ANSI_COLOR_RESET, + progress, P.res); + return false; + } + perception_last_partial_ = r.partial; + perception_last_residual_m_ = res_to_goal; + // Faithful to the Python prototype: the coverage audit is a DIAGNOSTIC, not a gate. + // The hard invariant enforced inside the lattice search already guarantees every + // swept unknown cell was coverage-feasible; blind_unknown_entries only reports where + // the independent (ladder-free) re-check disagrees with the planner's ladder credit. + // We log it for monitoring rather than rejecting an otherwise-feasible path. + if (r.blind_unknown_entries > 0) { + printf(ANSI_COLOR_YELLOW + "perception-aware audit: %d blind unknown cell(s) on the returned path " + "(diagnostic; path NOT rejected)\n" ANSI_COLOR_RESET, + r.blind_unknown_entries); + } + + // Convert (x, y, theta) poses -> vec_Vecf<3> path at the start's z plane. + const double z = start(2); + raw_path_.clear(); + raw_path_.reserve(r.states.size()); + for (const auto& s : r.states) { + raw_path_.emplace_back(Vec3f(s[0], s[1], z)); + } + // Pin the start exactly. Pin the END to the goal ONLY when the goal was actually + // reached -- for a partial path the last pose is the closest reachable node, and + // faking it to the goal would tell downstream the robot is at the goal when it is + // not (and would create a phantom final segment through unvetted space). + raw_path_.front() = Vec3f(start(0), start(1), z); + if (r.stop_reason == hgp::StopReason::GOAL) { + raw_path_.back() = Vec3f(goal(0), goal(1), z); + } + path_ = raw_path_; + final_g = r.cost; + status_ = 0; + + // Always surface a PARTIAL (goal-not-reached) result with how far short it stops, so + // repeated stuck-creeping is visible. Receding-horizon: the robot advances along the + // partial path and replans from there as the belief grows. + if (r.partial) { + printf(ANSI_COLOR_YELLOW + "perception-aware A*: PARTIAL path (stop=%s), %.3f m short of goal " + "(net progress=%.3f m, expanded=%d).\n" ANSI_COLOR_RESET, + hgp::stopReasonStr(r.stop_reason), res_to_goal, progress, r.expanded); + } + // Always surface a max-expansion cap hit (so it can be monitored for tuning); + // full stop-reason logging under planner_verbose_. + if (r.stop_reason == hgp::StopReason::MAX_EXPAND) { + printf(ANSI_COLOR_YELLOW + "perception-aware A*: hit max_expand=%d (expanded=%d) -> returning %s path " + "(blind_unknown=%d). Consider raising the perception max_expand.\n" ANSI_COLOR_RESET, + P.max_expand, r.expanded, r.partial ? "PARTIAL" : "full", r.blind_unknown_entries); + } else if (planner_verbose_) { + printf("perception-aware A*: stop=%s states=%zu cost=%.3f expanded=%d blind_unknown=%d partial=%d\n", + hgp::stopReasonStr(r.stop_reason), r.states.size(), r.cost, r.expanded, + r.blind_unknown_entries, (int)r.partial); + } + return true; +} diff --git a/src/hgp/perception_planner.cpp b/src/hgp/perception_planner.cpp new file mode 100644 index 0000000..3e97833 --- /dev/null +++ b/src/hgp/perception_planner.cpp @@ -0,0 +1,592 @@ +// /* ---------------------------------------------------------------------------- +// * Perception-aware lattice A* -- C++ port of the Python prototype. +// * See include/hgp/perception_planner.hpp for the model/invariant description. +// * -------------------------------------------------------------------------- */ +#include "hgp/perception_planner.hpp" + +#include +#include +#include +#include +#include +#include + +namespace hgp { + +namespace { +constexpr double kPi = 3.14159265358979323846; +constexpr double kInf = std::numeric_limits::infinity(); + +inline double wrapPi(double a) { + // wrap to (-pi, pi] + double r = std::fmod(a + kPi, 2.0 * kPi); + if (r < 0.0) r += 2.0 * kPi; + return r - kPi; +} +} // namespace + +const char* stopReasonStr(StopReason r) { + switch (r) { + case StopReason::GOAL: return "GOAL"; + case StopReason::MAX_EXPAND: return "MAX_EXPAND"; + case StopReason::TIMEOUT: return "TIMEOUT"; + case StopReason::EXHAUSTED: return "EXHAUSTED"; + case StopReason::NO_PROGRESS: return "NO_PROGRESS"; + } + return "?"; +} + +// =========================================================================== +// AnnulusSensor +// =========================================================================== +bool AnnulusSensor::rangeBounds(double rel_bearing, double& r_lo, double& r_hi) const { + if (fov_deg_ < 359.9) { + const double half = (fov_deg_ * kPi / 180.0) / 2.0; + if (std::abs(wrapPi(rel_bearing)) > half) return false; + } + r_lo = r_min_; + r_hi = r_max_; + return true; +} + +// =========================================================================== +// Mid360FOV -- faithful port of the prototype precompute +// =========================================================================== +Mid360FOV::Mid360FOV(double tilt_deg, double e_lo, double e_hi, double ground_start_fwd, double F, + double W, double xc, double bin_deg) + : bin_deg_(bin_deg) { + const double tau = tilt_deg * kPi / 180.0; + const double h = ground_start_fwd * std::tan((-e_lo) * kPi / 180.0 + tau); + + // Azimuth bins: edges = arange(-180, 180 + bin_deg, bin_deg) -> n = round(360/bin_deg). + n_ = static_cast(std::llround(360.0 / bin_deg_)); + r_near_.assign(n_, kInf); + r_far_.assign(n_, kInf); + + // --- near boundary: min ground-hit range per azimuth bin over the elevation band --- + const double phi_lo = -180.0, phi_hi = 180.0, phi_step = 0.1; // deg, [lo, hi) + const double ele_step = 0.2; // deg, [e_lo, e_hi] + const int n_ele = static_cast(std::floor((e_hi - e_lo) / ele_step + 1e-9)) + 1; + const int n_phi = static_cast(std::llround((phi_hi - phi_lo) / phi_step)); // 3600 + + for (int ie = 0; ie < n_ele; ++ie) { + const double ele = (e_lo + ie * ele_step) * kPi / 180.0; + const double ce = std::cos(ele), se = std::sin(ele); + for (int ip = 0; ip < n_phi; ++ip) { + const double phi = (phi_lo + ip * phi_step) * kPi / 180.0; + const double cp = std::cos(phi), sp = std::sin(phi); + const double dxw = ce * cp * std::cos(tau) + se * std::sin(tau); + const double dyw = ce * sp; + const double dzw = -ce * cp * std::sin(tau) + se * std::cos(tau); + if (dzw >= -1e-9) continue; // ray does not hit the ground plane below + const double t = h / std::max(-dzw, 1e-12); + const double gx = t * dxw, gy = t * dyw; + const double brg_deg = std::atan2(gy, gx) * 180.0 / kPi; // [-180, 180] + const double r = std::hypot(gx, gy); + // digitize(brg, edges) - 1, clipped to [0, n_-1] + int idx = static_cast(std::floor((brg_deg - phi_lo) / bin_deg_)); + if (idx < 0) idx = 0; + if (idx > n_ - 1) idx = n_ - 1; + if (r < r_near_[idx]) r_near_[idx] = r; + } + } + + // --- far boundary: elliptical trust envelope solved per bin-center bearing --- + const double a = F - xc, b = W; + const double A = 1.0 / (a * a), B = 1.0 / (b * b), C = -2.0 * xc / (a * a), + D = xc * xc / (a * a) - 1.0; + for (int i = 0; i < n_; ++i) { + const double bc = (phi_lo + (i + 0.5) * bin_deg_) * kPi / 180.0; // bin center, rad + const double cb = std::cos(bc), sb = std::sin(bc); + const double qa = A * cb * cb + B * sb * sb; + const double qb = C * cb; + const double disc = qb * qb - 4.0 * qa * D; + if (qa > 1e-12 && disc >= 0.0) { + r_far_[i] = (-qb + std::sqrt(disc)) / (2.0 * qa); + } else { + r_far_[i] = kInf; + } + } + + // --- derived: r_max and fov over bins where both boundaries are valid --- + double rmax = 0.0, absmax_deg = 0.0; + for (int i = 0; i < n_; ++i) { + const bool okb = std::isfinite(r_near_[i]) && std::isfinite(r_far_[i]) && (r_far_[i] > r_near_[i]); + if (!okb) continue; + rmax = std::max(rmax, r_far_[i]); + const double bc_deg = phi_lo + (i + 0.5) * bin_deg_; + absmax_deg = std::max(absmax_deg, std::abs(bc_deg)); + } + r_max_ = rmax; + fov_deg_ = 2.0 * absmax_deg; +} + +bool Mid360FOV::rangeBounds(double rel_bearing, double& r_lo, double& r_hi) const { + const double b_deg = wrapPi(rel_bearing) * 180.0 / kPi; // [-180, 180) + int i = static_cast((b_deg + 180.0) / bin_deg_); + if (i < 0) i = 0; + if (i > n_ - 1) i = n_ - 1; + const double lo = r_near_[i], hi = r_far_[i]; + if (!std::isfinite(lo) || hi <= lo) return false; + r_lo = lo; + r_hi = hi; + return true; +} + +// =========================================================================== +// Motion primitives +// =========================================================================== +std::vector> buildPrimitives(const PerceptionParams& P) { + const int NH = P.n_headings; + const double dth_rad = 2.0 * kPi / NH; + std::vector> prims(NH); + const int n_samp = std::max(8, static_cast(std::llround(P.prim_len / (P.res * 0.4)))); + + for (int hh = 0; hh < NH; ++hh) { + const double th0 = hh * dth_rad; + + for (int dth : {0, -1, 1}) { + double ex, ey; + if (dth == 0) { + ex = P.prim_len * std::cos(th0); + ey = P.prim_len * std::sin(th0); + } else { + const double dpsi = dth * dth_rad; + const double R = P.prim_len / std::abs(dpsi); + const double sign = dth > 0 ? 1.0 : -1.0; + const double cx = -R * std::sin(th0) * sign; + const double cy = R * std::cos(th0) * sign; + const double psi = sign * P.prim_len / R; + ex = cx + R * std::sin(th0 + psi) * sign; + ey = cy - R * std::cos(th0 + psi) * sign; + } + const int end_dx = static_cast(std::floor(ex / P.res + 0.5)); + const int end_dy = static_cast(std::floor(ey / P.res + 0.5)); + const double Px = end_dx * P.res, Py = end_dy * P.res; + const double c = std::hypot(Px, Py); + const double alpha = wrapPi(std::atan2(Py, Px) - th0); + + Primitive pr; + pr.kind = (dth == 0) ? Primitive::FWD : Primitive::ARC; + pr.dth = dth; + pr.end_dx = end_dx; + pr.end_dy = end_dy; + + std::vector> sweep; + std::unordered_map seen; + auto push_cell = [&](int cxi, int cyi) { + const uint64_t key = (static_cast(static_cast(cxi)) << 32) | + static_cast(cyi); + if (seen.find(key) == seen.end()) { + seen.emplace(key, 1); + sweep.push_back({cxi, cyi}); + } + }; + + double length; + if (std::abs(alpha) < 1e-9) { + length = c; + for (int k = 1; k <= n_samp; ++k) { + const double u = static_cast(k) / n_samp; + const double x = u * Px, y = u * Py; + push_cell(static_cast(std::floor(x / P.res + 0.5)), + static_cast(std::floor(y / P.res + 0.5))); + } + } else { + const double Rt = c / (2.0 * std::sin(alpha)); // signed radius + length = std::abs(c * alpha / std::sin(alpha)); + for (int k = 1; k <= n_samp; ++k) { + const double u = static_cast(k) / n_samp; + const double th_u = th0 + 2.0 * alpha * u; + const double x = Rt * (std::sin(th_u) - std::sin(th0)); + const double y = -Rt * (std::cos(th_u) - std::cos(th0)); + push_cell(static_cast(std::floor(x / P.res + 0.5)), + static_cast(std::floor(y / P.res + 0.5))); + } + } + + pr.sweep = std::move(sweep); + pr.cost = length + 5e-4 * std::abs(dth); // epsilon tie-break per heading change + pr.end_dx_m = Px; + pr.end_dy_m = Py; + pr.dtheta = dth * dth_rad; + prims[hh].push_back(std::move(pr)); + } + + for (int dth : {-1, 1}) { + Primitive pr; + pr.kind = Primitive::TURN; + pr.dth = dth; + pr.cost = P.turn_cost; + pr.dtheta = dth * dth_rad; + prims[hh].push_back(std::move(pr)); + } + } + return prims; +} + +// =========================================================================== +// Visibility +// =========================================================================== +bool Visibility::rayClear(int ix0, int iy0, int ix1, int iy1) { + const RayKey key{ix0, iy0, ix1, iy1}; + auto it = ray_cache_.find(key); + if (it != ray_cache_.end()) return it->second != 0; + + // Amanatides & Woo grid DDA over the segment between cell centers: visits every + // cell the ray actually crosses (no gaps, unlike fractional sampling). Cell i + // spans [i, i+1) with center i+0.5. Both endpoints (observer cell and target + // cell) are excluded; UNK is transparent (optimism), OOB counts as occupied. + bool ok = true; + if (!(ix0 == ix1 && iy0 == iy1)) { + const double x0 = ix0 + 0.5, y0 = iy0 + 0.5; + const double x1 = ix1 + 0.5, y1 = iy1 + 0.5; + const double dx = x1 - x0, dy = y1 - y0; + + const int stepx = (dx > 0) - (dx < 0); + const int stepy = (dy > 0) - (dy < 0); + // t (in [0,1]) to reach the first cell boundary, and to advance one full cell. + double tMaxX = kInf, tDeltaX = kInf; + if (stepx != 0) { + const double bx = (stepx > 0) ? (ix0 + 1) : ix0; // next x grid line + tMaxX = (bx - x0) / dx; + tDeltaX = std::abs(1.0 / dx); + } + double tMaxY = kInf, tDeltaY = kInf; + if (stepy != 0) { + const double by = (stepy > 0) ? (iy0 + 1) : iy0; + tMaxY = (by - y0) / dy; + tDeltaY = std::abs(1.0 / dy); + } + + int cx = ix0, cy = iy0; + while (true) { + if (tMaxX < tMaxY) { + cx += stepx; + tMaxX += tDeltaX; + } else { + cy += stepy; + tMaxY += tDeltaY; + } + if (cx == ix1 && cy == iy1) break; // reached target cell (endpoint) + if (tMaxX > 1.0 && tMaxY > 1.0) break; // passed the segment end + if (m_.isOccupied(cx, cy)) { // interior cell blocks the ray + ok = false; + break; + } + } + } + ray_cache_.emplace(key, ok ? 1 : 0); + return ok; +} + +bool Visibility::visible(double px, double py, double pth, int cix, int ciy) { + double cx, cy; + m_.gridToWorld(cix, ciy, cx, cy); + const double dx = cx - px, dy = cy - py; + const double d = std::hypot(dx, dy); + double r_lo, r_hi; + if (!s_.rangeBounds(std::atan2(dy, dx) - pth, r_lo, r_hi)) return false; + if (d < r_lo || d > r_hi) return false; + int pix, piy; + m_.worldToGrid(px, py, pix, piy); + return rayClear(pix, piy, cix, ciy); +} + +// =========================================================================== +// A* search +// =========================================================================== +namespace { + +// Open-list entry: min-heap on f, then g (matches the prototype's (f, g, state)). +struct OpenNode { + double f; + double g; + int64_t key; + bool operator>(const OpenNode& o) const { + if (f != o.f) return f > o.f; + return g > o.g; + } +}; + +} // namespace + +PerceptionPlanResult planPerceptionAware(const OccGrid2D& belief, const SensorModel& sensor, + const PerceptionParams& P, double start_x, double start_y, + double start_theta, double goal_x, double goal_y, + const SensorModel* audit_sensor) { + PerceptionPlanResult res; + const int NH = P.n_headings; + const double dth_rad = 2.0 * kPi / NH; + const int W = belief.width(); + const int H = belief.height(); + if (W <= 0 || H <= 0) return res; + + const auto prims = buildPrimitives(P); + Visibility vis(belief, sensor); + Visibility vis_audit(belief, audit_sensor ? *audit_sensor : sensor); + + // --- footprint inflation: cells blocked for the robot center (disc) + borders --- + const int rad = static_cast(std::ceil(P.robot_radius / P.res)); + std::vector blocked(static_cast(W) * H, 0); + for (int y = 0; y < H; ++y) { + for (int x = 0; x < W; ++x) { + if (!belief.isOccupied(x, y)) continue; + for (int dy = -rad; dy <= rad; ++dy) { + for (int dx = -rad; dx <= rad; ++dx) { + if (dx * dx + dy * dy > rad * rad) continue; + const int nx = x + dx, ny = y + dy; + if (nx < 0 || nx >= W || ny < 0 || ny >= H) continue; + blocked[static_cast(ny) * W + nx] = 1; + } + } + } + } + // Borders blocked (footprint would leave the map). + for (int y = 0; y < H; ++y) + for (int x = 0; x < W; ++x) + if (x <= rad || x >= W - rad - 1 || y <= rad || y >= H - rad - 1) + blocked[static_cast(y) * W + x] = 1; + + auto inBounds = [&](int ix, int iy) { return ix >= 0 && ix < W && iy >= 0 && iy < H; }; + auto isBlocked = [&](int ix, int iy) { + return !inBounds(ix, iy) || blocked[static_cast(iy) * W + ix] != 0; + }; + + int six, siy, gix, giy; + belief.worldToGrid(start_x, start_y, six, siy); + belief.worldToGrid(goal_x, goal_y, gix, giy); + (void)gix; + (void)giy; + int sih = static_cast(std::llround(start_theta / dth_rad)) % NH; + if (sih < 0) sih += NH; + + auto keyOf = [&](int ix, int iy, int ih, int mv) -> int64_t { + return (((static_cast(iy) * W + ix) * NH + ih) * 2) + mv; + }; + auto hFn = [&](int ix, int iy) { + double wx, wy; + belief.gridToWorld(ix, iy, wx, wy); + return std::hypot(goal_x - wx, goal_y - wy); + }; + + struct Came { + int64_t prev; + const Primitive* prim; // nullptr at the start + }; + std::unordered_map g; + std::unordered_map came; + std::unordered_map> unpack; // key -> (ix,iy,ih,mv) + + const int64_t start_key = keyOf(six, siy, sih, 0); + g[start_key] = 0.0; + came[start_key] = {-1, nullptr}; + unpack[start_key] = {six, siy, sih, 0}; + + // Best (closest-to-goal) node seen, for partial-path recovery on a resource + // limit or an exhausted open set -- mirrors MIGHTY's grid A* best_node behavior. + const double start_h = hFn(six, siy); + int64_t best_key = start_key; + double best_h = start_h; + + std::priority_queue, std::greater> open; + open.push({start_h, 0.0, start_key}); + + int64_t goal_key = -1; + int expanded = 0; + StopReason stop = StopReason::EXHAUSTED; // default: open set emptied + const auto t_start = std::chrono::steady_clock::now(); + + while (!open.empty()) { + const OpenNode top = open.top(); + open.pop(); + const int64_t st = top.key; + auto git = g.find(st); + if (git == g.end() || top.g > git->second + 1e-9) continue; + const auto s = unpack[st]; + const int ix = s[0], iy = s[1], ih = s[2], mv = s[3]; + ++expanded; + + // Resource guards (match MIGHTY's grid A*): on hitting either limit, stop and + // recover the best partial path found so far (handled after the loop). + if (P.max_expand > 0 && expanded >= P.max_expand) { + stop = StopReason::MAX_EXPAND; + break; + } + if (P.timeout_ms > 0 && + std::chrono::duration_cast( + std::chrono::steady_clock::now() - t_start) + .count() >= P.timeout_ms) { + stop = StopReason::TIMEOUT; + break; + } + + double wx, wy; + belief.gridToWorld(ix, iy, wx, wy); + const double h_cur = std::hypot(goal_x - wx, goal_y - wy); + if (h_cur < best_h) { // track closest-to-goal node for partial recovery + best_h = h_cur; + best_key = st; + } + if (h_cur <= P.goal_tol) { + goal_key = st; + stop = StopReason::GOAL; + break; + } + + const double px = wx, py = wy; + const double pth = ih * dth_rad; + const double gc = top.g; + + for (const Primitive& pr : prims[ih]) { + if (pr.kind == Primitive::TURN) { + const int nih = ((ih + pr.dth) % NH + NH) % NH; + const int64_t nst = keyOf(ix, iy, nih, 0); + const double ng = gc + pr.cost; + auto it = g.find(nst); + if (it == g.end() || ng < it->second - 1e-9) { + g[nst] = ng; + came[nst] = {st, &pr}; + unpack[nst] = {ix, iy, nih, 0}; + open.push({ng + hFn(ix, iy), ng, nst}); + } + continue; + } + + // forward / arc primitive: collision + coverage over the CENTERLINE swept + // cells (matches the Python prototype invariant). OCC is handled for the + // whole disc footprint by the robot_radius map inflation + // (centerline-vs-inflated == footprint-vs-raw occupied); each swept UNKNOWN + // cell must be predicted-observable before traversal. + bool feasible = true; + int unk_cells = 0; + for (const auto& off : pr.sweep) { + const int cix = ix + off[0], ciy = iy + off[1]; + if (isBlocked(cix, ciy)) { + feasible = false; + break; + } + if (!belief.isUnknown(cix, ciy)) continue; + ++unk_cells; + if (!P.use_coverage_rule) continue; + bool ok = vis.visible(px, py, pth, cix, ciy); + if (!ok && mv) { + // Back-projected virtual poses along the incoming heading. Armed after + // any primitive that reached this state with mv==1 (see mv_after), i.e. + // whenever the incoming lattice step is fine enough (<= 45 deg) for the + // straight-history assumption behind this ladder to hold. + for (int k = 1; k <= P.back_projection_steps; ++k) { + const double dlt = P.back_projection_step * k; + const double bx = px - dlt * std::cos(pth); + const double by = py - dlt * std::sin(pth); + if (vis.visible(bx, by, pth, cix, ciy)) { + ok = true; + break; + } + } + } + if (!ok) { + feasible = false; + break; + } + } + if (!feasible) continue; + + const int nih = ((ih + pr.dth) % NH + NH) % NH; + // Faithful to the Python prototype: the back-projection ladder assumes a + // ~straight incoming history along the current heading, which holds within one + // heading bin. Straights always arm it; arcs arm it too as long as the lattice + // step is <= 45 deg (coarser bins would break the straight-history assumption). + const int mv_after = (pr.dth == 0 || dth_rad <= kPi / 4.0 + 1e-9) ? 1 : 0; + const int nix = ix + pr.end_dx, niy = iy + pr.end_dy; + if (!inBounds(nix, niy)) continue; + const int64_t nst = keyOf(nix, niy, nih, mv_after); + const double ng = gc + pr.cost + P.w_unknown * unk_cells * P.res; + auto it = g.find(nst); + if (it == g.end() || ng < it->second - 1e-9) { + g[nst] = ng; + came[nst] = {st, &pr}; + unpack[nst] = {nix, niy, nih, mv_after}; + open.push({ng + hFn(nix, niy), ng, nst}); + } + } + } + + res.expanded = expanded; + + // Terminal node: the goal if reached, else the best (closest-to-goal) node for a + // partial path (MIGHTY best_node behavior). If nothing improved on the start + // (open set exhausted / limit hit with zero progress), report failure so the + // caller can fall back to the guarded grid search. + int64_t terminal = goal_key; + bool partial = false; + if (terminal < 0) { + if (best_key == start_key) { + res.stop_reason = StopReason::NO_PROGRESS; + res.ok = false; + return res; + } + terminal = best_key; + partial = true; + } + res.stop_reason = (goal_key >= 0) ? StopReason::GOAL : stop; // MAX_EXPAND/TIMEOUT/EXHAUSTED + + // --- reconstruct pose path from the terminal node --- + std::vector chain; + for (int64_t st = terminal; st != -1; st = came[st].prev) chain.push_back(st); + std::reverse(chain.begin(), chain.end()); + + for (int64_t st : chain) { + const auto s = unpack[st]; + double wx, wy; + belief.gridToWorld(s[0], s[1], wx, wy); + res.states.push_back({wx, wy, s[2] * dth_rad}); + } + res.cost = g[terminal]; + res.ok = true; + res.partial = partial; + + // --- coverage audit (faithful to the Python prototype) ---------------------- + // Diagnostic re-check of the returned path: walk each forward/arc primitive and, + // for every unknown CENTERLINE swept cell, confirm it was predicted-visible from + // the primitive's start node pose -- or, if that node was reached by motion + // (mv == 1), from one of the back-projected virtual poses along the incoming + // heading (the same ladder the planner uses). Uses vis_audit (the audit sensor if + // one was supplied, else the planner's sensor). Reported, never used to reject. + int blind = 0; + for (size_t i = 1; i < chain.size(); ++i) { + const Came& cm = came[chain[i]]; + if (cm.prim == nullptr || cm.prim->kind == Primitive::TURN) continue; + const auto sprev = unpack[chain[i - 1]]; + const int ix = sprev[0], iy = sprev[1], ih = sprev[2], mv = sprev[3]; + double px, py; + belief.gridToWorld(ix, iy, px, py); + const double pth = ih * dth_rad; + for (const auto& off : cm.prim->sweep) { + const int cix = ix + off[0], ciy = iy + off[1]; + if (!belief.isUnknown(cix, ciy)) continue; + bool ok = vis_audit.visible(px, py, pth, cix, ciy); + if (!ok && mv) { + for (int k = 1; k <= P.back_projection_steps; ++k) { + const double dlt = P.back_projection_step * k; + const double bx = px - dlt * std::cos(pth); + const double by = py - dlt * std::sin(pth); + if (vis_audit.visible(bx, by, pth, cix, ciy)) { + ok = true; + break; + } + } + } + if (!ok) { + ++blind; + double bwx, bwy; + belief.gridToWorld(cix, ciy, bwx, bwy); + res.blind_cells.push_back({bwx, bwy}); + } + } + } + res.blind_unknown_entries = blind; + return res; +} + +} // namespace hgp diff --git a/src/mighty/mighty_node.cpp b/src/mighty/mighty_node.cpp index 4734262..0149ad5 100644 --- a/src/mighty/mighty_node.cpp +++ b/src/mighty/mighty_node.cpp @@ -711,6 +711,7 @@ void MIGHTY_NODE::declareParameters() { // 2D ground robot planning parameters this->declare_parameter("use_2d_planning", false); + this->declare_parameter("perception_aware_planning", false); this->declare_parameter("robot_height", 0.5); this->declare_parameter("obstacle_min_height", 0.3); this->declare_parameter("use_column_any_occupied", true); @@ -1072,6 +1073,7 @@ void MIGHTY_NODE::setParameters() { // 2D ground robot planning parameters par_.use_2d_planning = this->get_parameter("use_2d_planning").as_bool(); + par_.perception_aware_planning = this->get_parameter("perception_aware_planning").as_bool(); par_.robot_height = this->get_parameter("robot_height").as_double(); par_.obstacle_min_height = this->get_parameter("obstacle_min_height").as_double(); par_.use_column_any_occupied = this->get_parameter("use_column_any_occupied").as_bool();