From 7d7d9dc5d6181a43c7393eacbd4913a8e2172e9c Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 28 Aug 2023 10:45:33 +0900 Subject: [PATCH 01/12] fix(system_monitor): extend command line to display (backport #4553) (#768) fix(system_monitor): extend command line to display (#4553) Signed-off-by: ito-san Co-authored-by: ito-san <57388357+ito-san@users.noreply.github.com> --- system/system_monitor/src/process_monitor/process_monitor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 1173431b0f7c2..458c374d4c033 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -543,7 +543,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { From d11da6a048c549b664256b632a14d21b91fba10a Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 19 Sep 2023 12:59:03 +0900 Subject: [PATCH 02/12] 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 Co-authored-by: Takayuki Murooka --- .../behavior_path_planner/behavior_path_planner.param.yaml | 1 + .../config/behavior_path_planner.param.yaml | 2 ++ .../include/behavior_path_planner/parameters.hpp | 2 ++ .../src/behavior_path_planner_node.cpp | 6 +++++- 4 files changed, 10 insertions(+), 1 deletion(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index dd8031b0438c6..90a6432f0b83a 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -14,3 +14,4 @@ drivable_area_margin: 6.0 refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 + path_interval: 2.0 diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index e809e47b05228..f0889cc638c02 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -17,4 +17,6 @@ refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 + path_interval: 2.0 + visualize_drivable_area_for_shared_linestrings_lanelet: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 9db58a23b70b3..31394714a9f08 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -37,6 +37,8 @@ struct BehaviorPathPlannerParameters double turn_light_on_threshold_dis_long; double turn_light_on_threshold_time; + double path_interval; + // vehicle info double wheel_base; double front_overhang; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bd392853bcb23..593a6cb2cf555 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -165,6 +165,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_light_on_threshold_dis_lat = declare_parameter("turn_light_on_threshold_dis_lat", 0.3); p.turn_light_on_threshold_dis_long = declare_parameter("turn_light_on_threshold_dis_long", 10.0); p.turn_light_on_threshold_time = declare_parameter("turn_light_on_threshold_time", 3.0); + p.path_interval = declare_parameter("path_interval"); p.visualize_drivable_area_for_shared_linestrings_lanelet = declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); @@ -535,7 +536,10 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO path->header.stamp = this->now(); RCLCPP_DEBUG( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); - return path; + + const auto resampled_path = + util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); + return std::make_shared(resampled_path); } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( From 1331a23fd3d2be211362651b0457ea682fdf0f25 Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Fri, 8 Sep 2023 11:36:37 +0900 Subject: [PATCH 03/12] 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> --- .github/workflows/dispatch-push-event.yaml | 45 ++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 .github/workflows/dispatch-push-event.yaml diff --git a/.github/workflows/dispatch-push-event.yaml b/.github/workflows/dispatch-push-event.yaml new file mode 100644 index 0000000000000..492083c8038f6 --- /dev/null +++ b/.github/workflows/dispatch-push-event.yaml @@ -0,0 +1,45 @@ +name: dispatch-push-event +on: + push: + +jobs: + search-dispatch-repo: + runs-on: ubuntu-latest + strategy: + matrix: + include: + - { version: beta/v0.3.**, dispatch-repo: pilot-auto.x1.eve } + outputs: + dispatch-repo: ${{ steps.search-dispatch-repo.outputs.value }} + steps: + - name: Search dispatch repo + id: search-dispatch-repo + run: | + if [[ ${{ github.ref_name }} =~ ${{ matrix.version }} ]]; then + echo ::set-output name=value::"${{ matrix.dispatch-repo }}" + echo "Detected beta branch: ${{ github.ref_name }}" + echo "Dispatch repository: ${{ matrix.dispatch-repo }}" + fi + + dispatch-push-event: + runs-on: ubuntu-latest + needs: search-dispatch-repo + if: ${{ needs.search-dispatch-repo.outputs.dispatch-repo != '' }} + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.INTERNAL_APP_ID }} + private_key: ${{ secrets.INTERNAL_PRIVATE_KEY }} + + # 注意: workflow_dispatchで指定するブランチはmain固定となっているため、dispatch-repoのmainブランチにupdate-beta-branch.yamlが存在することが前提条件。 + - name: Dispatch the update-beta-branch workflow + run: | + curl -L \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: Bearer ${{ steps.generate-token.outputs.token }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + https://api.github.com/repos/tier4/${{ needs.search-dispatch-repo.outputs.dispatch-repo }}/actions/workflows/update-beta-branch.yaml/dispatches \ + -d '{"ref":"main"}' From ffbf01b621b3943bda62d7e96515d7f03777deba Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 10 Oct 2023 14:08:45 +0900 Subject: [PATCH 04/12] 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> --- .../behavior_path_planner_node.hpp | 4 +++- .../src/behavior_path_planner_node.cpp | 13 +++++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index e5c1b89454f4e..073f9015d4616 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -50,6 +50,7 @@ #include #include +#include #include namespace behavior_path_planner @@ -136,7 +137,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ - PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out); + std::pair getPath( + const BehaviorModuleOutput & bt_out); /** * @brief extract path candidate from behavior tree output diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 593a6cb2cf555..c241ed0324353 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -478,7 +478,10 @@ void BehaviorPathPlannerNode::run() const auto output = bt_manager_->run(planner_data_); // path handling - const auto path = getPath(output); + const auto paths = getPath(output); + const auto path = paths.first; + const auto original_path = paths.second; + const auto path_candidate = getPathCandidate(output); planner_data_->prev_output_path = path; @@ -504,7 +507,7 @@ void BehaviorPathPlannerNode::run() hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { turn_signal = turn_signal_decider_.getTurnSignal( - *path, planner_data_->self_pose->pose, *(planner_data_->route_handler), + *original_path, planner_data_->self_pose->pose, *(planner_data_->route_handler), output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance); hazard_signal.command = HazardLightsCommand::DISABLE; } @@ -527,7 +530,9 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } -PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output) +// output: spline interpolated path, original path +std::pair BehaviorPathPlannerNode::getPath( + const BehaviorModuleOutput & bt_output) { // TODO(Horibe) do some error handling when path is not available. @@ -539,7 +544,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO const auto resampled_path = util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); - return std::make_shared(resampled_path); + return std::make_pair(std::make_shared(resampled_path), path); } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( From fbff6220197dec612ced3a01050b860c5707832a Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 13 Oct 2023 13:15:43 +0900 Subject: [PATCH 05/12] 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 c80c986e9b4b37f0d6af92c041bfc174819ef843. * 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 e6dd540d354a9cdb488da0b12157b55c8c895ae9. --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/path_utilities.cpp | 34 +++++++++++++------ .../behavior_velocity_planner/src/node.cpp | 4 +-- 2 files changed, 26 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 9a02ced1c41c4..34e5111c1a2cb 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -90,7 +90,18 @@ double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end) PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval) { const auto base_points = calcPathArcLengthArray(path); - const auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); + auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); + + constexpr double epsilon = 0.01; + for (const auto & b : base_points) { + const auto is_almost_same_value = std::find_if( + sampling_points.begin(), sampling_points.end(), + [&](const auto v) { return std::abs(v - b) < epsilon; }); + if (is_almost_same_value == sampling_points.end()) { + sampling_points.push_back(b); + } + } + std::sort(sampling_points.begin(), sampling_points.end()); if (base_points.empty() || sampling_points.empty()) { return path; @@ -122,22 +133,25 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv // For LaneIds, Type, Twist // - // ------|----|----|----|----|----|----|-------> resampled - // [0] [1] [2] [3] [4] [5] [6] + // ------|----|----|----|----|----|----|----|--------> resampled + // [0] [1] [2] [3] [4] [5] [6] [7] // - // --------|---------------|----------|---------> base - // [0] [1] [2] + // ------|---------------|----------|-------|---> base + // [0] [1] [2] [3] // - // resampled[0~3] = base[0] - // resampled[4~5] = base[1] - // resampled[6] = base[2] + // resampled[0] = base[0] + // resampled[1~3] = base[1] + // resampled[4~5] = base[2] + // resampled[6~7] = base[3] // size_t base_idx{0}; for (size_t i = 0; i < resampled_path.points.size(); ++i) { - while (base_idx < base_points.size() - 1 && sampling_points.at(i) > base_points.at(base_idx)) { + while (base_idx < base_points.size() - 1 && + ((sampling_points.at(i) > base_points.at(base_idx)) || + (sampling_points.at(i) - base_points.at(base_idx)) > 1e-3)) { ++base_idx; } - size_t ref_idx = std::max(static_cast(base_idx) - 1, 0); + size_t ref_idx = std::max(static_cast(base_idx), 0); if (i == resampled_path.points.size() - 1) { ref_idx = base_points.size() - 1; // for last point } diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index ad30db2917ecd..c8600e25716b9 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -420,10 +420,10 @@ void BehaviorVelocityPlannerNode::onTrigger( const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + // const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); // check stop point - auto output_path_msg = filterStopPathPoint(interpolated_path_msg); + auto output_path_msg = filterStopPathPoint(filtered_path); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); From 6ce082b1420e4c1dee8d509ff155e36caab640cb Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 13 Dec 2023 15:37:43 +0900 Subject: [PATCH 06/12] fix(system_monitor): fix program command line reading (backport #5191, #5430) (#995) * perf(system_monitor): fix program command line reading (#5191) * Fix program command line reading Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * fix spelling commandline->command_line Signed-off-by: Owen-Liuyuxuan --------- Signed-off-by: Owen-Liuyuxuan Co-authored-by: Owen-Liuyuxuan Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(system_monitor): output command line (#5430) * fix(system_monitor): output command line Signed-off-by: takeshi.iwanari * style(pre-commit): autofix --------- Signed-off-by: takeshi.iwanari Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Signed-off-by: Owen-Liuyuxuan Signed-off-by: takeshi.iwanari Co-authored-by: Yuxuan Liu <619684051@qq.com> Co-authored-by: Owen-Liuyuxuan Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: takeshi-iwanari Co-authored-by: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> --- system/system_monitor/src/process_monitor/process_monitor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 458c374d4c033..1173431b0f7c2 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -543,7 +543,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { From d56c191460a2939d76562df289e74b1d5588368b Mon Sep 17 00:00:00 2001 From: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> Date: Tue, 19 Dec 2023 14:02:54 +0900 Subject: [PATCH 07/12] feat(obstacle_stop): upd obstacle hunting (#1068) * Adapted from PR #1458 Signed-off-by: Shigekazu Fukuta * Adapted from PR #1627 Signed-off-by: Shigekazu Fukuta * fix parameter name Signed-off-by: Shigekazu Fukuta * Adapted from PR #2647 Signed-off-by: Shigekazu Fukuta * 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 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/obstacle_stop_planner/README.md | 6 ++ .../config/obstacle_stop_planner.param.yaml | 2 + .../include/obstacle_stop_planner/node.hpp | 45 ++++++++- planning/obstacle_stop_planner/src/node.cpp | 99 +++++++++++++------ 4 files changed, 118 insertions(+), 34 deletions(-) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 95274b6a4829d..ed46c3f6bc154 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -39,6 +39,12 @@ When the deceleration section is inserted, the start point of the section is ins ## Modules +### Common Parameter + +| Parameter | Type | Description | +| ---------------------- | ------ | ----------------------------------------------------------------------------------------- | +| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | + ### Obstacle Stop Planner #### Role diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index c4be8d7c35789..a0acc76b357f3 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] + stop_planner: stop_margin: 5.0 # stop margin distance from obstacle on the path [m] min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m] diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 0a4dc99493bc7..7ca2da6be2354 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include namespace motion_planning @@ -76,6 +77,8 @@ using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using vehicle_info_util::VehicleInfo; +using PointCloud = pcl::PointCloud; + struct StopPoint { TrajectoryPoint point{}; @@ -91,6 +94,17 @@ struct SlowDownSection double velocity; }; +struct ObstacleWithDetectionTime +{ + explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) + : detection_time(t), point(p) + { + } + + rclcpp::Time detection_time; + pcl::PointXYZ point; +}; + class ObstacleStopPlannerNode : public rclcpp::Node { public: @@ -101,7 +115,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node bool enable_slow_down; // set True, slow down for obstacle beside the path double max_velocity; // max velocity [m/s] double lowpass_gain; // smoothing calculated current acceleration [-] - double hunting_threshold; // keep slow down or stop state if obstacle vanished [s] + double chattering_threshold; // keep slow down or stop state if obstacle vanished [s] double max_yaw_deviation_rad; // maximum ego yaw deviation from trajectory [rad] (measures // against overlapping lanes) }; @@ -184,12 +198,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; std::shared_ptr lpf_acc_{nullptr}; - boost::optional latest_slow_down_section_{}; + boost::optional latest_slow_down_section_{boost::none}; + std::vector obstacle_history_{}; tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - rclcpp::Time last_detection_time_; nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; nav_msgs::msg::Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr}; @@ -305,6 +319,31 @@ class ObstacleStopPlannerNode : public rclcpp::Node void publishDebugData( const PlannerData & planner_data, const double current_acc, const double current_vel); + + void updateObstacleHistory(const rclcpp::Time & now, const double chattering_threshold) + { + for (auto itr = obstacle_history_.begin(); itr != obstacle_history_.end();) { + const auto expired = (now - itr->detection_time).seconds() > chattering_threshold; + + if (expired) { + itr = obstacle_history_.erase(itr); + continue; + } + + itr++; + } + } + + PointCloud::Ptr getOldPointCloudPtr() const + { + PointCloud::Ptr ret(new PointCloud); + + for (const auto & p : obstacle_history_) { + ret->push_back(p.point); + } + + return ret; + } }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 44de6e2cdf9cb..45a9757175f81 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -42,6 +42,8 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::findNearestIndex; using tier4_autoware_utils::getRPY; +using PointCloud = pcl::PointCloud; + namespace { rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) @@ -443,7 +445,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod auto & p = node_param_; p.enable_slow_down = declare_parameter("enable_slow_down", false); p.max_velocity = declare_parameter("max_velocity", 20.0); - p.hunting_threshold = declare_parameter("hunting_threshold", 0.5); + p.chattering_threshold = declare_parameter("chattering_threshold", 0.5); p.lowpass_gain = declare_parameter("lowpass_gain", 0.9); lpf_acc_ = std::make_shared(0.0, p.lowpass_gain); const double max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 90.0); @@ -503,7 +505,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod acc_controller_ = std::make_unique( this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m); debug_ptr_ = std::make_shared(this, i.max_longitudinal_offset_m); - last_detection_time_ = this->now(); // Publishers path_pub_ = this->create_publisher("~/output/trajectory", 1); @@ -632,9 +633,7 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu current_vel, stop_param); const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_; - const auto no_hunting = (rclcpp::Time(input_msg->header.stamp) - last_detection_time_).seconds() > - node_param_.hunting_threshold; - if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { + if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_) { resetExternalVelocityLimit(current_acc, current_vel); } @@ -662,6 +661,11 @@ void ObstacleStopPlannerNode::searchObstacle( return; } + const auto now = this->now(); + const bool is_stopping = (std::fabs(current_velocity_ptr->twist.twist.linear.x) < 0.001); + const double history_erase_sec = (is_stopping) ? node_param_.chattering_threshold : 0.0; + updateObstacleHistory(now, history_erase_sec); + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; @@ -721,37 +725,79 @@ void ObstacleStopPlannerNode::searchObstacle( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - planner_data.found_collision_points = withinPolygon( + const auto found_collision_points = withinPolygon( one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, next_center_point, slow_down_pointcloud_ptr, collision_pointcloud_ptr); - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; + if (found_collision_points) { + pcl::PointXYZ nearest_collision_point; + rclcpp::Time nearest_collision_point_time; getNearestPoint( - *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, - &planner_data.nearest_collision_point_time); + *collision_pointcloud_ptr, p_front, &nearest_collision_point, + &nearest_collision_point_time); - debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - - planner_data.stop_require = planner_data.found_collision_points; - acc_controller_->insertAdaptiveCruiseVelocity( - decimate_trajectory, planner_data.decimate_trajectory_collision_index, - planner_data.current_pose, planner_data.nearest_collision_point, - planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, - &planner_data.stop_require, &output, trajectory_header); + obstacle_history_.emplace_back(now, nearest_collision_point); break; } } } + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { + // create one step circle center for vehicle + const auto & p_front = decimate_trajectory.at(i).pose; + const auto & p_back = decimate_trajectory.at(i + 1).pose; + const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); + const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); + const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); + const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); + std::vector one_step_move_vehicle_polygon; + // create one step polygon for vehicle + createOneStepPolygon( + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.expand_stop_range); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, + PolygonType::Vehicle); + + PointCloud::Ptr collision_pointcloud_ptr(new PointCloud); + collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; + + // check new collision points + planner_data.found_collision_points = withinPolygon( + one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, + next_center_point, getOldPointCloudPtr(), collision_pointcloud_ptr); + + if (planner_data.found_collision_points) { + planner_data.decimate_trajectory_collision_index = i; + getNearestPoint( + *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, + &planner_data.nearest_collision_point_time); + + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); + + planner_data.stop_require = planner_data.found_collision_points; + + acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory, planner_data.decimate_trajectory_collision_index, + planner_data.current_pose, planner_data.nearest_collision_point, + planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, + &planner_data.stop_require, &output, trajectory_header); + + if (!planner_data.stop_require) { + obstacle_history_.clear(); + } + + break; + } + } } void ObstacleStopPlannerNode::insertVelocity( TrajectoryPoints & output, PlannerData & planner_data, - const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const double current_acc, const double current_vel, const StopParam & stop_param) + [[maybe_unused]] const std_msgs::msg::Header & trajectory_header, + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, + const StopParam & stop_param) { if (output.size() < 3) { return; @@ -799,17 +845,8 @@ void ObstacleStopPlannerNode::insertVelocity( index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc, current_vel); - if ( - !latest_slow_down_section_ && - dist_baselink_to_obstacle + index_with_dist_remain.get().second < - vehicle_info.max_longitudinal_offset_m) { - latest_slow_down_section_ = slow_down_section; - } - insertSlowDownSection(slow_down_section, output); } - - last_detection_time_ = trajectory_header.stamp; } if (node_param_.enable_slow_down && latest_slow_down_section_) { From 15062174c2e5585f3aec8ad2a410aa0ee899b280 Mon Sep 17 00:00:00 2001 From: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> Date: Wed, 5 Jun 2024 16:28:19 +0900 Subject: [PATCH 08/12] 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> --- .../obstacle_avoidance_planner/src/utils.cpp | 37 ++++++++++--------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp index 90e9daccb0a89..57eef51c89549 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -343,26 +343,29 @@ std::vector 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{}; + std::vector 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{}; + } } - } - std::vector 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{}; } - return interpolated_points; } From 2e5697ace40e1349085b8797607dad53b427e960 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 7 Jun 2024 15:23:50 +0900 Subject: [PATCH 09/12] fix(virtual traffic light): suppress lauch (#1290) * suppress launch Signed-off-by: Yuki Takagi * add existence check Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi Co-authored-by: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> --- .../virtual_traffic_light/manager.cpp | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) 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 index 7ac6918f13ed2..d5a302c3275c6 100644 --- 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 #include #include @@ -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( 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( module_id, *m.first, m.second, planner_param_, logger_.get_child("virtual_traffic_light_module"), clock_)); From 7cf13684b967f2e508b1eb3d8a39c0b7163c85e9 Mon Sep 17 00:00:00 2001 From: asa-naki Date: Tue, 17 Sep 2024 20:52:23 +0900 Subject: [PATCH 10/12] revert: #929,#909,#782 --- .../behavior_path_planner.param.yaml | 1 - .../config/behavior_path_planner.param.yaml | 2 -- .../behavior_path_planner_node.hpp | 4 +-- .../behavior_path_planner/parameters.hpp | 2 -- .../src/behavior_path_planner_node.cpp | 17 +++------- .../src/path_utilities.cpp | 34 ++++++------------- .../behavior_velocity_planner/src/node.cpp | 4 +-- 7 files changed, 17 insertions(+), 47 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index 90a6432f0b83a..dd8031b0438c6 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -14,4 +14,3 @@ drivable_area_margin: 6.0 refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 - path_interval: 2.0 diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index f0889cc638c02..e809e47b05228 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -17,6 +17,4 @@ refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 - path_interval: 2.0 - visualize_drivable_area_for_shared_linestrings_lanelet: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 073f9015d4616..e5c1b89454f4e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -50,7 +50,6 @@ #include #include -#include #include namespace behavior_path_planner @@ -137,8 +136,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ - std::pair getPath( - const BehaviorModuleOutput & bt_out); + PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out); /** * @brief extract path candidate from behavior tree output diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 31394714a9f08..9db58a23b70b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -37,8 +37,6 @@ struct BehaviorPathPlannerParameters double turn_light_on_threshold_dis_long; double turn_light_on_threshold_time; - double path_interval; - // vehicle info double wheel_base; double front_overhang; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index c241ed0324353..bd392853bcb23 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -165,7 +165,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_light_on_threshold_dis_lat = declare_parameter("turn_light_on_threshold_dis_lat", 0.3); p.turn_light_on_threshold_dis_long = declare_parameter("turn_light_on_threshold_dis_long", 10.0); p.turn_light_on_threshold_time = declare_parameter("turn_light_on_threshold_time", 3.0); - p.path_interval = declare_parameter("path_interval"); p.visualize_drivable_area_for_shared_linestrings_lanelet = declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); @@ -478,10 +477,7 @@ void BehaviorPathPlannerNode::run() const auto output = bt_manager_->run(planner_data_); // path handling - const auto paths = getPath(output); - const auto path = paths.first; - const auto original_path = paths.second; - + const auto path = getPath(output); const auto path_candidate = getPathCandidate(output); planner_data_->prev_output_path = path; @@ -507,7 +503,7 @@ void BehaviorPathPlannerNode::run() hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { turn_signal = turn_signal_decider_.getTurnSignal( - *original_path, planner_data_->self_pose->pose, *(planner_data_->route_handler), + *path, planner_data_->self_pose->pose, *(planner_data_->route_handler), output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance); hazard_signal.command = HazardLightsCommand::DISABLE; } @@ -530,9 +526,7 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } -// output: spline interpolated path, original path -std::pair BehaviorPathPlannerNode::getPath( - const BehaviorModuleOutput & bt_output) +PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output) { // TODO(Horibe) do some error handling when path is not available. @@ -541,10 +535,7 @@ std::pair BehaviorPathPlan path->header.stamp = this->now(); RCLCPP_DEBUG( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); - - const auto resampled_path = - util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); - return std::make_pair(std::make_shared(resampled_path), path); + return path; } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 34e5111c1a2cb..9a02ced1c41c4 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -90,18 +90,7 @@ double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end) PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval) { const auto base_points = calcPathArcLengthArray(path); - auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); - - constexpr double epsilon = 0.01; - for (const auto & b : base_points) { - const auto is_almost_same_value = std::find_if( - sampling_points.begin(), sampling_points.end(), - [&](const auto v) { return std::abs(v - b) < epsilon; }); - if (is_almost_same_value == sampling_points.end()) { - sampling_points.push_back(b); - } - } - std::sort(sampling_points.begin(), sampling_points.end()); + const auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); if (base_points.empty() || sampling_points.empty()) { return path; @@ -133,25 +122,22 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv // For LaneIds, Type, Twist // - // ------|----|----|----|----|----|----|----|--------> resampled - // [0] [1] [2] [3] [4] [5] [6] [7] + // ------|----|----|----|----|----|----|-------> resampled + // [0] [1] [2] [3] [4] [5] [6] // - // ------|---------------|----------|-------|---> base - // [0] [1] [2] [3] + // --------|---------------|----------|---------> base + // [0] [1] [2] // - // resampled[0] = base[0] - // resampled[1~3] = base[1] - // resampled[4~5] = base[2] - // resampled[6~7] = base[3] + // resampled[0~3] = base[0] + // resampled[4~5] = base[1] + // resampled[6] = base[2] // size_t base_idx{0}; for (size_t i = 0; i < resampled_path.points.size(); ++i) { - while (base_idx < base_points.size() - 1 && - ((sampling_points.at(i) > base_points.at(base_idx)) || - (sampling_points.at(i) - base_points.at(base_idx)) > 1e-3)) { + while (base_idx < base_points.size() - 1 && sampling_points.at(i) > base_points.at(base_idx)) { ++base_idx; } - size_t ref_idx = std::max(static_cast(base_idx), 0); + size_t ref_idx = std::max(static_cast(base_idx) - 1, 0); if (i == resampled_path.points.size() - 1) { ref_idx = base_points.size() - 1; // for last point } diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index c8600e25716b9..ad30db2917ecd 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -420,10 +420,10 @@ void BehaviorVelocityPlannerNode::onTrigger( const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - // const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); // check stop point - auto output_path_msg = filterStopPathPoint(filtered_path); + auto output_path_msg = filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); From 22f00a9e3f105c509e0ab090100aa2ee44dc05d0 Mon Sep 17 00:00:00 2001 From: asa-naki Date: Wed, 18 Sep 2024 17:13:35 +0900 Subject: [PATCH 11/12] revert: #1068 - d56c191460a2939d76562df289e74b1d5588368b. --- planning/obstacle_stop_planner/README.md | 6 -- .../config/obstacle_stop_planner.param.yaml | 2 - .../include/obstacle_stop_planner/node.hpp | 45 +-------- planning/obstacle_stop_planner/src/node.cpp | 99 ++++++------------- 4 files changed, 34 insertions(+), 118 deletions(-) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index ed46c3f6bc154..95274b6a4829d 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -39,12 +39,6 @@ When the deceleration section is inserted, the start point of the section is ins ## Modules -### Common Parameter - -| Parameter | Type | Description | -| ---------------------- | ------ | ----------------------------------------------------------------------------------------- | -| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | - ### Obstacle Stop Planner #### Role diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index a0acc76b357f3..c4be8d7c35789 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,7 +1,5 @@ /**: ros__parameters: - chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] - stop_planner: stop_margin: 5.0 # stop margin distance from obstacle on the path [m] min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m] diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 7ca2da6be2354..0a4dc99493bc7 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -57,7 +57,6 @@ #include #include #include -#include #include namespace motion_planning @@ -77,8 +76,6 @@ using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using vehicle_info_util::VehicleInfo; -using PointCloud = pcl::PointCloud; - struct StopPoint { TrajectoryPoint point{}; @@ -94,17 +91,6 @@ struct SlowDownSection double velocity; }; -struct ObstacleWithDetectionTime -{ - explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) - : detection_time(t), point(p) - { - } - - rclcpp::Time detection_time; - pcl::PointXYZ point; -}; - class ObstacleStopPlannerNode : public rclcpp::Node { public: @@ -115,7 +101,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node bool enable_slow_down; // set True, slow down for obstacle beside the path double max_velocity; // max velocity [m/s] double lowpass_gain; // smoothing calculated current acceleration [-] - double chattering_threshold; // keep slow down or stop state if obstacle vanished [s] + double hunting_threshold; // keep slow down or stop state if obstacle vanished [s] double max_yaw_deviation_rad; // maximum ego yaw deviation from trajectory [rad] (measures // against overlapping lanes) }; @@ -198,12 +184,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; std::shared_ptr lpf_acc_{nullptr}; - boost::optional latest_slow_down_section_{boost::none}; - std::vector obstacle_history_{}; + boost::optional latest_slow_down_section_{}; tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; + rclcpp::Time last_detection_time_; nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; nav_msgs::msg::Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr}; @@ -319,31 +305,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node void publishDebugData( const PlannerData & planner_data, const double current_acc, const double current_vel); - - void updateObstacleHistory(const rclcpp::Time & now, const double chattering_threshold) - { - for (auto itr = obstacle_history_.begin(); itr != obstacle_history_.end();) { - const auto expired = (now - itr->detection_time).seconds() > chattering_threshold; - - if (expired) { - itr = obstacle_history_.erase(itr); - continue; - } - - itr++; - } - } - - PointCloud::Ptr getOldPointCloudPtr() const - { - PointCloud::Ptr ret(new PointCloud); - - for (const auto & p : obstacle_history_) { - ret->push_back(p.point); - } - - return ret; - } }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 45a9757175f81..44de6e2cdf9cb 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -42,8 +42,6 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::findNearestIndex; using tier4_autoware_utils::getRPY; -using PointCloud = pcl::PointCloud; - namespace { rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) @@ -445,7 +443,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod auto & p = node_param_; p.enable_slow_down = declare_parameter("enable_slow_down", false); p.max_velocity = declare_parameter("max_velocity", 20.0); - p.chattering_threshold = declare_parameter("chattering_threshold", 0.5); + p.hunting_threshold = declare_parameter("hunting_threshold", 0.5); p.lowpass_gain = declare_parameter("lowpass_gain", 0.9); lpf_acc_ = std::make_shared(0.0, p.lowpass_gain); const double max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 90.0); @@ -505,6 +503,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod acc_controller_ = std::make_unique( this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m); debug_ptr_ = std::make_shared(this, i.max_longitudinal_offset_m); + last_detection_time_ = this->now(); // Publishers path_pub_ = this->create_publisher("~/output/trajectory", 1); @@ -633,7 +632,9 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu current_vel, stop_param); const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_; - if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_) { + const auto no_hunting = (rclcpp::Time(input_msg->header.stamp) - last_detection_time_).seconds() > + node_param_.hunting_threshold; + if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { resetExternalVelocityLimit(current_acc, current_vel); } @@ -661,11 +662,6 @@ void ObstacleStopPlannerNode::searchObstacle( return; } - const auto now = this->now(); - const bool is_stopping = (std::fabs(current_velocity_ptr->twist.twist.linear.x) < 0.001); - const double history_erase_sec = (is_stopping) ? node_param_.chattering_threshold : 0.0; - updateObstacleHistory(now, history_erase_sec); - for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; @@ -725,79 +721,37 @@ void ObstacleStopPlannerNode::searchObstacle( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - const auto found_collision_points = withinPolygon( + planner_data.found_collision_points = withinPolygon( one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, next_center_point, slow_down_pointcloud_ptr, collision_pointcloud_ptr); - if (found_collision_points) { - pcl::PointXYZ nearest_collision_point; - rclcpp::Time nearest_collision_point_time; + if (planner_data.found_collision_points) { + planner_data.decimate_trajectory_collision_index = i; getNearestPoint( - *collision_pointcloud_ptr, p_front, &nearest_collision_point, - &nearest_collision_point_time); - - obstacle_history_.emplace_back(now, nearest_collision_point); - - break; - } - } - } - for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { - // create one step circle center for vehicle - const auto & p_front = decimate_trajectory.at(i).pose; - const auto & p_back = decimate_trajectory.at(i + 1).pose; - const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); - const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); - const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); - const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); - std::vector one_step_move_vehicle_polygon; - // create one step polygon for vehicle - createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.expand_stop_range); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, - PolygonType::Vehicle); - - PointCloud::Ptr collision_pointcloud_ptr(new PointCloud); - collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - - // check new collision points - planner_data.found_collision_points = withinPolygon( - one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, - next_center_point, getOldPointCloudPtr(), collision_pointcloud_ptr); - - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; - getNearestPoint( - *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, - &planner_data.nearest_collision_point_time); - - debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); + *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, + &planner_data.nearest_collision_point_time); - planner_data.stop_require = planner_data.found_collision_points; + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - acc_controller_->insertAdaptiveCruiseVelocity( - decimate_trajectory, planner_data.decimate_trajectory_collision_index, - planner_data.current_pose, planner_data.nearest_collision_point, - planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, - &planner_data.stop_require, &output, trajectory_header); + planner_data.stop_require = planner_data.found_collision_points; + acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory, planner_data.decimate_trajectory_collision_index, + planner_data.current_pose, planner_data.nearest_collision_point, + planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, + &planner_data.stop_require, &output, trajectory_header); - if (!planner_data.stop_require) { - obstacle_history_.clear(); + break; } - - break; } } } void ObstacleStopPlannerNode::insertVelocity( TrajectoryPoints & output, PlannerData & planner_data, - [[maybe_unused]] const std_msgs::msg::Header & trajectory_header, - const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, - const StopParam & stop_param) + const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, + const double current_acc, const double current_vel, const StopParam & stop_param) { if (output.size() < 3) { return; @@ -845,8 +799,17 @@ void ObstacleStopPlannerNode::insertVelocity( index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc, current_vel); + if ( + !latest_slow_down_section_ && + dist_baselink_to_obstacle + index_with_dist_remain.get().second < + vehicle_info.max_longitudinal_offset_m) { + latest_slow_down_section_ = slow_down_section; + } + insertSlowDownSection(slow_down_section, output); } + + last_detection_time_ = trajectory_header.stamp; } if (node_param_.enable_slow_down && latest_slow_down_section_) { From 4cdd4f2418304227aa76c49d75a54138b8aaac42 Mon Sep 17 00:00:00 2001 From: asa-naki Date: Wed, 18 Sep 2024 20:07:36 +0900 Subject: [PATCH 12/12] revert #803 - 1331a23fd3d2be211362651b0457ea682fdf0f25. --- .github/workflows/dispatch-push-event.yaml | 45 ---------------------- 1 file changed, 45 deletions(-) delete mode 100644 .github/workflows/dispatch-push-event.yaml diff --git a/.github/workflows/dispatch-push-event.yaml b/.github/workflows/dispatch-push-event.yaml deleted file mode 100644 index 492083c8038f6..0000000000000 --- a/.github/workflows/dispatch-push-event.yaml +++ /dev/null @@ -1,45 +0,0 @@ -name: dispatch-push-event -on: - push: - -jobs: - search-dispatch-repo: - runs-on: ubuntu-latest - strategy: - matrix: - include: - - { version: beta/v0.3.**, dispatch-repo: pilot-auto.x1.eve } - outputs: - dispatch-repo: ${{ steps.search-dispatch-repo.outputs.value }} - steps: - - name: Search dispatch repo - id: search-dispatch-repo - run: | - if [[ ${{ github.ref_name }} =~ ${{ matrix.version }} ]]; then - echo ::set-output name=value::"${{ matrix.dispatch-repo }}" - echo "Detected beta branch: ${{ github.ref_name }}" - echo "Dispatch repository: ${{ matrix.dispatch-repo }}" - fi - - dispatch-push-event: - runs-on: ubuntu-latest - needs: search-dispatch-repo - if: ${{ needs.search-dispatch-repo.outputs.dispatch-repo != '' }} - steps: - - name: Generate token - id: generate-token - uses: tibdex/github-app-token@v1 - with: - app_id: ${{ secrets.INTERNAL_APP_ID }} - private_key: ${{ secrets.INTERNAL_PRIVATE_KEY }} - - # 注意: workflow_dispatchで指定するブランチはmain固定となっているため、dispatch-repoのmainブランチにupdate-beta-branch.yamlが存在することが前提条件。 - - name: Dispatch the update-beta-branch workflow - run: | - curl -L \ - -X POST \ - -H "Accept: application/vnd.github+json" \ - -H "Authorization: Bearer ${{ steps.generate-token.outputs.token }}" \ - -H "X-GitHub-Api-Version: 2022-11-28" \ - https://api.github.com/repos/tier4/${{ needs.search-dispatch-repo.outputs.dispatch-repo }}/actions/workflows/update-beta-branch.yaml/dispatches \ - -d '{"ref":"main"}'