feat: v0.3.19 rebase (#1540)

* fix(system_monitor): extend command line to display (backport autowarefoundation#4553) (#768) fix(system_monitor): extend command line to display (autowarefoundation#4553) Signed-off-by: ito-san <fumihito.ito@tier4.jp> Co-authored-by: ito-san <57388357+ito-san@users.noreply.github.com> * feat(behavior_path_planner): resample output path (backport #1604) (#782) feat(behavior_path_planner): resample output path (#1604) * feat(behavior_path_planner): resample output path * update param Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> * ci: add dispatch-push-event workflow (#803) * ci: add dispatch-push-event workflow Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * fix: change APP KEY Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * chore: use strategy Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * chore: change trigger Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * pre-commit fixes Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * Update .github/workflows/dispatch-push-event.yaml * Update .github/workflows/dispatch-push-event.yaml * style(pre-commit): autofix * Update .github/workflows/dispatch-push-event.yaml --------- Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909) * fix(behavior_path): only apply spline interpolate for output, not for turn_signal processing * fix implementation * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(behavior_path): fix base points vanishing and inconsistent lane_ids on the spline interpolated path (#929) * add base points to resampled path in behavior_path * Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)" This reverts commit c80c986. * ci(pre-commit): autofix * fix insert * fix: not interpolate behavior velocity path * Revert "Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)"" This reverts commit e6dd540. --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(system_monitor): fix program command line reading (backport autowarefoundation#5191, autowarefoundation#5430) (#995) * perf(system_monitor): fix program command line reading (autowarefoundation#5191) * Fix program command line reading Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> * style(pre-commit): autofix * fix spelling commandline->command_line Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> --------- Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Co-authored-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(system_monitor): output command line (autowarefoundation#5430) * fix(system_monitor): output command line Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> Co-authored-by: Yuxuan Liu <619684051@qq.com> Co-authored-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: takeshi-iwanari <takeshi.iwanari@tier4.jp> Co-authored-by: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> * feat(obstacle_stop): upd obstacle hunting (#1068) * Adapted from PR #1458 Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> * Adapted from PR #1627 Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> * fix parameter name Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> * Adapted from PR autowarefoundation#2647 Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> * ci(pre-commit): autofix * fix build error * ci(pre-commit): autofix * remove comment line * remove logic * Delete parameters other than those added this time * ci(pre-commit): autofix * add stop vehicle check * ci(pre-commit): autofix * fix stop velocity threshold * fix engage obstacle clear and stop threshold * ci(pre-commit): autofix --------- Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(obstacle_avoidance_planner): add empty check (#1285) * fix(obstacle_avoidance_planner): add empty check * ci(pre-commit): autofix * add invalid_argument * delete empty check * return code moved to end * add warning log * update rclcpp_debug * delete debug log * Delete unnecessary blank lines --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(virtual traffic light): suppress lauch (#1290) * suppress launch Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * add existence check Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> --------- Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> Co-authored-by: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> * revert: #929,#909,#782 * revert: #1068 - d56c191. --------- Signed-off-by: ito-san <fumihito.ito@tier4.jp> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: ito-san <57388357+ito-san@users.noreply.github.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yuxuan Liu <619684051@qq.com> Co-authored-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Co-authored-by: takeshi-iwanari <takeshi.iwanari@tier4.jp> Co-authored-by: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> Co-authored-by: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com>
tier4 · Sep 18, 2024 · cc1ae5f · cc1ae5f
1 parent a5b62af
commit cc1ae5f
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 18 deletions.
diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp
@@ -12,6 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <scene_module/virtual_traffic_light/manager.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
 
@@ -81,11 +84,28 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
 void VirtualTrafficLightModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
+  tier4_autoware_utils::LineString2d ego_path_linestring;
+  for (const auto & path_point : path.points) {
+    ego_path_linestring.push_back(
+      tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d());
+  }
+
   for (const auto & m : getRegElemMapOnPath<VirtualTrafficLight>(
          path, planner_data_->route_handler_->getLaneletMapPtr())) {
+    const auto stop_line_opt = m.first->getStopLine();
+    if (!stop_line_opt) {
+      RCLCPP_FATAL(
+        logger_, "No stop line at virtual_traffic_light_reg_elem_id = %ld, please fix the map!",
+        m.first->id());
+      continue;
+    }
+
     // Use lanelet_id to unregister module when the route is changed
     const auto module_id = m.second.id();
-    if (!isModuleRegistered(module_id)) {
+    if (
+      !isModuleRegistered(module_id) &&
+      boost::geometry::intersects(
+        ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) {
       registerModule(std::make_shared<VirtualTrafficLightModule>(
         module_id, *m.first, m.second, planner_param_,
         logger_.get_child("virtual_traffic_light_module"), clock_));

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -343,26 +343,29 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
   const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw);
 
   // spline interpolation
-  const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
-  const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
-  const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s);
-
-  for (size_t i = 0; i < interpolated_x.size(); i++) {
-    if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
-      return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
+  try {
+    const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
+    const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
+    const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s);
+
+    for (size_t i = 0; i < interpolated_x.size(); i++) {
+      if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
+        return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+      }
     }
-  }
 
-  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
-  for (size_t i = 0; i < interpolated_x.size(); i++) {
-    autoware_auto_planning_msgs::msg::TrajectoryPoint point;
-    point.pose.position.x = interpolated_x[i];
-    point.pose.position.y = interpolated_y[i];
-    point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
-      tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
-    interpolated_points.push_back(point);
+    for (size_t i = 0; i < interpolated_x.size(); i++) {
+      autoware_auto_planning_msgs::msg::TrajectoryPoint point;
+      point.pose.position.x = interpolated_x[i];
+      point.pose.position.y = interpolated_y[i];
+      point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
+        tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
+      interpolated_points.push_back(point);
+    }
+  } catch (const std::invalid_argument & e) {
+    return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
   }
-
   return interpolated_points;
 }