temp

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
zulfaqar-azmi-t4 · Oct 5, 2023 · 410d8dd · 410d8dd
1 parent 5405655
commit 410d8dd
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 2 deletions.
diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -1400,16 +1400,25 @@ void insertOrientation(T & points, const bool is_driving_forward)
 template <class T>
 void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
 {
+  std::cerr << __func__ << '\n';
   for (size_t i = 1; i < points.size();) {
     const double yaw1 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i - 1)).orientation);
     const double yaw2 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i)).orientation);
 
-    if (max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2))) {
+    std::cerr << max_yaw_diff << " "
+              << std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) << '\n';
+
+    if (max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)))
+    {
       points.erase(points.begin() + i);
-    } else {
+    }
+    else
+    {
       ++i;
     }
   }
+
+  std::cerr <<  __func__ << "end\n";
 }
 
 /**

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -93,6 +93,7 @@ std::pair<bool, bool> NormalLaneChange::getSafePath(LaneChangePath & safe_path)
     safe_path = valid_paths.front();
   }
 
+  std::cerr << __func__ << " " << safe_path.shifted_path.path.points.size() << '\n';
   return {true, found_safe_path};
 }
 
@@ -290,6 +291,7 @@ void NormalLaneChange::resetParameters()
 TurnSignalInfo NormalLaneChange::updateOutputTurnSignal()
 {
   TurnSignalInfo turn_signal_info = calcTurnSignalInfo();
+  std::cerr << __func__ << " " << status_.lane_change_path.shifted_path.path.points.size() << " " << status_.current_lanes.size() << '\n';
   const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal(
     status_.current_lanes, status_.lane_change_path.shifted_path,
     status_.lane_change_path.info.shift_line, getEgoPose(), getEgoTwist().linear.x,

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -353,6 +353,14 @@ std::optional<LaneChangePath> constructCandidatePath(
   candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path);
   candidate_path.shifted_path = shifted_path;
 
+  if(shifted_path.path.points.size() < 2){
+    std::cerr << __func__ << "reference path points " << target_lane_reference_path.points.size() << '\n';
+    std::cerr << __func__ << "lateral deviation " << tier4_autoware_utils::(shift_line.start, shift_line.end.position) << '\n';
+    std::cerr << __func__ << "shift line start end " << shift_line.start_shift_length - shift_line.end_shift_length << "\n";
+    std::cerr << __func__ << " " << lane_change_length.lane_changing << '\n';
+    throw std::runtime_error("points too small");
+  }
+
   return std::optional<LaneChangePath>{candidate_path};
 }
 

diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp
@@ -148,8 +148,12 @@ bool PathShifter::generate(
                              : applyLinearShifter(shifted_path);
 
   const bool is_driving_forward = true;
+
+  std::cerr << __func__ << "before insert orientation " << shifted_path->path.points.size() << '\n';
   insertOrientation(shifted_path->path.points, is_driving_forward);
+  std::cerr << __func__ << "after insert orientation " << shifted_path->path.points.size() << '\n';
   removeInvalidOrientationPoints(shifted_path->path.points);
+  std::cerr << __func__ << "after remove invalid orientation " << shifted_path->path.points.size() << '\n';
 
   // DEBUG
   RCLCPP_DEBUG_STREAM_THROTTLE(