Merge pull request #125 from tier4/fix/add_hotfix

fix: add hotfix
tier4 · Sep 16, 2022 · 42027c8 · 42027c8
2 parents dae7c1a + 4abc0ba
commit 42027c8
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 12 deletions.
diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp
@@ -56,7 +56,7 @@ std::vector<geometry_msgs::msg::Pose> resamplePath(
     if (ds < CLOSE_S_THRESHOLD) {
       continue;
     }
-    input_arclength.push_back(ds + input_arclength.at(i - 1));
+    input_arclength.push_back(ds + input_arclength.back());
     x.push_back(curr_pt.position.x);
     y.push_back(curr_pt.position.y);
     z.push_back(curr_pt.position.z);
@@ -157,7 +157,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
     if (ds < CLOSE_S_THRESHOLD) {
       continue;
     }
-    input_arclength.push_back(ds + input_arclength.at(i - 1));
+    input_arclength.push_back(ds + input_arclength.back());
     input_pose.push_back(curr_pt.pose);
     v_lon.push_back(curr_pt.longitudinal_velocity_mps);
     v_lat.push_back(curr_pt.lateral_velocity_mps);
@@ -321,7 +321,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
     if (ds < CLOSE_S_THRESHOLD) {
       continue;
     }
-    input_arclength.push_back(ds + input_arclength.at(i - 1));
+    input_arclength.push_back(ds + input_arclength.back());
     input_pose.push_back(curr_pt.pose);
     v_lon.push_back(curr_pt.longitudinal_velocity_mps);
     v_lat.push_back(curr_pt.lateral_velocity_mps);
@@ -418,7 +418,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
     if (ds < CLOSE_S_THRESHOLD) {
       continue;
     }
-    input_arclength.push_back(ds + input_arclength.at(i - 1));
+    input_arclength.push_back(ds + input_arclength.back());
     input_pose.push_back(curr_pt.pose);
     v_lon.push_back(curr_pt.longitudinal_velocity_mps);
     v_lat.push_back(curr_pt.lateral_velocity_mps);

diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp
@@ -16,6 +16,7 @@
 #define SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <tuple>
 #include <vector>
@@ -52,7 +53,8 @@ class TrafficLightModule : public SceneModuleInterface
     geometry_msgs::msg::Pose first_stop_pose;
     std::vector<geometry_msgs::msg::Pose> dead_line_poses;
     std::vector<geometry_msgs::msg::Point> traffic_light_points;
-    geometry_msgs::msg::Point highest_confidence_traffic_light_point;
+    std::optional<geometry_msgs::msg::Point> highest_confidence_traffic_light_point = {
+      std::nullopt};
   };
 
   struct PlannerParam

diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp
@@ -441,8 +441,9 @@ bool TrafficLightModule::getHighestConfidenceTrafficSignal(
       highest_confidence = tl_state.lights.front().confidence;
       highest_confidence_tl_state = *tl_state_stamped;
       try {
-        debug_data_.traffic_light_points.push_back(getTrafficLightPosition(traffic_light));
-        debug_data_.highest_confidence_traffic_light_point = getTrafficLightPosition(traffic_light);
+        auto tl_position = getTrafficLightPosition(traffic_light);
+        debug_data_.traffic_light_points.push_back(tl_position);
+        debug_data_.highest_confidence_traffic_light_point = std::make_optional(tl_position);
       } catch (const std::invalid_argument & ex) {
         RCLCPP_WARN_STREAM(logger_, ex.what());
         continue;
@@ -486,10 +487,11 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP
   // Get stop point and stop factor
   tier4_planning_msgs::msg::StopFactor stop_factor;
   stop_factor.stop_pose = debug_data_.first_stop_pose;
-  stop_factor.stop_factor_points =
-    std::vector<geometry_msgs::msg::Point>{debug_data_.highest_confidence_traffic_light_point};
-  planning_utils::appendStopReason(stop_factor, stop_reason);
-
+  if (debug_data_.highest_confidence_traffic_light_point != std::nullopt) {
+    stop_factor.stop_factor_points = std::vector<geometry_msgs::msg::Point>{
+      debug_data_.highest_confidence_traffic_light_point.value()};
+    planning_utils::appendStopReason(stop_factor, stop_reason);
+  }
   return modified_path;
 }
 

diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -37,7 +37,9 @@ tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray(
   // create stop factor
   tier4_planning_msgs::msg::StopFactor stop_factor;
   stop_factor.stop_pose = stop_pose;
-  stop_factor.stop_factor_points.emplace_back(stop_obstacle.collision_point);
+  geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point;
+  stop_factor_point.z = stop_pose.position.z;
+  stop_factor.stop_factor_points.emplace_back(stop_factor_point);
 
   // create stop reason stamped
   tier4_planning_msgs::msg::StopReason stop_reason_msg;