improving linear eval and omni obstac avoid logic

cobot · cobot · commit c52dae77f92e · 2025-12-15T11:00:42.000-06:00
diff --git a/config/navigation.lua b/config/navigation.lua
@@ -23,8 +23,7 @@ NavigationParameters = {
   base_link_offset_y = 0;
   max_free_path_length = 6.0;
   max_clearance = 1.0;
-  local_half_fov = deg2rad(90);
-  center_threshold = deg2rad(10);
+  lidar_fov_half_angle = deg2rad(60);
   can_traverse_stairs = false;
   target_dist_tolerance = 0.1;
   nudge_dist_tolerance = 0.3;
diff --git a/src/navigation/linear_evaluator.cc b/src/navigation/linear_evaluator.cc
@@ -48,10 +48,11 @@ using std::vector;
 using namespace geometry;
 using namespace math_util;
 
-// Cost function weights for path selection
-DEFINE_double(clearance_weight, -0.5, "Weight for obstacle clearance (negative = prefer higher)");
-DEFINE_double(freepath_weight, -1, "Weight for rollout length (negative = prefer longer)");
-DEFINE_double(subopt_tolerance, 1.5, "Max path length multiplier for clearance tradeoff");
+// Cost function weights for safety optimization in pass 2 (negative = prefer higher values)
+// Pass 1 handles goal-reaching (shortest total distance), pass 2 handles safety among near-optimal paths.
+DEFINE_double(clearance_weight, -0.5, "Weight for lateral clearance");
+DEFINE_double(freepath_weight, -1.0, "Weight for free path length");
+DEFINE_double(subopt_tolerance, 1.5, "Max total distance multiplier for constrained optimization");
 
 namespace motion_primitives {
 
@@ -60,18 +61,20 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(const vector<shared_ptr<Pa
 
     // Check line-of-sight (LOS) from each path's endpoint to the goal
     const size_t N = paths.size();
-    vector<float> los_clearance(N, 0.0f);          // clearance along LOS line to goal
     vector<float> remaining_dist(N, FLT_MAX);      // distance from endpoint to goal
     bool any_path_has_los = false;
 
 #pragma omp parallel
     {
         bool local_has_los = false;
-#pragma omp for schedule(runtime)
+        #pragma omp for schedule(runtime)
         for (int i = 0; i < static_cast<int>(N); ++i) {
             const auto endpoint = paths[i]->EndPoint().translation;
-            los_clearance[i] = LOSClearanceToLine(Line2f(endpoint, local_target), *point_cloud);
-            if (los_clearance[i] > 0.0f) {
+            const float los_clearance =
+                LOSClearanceToLine(Line2f(endpoint, local_target), *point_cloud);
+            // Require clearance >= half the robot's smallest dimension for meaningful LOS
+            const float min_los_clearance = 0.5f * std::min(nav_params.robot_width, nav_params.robot_length);
+            if (los_clearance > min_los_clearance) {
                 remaining_dist[i] = (endpoint - local_target).norm();
                 local_has_los = true;
             }
@@ -83,33 +86,37 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(const vector<shared_ptr<Pa
     }
 
     // Pass 1: Find shortest total path to goal
-    // With LOS: total = rollout + remaining distance. Without LOS: total = rollout only.
+    // Requires LOS: total = rollout + remaining distance. If no LOS exists, return no path.
+    if (!any_path_has_los) {
+        printf("No valid path found\n");
+        return nullptr;
+    }
     shared_ptr<PathRolloutBase> best = nullptr;
     float best_total_dist = FLT_MAX;
     for (size_t i = 0; i < paths.size(); ++i) {
         if (paths[i]->Length() <= 0.0f) continue;
-        const float total_dist =
-            any_path_has_los ? (paths[i]->Length() + remaining_dist[i]) : paths[i]->Length();
+        if (remaining_dist[i] == FLT_MAX) continue;
+        const float total_dist = paths[i]->Length() + remaining_dist[i];
         if (total_dist < best_total_dist) {
             best_total_dist = total_dist;
             best = paths[i];
         }
     }
-
     if (best == nullptr) {
         printf("No valid path found\n");
         return nullptr;
     }
 
-    // Pass 2: Among paths within subopt tolerance of shortest, find best clearance/length tradeoff
+    // Pass 2: Among LOS paths within subopt tolerance of shortest, find best clearance/FPL tradeoff
+    // ?? TODO: should this be FPL() or Length() ?
     const float max_allowed_dist = FLAGS_subopt_tolerance * best_total_dist;
-    float best_cost = FLAGS_freepath_weight * best->Length() + FLAGS_clearance_weight * best->Clearance();
+    float best_cost = FLAGS_freepath_weight * best->FPL() + FLAGS_clearance_weight * best->Clearance();
     for (size_t i = 0; i < paths.size(); ++i) {
         if (paths[i]->Length() <= 0.0f) continue;
-        const float total_dist =
-            any_path_has_los ? (paths[i]->Length() + remaining_dist[i]) : paths[i]->Length();
+        if (remaining_dist[i] == FLT_MAX) continue;
+        const float total_dist = paths[i]->Length() + remaining_dist[i];
         if (total_dist > max_allowed_dist) continue;
-        const float cost = FLAGS_freepath_weight * paths[i]->Length() + FLAGS_clearance_weight * paths[i]->Clearance();
+        const float cost = FLAGS_freepath_weight * paths[i]->FPL() + FLAGS_clearance_weight * paths[i]->Clearance();
         if (cost < best_cost) {
             best = paths[i];
             best_cost = cost;
diff --git a/src/navigation/motion_primitives.h b/src/navigation/motion_primitives.h
@@ -125,6 +125,8 @@ struct PathEvaluatorBase {
         point_cloud = &new_point_cloud;  // zero-copy view
     }
 
+    void SetNavParams(const navigation::NavigationParameters& new_params) { nav_params = new_params; }
+
     // Return the best path rollout from the provided set of paths.
     virtual std::shared_ptr<PathRolloutBase> FindBest(const std::vector<std::shared_ptr<PathRolloutBase>>& paths) = 0;
 
@@ -140,6 +142,8 @@ struct PathEvaluatorBase {
     Eigen::Vector2f local_target;
     // Non-owning view of obstacle point cloud.
     const std::vector<Eigen::Vector2f>* point_cloud = nullptr;
+    // Navigation parameters.
+    navigation::NavigationParameters nav_params;
 };
 
 float Run1DTimeOptimalControl(const navigation::MotionLimits& limits, const float x_init, const float v_init,
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
@@ -242,6 +242,7 @@ void Navigation::Initialize(const NavigationParameters& params, const string& ma
     PathEvaluatorBase* evaluator = nullptr;
     if (params_.evaluator_type == "linear") {
         evaluator = (PathEvaluatorBase*)new LinearEvaluator();
+        evaluator->SetNavParams(params);
     } else {
         printf("Unknown evaluator type %s\n", params_.evaluator_type.c_str());
         exit(1);
@@ -915,20 +916,21 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, float& cmd_angle_vel
         // "Nudge" window: when close to goal, prefer continuing OA over FOV-based turning
         const float goal_dist2 = (nav_goal_loc_ - robot_loc_fp_).squaredNorm();  // MAP-frame distance^2
         const bool near_goal_nudge = (goal_dist2 <= Sq(params_.nudge_dist_tolerance));
-        const bool fov_ok = (fabs(theta) <= params_.local_half_fov);
+        // Hysteresis-based lidar FOV check to prevent oscillation:
+        // - To START obstacle avoidance: target must be within conservative FOV (±0.8 * lidar_fov_half_angle)
+        // - To CONTINUE obstacle avoidance: target can be anywhere in full FOV (±lidar_fov_half_angle)
 
-        // Hysteresis-based FOV check to prevent oscillation:
-        // - To START obstacle avoidance: target must be well-centered (±center_threshold)
-        // - To CONTINUE obstacle avoidance: target can be anywhere in FOV (±local_half_fov)
+        const bool fov_ok_for_continue = (fabs(theta) <= params_.lidar_fov_half_angle);
+        const bool fov_ok_for_start = (fabs(theta) <= 0.8f * params_.lidar_fov_half_angle);
 
         // Check nudge condition first
         if (near_goal_nudge) {
-            fprintf(stderr, "DEBUG: Not in FOV but goal nudge is active\n");
+            fprintf(stderr, "DEBUG: Not in lidar FOV but goal nudge is active\n");
             RunObstacleAvoidance(cmd_vel, cmd_angle_vel);
         } else {
             if (in_obstacle_avoidance_mode_) {
-                // Already doing obstacle avoidance: keep going unless target leaves FOV
-                if (!fov_ok) {
+                // Already doing obstacle avoidance: keep going unless target leaves full FOV
+                if (!fov_ok_for_continue) {
                     // Target left FOV: switch back to turning
                     in_obstacle_avoidance_mode_ = false;
                     TurnInPlace(cmd_vel, cmd_angle_vel);
@@ -939,12 +941,12 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel, float& cmd_angle_vel
             } else {
                 // Currently turning: only start obstacle avoidance when target is well-centered
                 yaw_align_sp_init_ = false;
-                if (fabs(theta) <= params_.center_threshold) {
-                    // Target is centered: start obstacle avoidance
+                if (fov_ok_for_start) {
+                    // Target is well-centered in FOV: start obstacle avoidance
                     in_obstacle_avoidance_mode_ = true;
                     RunObstacleAvoidance(cmd_vel, cmd_angle_vel);
                 } else {
-                    // Target not centered: keep turning
+                    // Target not well-centered: keep turning
                     TurnInPlace(cmd_vel, cmd_angle_vel);
                 }
             }
diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc
@@ -109,8 +109,7 @@ CONFIG_FLOAT(base_link_offset_x, "NavigationParameters.base_link_offset_x");
 CONFIG_FLOAT(base_link_offset_y, "NavigationParameters.base_link_offset_y");
 CONFIG_FLOAT(max_free_path_length, "NavigationParameters.max_free_path_length");
 CONFIG_FLOAT(max_clearance, "NavigationParameters.max_clearance");
-CONFIG_FLOAT(local_half_fov, "NavigationParameters.local_half_fov");
-CONFIG_FLOAT(center_threshold, "NavigationParameters.center_threshold");
+CONFIG_FLOAT(lidar_fov_half_angle, "NavigationParameters.lidar_fov_half_angle");
 CONFIG_BOOL(can_traverse_stairs, "NavigationParameters.can_traverse_stairs");
 CONFIG_FLOAT(target_dist_tolerance, "NavigationParameters.target_dist_tolerance");
 CONFIG_FLOAT(nudge_dist_tolerance, "NavigationParameters.nudge_dist_tolerance");
@@ -596,9 +595,9 @@ class NavigationNode : public rclcpp::Node, public std::enable_shared_from_this<
         const uint32_t nudge_color = within_nudge ? 0xFF0000 : 0xE0E0E0;  // red if within, light gray otherwise
         visualization::DrawArc(goal_in_local, nudge_dist_tolerance, -M_PI, M_PI, nudge_color, local_viz_msg_);
 
-        // Draw FOV cone boundaries (dark yellow)
+        // Draw lidar FOV cone boundaries (dark yellow)
         const float fov_length = 2.0f;  // Length of FOV lines in meters
-        const float fov_half_angle = CONFIG_local_half_fov;
+        const float fov_half_angle = CONFIG_lidar_fov_half_angle;
         Eigen::Vector2f fov_left(fov_length * cos(fov_half_angle), fov_length * sin(fov_half_angle));
         Eigen::Vector2f fov_right(fov_length * cos(-fov_half_angle), fov_length * sin(-fov_half_angle));
         visualization::DrawLine(Eigen::Vector2f(0, 0), fov_left, 0xFFCC00, local_viz_msg_);
@@ -712,23 +711,24 @@ class NavigationNode : public rclcpp::Node, public std::enable_shared_from_this<
         std::vector<std::shared_ptr<motion_primitives::PathRolloutBase>> path_rollouts = navigation_.sampled_paths_;
         std::shared_ptr<motion_primitives::PathRolloutBase> best_option = navigation_.best_option_;
 
-        // Draw path options that participate in optimization (Length > 0) in blue
+        // Draw path options that participate in optimization (Length > 0) in light blue
+        constexpr uint32_t kCandidatePathColor = 0x80A0FF;  // Light blue for non-winning paths
         for (const auto& rollout : path_rollouts) {
             if (rollout->Length() <= 0.0f) continue;  // Skip zero-length paths (not in optimization)
 
             // Handle constant curvature arc paths
             const auto* arc = dynamic_cast<const motion_primitives::ConstantCurvatureArcPath*>(rollout.get());
             if (arc) {
-                // Draw arc path (blue: 0x0000FF)
-                visualization::DrawPathOption(arc->curvature, arc->Length(), arc->Clearance(), 0x0000FF, false,
+                // Draw arc path
+                visualization::DrawPathOption(arc->curvature, arc->Length(), arc->Clearance(), kCandidatePathColor, false,
                                               local_viz_msg_);
             }
             // Handle omnidirectional straight-line paths
             const auto* omni = dynamic_cast<const motion_primitives::OmnidirectionalMovePath*>(rollout.get());
             if (omni) {
-                // Draw straight line from origin to endpoint (blue: 0x0000FF)
+                // Draw straight line from origin to endpoint
                 Eigen::Vector2f endpoint = omni->EndPoint().translation;
-                visualization::DrawLine(Eigen::Vector2f(0, 0), endpoint, 0x0000FF, local_viz_msg_);
+                visualization::DrawLine(Eigen::Vector2f(0, 0), endpoint, kCandidatePathColor, local_viz_msg_);
             }
         }
 
@@ -745,17 +745,24 @@ class NavigationNode : public rclcpp::Node, public std::enable_shared_from_this<
             const auto* best_omni = dynamic_cast<const motion_primitives::OmnidirectionalMovePath*>(best_option.get());
             if (best_omni) {
                 // Draw selected straight path (red: 0xFF0000)
-                Eigen::Vector2f endpoint = best_omni->EndPoint().translation;
+                const Eigen::Vector2f endpoint = best_omni->EndPoint().translation;
                 visualization::DrawLine(Eigen::Vector2f(0, 0), endpoint, 0xFF0000, local_viz_msg_);
 
-                // Draw clearance boundaries showing minimum distance to obstacles (red: 0xFF0000)
+                // Draw clearance corridor: robot body extent + clearance on each side
+                // Path centerline is at base_link origin; support() gives distance from base_link to robot edge
+                const Eigen::Vector2f dir_lateral(-best_omni->direction.y(), best_omni->direction.x());
+                const motion_primitives::OffsetRect robot_body = {
+                    Eigen::Vector2f(navigation_.params_.base_link_offset_x, navigation_.params_.base_link_offset_y),
+                    0.5f * navigation_.params_.robot_length,
+                    0.5f * navigation_.params_.robot_width
+                };
                 const float clearance = best_omni->Clearance();
-                // Calculate perpendicular vector for clearance boundaries
-                Eigen::Vector2f perp(-best_omni->direction.y(), best_omni->direction.x());
-                Eigen::Vector2f clearance_offset = clearance * perp;
-                // Draw parallel lines showing clearance boundaries
-                visualization::DrawLine(clearance_offset, endpoint + clearance_offset, 0xFF0000, local_viz_msg_);
-                visualization::DrawLine(-clearance_offset, endpoint - clearance_offset, 0xFF0000, local_viz_msg_);
+                // Corridor edge = body extent (from base_link) + clearance
+                const Eigen::Vector2f offset_left = (clearance + robot_body.support(dir_lateral)) * dir_lateral;
+                const Eigen::Vector2f offset_right = (clearance + robot_body.support(-dir_lateral)) * (-dir_lateral);
+                constexpr uint32_t kCorridorColor = 0xFF8080;  // Light red
+                visualization::DrawLine(offset_left, endpoint + offset_left, kCorridorColor, local_viz_msg_);
+                visualization::DrawLine(offset_right, endpoint + offset_right, kCorridorColor, local_viz_msg_);
             }
         }
     }
@@ -776,8 +783,7 @@ class NavigationNode : public rclcpp::Node, public std::enable_shared_from_this<
         params->base_link_offset_y = CONFIG_base_link_offset_y;
         params->max_free_path_length = CONFIG_max_free_path_length;
         params->max_clearance = CONFIG_max_clearance;
-        params->local_half_fov = CONFIG_local_half_fov;
-        params->center_threshold = CONFIG_center_threshold;
+        params->lidar_fov_half_angle = CONFIG_lidar_fov_half_angle;
         params->can_traverse_stairs = CONFIG_can_traverse_stairs;
         params->target_dist_tolerance = CONFIG_target_dist_tolerance;
         params->nudge_dist_tolerance = CONFIG_nudge_dist_tolerance;
diff --git a/src/navigation/navigation_parameters.h b/src/navigation/navigation_parameters.h
@@ -68,12 +68,9 @@ struct NavigationParameters {
     float base_link_offset_y;
     float max_free_path_length;
     float max_clearance;
-    // Half-angle of the local field of view cone (radians).
-    // Full FOV cone is ±local_half_fov. Used to determine when obstacle avoidance can continue.
-    float local_half_fov;
-    // Angle threshold (radians) for starting obstacle avoidance in hysteresis logic.
-    // Target must be within ±center_threshold to start obstacle avoidance.
-    float center_threshold;
+    // Half-angle of the lidar field of view cone (radians).
+    // Full FOV cone is ±lidar_fov_half_angle. Used to determine when obstacle avoidance can run safely.
+    float lidar_fov_half_angle;
 
     bool can_traverse_stairs;
 
@@ -130,8 +127,7 @@ struct NavigationParameters {
           base_link_offset_y(0),
           max_free_path_length(10.0),
           max_clearance(1.0),
-          local_half_fov(1.57),
-          center_threshold(0.174),
+          lidar_fov_half_angle(1.57),
           can_traverse_stairs(false),
           target_dist_tolerance(0.1),
           nudge_dist_tolerance(0.3),
diff --git a/src/navigation/omnidirectional_motion_primitives.cc b/src/navigation/omnidirectional_motion_primitives.cc
diff --git a/src/navigation/omnidirectional_motion_primitives.h b/src/navigation/omnidirectional_motion_primitives.h
