trying to set maximum_avoid_distance

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -178,7 +178,7 @@
   />
   <let name="behavior_velocity_planner_launch_modules" value="$(eval &quot;'$(var behavior_velocity_planner_launch_modules)' + '$(var launch_module_list_end)'&quot;)"/>
 
-  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
+  <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" launch-prefix="xterm -e gdb -ex run --args" output="screen">
     <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
       <!-- topic remap -->
       <remap from="~/input/route" to="$(var input_route_topic_name)"/>

planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml

@@ -85,3 +85,11 @@
           bicycle: false                               # [-]
           motorcycle: false                            # [-]
           pedestrian: false                            # [-]
+
+      constraints:
+        # lateral constraints
+        lateral:
+          velocity: [1.0, 1.38, 11.1]                   # [m/s]
+          max_accel_values: [0.5, 0.5, 0.5]             # [m/ss]
+          min_jerk_values: [0.2, 0.2, 0.2]              # [m/sss]
+          max_jerk_values: [1.0, 1.0, 1.0]              # [m/sss]

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp

@@ -16,13 +16,15 @@
 #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_
 
 #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
+#include "behavior_path_avoidance_module/helper.hpp"
 #include "behavior_path_lane_change_module/scene.hpp"
 
 #include <memory>
 
 namespace behavior_path_planner
 {
 using AvoidanceDebugData = DebugData;
+using helper::avoidance::AvoidanceHelper;
 
 class AvoidanceByLaneChange : public NormalLaneChange
 {
@@ -46,6 +48,7 @@ class AvoidanceByLaneChange : public NormalLaneChange
 
   ObjectDataArray registered_objects_;
   mutable ObjectDataArray stopped_objects_;
+  std::shared_ptr<AvoidanceHelper> avoidance_helper_;
 
   ObjectData createObjectData(
     const AvoidancePlanningData & data, const PredictedObject & object) const;

planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -139,6 +139,18 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
       getOrDeclareParameter<double>(*node, ns + "ignore_area.crosswalk.behind_distance");
   }
 
+  // avoidance maneuver (longitudinal)
+  {
+    const std::string ns = "avoidance.avoidance.longitudinal.";
+    p.min_prepare_time = getOrDeclareParameter<double>(*node, ns + "min_prepare_time");
+    p.max_prepare_time = getOrDeclareParameter<double>(*node, ns + "max_prepare_time");
+    p.min_prepare_distance = getOrDeclareParameter<double>(*node, ns + "min_prepare_distance");
+    p.min_slow_down_speed = getOrDeclareParameter<double>(*node, ns + "min_slow_down_speed");
+    p.buf_slow_down_speed = getOrDeclareParameter<double>(*node, ns + "buf_slow_down_speed");
+    p.nominal_avoidance_speed =
+      getOrDeclareParameter<double>(*node, ns + "nominal_avoidance_speed");
+  }
+
   {
     const std::string ns = "avoidance.target_filtering.detection_area.";
     p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");
@@ -150,6 +162,34 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
       getOrDeclareParameter<double>(*node, ns + "backward_distance");
   }
 
+  // constraints (lateral)
+  {
+    const std::string ns = "avoidance_by_lane_change.constraints.lateral.";
+    p.velocity_map = getOrDeclareParameter<std::vector<double>>(*node, ns + "velocity");
+    p.lateral_max_accel_map =
+      getOrDeclareParameter<std::vector<double>>(*node, ns + "max_accel_values");
+    p.lateral_min_jerk_map =
+      getOrDeclareParameter<std::vector<double>>(*node, ns + "min_jerk_values");
+    p.lateral_max_jerk_map =
+      getOrDeclareParameter<std::vector<double>>(*node, ns + "max_jerk_values");
+
+    if (p.velocity_map.empty()) {
+      throw std::domain_error("invalid velocity map.");
+    }
+
+    if (p.velocity_map.size() != p.lateral_max_accel_map.size()) {
+      throw std::domain_error("inconsistency among the constraints map.");
+    }
+
+    if (p.velocity_map.size() != p.lateral_min_jerk_map.size()) {
+      throw std::domain_error("inconsistency among the constraints map.");
+    }
+
+    if (p.velocity_map.size() != p.lateral_max_jerk_map.size()) {
+      throw std::domain_error("inconsistency among the constraints map.");
+    }
+  }
+
   // safety check
   {
     const std::string ns = "avoidance.safety_check.";

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -41,7 +41,8 @@ AvoidanceByLaneChange::AvoidanceByLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters,
   std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters)
 : NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE),
-  avoidance_parameters_(std::move(avoidance_parameters))
+  avoidance_parameters_(std::move(avoidance_parameters)),
+  avoidance_helper_{std::make_shared<AvoidanceHelper>(avoidance_parameters_)}
 {
 }
 
@@ -72,19 +73,39 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
     return false;
   }
 
-  const auto & front_object = data.target_objects.front();
-
   const auto current_lanes = getCurrentLanes();
   if (current_lanes.empty()) {
     return false;
   }
+
+  const auto & nearest_object = data.target_objects.front();
+
+  // get minimum lane change distance
   const auto shift_intervals =
     getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
   const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
     *lane_change_parameters_, shift_intervals,
     lane_change_parameters_->backward_length_buffer_for_end_of_lane);
 
-  return (front_object.longitudinal > minimum_lane_change_length);
+  // get minimum avoid distance
+
+  const auto ego_width = getCommonParam().vehicle_width;
+  const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
+  const auto nearest_object_parameter =
+    avoidance_parameters_->object_parameters.at(nearest_object_type);
+  const auto avoid_margin =
+    nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
+    nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
+
+  avoidance_helper_->setData(planner_data_);
+  const auto shift_length = avoidance_helper_->getShiftLength(
+    nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin);
+
+  const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length);
+  std::cerr << "min lc " << minimum_lane_change_length << " maximum avoid "
+            << maximum_avoid_distance << '\n';
+
+  return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance);
 }
 
 bool AvoidanceByLaneChange::specialExpiredCheck() const

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -87,6 +87,7 @@ class AvoidanceHelper
   double getAvoidanceEgoSpeed() const
   {
     const auto & values = parameters_->velocity_map;
+
     const auto idx = getConstraintsMapIndex(getEgoSpeed(), values);
     return std::max(getEgoSpeed(), values.at(idx));
   }

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -686,6 +686,7 @@ double calcShiftLength(
 {
   const auto shift_length =
     is_object_on_right ? (overhang_dist + avoid_margin) : (overhang_dist - avoid_margin);
+  std::cerr << __func__ << " " << shift_length << '\n';
   return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0;
 }