From 88460a6bc9a961640fbf28c0e94a3ccb224ccf11 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 22 Sep 2023 17:25:17 +0900
Subject: [PATCH 001/115] first commit: add only necessary bpp code for
template
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../scene_module/template/manager.hpp | 51 ++
.../scene_module/template/template_module.hpp | 70 +++
.../utils/template/template_parameters.hpp | 31 ++
.../utils/template/util.hpp | 23 +
.../src/scene_module/template/manager.cpp | 37 ++
.../template/side_shift_module.cpp | 485 ++++++++++++++++++
6 files changed, 697 insertions(+)
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/template/template_parameters.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/template/util.hpp
create mode 100644 planning/behavior_path_planner/src/scene_module/template/manager.cpp
create mode 100644 planning/behavior_path_planner/src/scene_module/template/side_shift_module.cpp
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp
new file mode 100644
index 0000000000000..f9d8b57cd9503
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp
@@ -0,0 +1,51 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__MANAGER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__MANAGER_HPP_
+
+#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
+#include "behavior_path_planner/scene_module/template/template_module.hpp"
+#include "behavior_path_planner/utils/template/template_parameters.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+class TemplateModuleManager : public SceneModuleManagerInterface
+{
+public:
+ TemplateModuleManager(
+ rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
+
+ std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+ {
+ return std::make_unique<TemplateModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+ }
+
+ void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override{};
+
+private:
+ std::shared_ptr<TemplateParameters> parameters_;
+};
+
+} // namespace behavior_path_planner
+
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__MANAGER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp
new file mode 100644
index 0000000000000..544266ffc6fe8
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp
@@ -0,0 +1,70 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__TEMPLATE_MODULE_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__TEMPLATE_MODULE_HPP_
+
+#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <tier4_planning_msgs/msg/lateral_offset.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+namespace behavior_path_planner
+{
+class TemplateModule : public SceneModuleInterface
+{
+public:
+ TemplateModule(
+ const std::string & name, rclcpp::Node & node,
+ const std::shared_ptr<TemplateParameters> & parameters,
+ const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
+
+ bool isExecutionRequested() const override;
+ bool isExecutionReady() const override;
+ BehaviorModuleOutput plan() override;
+ CandidateOutput planCandidate() const override;
+ void setParameters(const std::shared_ptr<TemplateParameters> & parameters);
+
+ void updateModuleParams(const std::any & parameters) override
+ {
+ parameters_ = std::any_cast<std::shared_ptr<TemplateParameters>>(parameters);
+ }
+
+ void acceptVisitor(
+ [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
+ {
+ }
+
+private:
+ bool canTransitSuccessState() override { return false; }
+
+ bool canTransitFailureState() override { return false; }
+
+ bool canTransitIdleToRunningState() override { return false; }
+
+ // member
+ std::shared_ptr<TemplateParameters> parameters_;
+};
+
+} // namespace behavior_path_planner
+
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__TEMPLATE_MODULE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/template/template_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/template/template_parameters.hpp
new file mode 100644
index 0000000000000..30739bb773896
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/template/template_parameters.hpp
@@ -0,0 +1,31 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__TEMPLATE_PARAMETERS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__TEMPLATE_PARAMETERS_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+struct TemplateParameters
+{
+ double dummy_parameter{0.0};
+};
+
+} // namespace behavior_path_planner
+#endif // BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__TEMPLATE_PARAMETERS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/template/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/template/util.hpp
new file mode 100644
index 0000000000000..54ef9e75e379b
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/template/util.hpp
@@ -0,0 +1,23 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__UTIL_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__UTIL_HPP_
+
+namespace behavior_path_planner
+{
+
+} // namespace behavior_path_planner
+
+#endif // BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__UTIL_HPP_
diff --git a/planning/behavior_path_planner/src/scene_module/template/manager.cpp b/planning/behavior_path_planner/src/scene_module/template/manager.cpp
new file mode 100644
index 0000000000000..cab3a5600ca25
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/template/manager.cpp
@@ -0,0 +1,37 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "behavior_path_planner/scene_module/template/manager.hpp"
+
+#include "tier4_autoware_utils/ros/update_param.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+TemplateModuleManager::TemplateModuleManager(
+ rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config)
+: SceneModuleManagerInterface(node, name, config, {})
+{
+ TemplateParameters p{};
+ p.dummy_parameter = node->declare_parameter<double>(name + ".dummy_parameter");
+ parameters_ = std::make_shared<TemplateParameters>(p);
+}
+
+} // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/src/scene_module/template/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/template/side_shift_module.cpp
new file mode 100644
index 0000000000000..4e9680cee4d59
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/template/side_shift_module.cpp
@@ -0,0 +1,485 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
+
+#include "behavior_path_planner/marker_utils/utils.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/side_shift/util.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
+
+#include <lanelet2_extension/utility/utilities.hpp>
+
+#include <algorithm>
+#include <memory>
+#include <string>
+
+namespace behavior_path_planner
+{
+using geometry_msgs::msg::Point;
+using motion_utils::calcSignedArcLength;
+using motion_utils::findNearestIndex;
+using motion_utils::findNearestSegmentIndex;
+using tier4_autoware_utils::calcDistance2d;
+using tier4_autoware_utils::calcOffsetPose;
+using tier4_autoware_utils::getPoint;
+
+SideShiftModule::SideShiftModule(
+ const std::string & name, rclcpp::Node & node,
+ const std::shared_ptr<SideShiftParameters> & parameters,
+ const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map)
+: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}
+{
+ // If lateral offset is subscribed, it approves side shift module automatically
+ clearWaitingApproval();
+}
+
+void SideShiftModule::initVariables()
+{
+ reference_path_ = PathWithLaneId();
+ debug_data_.path_shifter.reset();
+ debug_marker_.markers.clear();
+ start_pose_reset_request_ = false;
+ requested_lateral_offset_ = 0.0;
+ inserted_lateral_offset_ = 0.0;
+ inserted_shift_line_ = ShiftLine{};
+ shift_status_ = SideShiftStatus{};
+ prev_output_ = ShiftedPath{};
+ prev_shift_line_ = ShiftLine{};
+ path_shifter_ = PathShifter{};
+ resetPathCandidate();
+ resetPathReference();
+}
+
+void SideShiftModule::processOnEntry()
+{
+ // write me... (Don't initialize variables, otherwise lateral offset gets zero on entry.)
+ start_pose_reset_request_ = false;
+}
+
+void SideShiftModule::processOnExit()
+{
+ // write me...
+ initVariables();
+}
+
+void SideShiftModule::setParameters(const std::shared_ptr<SideShiftParameters> & parameters)
+{
+ parameters_ = parameters;
+}
+
+bool SideShiftModule::isExecutionRequested() const
+{
+ if (current_state_ == ModuleStatus::RUNNING) {
+ return true;
+ }
+
+ // If the desired offset has a non-zero value, return true as we want to execute the plan.
+
+ const bool has_request = !isAlmostZero(requested_lateral_offset_);
+ RCLCPP_DEBUG_STREAM(
+ getLogger(), "ESS::isExecutionRequested() : " << std::boolalpha << has_request);
+
+ return has_request;
+}
+
+bool SideShiftModule::isExecutionReady() const
+{
+ return true; // TODO(Horibe) is it ok to say "always safe"?
+}
+
+bool SideShiftModule::isReadyForNextRequest(
+ const double & min_request_time_sec, bool override_requests) const noexcept
+{
+ rclcpp::Time current_time = clock_->now();
+ const auto interval_from_last_request_sec = current_time - last_requested_shift_change_time_;
+
+ if (interval_from_last_request_sec.seconds() >= min_request_time_sec && !override_requests) {
+ last_requested_shift_change_time_ = current_time;
+ return true;
+ }
+
+ return false;
+}
+
+ModuleStatus SideShiftModule::updateState()
+{
+ // Never return the FAILURE. When the desired offset is zero and the vehicle is in the original
+ // drivable area,this module can stop the computation and return SUCCESS.
+
+ const auto isOffsetDiffAlmostZero = [this]() noexcept {
+ const auto last_sp = path_shifter_.getLastShiftLine();
+ if (last_sp) {
+ const auto length = std::fabs(last_sp.get().end_shift_length);
+ const auto lateral_offset = std::fabs(requested_lateral_offset_);
+ const auto offset_diff = lateral_offset - length;
+ if (!isAlmostZero(offset_diff)) {
+ lateral_offset_change_request_ = true;
+ return false;
+ }
+ }
+ return true;
+ }();
+
+ const bool no_offset_diff = isOffsetDiffAlmostZero;
+ const bool no_request = isAlmostZero(requested_lateral_offset_);
+
+ const auto no_shifted_plan = [&]() {
+ if (prev_output_.shift_length.empty()) {
+ return true;
+ } else {
+ const auto max_planned_shift_length = std::max_element(
+ prev_output_.shift_length.begin(), prev_output_.shift_length.end(),
+ [](double a, double b) { return std::abs(a) < std::abs(b); });
+ return std::abs(*max_planned_shift_length) < 0.01;
+ }
+ }();
+
+ RCLCPP_DEBUG(
+ getLogger(), "ESS::updateState() : no_request = %d, no_shifted_plan = %d", no_request,
+ no_shifted_plan);
+
+ if (no_request && no_shifted_plan && no_offset_diff) {
+ return ModuleStatus::SUCCESS;
+ }
+
+ const auto & current_lanes = utils::getCurrentLanes(planner_data_);
+ const auto & current_pose = planner_data_->self_odometry->pose.pose;
+ const auto & inserted_shift_line_start_pose = inserted_shift_line_.start;
+ const auto & inserted_shift_line_end_pose = inserted_shift_line_.end;
+ const double self_to_shift_line_start_arc_length =
+ behavior_path_planner::utils::getSignedDistance(
+ current_pose, inserted_shift_line_start_pose, current_lanes);
+ const double self_to_shift_line_end_arc_length = behavior_path_planner::utils::getSignedDistance(
+ current_pose, inserted_shift_line_end_pose, current_lanes);
+ if (self_to_shift_line_start_arc_length >= 0) {
+ shift_status_ = SideShiftStatus::BEFORE_SHIFT;
+ } else if (self_to_shift_line_start_arc_length < 0 && self_to_shift_line_end_arc_length > 0) {
+ shift_status_ = SideShiftStatus::SHIFTING;
+ } else {
+ shift_status_ = SideShiftStatus::AFTER_SHIFT;
+ }
+ return ModuleStatus::RUNNING;
+}
+
+void SideShiftModule::updateData()
+{
+ if (
+ planner_data_->lateral_offset != nullptr &&
+ planner_data_->lateral_offset->stamp != latest_lateral_offset_stamp_) {
+ if (isReadyForNextRequest(parameters_->shift_request_time_limit)) {
+ lateral_offset_change_request_ = true;
+ requested_lateral_offset_ = planner_data_->lateral_offset->lateral_offset;
+ latest_lateral_offset_stamp_ = planner_data_->lateral_offset->stamp;
+ }
+ }
+
+ if (current_state_ != ModuleStatus::RUNNING && current_state_ != ModuleStatus::IDLE) {
+ return;
+ }
+
+ // special for avoidance: take behind distance upt ot shift-start-point if it exist.
+ const auto longest_dist_to_shift_line = [&]() {
+ double max_dist = 0.0;
+ for (const auto & pnt : path_shifter_.getShiftLines()) {
+ max_dist = std::max(max_dist, tier4_autoware_utils::calcDistance2d(getEgoPose(), pnt.start));
+ }
+ return max_dist;
+ }();
+
+ const auto reference_pose = prev_output_.shift_length.empty()
+ ? planner_data_->self_odometry->pose.pose
+ : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_);
+ if (prev_reference_.points.empty()) {
+ prev_reference_ = *getPreviousModuleOutput().path;
+ }
+ if (!getPreviousModuleOutput().reference_path) {
+ return;
+ }
+ const auto centerline_path = utils::calcCenterLinePath(
+ planner_data_, reference_pose, longest_dist_to_shift_line,
+ *getPreviousModuleOutput().reference_path);
+
+ constexpr double resample_interval = 1.0;
+ const auto backward_extened_path = extendBackwardLength(*getPreviousModuleOutput().path);
+ reference_path_ = utils::resamplePathWithSpline(backward_extened_path, resample_interval);
+
+ path_shifter_.setPath(reference_path_);
+
+ const auto & route_handler = planner_data_->route_handler;
+ const auto & p = planner_data_->parameters;
+
+ lanelet::ConstLanelet current_lane;
+ if (!route_handler->getClosestLaneletWithinRoute(reference_pose, ¤t_lane)) {
+ RCLCPP_ERROR_THROTTLE(
+ getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
+ }
+
+ // For current_lanes with desired length
+ current_lanelets_ = route_handler->getLaneletSequence(
+ current_lane, reference_pose, p.backward_path_length, p.forward_path_length);
+
+ const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
+ path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx);
+}
+
+void SideShiftModule::replaceShiftLine()
+{
+ auto shift_lines = path_shifter_.getShiftLines();
+ if (shift_lines.size() > 0) {
+ shift_lines.clear();
+ }
+
+ const auto new_sl = calcShiftLine();
+
+ // if no conflict, then add the new point.
+ shift_lines.push_back(new_sl);
+ const bool new_sl_is_same_with_previous =
+ new_sl.end_shift_length == prev_shift_line_.end_shift_length;
+
+ if (!new_sl_is_same_with_previous) {
+ prev_shift_line_ = new_sl;
+ }
+
+ // set to path_shifter
+ path_shifter_.setShiftLines(shift_lines);
+ lateral_offset_change_request_ = false;
+ inserted_lateral_offset_ = requested_lateral_offset_;
+ inserted_shift_line_ = new_sl;
+
+ return;
+}
+
+BehaviorModuleOutput SideShiftModule::plan()
+{
+ // Replace shift line
+ if (
+ lateral_offset_change_request_ && ((shift_status_ == SideShiftStatus::BEFORE_SHIFT) ||
+ (shift_status_ == SideShiftStatus::AFTER_SHIFT))) {
+ replaceShiftLine();
+ } else if (shift_status_ != SideShiftStatus::BEFORE_SHIFT) {
+ RCLCPP_DEBUG(getLogger(), "ego is shifting");
+ } else {
+ RCLCPP_DEBUG(getLogger(), "change is not requested");
+ }
+
+ // Refine path
+ ShiftedPath shifted_path;
+ path_shifter_.generate(&shifted_path);
+
+ // Reset orientation
+ setOrientation(&shifted_path.path);
+
+ auto output = adjustDrivableArea(shifted_path);
+ output.reference_path = getPreviousModuleOutput().reference_path;
+
+ prev_output_ = shifted_path;
+ path_reference_ = getPreviousModuleOutput().reference_path;
+
+ debug_data_.path_shifter = std::make_shared<PathShifter>(path_shifter_);
+
+ if (parameters_->publish_debug_marker) {
+ setDebugMarkersVisualization();
+ } else {
+ debug_marker_.markers.clear();
+ }
+
+ return output;
+}
+
+CandidateOutput SideShiftModule::planCandidate() const
+{
+ auto path_shifter_local = path_shifter_;
+
+ path_shifter_local.addShiftLine(calcShiftLine());
+
+ // Refine path
+ ShiftedPath shifted_path;
+ path_shifter_local.generate(&shifted_path);
+
+ // Reset orientation
+ setOrientation(&shifted_path.path);
+
+ return CandidateOutput(shifted_path.path);
+}
+
+BehaviorModuleOutput SideShiftModule::planWaitingApproval()
+{
+ // Refine path
+ ShiftedPath shifted_path;
+ path_shifter_.generate(&shifted_path);
+
+ // Reset orientation
+ setOrientation(&shifted_path.path);
+
+ auto output = adjustDrivableArea(shifted_path);
+
+ output.reference_path = getPreviousModuleOutput().reference_path;
+
+ path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
+ path_reference_ = getPreviousModuleOutput().reference_path;
+
+ prev_output_ = shifted_path;
+
+ waitApproval();
+
+ return output;
+}
+
+ShiftLine SideShiftModule::calcShiftLine() const
+{
+ const auto & p = parameters_;
+ const auto ego_speed = std::abs(planner_data_->self_odometry->twist.twist.linear.x);
+
+ const double dist_to_start =
+ std::max(p->min_distance_to_start_shifting, ego_speed * p->time_to_start_shifting);
+
+ const double dist_to_end = [&]() {
+ const double shift_length = requested_lateral_offset_ - getClosestShiftLength();
+ const double jerk_shifting_distance = path_shifter_.calcLongitudinalDistFromJerk(
+ shift_length, p->shifting_lateral_jerk, std::max(ego_speed, p->min_shifting_speed));
+ const double shifting_distance = std::max(jerk_shifting_distance, p->min_shifting_distance);
+ const double dist_to_end = dist_to_start + shifting_distance;
+ RCLCPP_DEBUG(
+ getLogger(), "min_distance_to_start_shifting = %f, dist_to_start = %f, dist_to_end = %f",
+ parameters_->min_distance_to_start_shifting, dist_to_start, dist_to_end);
+ return dist_to_end;
+ }();
+
+ const size_t nearest_idx = planner_data_->findEgoIndex(reference_path_.points);
+ ShiftLine shift_line;
+ shift_line.end_shift_length = requested_lateral_offset_;
+ shift_line.start_idx = utils::getIdxByArclength(reference_path_, nearest_idx, dist_to_start);
+ shift_line.start = reference_path_.points.at(shift_line.start_idx).point.pose;
+ shift_line.end_idx = utils::getIdxByArclength(reference_path_, nearest_idx, dist_to_end);
+ shift_line.end = reference_path_.points.at(shift_line.end_idx).point.pose;
+
+ return shift_line;
+}
+
+double SideShiftModule::getClosestShiftLength() const
+{
+ if (prev_output_.shift_length.empty()) {
+ return 0.0;
+ }
+
+ const auto ego_point = planner_data_->self_odometry->pose.pose.position;
+ const auto closest = motion_utils::findNearestIndex(prev_output_.path.points, ego_point);
+ return prev_output_.shift_length.at(closest);
+}
+
+BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & path) const
+{
+ BehaviorModuleOutput out;
+ const auto & p = planner_data_->parameters;
+
+ const auto & dp = planner_data_->drivable_area_expansion_parameters;
+ const auto itr = std::minmax_element(path.shift_length.begin(), path.shift_length.end());
+
+ constexpr double threshold = 0.1;
+ constexpr double margin = 0.5;
+ const double left_offset = std::max(
+ *itr.second + (*itr.first > threshold ? margin : 0.0), dp.drivable_area_left_bound_offset);
+ const double right_offset = -std::min(
+ *itr.first - (*itr.first < -threshold ? margin : 0.0), -dp.drivable_area_right_bound_offset);
+
+ // crop path which is too long here
+ auto output_path = path.path;
+ const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(output_path.points);
+ const auto & current_pose = planner_data_->self_odometry->pose.pose;
+ output_path.points = motion_utils::cropPoints(
+ output_path.points, current_pose.position, current_seg_idx, p.forward_path_length,
+ p.backward_path_length + p.input_path_interval);
+
+ const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_);
+ const auto shorten_lanes = utils::cutOverlappedLanes(output_path, drivable_lanes);
+ const auto expanded_lanes =
+ utils::expandLanelets(shorten_lanes, left_offset, right_offset, dp.drivable_area_types_to_skip);
+
+ { // for new architecture
+ // NOTE: side shift module is not launched with other modules. Therefore, drivable_lanes can be
+ // assigned without combining.
+ out.path = std::make_shared<PathWithLaneId>(output_path);
+ out.drivable_area_info.drivable_lanes = expanded_lanes;
+ out.drivable_area_info.is_already_expanded = true;
+ }
+
+ return out;
+}
+
+PathWithLaneId SideShiftModule::extendBackwardLength(const PathWithLaneId & original_path) const
+{
+ // special for avoidance: take behind distance upt ot shift-start-point if it exist.
+ const auto longest_dist_to_shift_point = [&]() {
+ double max_dist = 0.0;
+ for (const auto & pnt : path_shifter_.getShiftLines()) {
+ max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start));
+ }
+ return max_dist;
+ }();
+
+ const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line.
+ const auto backward_length = std::max(
+ planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin);
+
+ const size_t orig_ego_idx = findNearestIndex(original_path.points, getEgoPose().position);
+ const size_t prev_ego_idx = findNearestSegmentIndex(
+ prev_reference_.points, getPoint(original_path.points.at(orig_ego_idx)));
+
+ size_t clip_idx = 0;
+ for (size_t i = 0; i < prev_ego_idx; ++i) {
+ if (backward_length > calcSignedArcLength(prev_reference_.points, clip_idx, prev_ego_idx)) {
+ break;
+ }
+ clip_idx = i;
+ }
+
+ PathWithLaneId extended_path{};
+ {
+ extended_path.points.insert(
+ extended_path.points.end(), prev_reference_.points.begin() + clip_idx,
+ prev_reference_.points.begin() + prev_ego_idx);
+ }
+
+ {
+ extended_path.points.insert(
+ extended_path.points.end(), original_path.points.begin() + orig_ego_idx,
+ original_path.points.end());
+ }
+
+ return extended_path;
+}
+
+void SideShiftModule::setDebugMarkersVisualization() const
+{
+ using marker_utils::createShiftLineMarkerArray;
+
+ debug_marker_.markers.clear();
+
+ const auto add = [this](const MarkerArray & added) {
+ tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
+ };
+
+ const auto add_shift_line_marker = [this, add](
+ const auto & ns, auto r, auto g, auto b, double w = 0.1) {
+ add(createShiftLineMarkerArray(
+ debug_data_.path_shifter->getShiftLines(), debug_data_.path_shifter->getBaseOffset(), ns, r,
+ g, b, w));
+ };
+
+ if (debug_data_.path_shifter) {
+ add_shift_line_marker("side_shift_shift_points", 0.7, 0.7, 0.7, 0.4);
+ }
+}
+} // namespace behavior_path_planner
From 449aa8bf006cb5509737c646934c0a1fa45c1c85 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 22 Sep 2023 17:58:47 +0900
Subject: [PATCH 002/115] change name of file
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../template/{side_shift_module.cpp => template_module.cpp} | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
rename planning/behavior_path_planner/src/scene_module/template/{side_shift_module.cpp => template_module.cpp} (98%)
diff --git a/planning/behavior_path_planner/src/scene_module/template/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/template/template_module.cpp
similarity index 98%
rename from planning/behavior_path_planner/src/scene_module/template/side_shift_module.cpp
rename to planning/behavior_path_planner/src/scene_module/template/template_module.cpp
index 4e9680cee4d59..56f94adf3f953 100644
--- a/planning/behavior_path_planner/src/scene_module/template/side_shift_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/template/template_module.cpp
@@ -12,11 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
+#include "behavior_path_planner/scene_module/template/template_module.hpp"
#include "behavior_path_planner/marker_utils/utils.hpp"
#include "behavior_path_planner/utils/path_utils.hpp"
-#include "behavior_path_planner/utils/side_shift/util.hpp"
+#include "behavior_path_planner/utils/template/util.hpp"
#include "behavior_path_planner/utils/utils.hpp"
#include <lanelet2_extension/utility/utilities.hpp>
@@ -479,7 +479,7 @@ void SideShiftModule::setDebugMarkersVisualization() const
};
if (debug_data_.path_shifter) {
- add_shift_line_marker("side_shift_shift_points", 0.7, 0.7, 0.7, 0.4);
+ add_shift_line_marker("template_shift_points", 0.7, 0.7, 0.7, 0.4);
}
}
} // namespace behavior_path_planner
From e06ad6b07c2d0936c5dacd8f6d9c01e43928310b Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 25 Sep 2023 10:16:56 +0900
Subject: [PATCH 003/115] delete more unrelated code
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../scene_module/template/template_module.cpp | 134 ------------------
1 file changed, 134 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/template/template_module.cpp b/planning/behavior_path_planner/src/scene_module/template/template_module.cpp
index 56f94adf3f953..13fe5ec4bc083 100644
--- a/planning/behavior_path_planner/src/scene_module/template/template_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/template/template_module.cpp
@@ -337,37 +337,6 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval()
return output;
}
-ShiftLine SideShiftModule::calcShiftLine() const
-{
- const auto & p = parameters_;
- const auto ego_speed = std::abs(planner_data_->self_odometry->twist.twist.linear.x);
-
- const double dist_to_start =
- std::max(p->min_distance_to_start_shifting, ego_speed * p->time_to_start_shifting);
-
- const double dist_to_end = [&]() {
- const double shift_length = requested_lateral_offset_ - getClosestShiftLength();
- const double jerk_shifting_distance = path_shifter_.calcLongitudinalDistFromJerk(
- shift_length, p->shifting_lateral_jerk, std::max(ego_speed, p->min_shifting_speed));
- const double shifting_distance = std::max(jerk_shifting_distance, p->min_shifting_distance);
- const double dist_to_end = dist_to_start + shifting_distance;
- RCLCPP_DEBUG(
- getLogger(), "min_distance_to_start_shifting = %f, dist_to_start = %f, dist_to_end = %f",
- parameters_->min_distance_to_start_shifting, dist_to_start, dist_to_end);
- return dist_to_end;
- }();
-
- const size_t nearest_idx = planner_data_->findEgoIndex(reference_path_.points);
- ShiftLine shift_line;
- shift_line.end_shift_length = requested_lateral_offset_;
- shift_line.start_idx = utils::getIdxByArclength(reference_path_, nearest_idx, dist_to_start);
- shift_line.start = reference_path_.points.at(shift_line.start_idx).point.pose;
- shift_line.end_idx = utils::getIdxByArclength(reference_path_, nearest_idx, dist_to_end);
- shift_line.end = reference_path_.points.at(shift_line.end_idx).point.pose;
-
- return shift_line;
-}
-
double SideShiftModule::getClosestShiftLength() const
{
if (prev_output_.shift_length.empty()) {
@@ -379,107 +348,4 @@ double SideShiftModule::getClosestShiftLength() const
return prev_output_.shift_length.at(closest);
}
-BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & path) const
-{
- BehaviorModuleOutput out;
- const auto & p = planner_data_->parameters;
-
- const auto & dp = planner_data_->drivable_area_expansion_parameters;
- const auto itr = std::minmax_element(path.shift_length.begin(), path.shift_length.end());
-
- constexpr double threshold = 0.1;
- constexpr double margin = 0.5;
- const double left_offset = std::max(
- *itr.second + (*itr.first > threshold ? margin : 0.0), dp.drivable_area_left_bound_offset);
- const double right_offset = -std::min(
- *itr.first - (*itr.first < -threshold ? margin : 0.0), -dp.drivable_area_right_bound_offset);
-
- // crop path which is too long here
- auto output_path = path.path;
- const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(output_path.points);
- const auto & current_pose = planner_data_->self_odometry->pose.pose;
- output_path.points = motion_utils::cropPoints(
- output_path.points, current_pose.position, current_seg_idx, p.forward_path_length,
- p.backward_path_length + p.input_path_interval);
-
- const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_);
- const auto shorten_lanes = utils::cutOverlappedLanes(output_path, drivable_lanes);
- const auto expanded_lanes =
- utils::expandLanelets(shorten_lanes, left_offset, right_offset, dp.drivable_area_types_to_skip);
-
- { // for new architecture
- // NOTE: side shift module is not launched with other modules. Therefore, drivable_lanes can be
- // assigned without combining.
- out.path = std::make_shared<PathWithLaneId>(output_path);
- out.drivable_area_info.drivable_lanes = expanded_lanes;
- out.drivable_area_info.is_already_expanded = true;
- }
-
- return out;
-}
-
-PathWithLaneId SideShiftModule::extendBackwardLength(const PathWithLaneId & original_path) const
-{
- // special for avoidance: take behind distance upt ot shift-start-point if it exist.
- const auto longest_dist_to_shift_point = [&]() {
- double max_dist = 0.0;
- for (const auto & pnt : path_shifter_.getShiftLines()) {
- max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start));
- }
- return max_dist;
- }();
-
- const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line.
- const auto backward_length = std::max(
- planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin);
-
- const size_t orig_ego_idx = findNearestIndex(original_path.points, getEgoPose().position);
- const size_t prev_ego_idx = findNearestSegmentIndex(
- prev_reference_.points, getPoint(original_path.points.at(orig_ego_idx)));
-
- size_t clip_idx = 0;
- for (size_t i = 0; i < prev_ego_idx; ++i) {
- if (backward_length > calcSignedArcLength(prev_reference_.points, clip_idx, prev_ego_idx)) {
- break;
- }
- clip_idx = i;
- }
-
- PathWithLaneId extended_path{};
- {
- extended_path.points.insert(
- extended_path.points.end(), prev_reference_.points.begin() + clip_idx,
- prev_reference_.points.begin() + prev_ego_idx);
- }
-
- {
- extended_path.points.insert(
- extended_path.points.end(), original_path.points.begin() + orig_ego_idx,
- original_path.points.end());
- }
-
- return extended_path;
-}
-
-void SideShiftModule::setDebugMarkersVisualization() const
-{
- using marker_utils::createShiftLineMarkerArray;
-
- debug_marker_.markers.clear();
-
- const auto add = [this](const MarkerArray & added) {
- tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
- };
-
- const auto add_shift_line_marker = [this, add](
- const auto & ns, auto r, auto g, auto b, double w = 0.1) {
- add(createShiftLineMarkerArray(
- debug_data_.path_shifter->getShiftLines(), debug_data_.path_shifter->getBaseOffset(), ns, r,
- g, b, w));
- };
-
- if (debug_data_.path_shifter) {
- add_shift_line_marker("template_shift_points", 0.7, 0.7, 0.7, 0.4);
- }
-}
} // namespace behavior_path_planner
From 5c72414ee80ee8734d3ebc35e69363098f3c4aa1 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 25 Sep 2023 18:06:18 +0900
Subject: [PATCH 004/115] refactor
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../scene_module/template/template_module.hpp | 6 +-
.../scene_module/template/template_module.cpp | 316 +-----------------
2 files changed, 15 insertions(+), 307 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp
index 544266ffc6fe8..034200e62de51 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp
@@ -15,13 +15,18 @@
#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__TEMPLATE_MODULE_HPP_
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__TEMPLATE_MODULE_HPP_
+#include "behavior_path_planner/marker_utils/utils.hpp"
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/utils/template/template_parameters.hpp"
+#include "behavior_path_planner/utils/template/util.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
#include <rclcpp/rclcpp.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <tier4_planning_msgs/msg/lateral_offset.hpp>
+#include <algorithm>
#include <memory>
#include <string>
#include <unordered_map>
@@ -42,7 +47,6 @@ class TemplateModule : public SceneModuleInterface
bool isExecutionReady() const override;
BehaviorModuleOutput plan() override;
CandidateOutput planCandidate() const override;
- void setParameters(const std::shared_ptr<TemplateParameters> & parameters);
void updateModuleParams(const std::any & parameters) override
{
diff --git a/planning/behavior_path_planner/src/scene_module/template/template_module.cpp b/planning/behavior_path_planner/src/scene_module/template/template_module.cpp
index 13fe5ec4bc083..5b41f8011ac90 100644
--- a/planning/behavior_path_planner/src/scene_module/template/template_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/template/template_module.cpp
@@ -14,17 +14,6 @@
#include "behavior_path_planner/scene_module/template/template_module.hpp"
-#include "behavior_path_planner/marker_utils/utils.hpp"
-#include "behavior_path_planner/utils/path_utils.hpp"
-#include "behavior_path_planner/utils/template/util.hpp"
-#include "behavior_path_planner/utils/utils.hpp"
-
-#include <lanelet2_extension/utility/utilities.hpp>
-
-#include <algorithm>
-#include <memory>
-#include <string>
-
namespace behavior_path_planner
{
using geometry_msgs::msg::Point;
@@ -35,317 +24,32 @@ using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
using tier4_autoware_utils::getPoint;
-SideShiftModule::SideShiftModule(
+TemplateModule::TemplateModule(
const std::string & name, rclcpp::Node & node,
- const std::shared_ptr<SideShiftParameters> & parameters,
+ const std::shared_ptr<TemplateParameters> & parameters,
const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map)
: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}
{
- // If lateral offset is subscribed, it approves side shift module automatically
- clearWaitingApproval();
-}
-
-void SideShiftModule::initVariables()
-{
- reference_path_ = PathWithLaneId();
- debug_data_.path_shifter.reset();
- debug_marker_.markers.clear();
- start_pose_reset_request_ = false;
- requested_lateral_offset_ = 0.0;
- inserted_lateral_offset_ = 0.0;
- inserted_shift_line_ = ShiftLine{};
- shift_status_ = SideShiftStatus{};
- prev_output_ = ShiftedPath{};
- prev_shift_line_ = ShiftLine{};
- path_shifter_ = PathShifter{};
- resetPathCandidate();
- resetPathReference();
-}
-
-void SideShiftModule::processOnEntry()
-{
- // write me... (Don't initialize variables, otherwise lateral offset gets zero on entry.)
- start_pose_reset_request_ = false;
-}
-
-void SideShiftModule::processOnExit()
-{
- // write me...
- initVariables();
-}
-
-void SideShiftModule::setParameters(const std::shared_ptr<SideShiftParameters> & parameters)
-{
- parameters_ = parameters;
}
-bool SideShiftModule::isExecutionRequested() const
+bool TemplateModule::isExecutionRequested() const
{
- if (current_state_ == ModuleStatus::RUNNING) {
- return true;
- }
-
- // If the desired offset has a non-zero value, return true as we want to execute the plan.
-
- const bool has_request = !isAlmostZero(requested_lateral_offset_);
- RCLCPP_DEBUG_STREAM(
- getLogger(), "ESS::isExecutionRequested() : " << std::boolalpha << has_request);
-
- return has_request;
+ return true;
}
-bool SideShiftModule::isExecutionReady() const
+bool TemplateModule::isExecutionReady() const
{
- return true; // TODO(Horibe) is it ok to say "always safe"?
+ return true;
}
-bool SideShiftModule::isReadyForNextRequest(
- const double & min_request_time_sec, bool override_requests) const noexcept
+BehaviorModuleOutput TemplateModule::plan()
{
- rclcpp::Time current_time = clock_->now();
- const auto interval_from_last_request_sec = current_time - last_requested_shift_change_time_;
-
- if (interval_from_last_request_sec.seconds() >= min_request_time_sec && !override_requests) {
- last_requested_shift_change_time_ = current_time;
- return true;
- }
-
- return false;
+ return {};
}
-ModuleStatus SideShiftModule::updateState()
+CandidateOutput TemplateModule::planCandidate() const
{
- // Never return the FAILURE. When the desired offset is zero and the vehicle is in the original
- // drivable area,this module can stop the computation and return SUCCESS.
-
- const auto isOffsetDiffAlmostZero = [this]() noexcept {
- const auto last_sp = path_shifter_.getLastShiftLine();
- if (last_sp) {
- const auto length = std::fabs(last_sp.get().end_shift_length);
- const auto lateral_offset = std::fabs(requested_lateral_offset_);
- const auto offset_diff = lateral_offset - length;
- if (!isAlmostZero(offset_diff)) {
- lateral_offset_change_request_ = true;
- return false;
- }
- }
- return true;
- }();
-
- const bool no_offset_diff = isOffsetDiffAlmostZero;
- const bool no_request = isAlmostZero(requested_lateral_offset_);
-
- const auto no_shifted_plan = [&]() {
- if (prev_output_.shift_length.empty()) {
- return true;
- } else {
- const auto max_planned_shift_length = std::max_element(
- prev_output_.shift_length.begin(), prev_output_.shift_length.end(),
- [](double a, double b) { return std::abs(a) < std::abs(b); });
- return std::abs(*max_planned_shift_length) < 0.01;
- }
- }();
-
- RCLCPP_DEBUG(
- getLogger(), "ESS::updateState() : no_request = %d, no_shifted_plan = %d", no_request,
- no_shifted_plan);
-
- if (no_request && no_shifted_plan && no_offset_diff) {
- return ModuleStatus::SUCCESS;
- }
-
- const auto & current_lanes = utils::getCurrentLanes(planner_data_);
- const auto & current_pose = planner_data_->self_odometry->pose.pose;
- const auto & inserted_shift_line_start_pose = inserted_shift_line_.start;
- const auto & inserted_shift_line_end_pose = inserted_shift_line_.end;
- const double self_to_shift_line_start_arc_length =
- behavior_path_planner::utils::getSignedDistance(
- current_pose, inserted_shift_line_start_pose, current_lanes);
- const double self_to_shift_line_end_arc_length = behavior_path_planner::utils::getSignedDistance(
- current_pose, inserted_shift_line_end_pose, current_lanes);
- if (self_to_shift_line_start_arc_length >= 0) {
- shift_status_ = SideShiftStatus::BEFORE_SHIFT;
- } else if (self_to_shift_line_start_arc_length < 0 && self_to_shift_line_end_arc_length > 0) {
- shift_status_ = SideShiftStatus::SHIFTING;
- } else {
- shift_status_ = SideShiftStatus::AFTER_SHIFT;
- }
- return ModuleStatus::RUNNING;
-}
-
-void SideShiftModule::updateData()
-{
- if (
- planner_data_->lateral_offset != nullptr &&
- planner_data_->lateral_offset->stamp != latest_lateral_offset_stamp_) {
- if (isReadyForNextRequest(parameters_->shift_request_time_limit)) {
- lateral_offset_change_request_ = true;
- requested_lateral_offset_ = planner_data_->lateral_offset->lateral_offset;
- latest_lateral_offset_stamp_ = planner_data_->lateral_offset->stamp;
- }
- }
-
- if (current_state_ != ModuleStatus::RUNNING && current_state_ != ModuleStatus::IDLE) {
- return;
- }
-
- // special for avoidance: take behind distance upt ot shift-start-point if it exist.
- const auto longest_dist_to_shift_line = [&]() {
- double max_dist = 0.0;
- for (const auto & pnt : path_shifter_.getShiftLines()) {
- max_dist = std::max(max_dist, tier4_autoware_utils::calcDistance2d(getEgoPose(), pnt.start));
- }
- return max_dist;
- }();
-
- const auto reference_pose = prev_output_.shift_length.empty()
- ? planner_data_->self_odometry->pose.pose
- : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_);
- if (prev_reference_.points.empty()) {
- prev_reference_ = *getPreviousModuleOutput().path;
- }
- if (!getPreviousModuleOutput().reference_path) {
- return;
- }
- const auto centerline_path = utils::calcCenterLinePath(
- planner_data_, reference_pose, longest_dist_to_shift_line,
- *getPreviousModuleOutput().reference_path);
-
- constexpr double resample_interval = 1.0;
- const auto backward_extened_path = extendBackwardLength(*getPreviousModuleOutput().path);
- reference_path_ = utils::resamplePathWithSpline(backward_extened_path, resample_interval);
-
- path_shifter_.setPath(reference_path_);
-
- const auto & route_handler = planner_data_->route_handler;
- const auto & p = planner_data_->parameters;
-
- lanelet::ConstLanelet current_lane;
- if (!route_handler->getClosestLaneletWithinRoute(reference_pose, ¤t_lane)) {
- RCLCPP_ERROR_THROTTLE(
- getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
- }
-
- // For current_lanes with desired length
- current_lanelets_ = route_handler->getLaneletSequence(
- current_lane, reference_pose, p.backward_path_length, p.forward_path_length);
-
- const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
- path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx);
-}
-
-void SideShiftModule::replaceShiftLine()
-{
- auto shift_lines = path_shifter_.getShiftLines();
- if (shift_lines.size() > 0) {
- shift_lines.clear();
- }
-
- const auto new_sl = calcShiftLine();
-
- // if no conflict, then add the new point.
- shift_lines.push_back(new_sl);
- const bool new_sl_is_same_with_previous =
- new_sl.end_shift_length == prev_shift_line_.end_shift_length;
-
- if (!new_sl_is_same_with_previous) {
- prev_shift_line_ = new_sl;
- }
-
- // set to path_shifter
- path_shifter_.setShiftLines(shift_lines);
- lateral_offset_change_request_ = false;
- inserted_lateral_offset_ = requested_lateral_offset_;
- inserted_shift_line_ = new_sl;
-
- return;
-}
-
-BehaviorModuleOutput SideShiftModule::plan()
-{
- // Replace shift line
- if (
- lateral_offset_change_request_ && ((shift_status_ == SideShiftStatus::BEFORE_SHIFT) ||
- (shift_status_ == SideShiftStatus::AFTER_SHIFT))) {
- replaceShiftLine();
- } else if (shift_status_ != SideShiftStatus::BEFORE_SHIFT) {
- RCLCPP_DEBUG(getLogger(), "ego is shifting");
- } else {
- RCLCPP_DEBUG(getLogger(), "change is not requested");
- }
-
- // Refine path
- ShiftedPath shifted_path;
- path_shifter_.generate(&shifted_path);
-
- // Reset orientation
- setOrientation(&shifted_path.path);
-
- auto output = adjustDrivableArea(shifted_path);
- output.reference_path = getPreviousModuleOutput().reference_path;
-
- prev_output_ = shifted_path;
- path_reference_ = getPreviousModuleOutput().reference_path;
-
- debug_data_.path_shifter = std::make_shared<PathShifter>(path_shifter_);
-
- if (parameters_->publish_debug_marker) {
- setDebugMarkersVisualization();
- } else {
- debug_marker_.markers.clear();
- }
-
- return output;
-}
-
-CandidateOutput SideShiftModule::planCandidate() const
-{
- auto path_shifter_local = path_shifter_;
-
- path_shifter_local.addShiftLine(calcShiftLine());
-
- // Refine path
- ShiftedPath shifted_path;
- path_shifter_local.generate(&shifted_path);
-
- // Reset orientation
- setOrientation(&shifted_path.path);
-
- return CandidateOutput(shifted_path.path);
-}
-
-BehaviorModuleOutput SideShiftModule::planWaitingApproval()
-{
- // Refine path
- ShiftedPath shifted_path;
- path_shifter_.generate(&shifted_path);
-
- // Reset orientation
- setOrientation(&shifted_path.path);
-
- auto output = adjustDrivableArea(shifted_path);
-
- output.reference_path = getPreviousModuleOutput().reference_path;
-
- path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
- path_reference_ = getPreviousModuleOutput().reference_path;
-
- prev_output_ = shifted_path;
-
- waitApproval();
-
- return output;
-}
-
-double SideShiftModule::getClosestShiftLength() const
-{
- if (prev_output_.shift_length.empty()) {
- return 0.0;
- }
-
- const auto ego_point = planner_data_->self_odometry->pose.pose.position;
- const auto closest = motion_utils::findNearestIndex(prev_output_.path.points, ego_point);
- return prev_output_.shift_length.at(closest);
+ return {};
}
} // namespace behavior_path_planner
From 32b6ecc88acc37fe262e4c09790f38fce8b1a246 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 26 Sep 2023 15:18:43 +0900
Subject: [PATCH 005/115] fix manager
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../behavior_path_planner/scene_module/template/manager.hpp | 3 ++-
.../src/scene_module/template/manager.cpp | 3 ---
2 files changed, 2 insertions(+), 4 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp
index f9d8b57cd9503..82fb9a9b92939 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp
@@ -40,7 +40,8 @@ class TemplateModuleManager : public SceneModuleManagerInterface
return std::make_unique<TemplateModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
}
- void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override{};
+ void updateModuleParams(
+ [[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) override{};
private:
std::shared_ptr<TemplateParameters> parameters_;
diff --git a/planning/behavior_path_planner/src/scene_module/template/manager.cpp b/planning/behavior_path_planner/src/scene_module/template/manager.cpp
index cab3a5600ca25..3ddc2f345ebf6 100644
--- a/planning/behavior_path_planner/src/scene_module/template/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/template/manager.cpp
@@ -29,9 +29,6 @@ TemplateModuleManager::TemplateModuleManager(
rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config)
: SceneModuleManagerInterface(node, name, config, {})
{
- TemplateParameters p{};
- p.dummy_parameter = node->declare_parameter<double>(name + ".dummy_parameter");
- parameters_ = std::make_shared<TemplateParameters>(p);
}
} // namespace behavior_path_planner
From edafb155e2eaf80e0cf4587dde281e2d265191a4 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 26 Sep 2023 16:07:53 +0900
Subject: [PATCH 006/115] rebase Signed-off-by: Daniel Sanchez
<daniel.sanchez@tier4.jp>
---
.../manager.hpp | 19 +++++++++---------
.../sampling_planner_module.hpp} | 20 +++++++++----------
.../sampling_planner_parameters.hpp} | 8 ++++----
.../{template => sampling_planner}/util.hpp | 6 +++---
.../src/behavior_path_planner_node.cpp | 8 ++++++++
.../manager.cpp | 4 ++--
.../sampling_planner_module.cpp} | 14 ++++++-------
7 files changed, 44 insertions(+), 35 deletions(-)
rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/{template => sampling_planner}/manager.hpp (63%)
rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/{template/template_module.hpp => sampling_planner/sampling_planner_module.hpp} (71%)
rename planning/behavior_path_planner/include/behavior_path_planner/utils/{template/template_parameters.hpp => sampling_planner/sampling_planner_parameters.hpp} (71%)
rename planning/behavior_path_planner/include/behavior_path_planner/utils/{template => sampling_planner}/util.hpp (76%)
rename planning/behavior_path_planner/src/scene_module/{template => sampling_planner}/manager.cpp (87%)
rename planning/behavior_path_planner/src/scene_module/{template/template_module.cpp => sampling_planner/sampling_planner_module.cpp} (74%)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp
similarity index 63%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp
index 82fb9a9b92939..e805593dcff31 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp
@@ -12,12 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__MANAGER_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__MANAGER_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
+#include "behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp"
#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
-#include "behavior_path_planner/scene_module/template/template_module.hpp"
-#include "behavior_path_planner/utils/template/template_parameters.hpp"
+#include "behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp"
#include <rclcpp/rclcpp.hpp>
@@ -29,24 +29,25 @@
namespace behavior_path_planner
{
-class TemplateModuleManager : public SceneModuleManagerInterface
+class SamplingPlannerModuleManager : public SceneModuleManagerInterface
{
public:
- TemplateModuleManager(
+ SamplingPlannerModuleManager(
rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
{
- return std::make_unique<TemplateModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+ return std::make_unique<SamplingPlannerModule>(
+ name_, *node_, parameters_, rtc_interface_ptr_map_);
}
void updateModuleParams(
[[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) override{};
private:
- std::shared_ptr<TemplateParameters> parameters_;
+ std::shared_ptr<SamplingPlannerParameters> parameters_;
};
} // namespace behavior_path_planner
-#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__MANAGER_HPP_
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
similarity index 71%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 034200e62de51..848ad15ada3b7 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/template/template_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -12,13 +12,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__TEMPLATE_MODULE_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__TEMPLATE_MODULE_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
#include "behavior_path_planner/marker_utils/utils.hpp"
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
-#include "behavior_path_planner/utils/template/template_parameters.hpp"
-#include "behavior_path_planner/utils/template/util.hpp"
+#include "behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp"
+#include "behavior_path_planner/utils/sampling_planner/util.hpp"
#include "behavior_path_planner/utils/utils.hpp"
#include <rclcpp/rclcpp.hpp>
@@ -35,12 +35,12 @@
namespace behavior_path_planner
{
-class TemplateModule : public SceneModuleInterface
+class SamplingPlannerModule : public SceneModuleInterface
{
public:
- TemplateModule(
+ SamplingPlannerModule(
const std::string & name, rclcpp::Node & node,
- const std::shared_ptr<TemplateParameters> & parameters,
+ const std::shared_ptr<SamplingPlannerParameters> & parameters,
const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
bool isExecutionRequested() const override;
@@ -50,7 +50,7 @@ class TemplateModule : public SceneModuleInterface
void updateModuleParams(const std::any & parameters) override
{
- parameters_ = std::any_cast<std::shared_ptr<TemplateParameters>>(parameters);
+ parameters_ = std::any_cast<std::shared_ptr<SamplingPlannerParameters>>(parameters);
}
void acceptVisitor(
@@ -66,9 +66,9 @@ class TemplateModule : public SceneModuleInterface
bool canTransitIdleToRunningState() override { return false; }
// member
- std::shared_ptr<TemplateParameters> parameters_;
+ std::shared_ptr<SamplingPlannerParameters> parameters_;
};
} // namespace behavior_path_planner
-#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__TEMPLATE__TEMPLATE_MODULE_HPP_
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/template/template_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
similarity index 71%
rename from planning/behavior_path_planner/include/behavior_path_planner/utils/template/template_parameters.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
index 30739bb773896..c6e71c06b028f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/template/template_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__TEMPLATE_PARAMETERS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__TEMPLATE_PARAMETERS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__SAMPLING_PLANNER_PARAMETERS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__SAMPLING_PLANNER_PARAMETERS_HPP_
#include <memory>
#include <string>
@@ -22,10 +22,10 @@
namespace behavior_path_planner
{
-struct TemplateParameters
+struct SamplingPlannerParameters
{
double dummy_parameter{0.0};
};
} // namespace behavior_path_planner
-#endif // BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__TEMPLATE_PARAMETERS_HPP_
+#endif // BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__SAMPLING_PLANNER_PARAMETERS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/template/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
similarity index 76%
rename from planning/behavior_path_planner/include/behavior_path_planner/utils/template/util.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index 54ef9e75e379b..230cbaa90ce88 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/template/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -12,12 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__UTIL_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__UTIL_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
namespace behavior_path_planner
{
} // namespace behavior_path_planner
-#endif // BEHAVIOR_PATH_PLANNER__UTILS__TEMPLATE__UTIL_HPP_
+#endif // BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
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 3eb5eca5954fb..7e990b2236c8b 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -144,6 +144,14 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
path_reference_publishers_.emplace(
manager->name(), create_publisher<Path>(path_reference_name_space + manager->name(), 1));
}
+ {
+ ModuleConfigParameters config;
+ config.enable_module = true;
+ config.priority = 16;
+ auto manager =
+ std::make_shared<SamplingPlannerModuleManager>(this, "sampling_planner", config);
+ planner_manager_->registerSceneModuleManager(manager);
+ }
}
m_set_param_res = this->add_on_set_parameters_callback(
diff --git a/planning/behavior_path_planner/src/scene_module/template/manager.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
similarity index 87%
rename from planning/behavior_path_planner/src/scene_module/template/manager.cpp
rename to planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
index 3ddc2f345ebf6..5deb93d60ce34 100644
--- a/planning/behavior_path_planner/src/scene_module/template/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "behavior_path_planner/scene_module/template/manager.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/manager.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"
@@ -25,7 +25,7 @@
namespace behavior_path_planner
{
-TemplateModuleManager::TemplateModuleManager(
+SamplingPlannerModuleManager::SamplingPlannerModuleManager(
rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config)
: SceneModuleManagerInterface(node, name, config, {})
{
diff --git a/planning/behavior_path_planner/src/scene_module/template/template_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
similarity index 74%
rename from planning/behavior_path_planner/src/scene_module/template/template_module.cpp
rename to planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 5b41f8011ac90..f56e9f0580e33 100644
--- a/planning/behavior_path_planner/src/scene_module/template/template_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "behavior_path_planner/scene_module/template/template_module.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp"
namespace behavior_path_planner
{
@@ -24,30 +24,30 @@ using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
using tier4_autoware_utils::getPoint;
-TemplateModule::TemplateModule(
+SamplingPlannerModule::SamplingPlannerModule(
const std::string & name, rclcpp::Node & node,
- const std::shared_ptr<TemplateParameters> & parameters,
+ const std::shared_ptr<SamplingPlannerParameters> & parameters,
const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map)
: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}
{
}
-bool TemplateModule::isExecutionRequested() const
+bool SamplingPlannerModule::isExecutionRequested() const
{
return true;
}
-bool TemplateModule::isExecutionReady() const
+bool SamplingPlannerModule::isExecutionReady() const
{
return true;
}
-BehaviorModuleOutput TemplateModule::plan()
+BehaviorModuleOutput SamplingPlannerModule::plan()
{
return {};
}
-CandidateOutput TemplateModule::planCandidate() const
+CandidateOutput SamplingPlannerModule::planCandidate() const
{
return {};
}
From 271985c7f17aa2ff6250beddae7776b19502b1ab Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 26 Sep 2023 16:34:09 +0900
Subject: [PATCH 007/115] Copy sampling-based planner to behavior path planner
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner/bezier_sampler/README.md | 3 +
.../include/bezier_sampler/bezier.hpp | 75 ++++
.../bezier_sampler/bezier_sampling.hpp | 49 +++
.../sampling_planner/frenet_planner/README.md | 7 +
.../include/frenet_planner/conversions.hpp | 49 +++
.../include/frenet_planner/frenet_planner.hpp | 67 ++++
.../include/frenet_planner/polynomials.hpp | 55 +++
.../include/frenet_planner/structures.hpp | 156 ++++++++
.../path_sampler/common_structs.hpp | 136 +++++++
.../sampling_planner/path_sampler/node.hpp | 104 ++++++
.../path_sampler/parameters.hpp | 51 +++
.../path_sampler/path_generation.hpp | 52 +++
.../path_sampler/prepare_inputs.hpp | 54 +++
.../path_sampler/type_alias.hpp | 51 +++
.../sampler_common/constraints/footprint.hpp | 29 ++
.../constraints/hard_constraint.hpp | 26 ++
.../constraints/map_constraints.hpp | 60 +++
.../constraints/soft_constraint.hpp | 35 ++
.../sampler_common/structures.hpp | 352 ++++++++++++++++++
.../transform/spline_transform.hpp | 85 +++++
.../path_sampler_utils/geometry_utils.hpp | 56 +++
.../path_sampler_utils/trajectory_utils.hpp | 200 ++++++++++
.../bezier_sampler/bezier.cpp | 120 ++++++
.../bezier_sampler/bezier_sampling.cpp | 78 ++++
.../frenet_planner/frenet_planner.cpp | 227 +++++++++++
.../frenet_planner/polynomials.cpp | 75 ++++
.../sampling_planner/sampler_common/README.md | 6 +
.../sampler_common/constraints/footprint.cpp | 53 +++
.../constraints/hard_constraint.cpp | 56 +++
.../constraints/soft_constraint.cpp | 54 +++
.../transform/spline_transform.cpp | 315 ++++++++++++++++
31 files changed, 2736 insertions(+)
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/README.md
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/README.md
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/conversions.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/polynomials.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/structures.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp
create mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp
create mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp
create mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp
create mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp
create mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/README.md
create mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/footprint.cpp
create mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp
create mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp
create mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/README.md b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/README.md
new file mode 100644
index 0000000000000..654193006fad3
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/README.md
@@ -0,0 +1,3 @@
+# Bézier sampler
+
+Implementation of bézier curves and their generation following the sampling strategy from <https://ieeexplore.ieee.org/document/8932495>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier.hpp
new file mode 100644
index 0000000000000..93c6ec6b87f36
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier.hpp
@@ -0,0 +1,75 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEZIER_SAMPLER__BEZIER_HPP_
+#define BEZIER_SAMPLER__BEZIER_HPP_
+
+#include <eigen3/Eigen/Core>
+
+#include <vector>
+
+namespace bezier_sampler
+{
+
+// Coefficients for matrix calculation of the quintic Bézier curve.
+const Eigen::Matrix<double, 6, 6> quintic_bezier_coefficients(
+ (Eigen::Matrix<double, 6, 6>() << 1, 0, 0, 0, 0, 0, -5, 5, 0, 0, 0, 0, 10, -20, 10, 0, 0, 0, -10,
+ 30, -30, 10, 0, 0, 5, -20, 30, -20, 5, 0, -1, 5, -10, 10, -5, 1)
+ .finished());
+const Eigen::Matrix<double, 5, 6> quintic_bezier_velocity_coefficients(
+ (Eigen::Matrix<double, 5, 6>() << quintic_bezier_coefficients.row(1) * 1,
+ quintic_bezier_coefficients.row(2) * 2, quintic_bezier_coefficients.row(3) * 3,
+ quintic_bezier_coefficients.row(4) * 4, quintic_bezier_coefficients.row(5) * 5)
+ .finished());
+const Eigen::Matrix<double, 4, 6> quintic_bezier_acceleration_coefficients(
+ (Eigen::Matrix<double, 4, 6>() << quintic_bezier_velocity_coefficients.row(1) * 1,
+ quintic_bezier_velocity_coefficients.row(2) * 2, quintic_bezier_velocity_coefficients.row(3) * 3,
+ quintic_bezier_velocity_coefficients.row(4) * 4)
+ .finished());
+
+/// @brief Quintic Bezier curve
+class Bezier
+{
+ Eigen::Matrix<double, 6, 2> control_points_;
+
+public:
+ /// @brief constructor from a matrix
+ explicit Bezier(Eigen::Matrix<double, 6, 2> control_points);
+ /// @brief constructor from a set of control points
+ explicit Bezier(const std::vector<Eigen::Vector2d> & control_points);
+ /// @brief return the control points
+ [[nodiscard]] const Eigen::Matrix<double, 6, 2> & getControlPoints() const;
+ /// @brief return the curve in cartesian frame with the desired resolution
+ [[nodiscard]] std::vector<Eigen::Vector2d> cartesian(const double resolution) const;
+ /// @brief return the curve in cartesian frame with the desired number of points
+ [[nodiscard]] std::vector<Eigen::Vector2d> cartesian(const int nb_points) const;
+ /// @brief return the curve in cartesian frame (including angle) with the desired number of
+ /// points
+ [[nodiscard]] std::vector<Eigen::Vector3d> cartesianWithHeading(const int nb_points) const;
+ /// @brief calculate the curve value for the given parameter t
+ [[nodiscard]] Eigen::Vector2d value(const double t) const;
+ /// @brief calculate the curve value for the given parameter t (using matrix formulation)
+ [[nodiscard]] Eigen::Vector2d valueM(const double t) const;
+ /// @brief calculate the velocity (1st derivative) for the given parameter t
+ [[nodiscard]] Eigen::Vector2d velocity(const double t) const;
+ /// @brief calculate the acceleration (2nd derivative) for the given parameter t
+ [[nodiscard]] Eigen::Vector2d acceleration(const double t) const;
+ /// @brief return the heading (in radians) of the tangent for the given parameter t
+ [[nodiscard]] double heading(const double t) const;
+ /// @brief calculate the curvature for the given parameter t
+ [[nodiscard]] double curvature(const double t) const;
+};
+} // namespace bezier_sampler
+
+#endif // BEZIER_SAMPLER__BEZIER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp
new file mode 100644
index 0000000000000..624cd585b0a72
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp
@@ -0,0 +1,49 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_
+#define BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_
+
+#include <bezier_sampler/bezier.hpp>
+#include <eigen3/Eigen/Core>
+#include <sampler_common/structures.hpp>
+
+#include <vector>
+
+namespace bezier_sampler
+{
+struct SamplingParameters
+{
+ int nb_t; // Number of samples of normalized tangent vector magnitude
+ double mt_min; // Minimum normalized tangent vector magnitude
+ double mt_max; // Maximum normalized tangent vector magnitude
+ int nb_k; // Number of samples of normalized curvature vector magnitude
+ double mk_min; // Minimum normalized curvature vector magnitude
+ double mk_max; // Minimum normalized curvature vector magnitude
+};
+/// @brief sample Bezier curves given an initial and final state and sampling parameters
+// cspell: ignore Artuñedoet
+/// @details from Section IV of A. Artuñedoet al.: Real-Time Motion Planning Approach for Automated
+/// Driving in Urban Environments
+std::vector<Bezier> sample(
+ const sampler_common::State & initial, const sampler_common::State & final,
+ const SamplingParameters & params);
+/// @brief generate a Bezier curve for the given states, velocities, and accelerations
+Bezier generate(
+ const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose,
+ const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration,
+ const Eigen::Vector2d & final_velocity, const Eigen::Vector2d & final_acceleration);
+} // namespace bezier_sampler
+
+#endif // BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/README.md b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/README.md
new file mode 100644
index 0000000000000..083a880924604
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/README.md
@@ -0,0 +1,7 @@
+# Frenet planner
+
+Trajectory generation in Frenet frame.
+
+## Description
+
+[Original paper](https://www.researchgate.net/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/conversions.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/conversions.hpp
new file mode 100644
index 0000000000000..f8575d8f65936
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/conversions.hpp
@@ -0,0 +1,49 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FRENET_PLANNER__CONVERSIONS_HPP_
+#define FRENET_PLANNER__CONVERSIONS_HPP_
+
+#include "frenet_planner/polynomials.hpp"
+
+#include <sampler_common/structures.hpp>
+
+#include <vector>
+
+namespace frenet_planner
+{
+
+/// @brief calculate the lengths and yaws vectors of the given trajectory
+/// @param [inout] trajectory trajectory
+inline void calculateLengthsAndYaws(Trajectory & trajectory)
+{
+ if (trajectory.points.empty()) return;
+ trajectory.lengths.clear();
+ trajectory.yaws.clear();
+ trajectory.yaws.reserve(trajectory.points.size());
+ trajectory.lengths.reserve(trajectory.points.size());
+
+ trajectory.lengths.push_back(0.0);
+ for (auto it = trajectory.points.begin(); it != std::prev(trajectory.points.end()); ++it) {
+ const auto dx = std::next(it)->x() - it->x();
+ const auto dy = std::next(it)->y() - it->y();
+ trajectory.lengths.push_back(trajectory.lengths.back() + std::hypot(dx, dy));
+ trajectory.yaws.push_back(std::atan2(dy, dx));
+ }
+ const auto last_yaw = trajectory.yaws.empty() ? 0.0 : trajectory.yaws.back();
+ trajectory.yaws.push_back(last_yaw);
+}
+} // namespace frenet_planner
+
+#endif // FRENET_PLANNER__CONVERSIONS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp
new file mode 100644
index 0000000000000..c98e6d97969d4
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp
@@ -0,0 +1,67 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FRENET_PLANNER__FRENET_PLANNER_HPP_
+#define FRENET_PLANNER__FRENET_PLANNER_HPP_
+
+#include "frenet_planner/structures.hpp"
+#include "sampler_common/structures.hpp"
+#include "sampler_common/transform/spline_transform.hpp"
+
+#include <optional>
+#include <vector>
+
+namespace frenet_planner
+{
+/// @brief generate trajectories relative to the reference for the given initial state and sampling
+/// parameters
+std::vector<Trajectory> generateTrajectories(
+ const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
+ const SamplingParameters & sampling_parameters);
+/// @brief generate trajectories relative to the reference for the given initial state and sampling
+/// parameters
+std::vector<Trajectory> generateLowVelocityTrajectories(
+ const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
+ const SamplingParameters & sampling_parameters);
+/// @brief generate paths relative to the reference for the given initial state and sampling
+/// parameters
+std::vector<Path> generatePaths(
+ const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
+ const SamplingParameters & sampling_parameters);
+/// @brief generate a candidate path
+/// @details one polynomial for lateral motion (d) is calculated over the longitudinal displacement
+/// (s): d(s).
+Path generateCandidate(
+ const FrenetState & initial_state, const FrenetState & target_state, const double s_resolution);
+/// @brief generate a candidate trajectory
+/// @details the polynomials for lateral motion (d) and longitudinal motion (s) are calculated over
+/// time: d(t) and s(t).
+Trajectory generateCandidate(
+ const FrenetState & initial_state, const FrenetState & target_state, const double duration,
+ const double time_resolution);
+/// @brief generate a low velocity candidate trajectory
+/// @details the polynomial for lateral motion (d) is calculated over the longitudinal displacement
+/// (s) rather than over time: d(s) and s(t).
+Trajectory generateLowVelocityCandidate(
+ const FrenetState & initial_state, const FrenetState & target_state, const double duration,
+ const double time_resolution);
+/// @brief calculate the cartesian frame of the given path
+void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path);
+/// @brief calculate the cartesian frame of the given trajectory
+void calculateCartesian(
+ const sampler_common::transform::Spline2D & reference, Trajectory & trajectory);
+
+} // namespace frenet_planner
+
+#endif // FRENET_PLANNER__FRENET_PLANNER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/polynomials.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/polynomials.hpp
new file mode 100644
index 0000000000000..65157c76d5db3
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/polynomials.hpp
@@ -0,0 +1,55 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FRENET_PLANNER__POLYNOMIALS_HPP_
+#define FRENET_PLANNER__POLYNOMIALS_HPP_
+
+namespace frenet_planner
+{
+class Polynomial
+{
+ /// @brief polynomial coefficients
+ double a_;
+ double b_;
+ double c_;
+ double d_;
+ double e_;
+ double f_;
+
+public:
+ /// @brief Create a quintic polynomial between an initial and final state over a parameter length
+ /// @param x0 initial position
+ /// @param x0v initial velocity
+ /// @param x0a initial acceleration
+ /// @param xT final position
+ /// @param xTv final velocity
+ /// @param xTa final acceleration
+ /// @param T parameter length (arc length or duration)
+ Polynomial(
+ const double x0, const double x0v, const double x0a, const double xT, const double xTv,
+ const double xTa, const double T);
+ // TODO(Maxime CLEMENT) add quartic case for when final position is not given
+
+ /// @brief Get the position at the given time
+ [[nodiscard]] double position(const double t) const;
+ /// @brief Get the velocity at the given time
+ [[nodiscard]] double velocity(const double t) const;
+ /// @brief Get the acceleration at the given time
+ [[nodiscard]] double acceleration(const double t) const;
+ /// @brief Get the jerk at the given time
+ [[nodiscard]] double jerk(const double t) const;
+};
+} // namespace frenet_planner
+
+#endif // FRENET_PLANNER__POLYNOMIALS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/structures.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/structures.hpp
new file mode 100644
index 0000000000000..c44cb5d814d71
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/structures.hpp
@@ -0,0 +1,156 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FRENET_PLANNER__STRUCTURES_HPP_
+#define FRENET_PLANNER__STRUCTURES_HPP_
+
+#include "frenet_planner/polynomials.hpp"
+
+#include <sampler_common/structures.hpp>
+
+#include <optional>
+#include <vector>
+
+namespace frenet_planner
+{
+
+struct FrenetState
+{
+ sampler_common::FrenetPoint position = {0, 0};
+ double lateral_velocity{};
+ double longitudinal_velocity{};
+ double lateral_acceleration{};
+ double longitudinal_acceleration{};
+};
+
+struct Path : sampler_common::Path
+{
+ std::vector<sampler_common::FrenetPoint> frenet_points{};
+ std::optional<Polynomial> lateral_polynomial{};
+
+ Path() = default;
+ explicit Path(const sampler_common::Path & path) : sampler_common::Path(path) {}
+
+ void clear() override
+ {
+ sampler_common::Path::clear();
+ frenet_points.clear();
+ lateral_polynomial.reset();
+ }
+
+ void reserve(const size_t size) override
+ {
+ sampler_common::Path::reserve(size);
+ frenet_points.reserve(size);
+ }
+
+ [[nodiscard]] Path extend(const Path & path) const
+ {
+ Path extended_traj(sampler_common::Path::extend(path));
+ extended_traj.frenet_points.insert(
+ extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end());
+ extended_traj.frenet_points.insert(
+ extended_traj.frenet_points.end(), path.frenet_points.begin(), path.frenet_points.end());
+ // TODO(Maxime CLEMENT): direct copy from the 2nd trajectory. might need to be improved
+ extended_traj.lateral_polynomial = path.lateral_polynomial;
+ return extended_traj;
+ }
+
+ [[nodiscard]] Path * subset(const size_t from_idx, const size_t to_idx) const override
+ {
+ auto * subpath = new Path(*sampler_common::Path::subset(from_idx, to_idx));
+ assert(to_idx >= from_idx);
+ subpath->reserve(to_idx - from_idx);
+
+ const auto copy_subset = [&](const auto & from, auto & to) {
+ to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx));
+ };
+ copy_subset(frenet_points, subpath->frenet_points);
+ return subpath;
+ };
+};
+
+struct SamplingParameter
+{
+ double target_duration{};
+ FrenetState target_state;
+};
+
+struct Trajectory : sampler_common::Trajectory
+{
+ std::vector<sampler_common::FrenetPoint> frenet_points{};
+ std::optional<Polynomial> lateral_polynomial{};
+ std::optional<Polynomial> longitudinal_polynomial{};
+ SamplingParameter sampling_parameter;
+
+ Trajectory() = default;
+ explicit Trajectory(const sampler_common::Trajectory & traj) : sampler_common::Trajectory(traj) {}
+
+ void clear() override
+ {
+ sampler_common::Trajectory::clear();
+ frenet_points.clear();
+ lateral_polynomial.reset();
+ longitudinal_polynomial.reset();
+ }
+
+ void reserve(const size_t size) override
+ {
+ sampler_common::Trajectory::reserve(size);
+ frenet_points.reserve(size);
+ }
+
+ [[nodiscard]] Trajectory extend(const Trajectory & traj) const
+ {
+ Trajectory extended_traj(sampler_common::Trajectory::extend(traj));
+ extended_traj.frenet_points.insert(
+ extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end());
+ extended_traj.frenet_points.insert(
+ extended_traj.frenet_points.end(), traj.frenet_points.begin(), traj.frenet_points.end());
+ // TODO(Maxime CLEMENT): direct copy from the 2nd trajectory. might need to be improved
+ extended_traj.lateral_polynomial = traj.lateral_polynomial;
+ extended_traj.longitudinal_polynomial = traj.longitudinal_polynomial;
+ return extended_traj;
+ }
+
+ [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override
+ {
+ auto * sub_trajectory = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx));
+ assert(to_idx >= from_idx);
+ sub_trajectory->reserve(to_idx - from_idx);
+
+ const auto copy_subset = [&](const auto & from, auto & to) {
+ to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx));
+ };
+ copy_subset(frenet_points, sub_trajectory->frenet_points);
+ return sub_trajectory;
+ };
+};
+
+inline std::ostream & operator<<(std::ostream & os, const SamplingParameter & sp)
+{
+ const auto & s = sp.target_state;
+ return os << "["
+ << "T=" << sp.target_duration << ", s=" << s.position.s << ", d=" << s.position.d
+ << ", s'=" << s.longitudinal_velocity << ", d'=" << s.lateral_velocity
+ << ", s\"=" << s.longitudinal_acceleration << ", d\"=" << s.lateral_acceleration << "]";
+}
+struct SamplingParameters
+{
+ std::vector<SamplingParameter> parameters;
+ double resolution;
+};
+} // namespace frenet_planner
+
+#endif // FRENET_PLANNER__STRUCTURES_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp
new file mode 100644
index 0000000000000..7cf88cb75e13a
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp
@@ -0,0 +1,136 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PATH_SAMPLER__COMMON_STRUCTS_HPP_
+#define PATH_SAMPLER__COMMON_STRUCTS_HPP_
+
+#include "path_sampler/type_alias.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sampler_common/structures.hpp"
+#include "tier4_autoware_utils/ros/update_param.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
+
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
+
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace path_sampler
+{
+struct PlannerData
+{
+ // input
+ Header header;
+ std::vector<TrajectoryPoint> traj_points; // converted from the input path
+ std::vector<geometry_msgs::msg::Point> left_bound;
+ std::vector<geometry_msgs::msg::Point> right_bound;
+
+ // ego
+ geometry_msgs::msg::Pose ego_pose;
+ double ego_vel{};
+};
+
+struct TimeKeeper
+{
+ void init() { accumulated_msg = "\n"; }
+
+ template <typename T>
+ TimeKeeper & operator<<(const T & msg)
+ {
+ latest_stream << msg;
+ return *this;
+ }
+
+ void endLine()
+ {
+ const auto msg = latest_stream.str();
+ accumulated_msg += msg + "\n";
+ latest_stream.str("");
+
+ if (enable_calculation_time_info) {
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("path_sampler.time"), msg);
+ }
+ }
+
+ void tic(const std::string & func_name) { stop_watch_.tic(func_name); }
+
+ void toc(const std::string & func_name, const std::string & white_spaces)
+ {
+ const double elapsed_time = stop_watch_.toc(func_name);
+ *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]";
+ endLine();
+ }
+
+ std::string getLog() const { return accumulated_msg; }
+
+ bool enable_calculation_time_info;
+ std::string accumulated_msg = "\n";
+ std::stringstream latest_stream;
+
+ tier4_autoware_utils::StopWatch<
+ std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
+ stop_watch_;
+};
+
+struct DebugData
+{
+ std::vector<sampler_common::Path> sampled_candidates{};
+ size_t previous_sampled_candidates_nb = 0UL;
+ std::vector<tier4_autoware_utils::Polygon2d> obstacles{};
+ std::vector<tier4_autoware_utils::MultiPoint2d> footprints{};
+};
+
+struct TrajectoryParam
+{
+ TrajectoryParam() = default;
+ explicit TrajectoryParam(rclcpp::Node * node)
+ {
+ output_delta_arc_length = node->declare_parameter<double>("common.output_delta_arc_length");
+ }
+
+ void onParam(const std::vector<rclcpp::Parameter> & parameters)
+ {
+ using tier4_autoware_utils::updateParam;
+
+ // common
+ updateParam<double>(parameters, "common.output_delta_arc_length", output_delta_arc_length);
+ }
+
+ double output_delta_arc_length;
+};
+
+struct EgoNearestParam
+{
+ EgoNearestParam() = default;
+ explicit EgoNearestParam(rclcpp::Node * node)
+ {
+ dist_threshold = node->declare_parameter<double>("ego_nearest_dist_threshold");
+ yaw_threshold = node->declare_parameter<double>("ego_nearest_yaw_threshold");
+ }
+
+ void onParam(const std::vector<rclcpp::Parameter> & parameters)
+ {
+ using tier4_autoware_utils::updateParam;
+ updateParam<double>(parameters, "ego_nearest_dist_threshold", dist_threshold);
+ updateParam<double>(parameters, "ego_nearest_yaw_threshold", yaw_threshold);
+ }
+
+ double dist_threshold{0.0};
+ double yaw_threshold{0.0};
+};
+} // namespace path_sampler
+
+#endif // PATH_SAMPLER__COMMON_STRUCTS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp
new file mode 100644
index 0000000000000..a9002c8cf8a9f
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp
@@ -0,0 +1,104 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PATH_SAMPLER__NODE_HPP_
+#define PATH_SAMPLER__NODE_HPP_
+
+#include "path_sampler/common_structs.hpp"
+#include "path_sampler/parameters.hpp"
+#include "path_sampler/type_alias.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sampler_common/transform/spline_transform.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
+
+#include <sampler_common/structures.hpp>
+
+#include <algorithm>
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace path_sampler
+{
+class PathSampler : public rclcpp::Node
+{
+public:
+ explicit PathSampler(const rclcpp::NodeOptions & node_options);
+
+protected: // for the static_centerline_optimizer package
+ // argument variables
+ vehicle_info_util::VehicleInfo vehicle_info_{};
+ mutable DebugData debug_data_{};
+ mutable std::shared_ptr<TimeKeeper> time_keeper_ptr_{nullptr};
+
+ // parameters
+ TrajectoryParam traj_param_{};
+ EgoNearestParam ego_nearest_param_{};
+ Parameters params_;
+ size_t debug_id_ = 0;
+
+ // variables for subscribers
+ Odometry::SharedPtr ego_state_ptr_;
+ PredictedObjects::SharedPtr in_objects_ptr_ = std::make_shared<PredictedObjects>();
+
+ // variables for previous information
+ std::optional<sampler_common::Path> prev_path_;
+
+ // interface publisher
+ rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
+ rclcpp::Publisher<MarkerArray>::SharedPtr virtual_wall_pub_;
+
+ // interface subscriber
+ rclcpp::Subscription<Path>::SharedPtr path_sub_;
+ rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
+ rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
+
+ // debug publisher
+ rclcpp::Publisher<MarkerArray>::SharedPtr debug_markers_pub_;
+ rclcpp::Publisher<StringStamped>::SharedPtr debug_calculation_time_pub_;
+
+ // parameter callback
+ rcl_interfaces::msg::SetParametersResult onParam(
+ const std::vector<rclcpp::Parameter> & parameters);
+ OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+ // subscriber callback function
+ void objectsCallback(const PredictedObjects::SharedPtr msg);
+ void onPath(const Path::SharedPtr);
+
+ // main functions
+ bool isDataReady(const Path & path, rclcpp::Clock clock) const;
+ PlannerData createPlannerData(const Path & path) const;
+ std::vector<TrajectoryPoint> generateTrajectory(const PlannerData & planner_data);
+ std::vector<TrajectoryPoint> extendTrajectory(
+ const std::vector<TrajectoryPoint> & traj_points,
+ const std::vector<TrajectoryPoint> & optimized_points) const;
+ void resetPreviousData();
+ sampler_common::State getPlanningState(
+ sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const;
+
+ // sub-functions of generateTrajectory
+ void copyZ(
+ const std::vector<TrajectoryPoint> & from_traj, std::vector<TrajectoryPoint> & to_traj);
+ void copyVelocity(
+ const std::vector<TrajectoryPoint> & from_traj, std::vector<TrajectoryPoint> & to_traj,
+ const geometry_msgs::msg::Pose & ego_pose);
+ std::vector<TrajectoryPoint> generatePath(const PlannerData & planner_data);
+ void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const;
+ void publishDebugMarker(const std::vector<TrajectoryPoint> & traj_points) const;
+};
+} // namespace path_sampler
+
+#endif // PATH_SAMPLER__NODE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp
new file mode 100644
index 0000000000000..1c5c51e33b431
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp
@@ -0,0 +1,51 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PATH_SAMPLER__PARAMETERS_HPP_
+#define PATH_SAMPLER__PARAMETERS_HPP_
+
+#include "bezier_sampler/bezier_sampling.hpp"
+#include "sampler_common/structures.hpp"
+
+#include <vector>
+
+struct Parameters
+{
+ sampler_common::Constraints constraints;
+ struct
+ {
+ bool enable_frenet{};
+ bool enable_bezier{};
+ double resolution{};
+ int previous_path_reuse_points_nb{};
+ std::vector<double> target_lengths{};
+ std::vector<double> target_lateral_positions{};
+ int nb_target_lateral_positions{};
+ struct
+ {
+ std::vector<double> target_lateral_velocities{};
+ std::vector<double> target_lateral_accelerations{};
+ } frenet;
+ bezier_sampler::SamplingParameters bezier{};
+ } sampling;
+
+ struct
+ {
+ bool force_zero_deviation{};
+ bool force_zero_heading{};
+ bool smooth_reference{};
+ } preprocessing{};
+};
+
+#endif // PATH_SAMPLER__PARAMETERS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp
new file mode 100644
index 0000000000000..41493b74659fc
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp
@@ -0,0 +1,52 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PATH_SAMPLER__PATH_GENERATION_HPP_
+#define PATH_SAMPLER__PATH_GENERATION_HPP_
+
+#include "bezier_sampler/bezier_sampling.hpp"
+#include "frenet_planner/structures.hpp"
+#include "path_sampler/parameters.hpp"
+#include "sampler_common/constraints/hard_constraint.hpp"
+#include "sampler_common/structures.hpp"
+#include "sampler_common/transform/spline_transform.hpp"
+
+#include <autoware_auto_planning_msgs/msg/path.hpp>
+
+#include <vector>
+
+namespace path_sampler
+{
+/**
+ * @brief generate candidate paths for the given problem inputs
+ * @param [in] initial_state initial ego state
+ * @param [in] path_spline spline of the reference path
+ * @param [in] base_length base length of the reuse path (= 0.0 if not reusing a previous path)
+ * @param [in] params parameters
+ * @return candidate paths
+ */
+std::vector<sampler_common::Path> generateCandidatePaths(
+ const sampler_common::State & initial_state,
+ const sampler_common::transform::Spline2D & path_spline, const double base_length,
+ const Parameters & params);
+
+std::vector<sampler_common::Path> generateBezierPaths(
+ const sampler_common::State & initial_state, const double base_length,
+ const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
+std::vector<frenet_planner::Path> generateFrenetPaths(
+ const sampler_common::State & initial_state, const double base_length,
+ const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
+} // namespace path_sampler
+
+#endif // PATH_SAMPLER__PATH_GENERATION_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp
new file mode 100644
index 0000000000000..29cfddfbf8632
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp
@@ -0,0 +1,54 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PATH_SAMPLER__PREPARE_INPUTS_HPP_
+#define PATH_SAMPLER__PREPARE_INPUTS_HPP_
+
+#include "frenet_planner/structures.hpp"
+#include "path_sampler/parameters.hpp"
+#include "path_sampler/type_alias.hpp"
+#include "sampler_common/transform/spline_transform.hpp"
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/path.hpp>
+#include <autoware_auto_planning_msgs/msg/path_point.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/detail/occupancy_grid__struct.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <string>
+#include <vector>
+
+namespace path_sampler
+{
+/// @brief prepare constraints
+void prepareConstraints(
+ sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects,
+ const std::vector<geometry_msgs::msg::Point> & left_bound,
+ const std::vector<geometry_msgs::msg::Point> & right_bound);
+/// @brief prepare sampling parameters to generate Frenet paths
+frenet_planner::SamplingParameters prepareSamplingParameters(
+ const sampler_common::State & initial_state, const double base_length,
+ const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
+/// @brief prepare the 2D spline representation of the given Path message
+sampler_common::transform::Spline2D preparePathSpline(
+ const std::vector<TrajectoryPoint> & path_msg, const bool smooth_path);
+} // namespace path_sampler
+
+#endif // PATH_SAMPLER__PREPARE_INPUTS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp
new file mode 100644
index 0000000000000..8a792eba2f237
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp
@@ -0,0 +1,51 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_
+#define PATH_SAMPLER__TYPE_ALIAS_HPP_
+
+#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
+#include "autoware_auto_planning_msgs/msg/path.hpp"
+#include "autoware_auto_planning_msgs/msg/path_point.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "std_msgs/msg/header.hpp"
+#include "tier4_debug_msgs/msg/string_stamped.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+namespace path_sampler
+{
+// std_msgs
+using std_msgs::msg::Header;
+// planning
+using autoware_auto_planning_msgs::msg::Path;
+using autoware_auto_planning_msgs::msg::PathPoint;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+// perception
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+// navigation
+using nav_msgs::msg::Odometry;
+// visualization
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+// debug
+using tier4_debug_msgs::msg::StringStamped;
+} // namespace path_sampler
+
+#endif // PATH_SAMPLER__TYPE_ALIAS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp
new file mode 100644
index 0000000000000..061f922001a0f
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp
@@ -0,0 +1,29 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_
+#define SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_
+
+#include "sampler_common/structures.hpp"
+
+namespace sampler_common::constraints
+{
+/// @brief Calculate the footprint of the given path
+/// @param path sequence of pose used to build the footprint
+/// @param constraints input constraint object containing vehicle footprint offsets
+/// @return the polygon footprint of the path
+MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constraints);
+} // namespace sampler_common::constraints
+
+#endif // SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp
new file mode 100644
index 0000000000000..475aae4aeea18
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp
@@ -0,0 +1,26 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_
+#define SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_
+
+#include "sampler_common/structures.hpp"
+
+namespace sampler_common::constraints
+{
+/// @brief Check if the path satisfies the hard constraints
+MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints);
+} // namespace sampler_common::constraints
+
+#endif // SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp
new file mode 100644
index 0000000000000..ed04d8104a904
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp
@@ -0,0 +1,60 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_
+#define SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_
+
+#include "sampler_common/structures.hpp"
+
+#include <boost/geometry/algorithms/within.hpp>
+
+#include <vector>
+
+namespace sampler_common::constraints
+{
+struct MapConstraints
+{
+ MapConstraints(
+ const Point2d & ego_pose, const lanelet::LaneletMapConstPtr & map_ptr,
+ const std::vector<lanelet::Ids> route_ids)
+ {
+ }
+ /// @brief check the given path and return the corresponding cost
+ /// \return cost of the path. If negative then the path is invalid.
+ double check(const Path & path)
+ {
+ double cost = 0.0;
+ for (const auto & p : path.points) {
+ bool drivable = false;
+ for (size_t i = 0; i < drivable_polygons.size(); ++i) {
+ if (boost::geometry::within(p, drivable_polygons[i])) {
+ drivable = true;
+ cost += polygon_costs[i];
+ break;
+ }
+ }
+ if (!drivable) {
+ cost = -1;
+ break;
+ }
+ }
+ return cost;
+ }
+
+ MultiPolygon2d drivable_polygons;
+ std::vector<double> polygon_costs;
+};
+} // namespace sampler_common::constraints
+
+#endif // SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp
new file mode 100644
index 0000000000000..9fdb9fe137e49
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp
@@ -0,0 +1,35 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_
+#define SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_
+
+#include "sampler_common/structures.hpp"
+#include "sampler_common/transform/spline_transform.hpp"
+
+namespace sampler_common::constraints
+{
+/// @brief calculate the curvature cost of the given path
+void calculateCurvatureCost(Path & path, const Constraints & constraints);
+/// @brief calculate the length cost of the given path
+void calculateLengthCost(Path & path, const Constraints & constraints);
+/// @brief calculate the lateral deviation cost at the end of the given path
+void calculateLateralDeviationCost(
+ Path & path, const Constraints & constraints, const transform::Spline2D & reference);
+/// @brief calculate the overall cost of the given path
+void calculateCost(
+ Path & path, const Constraints & constraints, const transform::Spline2D & reference);
+} // namespace sampler_common::constraints
+
+#endif // SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp
new file mode 100644
index 0000000000000..1cb14fdf56198
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp
@@ -0,0 +1,352 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SAMPLER_COMMON__STRUCTURES_HPP_
+#define SAMPLER_COMMON__STRUCTURES_HPP_
+
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+
+#include <eigen3/Eigen/Core>
+#include <interpolation/linear_interpolation.hpp>
+
+#include <boost/geometry/core/cs.hpp>
+#include <boost/geometry/geometries/multi_polygon.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+
+#include <algorithm>
+#include <iostream>
+#include <memory>
+#include <numeric>
+#include <string>
+#include <vector>
+
+namespace sampler_common
+{
+using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::MultiPoint2d;
+using tier4_autoware_utils::MultiPolygon2d;
+using tier4_autoware_utils::Point2d;
+using tier4_autoware_utils::Polygon2d;
+
+/// @brief data about constraint check results of a given path
+struct ConstraintResults
+{
+ bool collision = true;
+ bool curvature = true;
+ bool drivable_area = true;
+
+ [[nodiscard]] bool isValid() const { return collision && curvature && drivable_area; }
+
+ void clear() { collision = curvature = drivable_area = true; }
+};
+struct FrenetPoint
+{
+ FrenetPoint(double s_, double d_) : s(s_), d(d_) {}
+ double s = 0.0;
+ double d = 0.0;
+};
+
+struct State
+{
+ Point2d pose{};
+ FrenetPoint frenet{0.0, 0.0};
+ double curvature{};
+ double heading{};
+};
+
+struct Configuration : State
+{
+ double velocity{};
+ double acceleration{};
+};
+
+/// @brief Path
+struct Path
+{
+ std::vector<Point2d> points{};
+ std::vector<double> curvatures{};
+ std::vector<double> yaws{};
+ std::vector<double> lengths{};
+ ConstraintResults constraint_results{};
+ double cost{};
+ std::string tag{}; // string tag used for debugging
+
+ Path() = default;
+ virtual ~Path() = default;
+
+ virtual void clear()
+ {
+ points.clear();
+ curvatures.clear();
+ yaws.clear();
+ lengths.clear();
+ constraint_results.clear();
+ tag = "";
+ cost = 0.0;
+ }
+
+ virtual void reserve(const size_t size)
+ {
+ points.reserve(size);
+ curvatures.reserve(size);
+ yaws.reserve(size);
+ lengths.reserve(size);
+ }
+
+ [[nodiscard]] Path extend(const Path & path) const
+ {
+ Path extended_path;
+ auto offset = 0l;
+ auto length_offset = 0.0;
+ if (!points.empty() && !path.points.empty()) {
+ if (
+ points.back().x() == path.points.front().x() &&
+ points.back().y() == path.points.front().y()) {
+ offset = 1l;
+ }
+ length_offset = std::hypot(
+ path.points[offset].x() - points.back().x(), path.points[offset].y() - points.back().y());
+ }
+ const auto ext = [&](auto & dest, const auto & first, const auto & second) {
+ dest.insert(dest.end(), first.begin(), first.end());
+ dest.insert(dest.end(), std::next(second.begin(), offset), second.end());
+ };
+ ext(extended_path.points, points, path.points);
+ ext(extended_path.curvatures, curvatures, path.curvatures);
+ ext(extended_path.yaws, yaws, path.yaws);
+ extended_path.lengths.insert(extended_path.lengths.end(), lengths.begin(), lengths.end());
+ const auto last_base_length = lengths.empty() ? 0.0 : lengths.back() + length_offset;
+ for (size_t i = offset; i < path.lengths.size(); ++i)
+ extended_path.lengths.push_back(last_base_length + path.lengths[i]);
+ extended_path.cost = path.cost;
+ extended_path.constraint_results = path.constraint_results;
+ extended_path.tag = path.tag;
+ return extended_path;
+ }
+
+ // Return a pointer to allow overriding classes to return the appropriate type
+ // Without pointer we are stuck with returning a Path
+ [[nodiscard]] virtual Path * subset(const size_t from_idx, const size_t to_idx) const
+ {
+ auto * subpath = new Path();
+ subpath->reserve(to_idx - from_idx);
+
+ const auto copy_subset = [&](const auto & from, auto & to) {
+ to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx));
+ };
+ copy_subset(points, subpath->points);
+ copy_subset(curvatures, subpath->curvatures);
+ copy_subset(yaws, subpath->yaws);
+ copy_subset(lengths, subpath->lengths);
+ return subpath;
+ };
+};
+
+struct Trajectory : Path
+{
+ std::vector<double> longitudinal_velocities{};
+ std::vector<double> longitudinal_accelerations{};
+ std::vector<double> lateral_velocities{};
+ std::vector<double> lateral_accelerations{};
+ std::vector<double> jerks{};
+ std::vector<double> times{};
+
+ Trajectory() = default;
+ ~Trajectory() override = default;
+ explicit Trajectory(const Path & path) : Path(path) {}
+
+ void clear() override
+ {
+ Path::clear();
+ longitudinal_velocities.clear();
+ longitudinal_accelerations.clear();
+ lateral_velocities.clear();
+ lateral_accelerations.clear();
+ jerks.clear();
+ times.clear();
+ }
+
+ void reserve(const size_t size) override
+ {
+ Path::reserve(size);
+ longitudinal_velocities.reserve(size);
+ longitudinal_accelerations.reserve(size);
+ lateral_velocities.reserve(size);
+ lateral_accelerations.reserve(size);
+ jerks.reserve(size);
+ times.reserve(size);
+ }
+
+ [[nodiscard]] Trajectory extend(const Trajectory & traj) const
+ {
+ Trajectory extended_traj(Path::extend(traj));
+ auto offset = 0l;
+ auto time_offset = 0.0;
+ // TODO(Maxime): remove these checks
+ if (!points.empty() && !traj.points.empty()) {
+ if (
+ points.back().x() == traj.points.front().x() &&
+ points.back().y() == traj.points.front().y())
+ offset = 1l;
+ const auto ds = std::hypot(
+ traj.points[offset].x() - points.back().x(), traj.points[offset].y() - points.back().y());
+ const auto v = std::max(
+ std::max(longitudinal_velocities.back(), traj.longitudinal_velocities[offset]), 0.1);
+ time_offset = std::abs(ds / v);
+ }
+ const auto ext = [&](auto & dest, const auto & first, const auto & second) {
+ dest.insert(dest.end(), first.begin(), first.end());
+ dest.insert(dest.end(), std::next(second.begin(), offset), second.end());
+ };
+ ext(
+ extended_traj.longitudinal_velocities, longitudinal_velocities, traj.longitudinal_velocities);
+ ext(
+ extended_traj.longitudinal_accelerations, longitudinal_accelerations,
+ traj.longitudinal_accelerations);
+ ext(extended_traj.lateral_velocities, lateral_velocities, traj.lateral_velocities);
+ ext(extended_traj.lateral_accelerations, lateral_accelerations, traj.lateral_accelerations);
+ ext(extended_traj.jerks, jerks, traj.jerks);
+ extended_traj.times.insert(extended_traj.times.end(), times.begin(), times.end());
+ const auto last_base_time = times.empty() ? 0.0 : times.back() + time_offset;
+ for (size_t i = offset; i < traj.times.size(); ++i)
+ extended_traj.times.push_back(last_base_time + traj.times[i]);
+ return extended_traj;
+ }
+
+ [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override
+ {
+ auto * sub_trajectory = new Trajectory(*Path::subset(from_idx, to_idx));
+
+ const auto copy_subset = [&](const auto & from, auto & to) {
+ to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx));
+ };
+
+ copy_subset(longitudinal_velocities, sub_trajectory->longitudinal_velocities);
+ copy_subset(longitudinal_accelerations, sub_trajectory->longitudinal_accelerations);
+ copy_subset(lateral_velocities, sub_trajectory->lateral_velocities);
+ copy_subset(lateral_accelerations, sub_trajectory->lateral_accelerations);
+ copy_subset(jerks, sub_trajectory->jerks);
+ copy_subset(times, sub_trajectory->times);
+ return sub_trajectory;
+ }
+
+ [[nodiscard]] Trajectory resample(const double fixed_interval) const
+ {
+ Trajectory t;
+ if (lengths.size() < 2 || fixed_interval <= 0.0) return *this;
+
+ const auto new_size =
+ static_cast<size_t>((lengths.back() - lengths.front()) / fixed_interval) + 1;
+ t.times.reserve(new_size);
+ t.lengths.reserve(new_size);
+ for (auto i = 0lu; i < new_size; ++i)
+ t.lengths.push_back(lengths.front() + static_cast<double>(i) * fixed_interval);
+ t.times = interpolation::lerp(lengths, times, t.lengths);
+ std::vector<double> xs;
+ std::vector<double> ys;
+ xs.reserve(points.size());
+ ys.reserve(points.size());
+ for (const auto & p : points) {
+ xs.push_back(p.x());
+ ys.push_back(p.y());
+ }
+ const auto new_xs = interpolation::lerp(times, xs, t.times);
+ const auto new_ys = interpolation::lerp(times, ys, t.times);
+ for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]);
+ t.curvatures = interpolation::lerp(times, curvatures, t.times);
+ t.jerks = interpolation::lerp(times, jerks, t.times);
+ t.yaws = interpolation::lerp(times, yaws, t.times);
+ t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times);
+ t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times);
+ t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times);
+ t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times);
+ t.constraint_results = constraint_results;
+ t.cost = cost;
+ return t;
+ }
+
+ [[nodiscard]] Trajectory resampleTimeFromZero(const double fixed_interval) const
+ {
+ Trajectory t;
+ if (times.size() < 2 || times.back() < 0.0 || fixed_interval <= 0.0) return *this;
+
+ const auto min_time = 0;
+ const auto max_time = times.back();
+ const auto new_size = static_cast<size_t>((max_time - min_time) / fixed_interval) + 1;
+ t.times.reserve(new_size);
+ t.lengths.reserve(new_size);
+ for (auto i = 0lu; i < new_size; ++i)
+ t.times.push_back(static_cast<double>(i) * fixed_interval);
+ t.lengths = interpolation::lerp(times, lengths, t.times);
+ std::vector<double> xs;
+ std::vector<double> ys;
+ xs.reserve(points.size());
+ ys.reserve(points.size());
+ for (const auto & p : points) {
+ xs.push_back(p.x());
+ ys.push_back(p.y());
+ }
+ const auto new_xs = interpolation::lerp(times, xs, t.times);
+ const auto new_ys = interpolation::lerp(times, ys, t.times);
+ for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]);
+ t.curvatures = interpolation::lerp(times, curvatures, t.times);
+ t.jerks = interpolation::lerp(times, jerks, t.times);
+ t.yaws = interpolation::lerp(times, yaws, t.times);
+ t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times);
+ t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times);
+ t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times);
+ t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times);
+ t.constraint_results = constraint_results;
+ t.cost = cost;
+ return t;
+ }
+};
+
+struct DynamicObstacle
+{
+ std::vector<Polygon2d> footprint_per_time;
+ double time_step; // [s] time step between each footprint
+};
+
+struct Constraints
+{
+ struct
+ {
+ double lateral_deviation_weight;
+ double length_weight;
+ double curvature_weight;
+ } soft{};
+ struct
+ {
+ double min_curvature;
+ double max_curvature;
+ } hard{};
+ LinearRing2d ego_footprint;
+ double ego_width;
+ double ego_length;
+ MultiPolygon2d obstacle_polygons;
+ MultiPolygon2d drivable_polygons;
+ std::vector<DynamicObstacle> dynamic_obstacles;
+};
+
+struct ReusableTrajectory
+{
+ Trajectory trajectory; // base trajectory
+ Configuration planning_configuration; // planning configuration at the end of the trajectory
+};
+
+} // namespace sampler_common
+
+#endif // SAMPLER_COMMON__STRUCTURES_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp
new file mode 100644
index 0000000000000..ce66141832dbf
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp
@@ -0,0 +1,85 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_
+#define SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_
+
+#include "sampler_common/structures.hpp"
+
+#include <vector>
+
+namespace sampler_common::transform
+{
+using sampler_common::FrenetPoint;
+
+class Spline
+{
+ std::vector<double> a_{};
+ std::vector<double> b_{};
+ std::vector<double> c_{};
+ std::vector<double> d_{};
+ std::vector<double> h_{};
+
+public:
+ Spline() = default;
+ Spline(const std::vector<double> & base_index, const std::vector<double> & base_value);
+ explicit Spline(const std::vector<Point2d> & points);
+ bool interpolate(
+ const std::vector<double> & base_index, const std::vector<double> & base_value,
+ const std::vector<double> & return_index, std::vector<double> & return_value);
+ [[nodiscard]] double value(const double query, const std::vector<double> & base_index) const;
+ [[nodiscard]] double velocity(const double query, const std::vector<double> & base_index) const;
+ [[nodiscard]] double acceleration(
+ const double query, const std::vector<double> & base_index) const;
+
+private:
+ void generateSpline(
+ const std::vector<double> & base_index, const std::vector<double> & base_value);
+ [[nodiscard]] static bool isIncrease(const std::vector<double> & x);
+ [[nodiscard]] static bool isValidInput(
+ const std::vector<double> & base_index, const std::vector<double> & base_value,
+ const std::vector<double> & return_index);
+ [[nodiscard]] std::vector<double> solveLinearSystem(
+ const double omega, const size_t max_iter) const;
+ [[nodiscard]] static bool isConvergeL1(
+ const std::vector<double> & r1, const std::vector<double> & r2, const double converge_range);
+};
+
+class Spline2D
+{
+ std::vector<double> s_{};
+ Spline x_spline_{};
+ Spline y_spline_{};
+
+ std::vector<Point2d> original_points_{};
+
+public:
+ Spline2D() = default;
+ Spline2D(const std::vector<double> & x, const std::vector<double> & y);
+ [[nodiscard]] FrenetPoint frenet_naive(const Point2d & p, const double precision = 0.01) const;
+ [[nodiscard]] FrenetPoint frenet(const Point2d & p, const double precision = 0.01) const;
+ [[nodiscard]] Point2d cartesian(const double s) const;
+ [[nodiscard]] Point2d cartesian(const FrenetPoint & fp) const;
+ [[nodiscard]] double curvature(const double s) const;
+ [[nodiscard]] double yaw(const double s) const;
+ [[nodiscard]] double firstS() const { return s_.empty() ? 0.0 : s_.front(); }
+ [[nodiscard]] double lastS() const { return s_.empty() ? 0.0 : s_.back(); }
+
+private:
+ static std::vector<double> arcLength(
+ const std::vector<double> & x, const std::vector<double> & y);
+};
+} // namespace sampler_common::transform
+
+#endif // SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp
new file mode 100644
index 0000000000000..3e5afdbdad696
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp
@@ -0,0 +1,56 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_
+#define PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_
+
+#include "eigen3/Eigen/Core"
+#include "interpolation/linear_interpolation.hpp"
+#include "interpolation/spline_interpolation.hpp"
+#include "interpolation/spline_interpolation_points_2d.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "path_sampler/common_structs.hpp"
+#include "path_sampler/type_alias.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
+
+#include "autoware_auto_planning_msgs/msg/path_point.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+
+#include "boost/optional/optional_fwd.hpp"
+
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace path_sampler
+{
+namespace geometry_utils
+{
+template <typename T1, typename T2>
+bool isSamePoint(const T1 & t1, const T2 & t2)
+{
+ const auto p1 = tier4_autoware_utils::getPoint(t1);
+ const auto p2 = tier4_autoware_utils::getPoint(t2);
+
+ constexpr double epsilon = 1e-6;
+ if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) {
+ return false;
+ }
+ return true;
+}
+} // namespace geometry_utils
+} // namespace path_sampler
+#endif // PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp
new file mode 100644
index 0000000000000..c0a3c1d917c25
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp
@@ -0,0 +1,200 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_
+#define PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_
+
+#include "eigen3/Eigen/Core"
+#include "interpolation/linear_interpolation.hpp"
+#include "interpolation/spline_interpolation.hpp"
+#include "interpolation/spline_interpolation_points_2d.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
+#include "path_sampler/common_structs.hpp"
+#include "path_sampler/type_alias.hpp"
+#include "sampler_common/structures.hpp"
+
+#include "autoware_auto_planning_msgs/msg/path_point.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace path_sampler
+{
+namespace trajectory_utils
+{
+template <typename T>
+std::optional<size_t> getPointIndexAfter(
+ const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx,
+ const double offset)
+{
+ if (points.empty()) {
+ return std::nullopt;
+ }
+
+ double sum_length =
+ -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos);
+
+ // search forward
+ if (sum_length < offset) {
+ for (size_t i = target_seg_idx + 1; i < points.size(); ++i) {
+ sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
+ if (offset < sum_length) {
+ return i - 1;
+ }
+ }
+
+ return std::nullopt;
+ }
+
+ // search backward
+ for (size_t i = target_seg_idx; 0 < i;
+ --i) { // NOTE: use size_t since i is always positive value
+ sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
+ if (sum_length < offset) {
+ return i - 1;
+ }
+ }
+
+ return 0;
+}
+
+template <typename T>
+TrajectoryPoint convertToTrajectoryPoint(const T & point)
+{
+ TrajectoryPoint traj_point;
+ traj_point.pose = tier4_autoware_utils::getPose(point);
+ traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps;
+ traj_point.lateral_velocity_mps = point.lateral_velocity_mps;
+ traj_point.heading_rate_rps = point.heading_rate_rps;
+ return traj_point;
+}
+
+template <typename T>
+std::vector<TrajectoryPoint> convertToTrajectoryPoints(const std::vector<T> & points)
+{
+ std::vector<TrajectoryPoint> traj_points;
+ for (const auto & point : points) {
+ const auto traj_point = convertToTrajectoryPoint(point);
+ traj_points.push_back(traj_point);
+ }
+ return traj_points;
+}
+
+inline std::vector<TrajectoryPoint> convertToTrajectoryPoints(const sampler_common::Path & path)
+{
+ std::vector<TrajectoryPoint> traj_points;
+ for (auto i = 0UL; i < path.points.size(); ++i) {
+ TrajectoryPoint p;
+ p.pose.position.x = path.points[i].x();
+ p.pose.position.y = path.points[i].y();
+ auto quat = tf2::Quaternion();
+ quat.setRPY(0.0, 0.0, path.yaws[i]);
+ p.pose.orientation.w = quat.w();
+ p.pose.orientation.x = quat.x();
+ p.pose.orientation.y = quat.y();
+ p.pose.orientation.z = quat.z();
+ p.longitudinal_velocity_mps = 0.0;
+ p.lateral_velocity_mps = 0.0;
+ p.heading_rate_rps = 0.0;
+ traj_points.push_back(p);
+ }
+ return traj_points;
+}
+
+void compensateLastPose(
+ const PathPoint & last_path_point, std::vector<TrajectoryPoint> & traj_points,
+ const double delta_dist_threshold, const double delta_yaw_threshold);
+
+template <class T>
+size_t findEgoIndex(
+ const std::vector<T> & points, const geometry_msgs::msg::Pose & ego_pose,
+ const EgoNearestParam & ego_nearest_param)
+{
+ return motion_utils::findFirstNearestIndexWithSoftConstraints(
+ points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold);
+}
+
+template <class T>
+size_t findEgoSegmentIndex(
+ const std::vector<T> & points, const geometry_msgs::msg::Pose & ego_pose,
+ const EgoNearestParam & ego_nearest_param)
+{
+ return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+ points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold);
+}
+
+Trajectory createTrajectory(
+ const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points);
+
+std::vector<TrajectoryPoint> resampleTrajectoryPoints(
+ const std::vector<TrajectoryPoint> traj_points, const double interval);
+
+std::vector<TrajectoryPoint> resampleTrajectoryPointsWithoutStopPoint(
+ const std::vector<TrajectoryPoint> traj_points, const double interval);
+
+template <typename T>
+std::optional<size_t> updateFrontPointForFix(
+ std::vector<T> & points, std::vector<T> & points_for_fix, const double delta_arc_length,
+ const EgoNearestParam & ego_nearest_param)
+{
+ // calculate front point to insert in points as a fixed point
+ const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex(
+ points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param);
+ const size_t front_point_idx_for_fix = front_seg_idx_for_fix;
+ const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix);
+
+ // check if the points_for_fix is longer in front than points
+ const double lon_offset_to_prev_front =
+ motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position);
+ if (0 < lon_offset_to_prev_front) {
+ RCLCPP_WARN(
+ rclcpp::get_logger("path_sampler.trajectory_utils"),
+ "Fixed point will not be inserted due to the error during calculation.");
+ return std::nullopt;
+ }
+
+ const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point);
+
+ // check if deviation is not too large
+ constexpr double max_lat_error = 3.0;
+ if (max_lat_error < dist) {
+ RCLCPP_WARN(
+ rclcpp::get_logger("path_sampler.trajectory_utils"),
+ "New Fixed point is too far from points %f [m]", dist);
+ }
+
+ // update pose
+ if (dist < delta_arc_length) {
+ // only pose is updated
+ points.front() = front_fix_point;
+ } else {
+ // add new front point
+ T new_front_point;
+ new_front_point = front_fix_point;
+ points.insert(points.begin(), new_front_point);
+ }
+
+ return front_point_idx_for_fix;
+}
+
+void insertStopPoint(
+ std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & input_stop_pose,
+ const size_t stop_seg_idx);
+} // namespace trajectory_utils
+} // namespace path_sampler
+#endif // PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp
new file mode 100644
index 0000000000000..fc4c09840f507
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp
@@ -0,0 +1,120 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <bezier_sampler/bezier.hpp>
+
+#include <iostream>
+
+namespace bezier_sampler
+{
+Bezier::Bezier(Eigen::Matrix<double, 6, 2> control_points)
+: control_points_(std::move(control_points))
+{
+}
+Bezier::Bezier(const std::vector<Eigen::Vector2d> & control_points)
+{
+ if (control_points.size() != 6) {
+ std::cerr << "Trying to initialize a quintic bezier curve with " << control_points.size()
+ << " (!= 6) control points." << std::endl;
+ } else {
+ control_points_ << control_points[0], control_points[1], control_points[2], control_points[3],
+ control_points[4], control_points[5];
+ }
+}
+
+const Eigen::Matrix<double, 6, 2> & Bezier::getControlPoints() const
+{
+ return control_points_;
+}
+
+Eigen::Vector2d Bezier::value(const double t) const
+{
+ Eigen::Vector2d point = {0.0, 0.0};
+ // sum( binomial(i in 5) * (1 - t)^(5-i) * t^i * control_points_[i] )
+ point += std::pow((1 - t), 5) * control_points_.row(0);
+ point += 5 * std::pow((1 - t), 4) * t * control_points_.row(1);
+ point += 10 * std::pow((1 - t), 3) * t * t * control_points_.row(2);
+ point += 10 * std::pow((1 - t), 2) * t * t * t * control_points_.row(3);
+ point += 5 * (1 - t) * t * t * t * t * control_points_.row(4);
+ point += t * t * t * t * t * control_points_.row(5);
+ return point;
+}
+
+Eigen::Vector2d Bezier::valueM(const double t) const
+{
+ Eigen::Matrix<double, 1, 6> ts;
+ ts << 1, t, t * t, t * t * t, t * t * t * t, t * t * t * t * t;
+ return ts * quintic_bezier_coefficients * control_points_;
+}
+
+std::vector<Eigen::Vector2d> Bezier::cartesian(const int nb_points) const
+{
+ std::vector<Eigen::Vector2d> points;
+ points.reserve(nb_points);
+ const double step = 1.0 / (nb_points - 1);
+ for (double t = 0.0; t <= 1.0; t += step) points.push_back(valueM(t));
+ return points;
+}
+
+std::vector<Eigen::Vector2d> Bezier::cartesian(const double resolution) const
+{
+ std::vector<Eigen::Vector2d> points;
+ points.reserve(static_cast<int>(1 / resolution));
+ for (double t = 0.0; t <= 1.0; t += resolution) points.push_back(valueM(t));
+ return points;
+}
+
+std::vector<Eigen::Vector3d> Bezier::cartesianWithHeading(const int nb_points) const
+{
+ std::vector<Eigen::Vector3d> points;
+ points.reserve(nb_points);
+ const double step = 1.0 / (nb_points - 1);
+ for (double t = 0.0; t <= 1.0; t += step) {
+ Eigen::Vector2d point = valueM(t);
+ points.emplace_back(point.x(), point.y(), heading(t));
+ }
+ return points;
+}
+
+Eigen::Vector2d Bezier::velocity(const double t) const
+{
+ Eigen::Matrix<double, 1, 5> ts;
+ ts << 1, t, t * t, t * t * t, t * t * t * t;
+ return ts * quintic_bezier_velocity_coefficients * control_points_;
+}
+
+Eigen::Vector2d Bezier::acceleration(const double t) const
+{
+ Eigen::Matrix<double, 1, 4> ts;
+ ts << 1, t, t * t, t * t * t;
+ return ts * quintic_bezier_acceleration_coefficients * control_points_;
+}
+
+double Bezier::curvature(const double t) const
+{
+ const Eigen::Vector2d vel = velocity(t);
+ const Eigen::Vector2d accel = acceleration(t);
+ double curvature = std::numeric_limits<double>::infinity();
+ const double denominator = std::pow(vel.x() * vel.x() + vel.y() * vel.y(), 3.0 / 2.0);
+ if (denominator != 0) curvature = (vel.x() * accel.y() - accel.x() * vel.y()) / denominator;
+ return curvature;
+}
+
+double Bezier::heading(const double t) const
+{
+ const Eigen::Vector2d vel = velocity(t);
+ return std::atan2(vel.y(), vel.x());
+}
+
+} // namespace bezier_sampler
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp
new file mode 100644
index 0000000000000..a9fa318980250
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp
@@ -0,0 +1,78 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <bezier_sampler/bezier_sampling.hpp>
+
+namespace bezier_sampler
+{
+std::vector<Bezier> sample(
+ const sampler_common::State & initial, const sampler_common::State & final,
+ const SamplingParameters & params)
+{
+ std::vector<Bezier> curves;
+ curves.reserve(params.nb_t * params.nb_t * params.nb_k);
+
+ double distance_initial_to_final = std::sqrt(
+ (initial.pose.x() - final.pose.x()) * (initial.pose.x() - final.pose.x()) +
+ (initial.pose.y() - final.pose.y()) * (initial.pose.y() - final.pose.y()));
+ // Tangent vectors
+ const Eigen::Vector2d initial_tangent_unit(std::cos(initial.heading), std::sin(initial.heading));
+ const Eigen::Vector2d final_tangent_unit(std::cos(final.heading), std::sin(final.heading));
+ // Unit vectors
+ const Eigen::Vector2d initial_normal_unit = initial_tangent_unit.unitOrthogonal();
+ const Eigen::Vector2d final_normal_unit = final_tangent_unit.unitOrthogonal();
+
+ double step_t = (params.mt_max - params.mt_min) / (params.nb_t - 1);
+ double step_k = (params.mk_max - params.mk_min) / (params.nb_k - 1);
+ for (double m_initial = params.mt_min; m_initial <= params.mt_max; m_initial += step_t) {
+ double initial_tangent_length = m_initial * distance_initial_to_final;
+ for (double m_final = params.mt_min; m_final <= params.mt_max; m_final += step_t) {
+ double final_tangent_length = m_final * distance_initial_to_final;
+ for (double k = params.mk_min; k <= params.mk_max; k += step_k) {
+ double acceleration_length = k * distance_initial_to_final;
+ Eigen::Vector2d initial_acceleration =
+ acceleration_length * initial_tangent_unit +
+ initial.curvature * initial_tangent_length * initial_tangent_length * initial_normal_unit;
+ Eigen::Vector2d final_acceleration =
+ acceleration_length * final_tangent_unit +
+ final.curvature * final_tangent_length * final_tangent_length * final_normal_unit;
+ curves.push_back(generate(
+ {initial.pose.x(), initial.pose.y()}, {final.pose.x(), final.pose.y()},
+ initial_tangent_unit * initial_tangent_length, initial_acceleration,
+ final_tangent_unit * final_tangent_length, final_acceleration));
+ }
+ }
+ }
+ return curves;
+}
+Bezier generate(
+ const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose,
+ const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration,
+ const Eigen::Vector2d & final_velocity, const Eigen::Vector2d & final_acceleration)
+{
+ Eigen::Matrix<double, 6, 2> control_points;
+ // P0 and P5 correspond to the initial and final configurations
+ control_points.row(0) = initial_pose;
+ control_points.row(5) = final_pose;
+ // P1 and P4 depend on P0, P5, and the initial/final velocities
+ control_points.row(1) = control_points.row(0) + (1.0 / 5.0) * initial_velocity.transpose();
+ control_points.row(4) = control_points.row(5) - (1.0 / 5.0) * final_velocity.transpose();
+ // P2 and P3 depend on P1, P4, and the initial/final accelerations
+ control_points.row(2) = 2 * control_points.row(1) - control_points.row(0) +
+ (1.0 / 20.0) * initial_acceleration.transpose();
+ control_points.row(3) = 2 * control_points.row(4) - control_points.row(5) -
+ (1.0 / 20.0) * final_acceleration.transpose();
+ return Bezier(control_points);
+}
+} // namespace bezier_sampler
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp
new file mode 100644
index 0000000000000..3368c49459a55
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp
@@ -0,0 +1,227 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "frenet_planner/frenet_planner.hpp"
+
+#include <eigen3/Eigen/Eigen>
+#include <frenet_planner/conversions.hpp>
+#include <frenet_planner/polynomials.hpp>
+#include <frenet_planner/structures.hpp>
+#include <helper_functions/angle_utils.hpp>
+#include <sampler_common/structures.hpp>
+#include <sampler_common/transform/spline_transform.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+
+namespace frenet_planner
+{
+std::vector<Trajectory> generateTrajectories(
+ const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
+ const SamplingParameters & sampling_parameters)
+{
+ std::vector<Trajectory> trajectories;
+ trajectories.reserve(sampling_parameters.parameters.size());
+ for (const auto & parameter : sampling_parameters.parameters) {
+ auto trajectory = generateCandidate(
+ initial_state, parameter.target_state, parameter.target_duration,
+ sampling_parameters.resolution);
+ trajectory.sampling_parameter = parameter;
+ calculateCartesian(reference_spline, trajectory);
+ std::stringstream ss;
+ ss << parameter;
+ trajectory.tag = ss.str();
+ trajectories.push_back(trajectory);
+ }
+ return trajectories;
+}
+
+std::vector<Trajectory> generateLowVelocityTrajectories(
+ const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
+ const SamplingParameters & sampling_parameters)
+{
+ std::vector<Trajectory> trajectories;
+ trajectories.reserve(sampling_parameters.parameters.size());
+ for (const auto & parameter : sampling_parameters.parameters) {
+ auto trajectory = generateLowVelocityCandidate(
+ initial_state, parameter.target_state, parameter.target_duration,
+ sampling_parameters.resolution);
+ calculateCartesian(reference_spline, trajectory);
+ std::stringstream ss;
+ ss << parameter;
+ trajectory.tag = ss.str();
+ trajectories.push_back(trajectory);
+ }
+ return trajectories;
+}
+
+std::vector<Path> generatePaths(
+ const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
+ const SamplingParameters & sampling_parameters)
+{
+ std::vector<Path> candidates;
+ candidates.reserve(sampling_parameters.parameters.size());
+ for (const auto parameter : sampling_parameters.parameters) {
+ auto candidate =
+ generateCandidate(initial_state, parameter.target_state, sampling_parameters.resolution);
+ calculateCartesian(reference_spline, candidate);
+ candidates.push_back(candidate);
+ }
+ return candidates;
+}
+
+Trajectory generateCandidate(
+ const FrenetState & initial_state, const FrenetState & target_state, const double duration,
+ const double time_resolution)
+{
+ Trajectory trajectory;
+ trajectory.longitudinal_polynomial = Polynomial(
+ initial_state.position.s, initial_state.longitudinal_velocity,
+ initial_state.longitudinal_acceleration, target_state.position.s,
+ target_state.longitudinal_velocity, target_state.longitudinal_acceleration, duration);
+ trajectory.lateral_polynomial = Polynomial(
+ initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration,
+ target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration,
+ duration);
+ for (double t = 0.0; t <= duration; t += time_resolution) {
+ trajectory.times.push_back(t);
+ trajectory.frenet_points.emplace_back(
+ trajectory.longitudinal_polynomial->position(t), trajectory.lateral_polynomial->position(t));
+ }
+ return trajectory;
+}
+
+Trajectory generateLowVelocityCandidate(
+ const FrenetState & initial_state, const FrenetState & target_state, const double duration,
+ const double time_resolution)
+{
+ Trajectory trajectory;
+ trajectory.longitudinal_polynomial = Polynomial(
+ initial_state.position.s, initial_state.longitudinal_velocity,
+ initial_state.longitudinal_acceleration, target_state.position.s,
+ target_state.longitudinal_velocity, target_state.longitudinal_acceleration, duration);
+ const auto delta_s = target_state.position.s - initial_state.position.s;
+ trajectory.lateral_polynomial = Polynomial(
+ initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration,
+ target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration,
+ delta_s);
+ for (double t = 0.0; t <= duration; t += time_resolution) {
+ trajectory.times.push_back(t);
+ const auto s = trajectory.longitudinal_polynomial->position(t);
+ const auto ds = s - initial_state.position.s;
+ const auto d = trajectory.lateral_polynomial->position(ds);
+ trajectory.frenet_points.emplace_back(s, d);
+ }
+ return trajectory;
+}
+
+Path generateCandidate(
+ const FrenetState & initial_state, const FrenetState & target_state, const double s_resolution)
+{
+ const auto delta_s = target_state.position.s - initial_state.position.s;
+ Path path;
+ path.lateral_polynomial = Polynomial(
+ initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration,
+ target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration,
+ delta_s);
+ for (double s = s_resolution; s <= delta_s; s += s_resolution) {
+ path.frenet_points.emplace_back(
+ initial_state.position.s + s, path.lateral_polynomial->position(s));
+ }
+ return path;
+}
+
+void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path)
+{
+ if (!path.frenet_points.empty()) {
+ path.points.reserve(path.frenet_points.size());
+ path.yaws.reserve(path.frenet_points.size());
+ path.lengths.reserve(path.frenet_points.size());
+ path.curvatures.reserve(path.frenet_points.size());
+ // Calculate cartesian positions
+ for (const auto & fp : path.frenet_points) {
+ path.points.push_back(reference.cartesian(fp));
+ }
+ // TODO(Maxime CLEMENT): more precise calculations are proposed in Appendix I of the paper:
+ // Optimal path Generation for Dynamic Street Scenarios in a Frenet Frame (Werling2010)
+ // Calculate cartesian yaw and interval values
+ path.lengths.push_back(0.0);
+ for (auto it = path.points.begin(); it != std::prev(path.points.end()); ++it) {
+ const auto dx = std::next(it)->x() - it->x();
+ const auto dy = std::next(it)->y() - it->y();
+ path.yaws.push_back(std::atan2(dy, dx));
+ path.lengths.push_back(path.lengths.back() + std::hypot(dx, dy));
+ }
+ path.yaws.push_back(path.yaws.back());
+ // Calculate curvatures
+ for (size_t i = 1; i < path.yaws.size(); ++i) {
+ const auto dyaw =
+ autoware::common::helper_functions::wrap_angle(path.yaws[i] - path.yaws[i - 1]);
+ path.curvatures.push_back(dyaw / (path.lengths[i - 1], path.lengths[i]));
+ }
+ path.curvatures.push_back(path.curvatures.back());
+ }
+}
+void calculateCartesian(
+ const sampler_common::transform::Spline2D & reference, Trajectory & trajectory)
+{
+ if (!trajectory.frenet_points.empty()) {
+ trajectory.points.reserve(trajectory.frenet_points.size());
+ // Calculate cartesian positions
+ for (const auto & fp : trajectory.frenet_points)
+ trajectory.points.push_back(reference.cartesian(fp));
+ calculateLengthsAndYaws(trajectory);
+ std::vector<double> d_yaws;
+ d_yaws.reserve(trajectory.yaws.size());
+ for (size_t i = 0; i + 1 < trajectory.yaws.size(); ++i)
+ d_yaws.push_back(autoware::common::helper_functions::wrap_angle(
+ trajectory.yaws[i + 1] - trajectory.yaws[i]));
+ d_yaws.push_back(0.0);
+ // Calculate curvatures
+ for (size_t i = 1; i < trajectory.yaws.size(); ++i) {
+ const auto curvature = trajectory.lengths[i] == trajectory.lengths[i - 1]
+ ? 0.0
+ : d_yaws[i] / (trajectory.lengths[i] - trajectory.lengths[i - 1]);
+ trajectory.curvatures.push_back(curvature);
+ }
+ const auto last_curvature = trajectory.curvatures.empty() ? 0.0 : trajectory.curvatures.back();
+ trajectory.curvatures.push_back(last_curvature);
+ // Calculate velocities, accelerations, jerk
+ for (size_t i = 0; i < trajectory.times.size(); ++i) {
+ const auto time = trajectory.times[i];
+ const auto s_vel = trajectory.longitudinal_polynomial->velocity(time);
+ const auto s_acc = trajectory.longitudinal_polynomial->acceleration(time);
+ const auto d_vel = trajectory.lateral_polynomial->velocity(time);
+ const auto d_acc = trajectory.lateral_polynomial->acceleration(time);
+ Eigen::Rotation2D rotation(d_yaws[i]);
+ Eigen::Vector2d vel_vector{s_vel, d_vel};
+ Eigen::Vector2d acc_vector{s_acc, d_acc};
+ const auto vel = rotation * vel_vector;
+ const auto acc = rotation * acc_vector;
+ trajectory.longitudinal_velocities.push_back(vel.x());
+ trajectory.lateral_velocities.push_back(vel.y());
+ trajectory.longitudinal_accelerations.push_back(acc.x());
+ trajectory.lateral_accelerations.push_back(acc.y());
+ trajectory.jerks.push_back(
+ trajectory.longitudinal_polynomial->jerk(time) + trajectory.lateral_polynomial->jerk(time));
+ }
+ if (trajectory.longitudinal_accelerations.empty()) {
+ trajectory.longitudinal_accelerations.push_back(0.0);
+ trajectory.lateral_accelerations.push_back(0.0);
+ }
+ }
+}
+
+} // namespace frenet_planner
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp
new file mode 100644
index 0000000000000..13f71dc88f9d6
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp
@@ -0,0 +1,75 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <eigen3/Eigen/Eigen>
+#include <frenet_planner/polynomials.hpp>
+
+namespace frenet_planner
+{
+// @brief Create a polynomial connecting the given initial and target configuration over the given
+// duration
+Polynomial::Polynomial(
+ const double x0, const double x0v, const double x0a, const double xT, const double xTv,
+ const double xTa, const double T)
+{
+ d_ = x0a / 2;
+ e_ = x0v;
+ f_ = x0;
+
+ const double T2 = T * T;
+ const double T3 = T2 * T;
+ const double T4 = T3 * T;
+ const double T5 = T4 * T;
+
+ Eigen::Matrix3d A;
+ A << T5, T4, T3, 5 * T4, 4 * T3, 3 * T2, 20 * T3, 12 * T2, 6 * T;
+ Eigen::Vector3d B;
+ B << xT - x0 - x0v * T - 0.5 * x0a * T2, xTv - x0v - x0a * T, xTa - x0a;
+
+ const Eigen::Vector3d X = A.colPivHouseholderQr().solve(B);
+ a_ = X(0);
+ b_ = X(1);
+ c_ = X(2);
+}
+
+double Polynomial::position(const double t) const
+{
+ const double t2 = t * t;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ const double t5 = t4 * t;
+ return a_ * t5 + b_ * t4 + c_ * t3 + d_ * t2 + e_ * t + f_;
+}
+
+double Polynomial::velocity(const double t) const
+{
+ const double t2 = t * t;
+ const double t3 = t2 * t;
+ const double t4 = t3 * t;
+ return 5 * a_ * t4 + 4 * b_ * t3 + 3 * c_ * t2 + 2 * d_ * t + e_;
+}
+
+double Polynomial::acceleration(const double t) const
+{
+ const double t2 = t * t;
+ const double t3 = t2 * t;
+ return 20 * a_ * t3 + 12 * b_ * t2 + 6 * c_ * t + 2 * d_;
+}
+
+double Polynomial::jerk(const double t) const
+{
+ const double t2 = t * t;
+ return 60 * a_ * t2 + 24 * b_ * t + 6 * c_;
+}
+} // namespace frenet_planner
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/README.md b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/README.md
new file mode 100644
index 0000000000000..4bae0bf5d6d7d
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/README.md
@@ -0,0 +1,6 @@
+# Sampler Common
+
+Common functions for sampling based planners.
+This includes classes for representing paths and trajectories,
+hard and soft constraints,
+conversion between cartesian and frenet frames, ...
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/footprint.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/footprint.cpp
new file mode 100644
index 0000000000000..92c8ae65267db
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/footprint.cpp
@@ -0,0 +1,53 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "sampler_common/constraints/footprint.hpp"
+
+#include "sampler_common/structures.hpp"
+
+#include <eigen3/Eigen/Core>
+
+#include <boost/geometry/algorithms/append.hpp>
+#include <boost/geometry/algorithms/correct.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+namespace sampler_common::constraints
+{
+
+namespace
+{
+const auto to_eigen = [](const Point2d & p) { return Eigen::Vector2d(p.x(), p.y()); };
+} // namespace
+
+MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constraints)
+{
+ MultiPoint2d footprint;
+
+ footprint.reserve(path.points.size() * 4);
+ for (auto i = 0UL; i < path.points.size(); ++i) {
+ const Eigen::Vector2d p = to_eigen(path.points[i]);
+ const double heading = path.yaws[i];
+ Eigen::Matrix2d rotation;
+ rotation << std::cos(heading), -std::sin(heading), std::sin(heading), std::cos(heading);
+ for (const auto & fp : constraints.ego_footprint) {
+ const Eigen::Vector2d fp_point = p + rotation * fp;
+ footprint.emplace_back(fp_point.x(), fp_point.y());
+ }
+ }
+ return footprint;
+}
+} // namespace sampler_common::constraints
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp
new file mode 100644
index 0000000000000..40d7efa739e6b
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp
@@ -0,0 +1,56 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "sampler_common/constraints/hard_constraint.hpp"
+
+#include "sampler_common/constraints/footprint.hpp"
+
+#include <boost/geometry/algorithms/within.hpp>
+
+#include <vector>
+
+namespace sampler_common::constraints
+{
+bool satisfyMinMax(const std::vector<double> & values, const double min, const double max)
+{
+ for (const auto value : values) {
+ if (value < min || value > max) return false;
+ }
+ return true;
+}
+
+bool has_collision(const MultiPoint2d & footprint, const MultiPolygon2d & obstacles)
+{
+ for (const auto & o : obstacles)
+ for (const auto & p : footprint)
+ if (boost::geometry::within(p, o)) return true;
+ return false;
+}
+
+MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints)
+{
+ const auto footprint = buildFootprintPoints(path, constraints);
+ if (!footprint.empty()) {
+ if (!boost::geometry::within(footprint, constraints.drivable_polygons)) {
+ path.constraint_results.drivable_area = false;
+ }
+ }
+ path.constraint_results.collision = !has_collision(footprint, constraints.obstacle_polygons);
+ if (!satisfyMinMax(
+ path.curvatures, constraints.hard.min_curvature, constraints.hard.max_curvature)) {
+ path.constraint_results.curvature = false;
+ }
+ return footprint;
+}
+} // namespace sampler_common::constraints
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp
new file mode 100644
index 0000000000000..b1d390e68c49f
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp
@@ -0,0 +1,54 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "sampler_common/constraints/soft_constraint.hpp"
+
+#include "sampler_common/structures.hpp"
+#include "sampler_common/transform/spline_transform.hpp"
+
+#include <numeric>
+
+namespace sampler_common::constraints
+{
+void calculateCurvatureCost(Path & path, const Constraints & constraints)
+{
+ double curvature_sum = 0.0;
+ for (const auto curvature : path.curvatures) {
+ curvature_sum += std::abs(curvature);
+ }
+ path.cost +=
+ constraints.soft.curvature_weight * curvature_sum / static_cast<double>(path.curvatures.size());
+}
+void calculateLengthCost(Path & path, const Constraints & constraints)
+{
+ if (!path.lengths.empty()) path.cost -= constraints.soft.length_weight * path.lengths.back();
+}
+
+void calculateLateralDeviationCost(
+ Path & path, const Constraints & constraints, const transform::Spline2D & reference)
+{
+ const auto fp = reference.frenet(path.points.back());
+ const double lateral_deviation = std::abs(fp.d);
+ path.cost += constraints.soft.lateral_deviation_weight * lateral_deviation;
+}
+
+void calculateCost(
+ Path & path, const Constraints & constraints, const transform::Spline2D & reference)
+{
+ if (path.points.empty()) return;
+ calculateCurvatureCost(path, constraints);
+ calculateLengthCost(path, constraints);
+ calculateLateralDeviationCost(path, constraints, reference);
+}
+} // namespace sampler_common::constraints
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp
new file mode 100644
index 0000000000000..cf5f1e5478fe5
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp
@@ -0,0 +1,315 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <sampler_common/transform/spline_transform.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <limits>
+
+namespace sampler_common::transform
+{
+Spline::Spline(const std::vector<double> & base_index, const std::vector<double> & base_value)
+{
+ generateSpline(base_index, base_value);
+}
+
+Spline::Spline(const std::vector<Point2d> & points)
+{
+ std::vector<double> xs;
+ std::vector<double> ys;
+ xs.reserve(points.size());
+ ys.reserve(points.size());
+ for (const auto & p : points) {
+ xs.push_back(p.x());
+ ys.push_back(p.y());
+ }
+ generateSpline(xs, ys);
+}
+
+void Spline::generateSpline(
+ const std::vector<double> & base_index, const std::vector<double> & base_value)
+{
+ const size_t N = base_value.size();
+
+ a_.clear();
+ b_.clear();
+ c_.clear();
+ d_.clear();
+ h_.clear();
+
+ a_ = base_value;
+
+ for (size_t i = 0; i + 1 < N; ++i) {
+ h_.push_back(base_index[i + 1] - base_index[i]);
+ }
+
+ c_ = solveLinearSystem(1.8, 100);
+
+ for (size_t i = 0; i < N - 1; i++) {
+ d_.push_back((c_[i + 1] - c_[i]) / (3.0 * h_[i]));
+ b_.push_back((a_[i + 1] - a_[i]) / h_[i] - h_[i] * (2.0 * c_[i] + c_[i + 1]) / 3.0);
+ }
+
+ d_.push_back(0.0);
+ b_.push_back(0.0);
+}
+
+double Spline::value(const double query, const std::vector<double> & base_index) const
+{
+ const auto ub = std::upper_bound(base_index.begin(), base_index.end(), query);
+ const size_t i = std::distance(base_index.begin(), ub) - 1;
+ const double ds = query - base_index[i];
+ return a_[i] + (b_[i] + (c_[i] + d_[i] * ds) * ds) * ds;
+}
+
+double Spline::velocity(const double query, const std::vector<double> & base_index) const
+{
+ const auto lb = std::lower_bound(base_index.begin(), base_index.end(), query);
+ const size_t i = std::distance(base_index.begin(), lb) - 1;
+ const auto delta = query - base_index[i];
+ return b_[i] + 2.0 * c_[i] * delta + 3.0 * d_[i] * delta * delta;
+}
+
+double Spline::acceleration(const double query, const std::vector<double> & base_index) const
+{
+ const auto lb = std::lower_bound(base_index.begin(), base_index.end(), query);
+ const size_t i = std::distance(base_index.begin(), lb) - 1;
+ return 2.0 * c_[i] + 6.0 * d_[i] * (query - base_index[i]);
+}
+
+bool Spline::isIncrease(const std::vector<double> & x)
+{
+ for (int i = 0; i < static_cast<int>(x.size()) - 1; ++i) {
+ if (x[i] > x[i + 1]) return false;
+ }
+ return true;
+}
+
+bool Spline::isValidInput(
+ const std::vector<double> & base_index, const std::vector<double> & base_value,
+ const std::vector<double> & return_index)
+{
+ if (base_index.empty() || base_value.empty() || return_index.empty()) {
+ std::cerr << "bad index : some vector is empty. base_index: " << base_index.size()
+ << ", base_value: " << base_value.size() << ", return_index: " << return_index.size()
+ << std::endl;
+ return false;
+ }
+ if (!isIncrease(base_index)) {
+ std::cerr << "bad index : base_index is not monotonically increasing. base_index = ["
+ << base_index.front() << ", " << base_index.back() << "]" << std::endl;
+ return false;
+ }
+ if (!isIncrease(return_index)) {
+ std::cerr << "bad index : base_index is not monotonically increasing. return_index = ["
+ << return_index.front() << ", " << return_index.back() << "]" << std::endl;
+ return false;
+ }
+ if (return_index.front() < base_index.front()) {
+ std::cerr << "bad index : return_index.front() < base_index.front()" << std::endl;
+ return false;
+ }
+ if (base_index.back() < return_index.back()) {
+ std::cerr << "bad index : base_index.back() < return_index.back()" << std::endl;
+ return false;
+ }
+ if (base_index.size() != base_value.size()) {
+ std::cerr << "bad index : base_index.size() != base_value.size()" << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+std::vector<double> Spline::solveLinearSystem(const double omega, const size_t max_iter) const
+{
+ // solve A * ans = rhs by SOR method
+ constexpr double converge_range = 0.00001;
+ std::vector<double> ans(a_.size(), 1.0);
+ std::vector<double> ans_next(a_.size(), 0.0);
+ size_t num_iter = 0;
+
+ while (!isConvergeL1(ans, ans_next, converge_range) && num_iter <= max_iter) {
+ ans = ans_next;
+ for (size_t i = 1; i < a_.size() - 1; ++i) {
+ const double rhs = 3.0 / h_[i] * (a_[i + 1] - a_[i]) - 3.0 / h_[i - 1] * (a_[i] - a_[i - 1]);
+ ans_next[i] += omega / (2.0 * (h_[i - 1] + h_[i])) *
+ (rhs - (h_[i - 1] * ans_next[i - 1] + 2.0 * (h_[i - 1] + h_[i]) * ans[i] +
+ h_[i] * ans[i + 1]));
+ }
+ ++num_iter;
+ }
+
+ if (num_iter > max_iter) std::cerr << "[Spline::solveLinearSystem] unconverged!\n";
+ return ans_next;
+}
+
+bool Spline::isConvergeL1(
+ const std::vector<double> & r1, const std::vector<double> & r2, const double converge_range)
+{
+ double d = 0.0;
+ for (size_t i = 0; i < r1.size(); ++i) {
+ d += std::fabs(r1.at(i) - r2.at(i));
+ }
+ return d < converge_range;
+}
+
+/*
+ * 2D Spline
+ */
+
+Spline2D::Spline2D(const std::vector<double> & x, const std::vector<double> & y)
+: s_(arcLength(x, y)), x_spline_{s_, x}, y_spline_{s_, y}
+{
+ original_points_.reserve(x.size());
+ for (size_t i = 0; i < x.size(); ++i) {
+ original_points_.emplace_back(x[i], y[i]);
+ }
+}
+
+// @brief Calculate the distances of the points along the path
+// @details approximation using linear segments
+std::vector<double> Spline2D::arcLength(
+ const std::vector<double> & x, const std::vector<double> & y)
+{
+ std::vector<double> s = {0.0};
+ s.reserve(x.size() - 1);
+ for (size_t i = 0; i < x.size() - 1; ++i) {
+ const double ds = std::hypot((x[i + 1] - x[i]), (y[i + 1] - y[i]));
+ s.push_back(s.back() + ds);
+ }
+ return s;
+}
+
+// @brief Convert the given point to the Frenet frame of this spline
+// @details sample points along the splines and return the closest one
+FrenetPoint Spline2D::frenet_naive(const Point2d & p, double precision) const
+{
+ double closest_d = std::numeric_limits<double>::max();
+ double arc_length = std::numeric_limits<double>::min();
+ for (double s = s_.front(); s < s_.back(); s += precision) {
+ const double x_s = x_spline_.value(s, s_);
+ const double y_s = y_spline_.value(s, s_);
+ const double d = std::hypot(p.x() - x_s, p.y() - y_s);
+ if (d <= closest_d) // also accept equality to return the largest possible arc length
+ {
+ closest_d = d;
+ arc_length = s;
+ }
+ }
+ // check sign of d
+ const double x0 = x_spline_.value(arc_length, s_);
+ const double y0 = y_spline_.value(arc_length, s_);
+ const double x1 = x_spline_.value(arc_length + precision, s_);
+ const double y1 = y_spline_.value(arc_length + precision, s_);
+ if ((x1 - x0) * (p.y() - y0) - (y1 - y0) * (p.x() - x0) < 0) {
+ closest_d *= -1.0;
+ }
+ return {arc_length, closest_d};
+}
+
+// @brief Convert the given point to the Frenet frame of this spline
+// @details find closest point in the lookup table
+// @warning can fail if the original points are not smooth or if some points are very far apart
+FrenetPoint Spline2D::frenet(const Point2d & p, const double precision) const
+{
+ const auto distance = [&](const Point2d & p2) {
+ return std::hypot(p.x() - p2.x(), p.y() - p2.y());
+ };
+ size_t min_i{};
+ auto min_dist = std::numeric_limits<double>::max();
+ for (size_t i = 0; i < original_points_.size(); ++i) {
+ const auto dist = distance(original_points_[i]);
+ if (dist <= min_dist) {
+ min_dist = dist;
+ min_i = i;
+ }
+ }
+ auto lb_i = min_i == 0 ? min_i : min_i - 1;
+ auto ub_i = min_i + 1 == original_points_.size() ? min_i : min_i + 1;
+ auto best_s = s_[min_i];
+ // real closest s is either in interval [lb_i:min_i] or interval [min_i:ub]
+ // continue exploring the interval whose middle point is closest to the input point
+ std::vector<double> s_interval = {s_[lb_i], {}, s_[min_i], {}, s_[ub_i]};
+ std::vector<double> d_interval = {
+ distance(original_points_[lb_i]),
+ {},
+ distance(original_points_[min_i]),
+ {},
+ distance(original_points_[ub_i])};
+ while (s_interval[4] - s_interval[0] > precision) {
+ s_interval[1] = s_interval[0] + (s_interval[2] - s_interval[0]) / 2;
+ s_interval[3] = s_interval[2] + (s_interval[4] - s_interval[2]) / 2;
+ d_interval[1] =
+ distance({x_spline_.value(s_interval[1], s_), y_spline_.value(s_interval[1], s_)});
+ d_interval[3] =
+ distance({x_spline_.value(s_interval[3], s_), y_spline_.value(s_interval[3], s_)});
+
+ for (auto i = 0; i < 5; ++i) {
+ if (d_interval[i] <= min_dist) {
+ min_dist = d_interval[i];
+ min_i = i;
+ }
+ }
+
+ best_s = s_interval[min_i];
+ lb_i = min_i == 0 ? min_i : min_i - 1;
+ ub_i = min_i == 4 ? min_i : min_i + 1;
+ s_interval = {s_interval[lb_i], {}, s_interval[min_i], {}, s_interval[ub_i]};
+ d_interval = {d_interval[lb_i], {}, d_interval[min_i], {}, d_interval[ub_i]};
+ }
+ // check sign of d
+ const double x0 = x_spline_.value(best_s, s_);
+ const double y0 = y_spline_.value(best_s, s_);
+ const double x1 = x_spline_.value(best_s + precision, s_);
+ const double y1 = y_spline_.value(best_s + precision, s_);
+ if ((x1 - x0) * (p.y() - y0) - (y1 - y0) * (p.x() - x0) < 0) {
+ min_dist *= -1.0;
+ }
+ return {best_s, min_dist};
+}
+
+Point2d Spline2D::cartesian(const double s) const
+{
+ return {x_spline_.value(s, s_), y_spline_.value(s, s_)};
+}
+
+Point2d Spline2D::cartesian(const FrenetPoint & fp) const
+{
+ const auto heading = yaw(fp.s);
+ const auto x = x_spline_.value(fp.s, s_);
+ const auto y = y_spline_.value(fp.s, s_);
+ return {x + fp.d * std::cos(heading + M_PI_2), y + fp.d * std::sin(heading + M_PI_2)};
+}
+
+double Spline2D::curvature(const double s) const
+{
+ // TODO(Maxime CLEMENT) search for s in s_ here and pass index
+ const double x_vel = x_spline_.velocity(s, s_);
+ const double y_vel = y_spline_.velocity(s, s_);
+ const double x_acc = x_spline_.acceleration(s, s_);
+ const double y_acc = y_spline_.acceleration(s, s_);
+ return (y_acc * x_vel - x_acc * y_vel) / std::pow(x_vel * x_vel + y_vel * y_vel, 3.0 / 2.0);
+}
+
+double Spline2D::yaw(const double s) const
+{
+ const double x_vel = x_spline_.velocity(s, s_);
+ const double y_vel = y_spline_.velocity(s, s_);
+ return std::atan2(y_vel, x_vel);
+}
+
+} // namespace sampler_common::transform
From ea550953f2810af63dd7dad0210ac94ce9bd9262 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 26 Sep 2023 17:20:18 +0900
Subject: [PATCH 008/115] fix include paths
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../{include/bezier_sampler => }/bezier.hpp | 0
.../{include/bezier_sampler => }/bezier_sampling.hpp | 5 +++--
.../{include/frenet_planner => }/conversions.hpp | 5 ++---
.../{include/frenet_planner => }/frenet_planner.hpp | 6 +++---
.../{include/frenet_planner => }/polynomials.hpp | 0
.../{include/frenet_planner => }/structures.hpp | 5 ++---
.../sampling_planner/path_sampler/common_structs.hpp | 4 ++--
.../sampling_planner/path_sampler/node.hpp | 11 +++++------
.../sampling_planner/path_sampler/parameters.hpp | 4 ++--
.../path_sampler/path_generation.hpp | 12 ++++++------
.../sampling_planner/path_sampler/prepare_inputs.hpp | 8 ++++----
.../sampler_common/constraints/footprint.hpp | 2 +-
.../sampler_common/constraints/hard_constraint.hpp | 2 +-
.../sampler_common/constraints/map_constraints.hpp | 2 +-
.../sampler_common/constraints/soft_constraint.hpp | 4 ++--
.../sampler_common/transform/spline_transform.hpp | 2 +-
.../path_sampler_utils/geometry_utils.hpp | 4 ++--
.../path_sampler_utils/trajectory_utils.hpp | 6 +++---
.../sampling_planner/bezier_sampler/bezier.cpp | 2 +-
.../bezier_sampler/bezier_sampling.cpp | 2 +-
.../frenet_planner/frenet_planner.cpp | 11 ++++++-----
.../sampling_planner/frenet_planner/polynomials.cpp | 3 ++-
.../sampler_common => }/constraints/footprint.cpp | 4 ++--
.../constraints/hard_constraint.cpp | 4 ++--
.../constraints/soft_constraint.cpp | 6 +++---
.../transform/spline_transform.cpp | 2 +-
26 files changed, 58 insertions(+), 58 deletions(-)
rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/{include/bezier_sampler => }/bezier.hpp (100%)
rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/{include/bezier_sampler => }/bezier_sampling.hpp (91%)
rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/{include/frenet_planner => }/conversions.hpp (89%)
rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/{include/frenet_planner => }/frenet_planner.hpp (91%)
rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/{include/frenet_planner => }/polynomials.hpp (100%)
rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/{include/frenet_planner => }/structures.hpp (96%)
rename planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/{src/sampler_common => }/constraints/footprint.cpp (89%)
rename planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/{src/sampler_common => }/constraints/hard_constraint.cpp (89%)
rename planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/{src/sampler_common => }/constraints/soft_constraint.cpp (85%)
rename planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/{src/sampler_common => }/transform/spline_transform.cpp (98%)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp
similarity index 100%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp
similarity index 91%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp
index 624cd585b0a72..1d436191326c9 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp
@@ -15,9 +15,10 @@
#ifndef BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_
#define BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_
-#include <bezier_sampler/bezier.hpp>
+#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
+
#include <eigen3/Eigen/Core>
-#include <sampler_common/structures.hpp>
#include <vector>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/conversions.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/conversions.hpp
similarity index 89%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/conversions.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/conversions.hpp
index f8575d8f65936..9979853a046b7 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/conversions.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/conversions.hpp
@@ -15,9 +15,8 @@
#ifndef FRENET_PLANNER__CONVERSIONS_HPP_
#define FRENET_PLANNER__CONVERSIONS_HPP_
-#include "frenet_planner/polynomials.hpp"
-
-#include <sampler_common/structures.hpp>
+#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
#include <vector>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/frenet_planner.hpp
similarity index 91%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/frenet_planner.hpp
index c98e6d97969d4..a843dc64fef9f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/frenet_planner.hpp
@@ -15,9 +15,9 @@
#ifndef FRENET_PLANNER__FRENET_PLANNER_HPP_
#define FRENET_PLANNER__FRENET_PLANNER_HPP_
-#include "frenet_planner/structures.hpp"
-#include "sampler_common/structures.hpp"
-#include "sampler_common/transform/spline_transform.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
#include <optional>
#include <vector>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/polynomials.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp
similarity index 100%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/polynomials.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/structures.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp
similarity index 96%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/structures.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp
index c44cb5d814d71..ed8c8579eecff 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/include/frenet_planner/structures.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp
@@ -15,9 +15,8 @@
#ifndef FRENET_PLANNER__STRUCTURES_HPP_
#define FRENET_PLANNER__STRUCTURES_HPP_
-#include "frenet_planner/polynomials.hpp"
-
-#include <sampler_common/structures.hpp>
+#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
#include <optional>
#include <vector>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp
index 7cf88cb75e13a..b3ebb592bb393 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp
@@ -15,9 +15,9 @@
#ifndef PATH_SAMPLER__COMMON_STRUCTS_HPP_
#define PATH_SAMPLER__COMMON_STRUCTS_HPP_
-#include "path_sampler/type_alias.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
#include "rclcpp/rclcpp.hpp"
-#include "sampler_common/structures.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"
#include "tier4_autoware_utils/system/stop_watch.hpp"
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp
index a9002c8cf8a9f..c9bdb68f35a50 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp
@@ -15,15 +15,14 @@
#ifndef PATH_SAMPLER__NODE_HPP_
#define PATH_SAMPLER__NODE_HPP_
-#include "path_sampler/common_structs.hpp"
-#include "path_sampler/parameters.hpp"
-#include "path_sampler/type_alias.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
#include "rclcpp/rclcpp.hpp"
-#include "sampler_common/transform/spline_transform.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"
-#include <sampler_common/structures.hpp>
-
#include <algorithm>
#include <memory>
#include <optional>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp
index 1c5c51e33b431..34446a9db5e85 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp
@@ -15,8 +15,8 @@
#ifndef PATH_SAMPLER__PARAMETERS_HPP_
#define PATH_SAMPLER__PARAMETERS_HPP_
-#include "bezier_sampler/bezier_sampling.hpp"
-#include "sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
#include <vector>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp
index 41493b74659fc..f9b9781b2a14c 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp
@@ -15,12 +15,12 @@
#ifndef PATH_SAMPLER__PATH_GENERATION_HPP_
#define PATH_SAMPLER__PATH_GENERATION_HPP_
-#include "bezier_sampler/bezier_sampling.hpp"
-#include "frenet_planner/structures.hpp"
-#include "path_sampler/parameters.hpp"
-#include "sampler_common/constraints/hard_constraint.hpp"
-#include "sampler_common/structures.hpp"
-#include "sampler_common/transform/spline_transform.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
#include <autoware_auto_planning_msgs/msg/path.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp
index 29cfddfbf8632..b20131a817135 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp
@@ -15,10 +15,10 @@
#ifndef PATH_SAMPLER__PREPARE_INPUTS_HPP_
#define PATH_SAMPLER__PREPARE_INPUTS_HPP_
-#include "frenet_planner/structures.hpp"
-#include "path_sampler/parameters.hpp"
-#include "path_sampler/type_alias.hpp"
-#include "sampler_common/transform/spline_transform.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_planning_msgs/msg/path.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp
index 061f922001a0f..b063ba3cc3e5f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp
@@ -15,7 +15,7 @@
#ifndef SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_
#define SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_
-#include "sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
namespace sampler_common::constraints
{
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp
index 475aae4aeea18..aa9c82cd9e966 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp
@@ -15,7 +15,7 @@
#ifndef SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_
#define SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_
-#include "sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
namespace sampler_common::constraints
{
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp
index ed04d8104a904..e8318ab1e1023 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp
@@ -15,7 +15,7 @@
#ifndef SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_
#define SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_
-#include "sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
#include <boost/geometry/algorithms/within.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp
index 9fdb9fe137e49..4184362497425 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp
@@ -15,8 +15,8 @@
#ifndef SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_
#define SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_
-#include "sampler_common/structures.hpp"
-#include "sampler_common/transform/spline_transform.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
namespace sampler_common::constraints
{
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp
index ce66141832dbf..7110a3828ad01 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp
@@ -15,7 +15,7 @@
#ifndef SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_
#define SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_
-#include "sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
#include <vector>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp
index 3e5afdbdad696..c4cac93b605aa 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp
@@ -15,13 +15,13 @@
#ifndef PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_
#define PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
#include "eigen3/Eigen/Core"
#include "interpolation/linear_interpolation.hpp"
#include "interpolation/spline_interpolation.hpp"
#include "interpolation/spline_interpolation_points_2d.hpp"
#include "motion_utils/trajectory/trajectory.hpp"
-#include "path_sampler/common_structs.hpp"
-#include "path_sampler/type_alias.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"
#include "autoware_auto_planning_msgs/msg/path_point.hpp"
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp
index c0a3c1d917c25..77072c9045133 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp
@@ -15,14 +15,14 @@
#ifndef PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_
#define PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
#include "eigen3/Eigen/Core"
#include "interpolation/linear_interpolation.hpp"
#include "interpolation/spline_interpolation.hpp"
#include "interpolation/spline_interpolation_points_2d.hpp"
#include "motion_utils/trajectory/trajectory.hpp"
-#include "path_sampler/common_structs.hpp"
-#include "path_sampler/type_alias.hpp"
-#include "sampler_common/structures.hpp"
#include "autoware_auto_planning_msgs/msg/path_point.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp
index fc4c09840f507..92598948ca1e4 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <bezier_sampler/bezier.hpp>
+#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp"
#include <iostream>
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp
index a9fa318980250..617ee4d7f1d79 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <bezier_sampler/bezier_sampling.hpp>
+#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp"
namespace bezier_sampler
{
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp
index 3368c49459a55..24415692e34b5 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp
@@ -14,13 +14,14 @@
#include "frenet_planner/frenet_planner.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/conversions.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
+
#include <eigen3/Eigen/Eigen>
-#include <frenet_planner/conversions.hpp>
-#include <frenet_planner/polynomials.hpp>
-#include <frenet_planner/structures.hpp>
#include <helper_functions/angle_utils.hpp>
-#include <sampler_common/structures.hpp>
-#include <sampler_common/transform/spline_transform.hpp>
#include <algorithm>
#include <cmath>
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp
index 13f71dc88f9d6..845a421fc3769 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp
@@ -12,8 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp"
+
#include <eigen3/Eigen/Eigen>
-#include <frenet_planner/polynomials.hpp>
namespace frenet_planner
{
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/footprint.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/footprint.cpp
similarity index 89%
rename from planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/footprint.cpp
rename to planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/footprint.cpp
index 92c8ae65267db..1a9df044c39d7 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/footprint.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/footprint.cpp
@@ -12,9 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "sampler_common/constraints/footprint.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp"
-#include "sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
#include <eigen3/Eigen/Core>
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.cpp
similarity index 89%
rename from planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp
rename to planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.cpp
index 40d7efa739e6b..54e39a9b04fd9 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.cpp
@@ -12,9 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "sampler_common/constraints/hard_constraint.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp"
-#include "sampler_common/constraints/footprint.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp"
#include <boost/geometry/algorithms/within.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.cpp
similarity index 85%
rename from planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp
rename to planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.cpp
index b1d390e68c49f..175072bcde7da 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.cpp
@@ -12,10 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "sampler_common/constraints/soft_constraint.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp"
-#include "sampler_common/structures.hpp"
-#include "sampler_common/transform/spline_transform.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
+#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
#include <numeric>
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/transform/spline_transform.cpp
similarity index 98%
rename from planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp
rename to planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/transform/spline_transform.cpp
index cf5f1e5478fe5..c3e06e4a170b9 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/transform/spline_transform.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include <sampler_common/transform/spline_transform.hpp>
+#include <behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp>
#include <algorithm>
#include <cmath>
From e30778b3192c0980d3fc80ce3be9d5c0ee9070a6 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 27 Sep 2023 17:35:42 +0900
Subject: [PATCH 009/115] rebase Signed-off-by: Daniel Sanchez
<danielsanchezaran@gmail.com>
---
.../behavior_planning.launch.py | 321 ++++++++++++++++++
.../sampling_planner.param.yaml | 139 ++++++++
.../config/scene_module_manager.param.yaml | 9 +
.../behavior_path_planner/parameters.hpp | 148 ++++++++
.../src/behavior_path_planner_node.cpp | 1 +
5 files changed, 618 insertions(+)
create mode 100644 launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
create mode 100644 planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
new file mode 100644
index 0000000000000..b44678d700b96
--- /dev/null
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -0,0 +1,321 @@
+# Copyright 2021-2023 TIER IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.actions import GroupAction
+from launch.actions import IncludeLaunchDescription
+from launch.actions import OpaqueFunction
+from launch.actions import SetLaunchConfiguration
+from launch.conditions import IfCondition
+from launch.conditions import UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PythonExpression
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
+import yaml
+
+
+def launch_setup(context, *args, **kwargs):
+ # vehicle information parameter
+ vehicle_param_path = LaunchConfiguration("vehicle_param_file").perform(context)
+ with open(vehicle_param_path, "r") as f:
+ vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+ # common parameter
+ with open(LaunchConfiguration("common_param_path").perform(context), "r") as f:
+ common_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ # nearest search parameter
+ with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f:
+ nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+ # behavior path planner
+ with open(LaunchConfiguration("side_shift_param_path").perform(context), "r") as f:
+ side_shift_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("avoidance_param_path").perform(context), "r") as f:
+ avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("avoidance_by_lc_param_path").perform(context), "r") as f:
+ avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("dynamic_avoidance_param_path").perform(context), "r") as f:
+ dynamic_avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("sampling_planner_param_path").perform(context), "r") as f:
+ sampling_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f:
+ lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f:
+ goal_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("start_planner_param_path").perform(context), "r") as f:
+ start_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f:
+ drivable_area_expansion_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("scene_module_manager_param_path").perform(context), "r") as f:
+ scene_module_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f:
+ behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+ glog_component = ComposableNode(
+ package="glog_component",
+ plugin="GlogComponent",
+ name="glog_component",
+ )
+
+ behavior_path_planner_component = ComposableNode(
+ package="behavior_path_planner",
+ plugin="behavior_path_planner::BehaviorPathPlannerNode",
+ name="behavior_path_planner",
+ namespace="",
+ remappings=[
+ ("~/input/route", LaunchConfiguration("input_route_topic_name")),
+ ("~/input/vector_map", LaunchConfiguration("map_topic_name")),
+ ("~/input/perception", "/perception/object_recognition/objects"),
+ ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"),
+ (
+ "~/input/costmap",
+ "/planning/scenario_planning/parking/costmap_generator/occupancy_grid",
+ ),
+ ("~/input/odometry", "/localization/kinematic_state"),
+ ("~/input/accel", "/localization/acceleration"),
+ ("~/input/scenario", "/planning/scenario_planning/scenario"),
+ ("~/output/path", "path_with_lane_id"),
+ ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"),
+ ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"),
+ ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"),
+ ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
+ ],
+ parameters=[
+ common_param,
+ nearest_search_param,
+ side_shift_param,
+ avoidance_param,
+ avoidance_by_lc_param,
+ dynamic_avoidance_param,
+ sampling_planner_param,
+ lane_change_param,
+ goal_planner_param,
+ start_planner_param,
+ drivable_area_expansion_param,
+ scene_module_manager_param,
+ behavior_path_planner_param,
+ vehicle_param,
+ {
+ "lane_change.enable_collision_check_at_prepare_phase": LaunchConfiguration(
+ "use_experimental_lane_change_function"
+ ),
+ "lane_change.use_predicted_path_outside_lanelet": LaunchConfiguration(
+ "use_experimental_lane_change_function"
+ ),
+ "lane_change.use_all_predicted_path": LaunchConfiguration(
+ "use_experimental_lane_change_function"
+ ),
+ },
+ ],
+ extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ )
+
+ # smoother param
+ with open(
+ LaunchConfiguration("motion_velocity_smoother_param_path").perform(context), "r"
+ ) as f:
+ motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(
+ LaunchConfiguration("behavior_velocity_smoother_type_param_path").perform(context), "r"
+ ) as f:
+ behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+ # behavior velocity planner
+ behavior_velocity_planner_common_param_path = LaunchConfiguration(
+ "behavior_velocity_planner_common_param_path"
+ ).perform(context)
+ behavior_velocity_planner_module_param_paths = LaunchConfiguration(
+ "behavior_velocity_planner_module_param_paths"
+ ).perform(context)
+
+ behavior_velocity_planner_params_paths = [
+ behavior_velocity_planner_common_param_path,
+ *yaml.safe_load(behavior_velocity_planner_module_param_paths),
+ ]
+
+ behavior_velocity_planner_params = {}
+ for path in behavior_velocity_planner_params_paths:
+ with open(path) as f:
+ behavior_velocity_planner_params.update(yaml.safe_load(f)["/**"]["ros__parameters"])
+
+ behavior_velocity_planner_component = ComposableNode(
+ package="behavior_velocity_planner",
+ plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode",
+ name="behavior_velocity_planner",
+ namespace="",
+ remappings=[
+ ("~/input/path_with_lane_id", "path_with_lane_id"),
+ ("~/input/vector_map", "/map/vector_map"),
+ ("~/input/vehicle_odometry", "/localization/kinematic_state"),
+ ("~/input/accel", "/localization/acceleration"),
+ ("~/input/dynamic_objects", "/perception/object_recognition/objects"),
+ (
+ "~/input/no_ground_pointcloud",
+ "/perception/obstacle_segmentation/pointcloud",
+ ),
+ (
+ "~/input/compare_map_filtered_pointcloud",
+ "compare_map_filtered/pointcloud",
+ ),
+ (
+ "~/input/vector_map_inside_area_filtered_pointcloud",
+ "vector_map_inside_area_filtered/pointcloud",
+ ),
+ (
+ "~/input/traffic_signals",
+ "/perception/traffic_light_recognition/traffic_signals",
+ ),
+ (
+ "~/input/external_velocity_limit_mps",
+ "/planning/scenario_planning/max_velocity_default",
+ ),
+ ("~/input/virtual_traffic_light_states", "/awapi/tmp/virtual_traffic_light_states"),
+ (
+ "~/input/occupancy_grid",
+ "/perception/occupancy_grid_map/map",
+ ),
+ ("~/output/path", "path"),
+ ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
+ (
+ "~/output/infrastructure_commands",
+ "/planning/scenario_planning/status/infrastructure_commands",
+ ),
+ ("~/output/traffic_signal", "debug/traffic_signal"),
+ ],
+ parameters=[
+ nearest_search_param,
+ behavior_velocity_planner_params,
+ vehicle_param,
+ common_param,
+ motion_velocity_smoother_param,
+ behavior_velocity_smoother_type_param,
+ ],
+ extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ )
+
+ container = ComposableNodeContainer(
+ name="behavior_planning_container",
+ namespace="",
+ package="rclcpp_components",
+ executable=LaunchConfiguration("container_executable"),
+ composable_node_descriptions=[
+ behavior_path_planner_component,
+ behavior_velocity_planner_component,
+ glog_component,
+ ],
+ output="screen",
+ )
+
+ # This condition is true if run_out module is enabled and its detection method is Points
+ run_out_module = "behavior_velocity_planner::RunOutModulePlugin"
+ run_out_method = behavior_velocity_planner_params.get("run_out", {}).get("detection_method")
+ launch_run_out = run_out_module in behavior_velocity_planner_params["launch_modules"]
+ launch_run_out_with_points_method = launch_run_out and run_out_method == "Points"
+
+ # load compare map for run_out module
+ load_compare_map = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ [
+ FindPackageShare("tier4_planning_launch"),
+ "/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py",
+ ],
+ ),
+ launch_arguments={
+ "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
+ "container_name": LaunchConfiguration("container_name"),
+ "use_multithread": "true",
+ }.items(),
+ # launch compare map only when run_out module is enabled and detection method is Points
+ condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))),
+ )
+
+ load_vector_map_inside_area_filter = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ [
+ FindPackageShare("tier4_planning_launch"),
+ "/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py",
+ ]
+ ),
+ launch_arguments={
+ "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
+ "container_name": LaunchConfiguration("container_name"),
+ "use_multithread": "true",
+ "polygon_type": "no_obstacle_segmentation_area_for_run_out",
+ }.items(),
+ # launch vector map filter only when run_out module is enabled and detection method is Points
+ condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))),
+ )
+
+ group = GroupAction(
+ [
+ container,
+ load_compare_map,
+ load_vector_map_inside_area_filter,
+ ]
+ )
+
+ return [group]
+
+
+def generate_launch_description():
+ launch_arguments = []
+
+ def add_launch_arg(name: str, default_value=None, description=None):
+ launch_arguments.append(
+ DeclareLaunchArgument(name, default_value=default_value, description=description)
+ )
+
+ # vehicle parameter
+ add_launch_arg("vehicle_param_file")
+
+ # interface parameter
+ add_launch_arg(
+ "input_route_topic_name", "/planning/mission_planning/route", "input topic of route"
+ )
+ add_launch_arg("map_topic_name", "/map/vector_map", "input topic of map")
+
+ # package parameter
+ add_launch_arg("use_experimental_lane_change_function")
+
+ # component
+ add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication")
+ add_launch_arg("use_multithread", "false", "use multithread")
+
+ # for points filter of run out module
+ add_launch_arg("use_pointcloud_container", "true")
+ add_launch_arg("container_name", "pointcloud_container")
+
+ set_container_executable = SetLaunchConfiguration(
+ "container_executable",
+ "component_container",
+ condition=UnlessCondition(LaunchConfiguration("use_multithread")),
+ )
+ set_container_mt_executable = SetLaunchConfiguration(
+ "container_executable",
+ "component_container_mt",
+ condition=IfCondition(LaunchConfiguration("use_multithread")),
+ )
+
+ return launch.LaunchDescription(
+ launch_arguments
+ + [
+ set_container_executable,
+ set_container_mt_executable,
+ ]
+ + [OpaqueFunction(function=launch_setup)]
+ )
diff --git a/planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml b/planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml
new file mode 100644
index 0000000000000..6760ec787bb03
--- /dev/null
+++ b/planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml
@@ -0,0 +1,139 @@
+/**:
+ ros__parameters:
+ start_planner:
+ th_arrived_distance: 1.0
+ th_stopped_velocity: 0.01
+ th_stopped_time: 1.0
+ collision_check_margin: 1.0
+ collision_check_distance_from_end: 1.0
+ th_moving_object_velocity: 1.0
+ th_distance_to_middle_of_the_road: 0.1
+ center_line_path_interval: 1.0
+ # shift pull out
+ enable_shift_pull_out: true
+ check_shift_path_lane_departure: false
+ minimum_shift_pull_out_distance: 0.0
+ deceleration_interval: 15.0
+ lateral_jerk: 0.5
+ lateral_acceleration_sampling_num: 3
+ minimum_lateral_acc: 0.15
+ maximum_lateral_acc: 0.5
+ maximum_curvature: 0.07
+ # geometric pull out
+ enable_geometric_pull_out: true
+ divide_pull_out_path: false
+ geometric_pull_out_velocity: 1.0
+ arc_path_interval: 1.0
+ lane_departure_margin: 0.2
+ backward_velocity: -1.0
+ pull_out_max_steer_angle: 0.26 # 15deg
+ # search start pose backward
+ enable_back: true
+ search_priority: "efficient_path" # "efficient_path" or "short_back_distance"
+ max_back_distance: 30.0
+ backward_search_resolution: 2.0
+ backward_path_update_duration: 3.0
+ ignore_distance_from_lane_end: 15.0
+ # turns signal
+ th_turn_signal_on_lateral_offset: 1.0
+ intersection_search_length: 30.0
+ length_ratio_for_turn_signal_deactivation_near_intersection: 0.5
+ # freespace planner
+ freespace_planner:
+ enable_freespace_planner: true
+ end_pose_search_start_distance: 20.0
+ end_pose_search_end_distance: 30.0
+ end_pose_search_interval: 2.0
+ freespace_planner_algorithm: "astar" # options: astar, rrtstar
+ velocity: 1.0
+ vehicle_shape_margin: 1.0
+ time_limit: 3000.0
+ minimum_turning_radius: 5.0
+ maximum_turning_radius: 5.0
+ turning_radius_size: 1
+ # search configs
+ search_configs:
+ theta_size: 144
+ angle_goal_range: 6.0
+ curve_weight: 1.2
+ reverse_weight: 1.0
+ lateral_goal_range: 0.5
+ longitudinal_goal_range: 2.0
+ # costmap configs
+ costmap_configs:
+ obstacle_threshold: 30
+ # -- A* search Configurations --
+ astar:
+ only_behind_solutions: false
+ use_back: false
+ distance_heuristic_weight: 1.0
+ # -- RRT* search Configurations --
+ rrtstar:
+ enable_update: true
+ use_informed_sampling: true
+ max_planning_time: 150.0
+ neighbor_radius: 8.0
+ margin: 1.0
+
+ stop_condition:
+ maximum_deceleration_for_stop: 1.0
+ maximum_jerk_for_stop: 1.0
+ path_safety_check:
+ # EgoPredictedPath
+ ego_predicted_path:
+ min_velocity: 0.0
+ acceleration: 1.0
+ max_velocity: 1.0
+ time_horizon_for_front_object: 10.0
+ time_horizon_for_rear_object: 10.0
+ time_resolution: 0.5
+ delay_until_departure: 1.0
+ # For target object filtering
+ target_filtering:
+ safety_check_time_horizon: 5.0
+ safety_check_time_resolution: 1.0
+ # detection range
+ object_check_forward_distance: 10.0
+ object_check_backward_distance: 100.0
+ ignore_object_velocity_threshold: 0.0
+ # ObjectTypesToCheck
+ object_types_to_check:
+ check_car: true
+ check_truck: true
+ check_bus: true
+ check_trailer: true
+ check_bicycle: true
+ check_motorcycle: true
+ check_pedestrian: true
+ check_unknown: false
+ # ObjectLaneConfiguration
+ object_lane_configuration:
+ check_current_lane: true
+ check_right_side_lane: true
+ check_left_side_lane: true
+ check_shoulder_lane: true
+ check_other_lane: false
+ include_opposite_lane: false
+ invert_opposite_lane: false
+ check_all_predicted_path: true
+ use_all_predicted_path: true
+ use_predicted_path_outside_lanelet: false
+
+ # For safety check
+ safety_check_params:
+ # safety check configuration
+ enable_safety_check: true
+ # collision check parameters
+ check_all_predicted_path: true
+ publish_debug_marker: false
+ rss_params:
+ rear_vehicle_reaction_time: 1.0
+ rear_vehicle_safety_time_margin: 1.0
+ lateral_distance_max_threshold: 1.0
+ longitudinal_distance_min_threshold: 1.0
+ longitudinal_velocity_delta_time: 1.0
+ # hysteresis factor to expand/shrink polygon
+ hysteresis_factor_expand_rate: 1.0
+ # temporary
+ backward_path_length: 30.0
+ forward_path_length: 100.0
diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml
index 439e08fd7e94f..c2b6babc402a2 100644
--- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml
+++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml
@@ -82,3 +82,12 @@
keep_last: false
priority: 7
max_module_size: 1
+
+ sampling_planner:
+ enable_module: true
+ enable_rtc: false
+ enable_simultaneous_execution_as_approved_module: false
+ enable_simultaneous_execution_as_candidate_module: false
+ keep_last: true
+ priority: 16
+ max_module_size: 1
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
new file mode 100644
index 0000000000000..ba7b554a8444a
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
@@ -0,0 +1,148 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
+#define BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
+
+#include <interpolation/linear_interpolation.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <string>
+#include <utility>
+#include <vector>
+
+struct ModuleConfigParameters
+{
+ bool enable_module{false};
+ bool enable_rtc{false};
+ bool enable_simultaneous_execution_as_approved_module{false};
+ bool enable_simultaneous_execution_as_candidate_module{false};
+ bool keep_last{false};
+ uint8_t priority{0};
+ uint8_t max_module_size{0};
+};
+
+struct LateralAccelerationMap
+{
+ std::vector<double> base_vel;
+ std::vector<double> base_min_acc;
+ std::vector<double> base_max_acc;
+
+ void add(const double velocity, const double min_acc, const double max_acc)
+ {
+ if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) {
+ return;
+ }
+
+ size_t idx = 0;
+ for (size_t i = 0; i < base_vel.size(); ++i) {
+ if (velocity < base_vel.at(i)) {
+ break;
+ }
+ idx = i + 1;
+ }
+
+ base_vel.insert(base_vel.begin() + idx, velocity);
+ base_min_acc.insert(base_min_acc.begin() + idx, min_acc);
+ base_max_acc.insert(base_max_acc.begin() + idx, max_acc);
+ }
+
+ std::pair<double, double> find(const double velocity) const
+ {
+ if (!base_vel.empty() && velocity < base_vel.front()) {
+ return std::make_pair(base_min_acc.front(), base_max_acc.front());
+ }
+ if (!base_vel.empty() && velocity > base_vel.back()) {
+ return std::make_pair(base_min_acc.back(), base_max_acc.back());
+ }
+
+ const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity);
+ const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity);
+
+ return std::make_pair(min_acc, max_acc);
+ }
+};
+
+struct BehaviorPathPlannerParameters
+{
+ bool verbose;
+
+ ModuleConfigParameters config_avoidance;
+ ModuleConfigParameters config_avoidance_by_lc;
+ ModuleConfigParameters config_dynamic_avoidance;
+ ModuleConfigParameters config_start_planner;
+ ModuleConfigParameters config_goal_planner;
+ ModuleConfigParameters config_side_shift;
+ ModuleConfigParameters config_lane_change_left;
+ ModuleConfigParameters config_lane_change_right;
+ ModuleConfigParameters config_sampling_planner;
+ ModuleConfigParameters config_ext_request_lane_change_left;
+ ModuleConfigParameters config_ext_request_lane_change_right;
+
+ double backward_path_length;
+ double forward_path_length;
+ double backward_length_buffer_for_end_of_lane;
+ double backward_length_buffer_for_end_of_pull_over;
+ double backward_length_buffer_for_end_of_pull_out;
+
+ // common parameters
+ double min_acc;
+ double max_acc;
+
+ // lane change parameters
+ double lane_changing_lateral_jerk{0.5};
+ double minimum_lane_changing_velocity{5.6};
+ double lane_change_prepare_duration{4.0};
+ double lane_change_finish_judge_buffer{3.0};
+ LateralAccelerationMap lane_change_lat_acc_map;
+
+ double minimum_pull_over_length;
+ double minimum_pull_out_length;
+ double drivable_area_resolution;
+
+ double refine_goal_search_radius_range;
+
+ double turn_signal_intersection_search_distance;
+ double turn_signal_intersection_angle_threshold_deg;
+ double turn_signal_search_time;
+ double turn_signal_minimum_search_distance;
+ double turn_signal_shift_length_threshold;
+ bool turn_signal_on_swerving;
+
+ bool enable_akima_spline_first;
+ bool enable_cog_on_centerline;
+ double input_path_interval;
+ double output_path_interval;
+
+ double ego_nearest_dist_threshold;
+ double ego_nearest_yaw_threshold;
+
+ // vehicle info
+ vehicle_info_util::VehicleInfo vehicle_info;
+ double wheel_base;
+ double front_overhang;
+ double rear_overhang;
+ double vehicle_width;
+ double vehicle_length;
+ double wheel_tread;
+ double left_over_hang;
+ double right_over_hang;
+ double base_link2front;
+ double base_link2rear;
+
+ // maximum drivable area visualization
+ bool visualize_maximum_drivable_area;
+};
+
+#endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
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 7e990b2236c8b..91d582acabee1 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -145,6 +145,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
manager->name(), create_publisher<Path>(path_reference_name_space + manager->name(), 1));
}
{
+ RCLCPP_INFO(get_logger(), "The priority is %d", p.config_sampling_planner.priority);
ModuleConfigParameters config;
config.enable_module = true;
config.priority = 16;
From e60648a373fa64d3e1f11613436b3174498c070f Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 27 Sep 2023 17:40:38 +0900
Subject: [PATCH 010/115] eliminate unused code
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/behavior_path_planner_node.cpp | 10 ++++------
1 file changed, 4 insertions(+), 6 deletions(-)
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 91d582acabee1..6f8bfff3d6522 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -144,13 +144,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
path_reference_publishers_.emplace(
manager->name(), create_publisher<Path>(path_reference_name_space + manager->name(), 1));
}
- {
+ if (p.config_sampling_planner.enable_module) {
RCLCPP_INFO(get_logger(), "The priority is %d", p.config_sampling_planner.priority);
- ModuleConfigParameters config;
- config.enable_module = true;
- config.priority = 16;
- auto manager =
- std::make_shared<SamplingPlannerModuleManager>(this, "sampling_planner", config);
+
+ auto manager = std::make_shared<SamplingPlannerModuleManager>(
+ this, "sampling_planner", p.config_sampling_planner);
planner_manager_->registerSceneModuleManager(manager);
}
}
From 45be300e4025a09409529c9c56a7ac0e0726d075 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 27 Sep 2023 17:51:17 +0900
Subject: [PATCH 011/115] delete repeated code
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner/bezier_sampler/README.md | 3 -
.../bezier_sampler/bezier.hpp | 75 ----
.../bezier_sampler/bezier_sampling.hpp | 50 ---
.../sampling_planner/frenet_planner/README.md | 7 -
.../frenet_planner/conversions.hpp | 48 ---
.../frenet_planner/frenet_planner.hpp | 67 ----
.../frenet_planner/polynomials.hpp | 55 ---
.../frenet_planner/structures.hpp | 155 --------
.../path_sampler/common_structs.hpp | 136 -------
.../sampling_planner/path_sampler/node.hpp | 103 -----
.../path_sampler/parameters.hpp | 51 ---
.../path_sampler/path_generation.hpp | 52 ---
.../path_sampler/prepare_inputs.hpp | 54 ---
.../path_sampler/type_alias.hpp | 51 ---
.../sampler_common/constraints/footprint.hpp | 29 --
.../constraints/hard_constraint.hpp | 26 --
.../constraints/map_constraints.hpp | 60 ---
.../constraints/soft_constraint.hpp | 35 --
.../sampler_common/structures.hpp | 352 ------------------
.../transform/spline_transform.hpp | 85 -----
.../path_sampler_utils/geometry_utils.hpp | 56 ---
.../path_sampler_utils/trajectory_utils.hpp | 200 ----------
.../bezier_sampler/bezier.cpp | 120 ------
.../bezier_sampler/bezier_sampling.cpp | 78 ----
.../frenet_planner/frenet_planner.cpp | 228 ------------
.../frenet_planner/polynomials.cpp | 76 ----
.../sampling_planner/sampler_common/README.md | 6 -
.../sampler_common/constraints/footprint.cpp | 53 ---
.../constraints/hard_constraint.cpp | 56 ---
.../constraints/soft_constraint.cpp | 54 ---
.../transform/spline_transform.cpp | 315 ----------------
31 files changed, 2736 deletions(-)
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/README.md
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/README.md
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/conversions.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/frenet_planner.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp
delete mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp
delete mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp
delete mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp
delete mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp
delete mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/README.md
delete mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/footprint.cpp
delete mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.cpp
delete mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.cpp
delete mode 100644 planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/transform/spline_transform.cpp
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/README.md b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/README.md
deleted file mode 100644
index 654193006fad3..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/README.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# Bézier sampler
-
-Implementation of bézier curves and their generation following the sampling strategy from <https://ieeexplore.ieee.org/document/8932495>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp
deleted file mode 100644
index 93c6ec6b87f36..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp
+++ /dev/null
@@ -1,75 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef BEZIER_SAMPLER__BEZIER_HPP_
-#define BEZIER_SAMPLER__BEZIER_HPP_
-
-#include <eigen3/Eigen/Core>
-
-#include <vector>
-
-namespace bezier_sampler
-{
-
-// Coefficients for matrix calculation of the quintic Bézier curve.
-const Eigen::Matrix<double, 6, 6> quintic_bezier_coefficients(
- (Eigen::Matrix<double, 6, 6>() << 1, 0, 0, 0, 0, 0, -5, 5, 0, 0, 0, 0, 10, -20, 10, 0, 0, 0, -10,
- 30, -30, 10, 0, 0, 5, -20, 30, -20, 5, 0, -1, 5, -10, 10, -5, 1)
- .finished());
-const Eigen::Matrix<double, 5, 6> quintic_bezier_velocity_coefficients(
- (Eigen::Matrix<double, 5, 6>() << quintic_bezier_coefficients.row(1) * 1,
- quintic_bezier_coefficients.row(2) * 2, quintic_bezier_coefficients.row(3) * 3,
- quintic_bezier_coefficients.row(4) * 4, quintic_bezier_coefficients.row(5) * 5)
- .finished());
-const Eigen::Matrix<double, 4, 6> quintic_bezier_acceleration_coefficients(
- (Eigen::Matrix<double, 4, 6>() << quintic_bezier_velocity_coefficients.row(1) * 1,
- quintic_bezier_velocity_coefficients.row(2) * 2, quintic_bezier_velocity_coefficients.row(3) * 3,
- quintic_bezier_velocity_coefficients.row(4) * 4)
- .finished());
-
-/// @brief Quintic Bezier curve
-class Bezier
-{
- Eigen::Matrix<double, 6, 2> control_points_;
-
-public:
- /// @brief constructor from a matrix
- explicit Bezier(Eigen::Matrix<double, 6, 2> control_points);
- /// @brief constructor from a set of control points
- explicit Bezier(const std::vector<Eigen::Vector2d> & control_points);
- /// @brief return the control points
- [[nodiscard]] const Eigen::Matrix<double, 6, 2> & getControlPoints() const;
- /// @brief return the curve in cartesian frame with the desired resolution
- [[nodiscard]] std::vector<Eigen::Vector2d> cartesian(const double resolution) const;
- /// @brief return the curve in cartesian frame with the desired number of points
- [[nodiscard]] std::vector<Eigen::Vector2d> cartesian(const int nb_points) const;
- /// @brief return the curve in cartesian frame (including angle) with the desired number of
- /// points
- [[nodiscard]] std::vector<Eigen::Vector3d> cartesianWithHeading(const int nb_points) const;
- /// @brief calculate the curve value for the given parameter t
- [[nodiscard]] Eigen::Vector2d value(const double t) const;
- /// @brief calculate the curve value for the given parameter t (using matrix formulation)
- [[nodiscard]] Eigen::Vector2d valueM(const double t) const;
- /// @brief calculate the velocity (1st derivative) for the given parameter t
- [[nodiscard]] Eigen::Vector2d velocity(const double t) const;
- /// @brief calculate the acceleration (2nd derivative) for the given parameter t
- [[nodiscard]] Eigen::Vector2d acceleration(const double t) const;
- /// @brief return the heading (in radians) of the tangent for the given parameter t
- [[nodiscard]] double heading(const double t) const;
- /// @brief calculate the curvature for the given parameter t
- [[nodiscard]] double curvature(const double t) const;
-};
-} // namespace bezier_sampler
-
-#endif // BEZIER_SAMPLER__BEZIER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp
deleted file mode 100644
index 1d436191326c9..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp
+++ /dev/null
@@ -1,50 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_
-#define BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-
-#include <eigen3/Eigen/Core>
-
-#include <vector>
-
-namespace bezier_sampler
-{
-struct SamplingParameters
-{
- int nb_t; // Number of samples of normalized tangent vector magnitude
- double mt_min; // Minimum normalized tangent vector magnitude
- double mt_max; // Maximum normalized tangent vector magnitude
- int nb_k; // Number of samples of normalized curvature vector magnitude
- double mk_min; // Minimum normalized curvature vector magnitude
- double mk_max; // Minimum normalized curvature vector magnitude
-};
-/// @brief sample Bezier curves given an initial and final state and sampling parameters
-// cspell: ignore Artuñedoet
-/// @details from Section IV of A. Artuñedoet al.: Real-Time Motion Planning Approach for Automated
-/// Driving in Urban Environments
-std::vector<Bezier> sample(
- const sampler_common::State & initial, const sampler_common::State & final,
- const SamplingParameters & params);
-/// @brief generate a Bezier curve for the given states, velocities, and accelerations
-Bezier generate(
- const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose,
- const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration,
- const Eigen::Vector2d & final_velocity, const Eigen::Vector2d & final_acceleration);
-} // namespace bezier_sampler
-
-#endif // BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/README.md b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/README.md
deleted file mode 100644
index 083a880924604..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/README.md
+++ /dev/null
@@ -1,7 +0,0 @@
-# Frenet planner
-
-Trajectory generation in Frenet frame.
-
-## Description
-
-[Original paper](https://www.researchgate.net/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/conversions.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/conversions.hpp
deleted file mode 100644
index 9979853a046b7..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/conversions.hpp
+++ /dev/null
@@ -1,48 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef FRENET_PLANNER__CONVERSIONS_HPP_
-#define FRENET_PLANNER__CONVERSIONS_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-
-#include <vector>
-
-namespace frenet_planner
-{
-
-/// @brief calculate the lengths and yaws vectors of the given trajectory
-/// @param [inout] trajectory trajectory
-inline void calculateLengthsAndYaws(Trajectory & trajectory)
-{
- if (trajectory.points.empty()) return;
- trajectory.lengths.clear();
- trajectory.yaws.clear();
- trajectory.yaws.reserve(trajectory.points.size());
- trajectory.lengths.reserve(trajectory.points.size());
-
- trajectory.lengths.push_back(0.0);
- for (auto it = trajectory.points.begin(); it != std::prev(trajectory.points.end()); ++it) {
- const auto dx = std::next(it)->x() - it->x();
- const auto dy = std::next(it)->y() - it->y();
- trajectory.lengths.push_back(trajectory.lengths.back() + std::hypot(dx, dy));
- trajectory.yaws.push_back(std::atan2(dy, dx));
- }
- const auto last_yaw = trajectory.yaws.empty() ? 0.0 : trajectory.yaws.back();
- trajectory.yaws.push_back(last_yaw);
-}
-} // namespace frenet_planner
-
-#endif // FRENET_PLANNER__CONVERSIONS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/frenet_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/frenet_planner.hpp
deleted file mode 100644
index a843dc64fef9f..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/frenet_planner.hpp
+++ /dev/null
@@ -1,67 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef FRENET_PLANNER__FRENET_PLANNER_HPP_
-#define FRENET_PLANNER__FRENET_PLANNER_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
-
-#include <optional>
-#include <vector>
-
-namespace frenet_planner
-{
-/// @brief generate trajectories relative to the reference for the given initial state and sampling
-/// parameters
-std::vector<Trajectory> generateTrajectories(
- const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
- const SamplingParameters & sampling_parameters);
-/// @brief generate trajectories relative to the reference for the given initial state and sampling
-/// parameters
-std::vector<Trajectory> generateLowVelocityTrajectories(
- const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
- const SamplingParameters & sampling_parameters);
-/// @brief generate paths relative to the reference for the given initial state and sampling
-/// parameters
-std::vector<Path> generatePaths(
- const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
- const SamplingParameters & sampling_parameters);
-/// @brief generate a candidate path
-/// @details one polynomial for lateral motion (d) is calculated over the longitudinal displacement
-/// (s): d(s).
-Path generateCandidate(
- const FrenetState & initial_state, const FrenetState & target_state, const double s_resolution);
-/// @brief generate a candidate trajectory
-/// @details the polynomials for lateral motion (d) and longitudinal motion (s) are calculated over
-/// time: d(t) and s(t).
-Trajectory generateCandidate(
- const FrenetState & initial_state, const FrenetState & target_state, const double duration,
- const double time_resolution);
-/// @brief generate a low velocity candidate trajectory
-/// @details the polynomial for lateral motion (d) is calculated over the longitudinal displacement
-/// (s) rather than over time: d(s) and s(t).
-Trajectory generateLowVelocityCandidate(
- const FrenetState & initial_state, const FrenetState & target_state, const double duration,
- const double time_resolution);
-/// @brief calculate the cartesian frame of the given path
-void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path);
-/// @brief calculate the cartesian frame of the given trajectory
-void calculateCartesian(
- const sampler_common::transform::Spline2D & reference, Trajectory & trajectory);
-
-} // namespace frenet_planner
-
-#endif // FRENET_PLANNER__FRENET_PLANNER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp
deleted file mode 100644
index 65157c76d5db3..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp
+++ /dev/null
@@ -1,55 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef FRENET_PLANNER__POLYNOMIALS_HPP_
-#define FRENET_PLANNER__POLYNOMIALS_HPP_
-
-namespace frenet_planner
-{
-class Polynomial
-{
- /// @brief polynomial coefficients
- double a_;
- double b_;
- double c_;
- double d_;
- double e_;
- double f_;
-
-public:
- /// @brief Create a quintic polynomial between an initial and final state over a parameter length
- /// @param x0 initial position
- /// @param x0v initial velocity
- /// @param x0a initial acceleration
- /// @param xT final position
- /// @param xTv final velocity
- /// @param xTa final acceleration
- /// @param T parameter length (arc length or duration)
- Polynomial(
- const double x0, const double x0v, const double x0a, const double xT, const double xTv,
- const double xTa, const double T);
- // TODO(Maxime CLEMENT) add quartic case for when final position is not given
-
- /// @brief Get the position at the given time
- [[nodiscard]] double position(const double t) const;
- /// @brief Get the velocity at the given time
- [[nodiscard]] double velocity(const double t) const;
- /// @brief Get the acceleration at the given time
- [[nodiscard]] double acceleration(const double t) const;
- /// @brief Get the jerk at the given time
- [[nodiscard]] double jerk(const double t) const;
-};
-} // namespace frenet_planner
-
-#endif // FRENET_PLANNER__POLYNOMIALS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp
deleted file mode 100644
index ed8c8579eecff..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp
+++ /dev/null
@@ -1,155 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef FRENET_PLANNER__STRUCTURES_HPP_
-#define FRENET_PLANNER__STRUCTURES_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-
-#include <optional>
-#include <vector>
-
-namespace frenet_planner
-{
-
-struct FrenetState
-{
- sampler_common::FrenetPoint position = {0, 0};
- double lateral_velocity{};
- double longitudinal_velocity{};
- double lateral_acceleration{};
- double longitudinal_acceleration{};
-};
-
-struct Path : sampler_common::Path
-{
- std::vector<sampler_common::FrenetPoint> frenet_points{};
- std::optional<Polynomial> lateral_polynomial{};
-
- Path() = default;
- explicit Path(const sampler_common::Path & path) : sampler_common::Path(path) {}
-
- void clear() override
- {
- sampler_common::Path::clear();
- frenet_points.clear();
- lateral_polynomial.reset();
- }
-
- void reserve(const size_t size) override
- {
- sampler_common::Path::reserve(size);
- frenet_points.reserve(size);
- }
-
- [[nodiscard]] Path extend(const Path & path) const
- {
- Path extended_traj(sampler_common::Path::extend(path));
- extended_traj.frenet_points.insert(
- extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end());
- extended_traj.frenet_points.insert(
- extended_traj.frenet_points.end(), path.frenet_points.begin(), path.frenet_points.end());
- // TODO(Maxime CLEMENT): direct copy from the 2nd trajectory. might need to be improved
- extended_traj.lateral_polynomial = path.lateral_polynomial;
- return extended_traj;
- }
-
- [[nodiscard]] Path * subset(const size_t from_idx, const size_t to_idx) const override
- {
- auto * subpath = new Path(*sampler_common::Path::subset(from_idx, to_idx));
- assert(to_idx >= from_idx);
- subpath->reserve(to_idx - from_idx);
-
- const auto copy_subset = [&](const auto & from, auto & to) {
- to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx));
- };
- copy_subset(frenet_points, subpath->frenet_points);
- return subpath;
- };
-};
-
-struct SamplingParameter
-{
- double target_duration{};
- FrenetState target_state;
-};
-
-struct Trajectory : sampler_common::Trajectory
-{
- std::vector<sampler_common::FrenetPoint> frenet_points{};
- std::optional<Polynomial> lateral_polynomial{};
- std::optional<Polynomial> longitudinal_polynomial{};
- SamplingParameter sampling_parameter;
-
- Trajectory() = default;
- explicit Trajectory(const sampler_common::Trajectory & traj) : sampler_common::Trajectory(traj) {}
-
- void clear() override
- {
- sampler_common::Trajectory::clear();
- frenet_points.clear();
- lateral_polynomial.reset();
- longitudinal_polynomial.reset();
- }
-
- void reserve(const size_t size) override
- {
- sampler_common::Trajectory::reserve(size);
- frenet_points.reserve(size);
- }
-
- [[nodiscard]] Trajectory extend(const Trajectory & traj) const
- {
- Trajectory extended_traj(sampler_common::Trajectory::extend(traj));
- extended_traj.frenet_points.insert(
- extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end());
- extended_traj.frenet_points.insert(
- extended_traj.frenet_points.end(), traj.frenet_points.begin(), traj.frenet_points.end());
- // TODO(Maxime CLEMENT): direct copy from the 2nd trajectory. might need to be improved
- extended_traj.lateral_polynomial = traj.lateral_polynomial;
- extended_traj.longitudinal_polynomial = traj.longitudinal_polynomial;
- return extended_traj;
- }
-
- [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override
- {
- auto * sub_trajectory = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx));
- assert(to_idx >= from_idx);
- sub_trajectory->reserve(to_idx - from_idx);
-
- const auto copy_subset = [&](const auto & from, auto & to) {
- to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx));
- };
- copy_subset(frenet_points, sub_trajectory->frenet_points);
- return sub_trajectory;
- };
-};
-
-inline std::ostream & operator<<(std::ostream & os, const SamplingParameter & sp)
-{
- const auto & s = sp.target_state;
- return os << "["
- << "T=" << sp.target_duration << ", s=" << s.position.s << ", d=" << s.position.d
- << ", s'=" << s.longitudinal_velocity << ", d'=" << s.lateral_velocity
- << ", s\"=" << s.longitudinal_acceleration << ", d\"=" << s.lateral_acceleration << "]";
-}
-struct SamplingParameters
-{
- std::vector<SamplingParameter> parameters;
- double resolution;
-};
-} // namespace frenet_planner
-
-#endif // FRENET_PLANNER__STRUCTURES_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp
deleted file mode 100644
index b3ebb592bb393..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp
+++ /dev/null
@@ -1,136 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PATH_SAMPLER__COMMON_STRUCTS_HPP_
-#define PATH_SAMPLER__COMMON_STRUCTS_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "tier4_autoware_utils/ros/update_param.hpp"
-#include "tier4_autoware_utils/system/stop_watch.hpp"
-
-#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
-
-#include <memory>
-#include <optional>
-#include <string>
-#include <vector>
-
-namespace path_sampler
-{
-struct PlannerData
-{
- // input
- Header header;
- std::vector<TrajectoryPoint> traj_points; // converted from the input path
- std::vector<geometry_msgs::msg::Point> left_bound;
- std::vector<geometry_msgs::msg::Point> right_bound;
-
- // ego
- geometry_msgs::msg::Pose ego_pose;
- double ego_vel{};
-};
-
-struct TimeKeeper
-{
- void init() { accumulated_msg = "\n"; }
-
- template <typename T>
- TimeKeeper & operator<<(const T & msg)
- {
- latest_stream << msg;
- return *this;
- }
-
- void endLine()
- {
- const auto msg = latest_stream.str();
- accumulated_msg += msg + "\n";
- latest_stream.str("");
-
- if (enable_calculation_time_info) {
- RCLCPP_INFO_STREAM(rclcpp::get_logger("path_sampler.time"), msg);
- }
- }
-
- void tic(const std::string & func_name) { stop_watch_.tic(func_name); }
-
- void toc(const std::string & func_name, const std::string & white_spaces)
- {
- const double elapsed_time = stop_watch_.toc(func_name);
- *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]";
- endLine();
- }
-
- std::string getLog() const { return accumulated_msg; }
-
- bool enable_calculation_time_info;
- std::string accumulated_msg = "\n";
- std::stringstream latest_stream;
-
- tier4_autoware_utils::StopWatch<
- std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
- stop_watch_;
-};
-
-struct DebugData
-{
- std::vector<sampler_common::Path> sampled_candidates{};
- size_t previous_sampled_candidates_nb = 0UL;
- std::vector<tier4_autoware_utils::Polygon2d> obstacles{};
- std::vector<tier4_autoware_utils::MultiPoint2d> footprints{};
-};
-
-struct TrajectoryParam
-{
- TrajectoryParam() = default;
- explicit TrajectoryParam(rclcpp::Node * node)
- {
- output_delta_arc_length = node->declare_parameter<double>("common.output_delta_arc_length");
- }
-
- void onParam(const std::vector<rclcpp::Parameter> & parameters)
- {
- using tier4_autoware_utils::updateParam;
-
- // common
- updateParam<double>(parameters, "common.output_delta_arc_length", output_delta_arc_length);
- }
-
- double output_delta_arc_length;
-};
-
-struct EgoNearestParam
-{
- EgoNearestParam() = default;
- explicit EgoNearestParam(rclcpp::Node * node)
- {
- dist_threshold = node->declare_parameter<double>("ego_nearest_dist_threshold");
- yaw_threshold = node->declare_parameter<double>("ego_nearest_yaw_threshold");
- }
-
- void onParam(const std::vector<rclcpp::Parameter> & parameters)
- {
- using tier4_autoware_utils::updateParam;
- updateParam<double>(parameters, "ego_nearest_dist_threshold", dist_threshold);
- updateParam<double>(parameters, "ego_nearest_yaw_threshold", yaw_threshold);
- }
-
- double dist_threshold{0.0};
- double yaw_threshold{0.0};
-};
-} // namespace path_sampler
-
-#endif // PATH_SAMPLER__COMMON_STRUCTS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp
deleted file mode 100644
index c9bdb68f35a50..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/node.hpp
+++ /dev/null
@@ -1,103 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PATH_SAMPLER__NODE_HPP_
-#define PATH_SAMPLER__NODE_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "vehicle_info_util/vehicle_info_util.hpp"
-
-#include <algorithm>
-#include <memory>
-#include <optional>
-#include <string>
-#include <vector>
-
-namespace path_sampler
-{
-class PathSampler : public rclcpp::Node
-{
-public:
- explicit PathSampler(const rclcpp::NodeOptions & node_options);
-
-protected: // for the static_centerline_optimizer package
- // argument variables
- vehicle_info_util::VehicleInfo vehicle_info_{};
- mutable DebugData debug_data_{};
- mutable std::shared_ptr<TimeKeeper> time_keeper_ptr_{nullptr};
-
- // parameters
- TrajectoryParam traj_param_{};
- EgoNearestParam ego_nearest_param_{};
- Parameters params_;
- size_t debug_id_ = 0;
-
- // variables for subscribers
- Odometry::SharedPtr ego_state_ptr_;
- PredictedObjects::SharedPtr in_objects_ptr_ = std::make_shared<PredictedObjects>();
-
- // variables for previous information
- std::optional<sampler_common::Path> prev_path_;
-
- // interface publisher
- rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
- rclcpp::Publisher<MarkerArray>::SharedPtr virtual_wall_pub_;
-
- // interface subscriber
- rclcpp::Subscription<Path>::SharedPtr path_sub_;
- rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
- rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
-
- // debug publisher
- rclcpp::Publisher<MarkerArray>::SharedPtr debug_markers_pub_;
- rclcpp::Publisher<StringStamped>::SharedPtr debug_calculation_time_pub_;
-
- // parameter callback
- rcl_interfaces::msg::SetParametersResult onParam(
- const std::vector<rclcpp::Parameter> & parameters);
- OnSetParametersCallbackHandle::SharedPtr set_param_res_;
-
- // subscriber callback function
- void objectsCallback(const PredictedObjects::SharedPtr msg);
- void onPath(const Path::SharedPtr);
-
- // main functions
- bool isDataReady(const Path & path, rclcpp::Clock clock) const;
- PlannerData createPlannerData(const Path & path) const;
- std::vector<TrajectoryPoint> generateTrajectory(const PlannerData & planner_data);
- std::vector<TrajectoryPoint> extendTrajectory(
- const std::vector<TrajectoryPoint> & traj_points,
- const std::vector<TrajectoryPoint> & optimized_points) const;
- void resetPreviousData();
- sampler_common::State getPlanningState(
- sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const;
-
- // sub-functions of generateTrajectory
- void copyZ(
- const std::vector<TrajectoryPoint> & from_traj, std::vector<TrajectoryPoint> & to_traj);
- void copyVelocity(
- const std::vector<TrajectoryPoint> & from_traj, std::vector<TrajectoryPoint> & to_traj,
- const geometry_msgs::msg::Pose & ego_pose);
- std::vector<TrajectoryPoint> generatePath(const PlannerData & planner_data);
- void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const;
- void publishDebugMarker(const std::vector<TrajectoryPoint> & traj_points) const;
-};
-} // namespace path_sampler
-
-#endif // PATH_SAMPLER__NODE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp
deleted file mode 100644
index 34446a9db5e85..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp
+++ /dev/null
@@ -1,51 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PATH_SAMPLER__PARAMETERS_HPP_
-#define PATH_SAMPLER__PARAMETERS_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-
-#include <vector>
-
-struct Parameters
-{
- sampler_common::Constraints constraints;
- struct
- {
- bool enable_frenet{};
- bool enable_bezier{};
- double resolution{};
- int previous_path_reuse_points_nb{};
- std::vector<double> target_lengths{};
- std::vector<double> target_lateral_positions{};
- int nb_target_lateral_positions{};
- struct
- {
- std::vector<double> target_lateral_velocities{};
- std::vector<double> target_lateral_accelerations{};
- } frenet;
- bezier_sampler::SamplingParameters bezier{};
- } sampling;
-
- struct
- {
- bool force_zero_deviation{};
- bool force_zero_heading{};
- bool smooth_reference{};
- } preprocessing{};
-};
-
-#endif // PATH_SAMPLER__PARAMETERS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp
deleted file mode 100644
index f9b9781b2a14c..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/path_generation.hpp
+++ /dev/null
@@ -1,52 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PATH_SAMPLER__PATH_GENERATION_HPP_
-#define PATH_SAMPLER__PATH_GENERATION_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
-
-#include <autoware_auto_planning_msgs/msg/path.hpp>
-
-#include <vector>
-
-namespace path_sampler
-{
-/**
- * @brief generate candidate paths for the given problem inputs
- * @param [in] initial_state initial ego state
- * @param [in] path_spline spline of the reference path
- * @param [in] base_length base length of the reuse path (= 0.0 if not reusing a previous path)
- * @param [in] params parameters
- * @return candidate paths
- */
-std::vector<sampler_common::Path> generateCandidatePaths(
- const sampler_common::State & initial_state,
- const sampler_common::transform::Spline2D & path_spline, const double base_length,
- const Parameters & params);
-
-std::vector<sampler_common::Path> generateBezierPaths(
- const sampler_common::State & initial_state, const double base_length,
- const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
-std::vector<frenet_planner::Path> generateFrenetPaths(
- const sampler_common::State & initial_state, const double base_length,
- const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
-} // namespace path_sampler
-
-#endif // PATH_SAMPLER__PATH_GENERATION_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp
deleted file mode 100644
index b20131a817135..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/prepare_inputs.hpp
+++ /dev/null
@@ -1,54 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PATH_SAMPLER__PREPARE_INPUTS_HPP_
-#define PATH_SAMPLER__PREPARE_INPUTS_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/parameters.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
-
-#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
-#include <autoware_auto_planning_msgs/msg/path.hpp>
-#include <autoware_auto_planning_msgs/msg/path_point.hpp>
-#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
-#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
-#include <geometry_msgs/msg/point.hpp>
-#include <geometry_msgs/msg/pose.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
-#include <nav_msgs/msg/detail/occupancy_grid__struct.hpp>
-#include <nav_msgs/msg/occupancy_grid.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-
-#include <string>
-#include <vector>
-
-namespace path_sampler
-{
-/// @brief prepare constraints
-void prepareConstraints(
- sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects,
- const std::vector<geometry_msgs::msg::Point> & left_bound,
- const std::vector<geometry_msgs::msg::Point> & right_bound);
-/// @brief prepare sampling parameters to generate Frenet paths
-frenet_planner::SamplingParameters prepareSamplingParameters(
- const sampler_common::State & initial_state, const double base_length,
- const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
-/// @brief prepare the 2D spline representation of the given Path message
-sampler_common::transform::Spline2D preparePathSpline(
- const std::vector<TrajectoryPoint> & path_msg, const bool smooth_path);
-} // namespace path_sampler
-
-#endif // PATH_SAMPLER__PREPARE_INPUTS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp
deleted file mode 100644
index 8a792eba2f237..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp
+++ /dev/null
@@ -1,51 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_
-#define PATH_SAMPLER__TYPE_ALIAS_HPP_
-
-#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
-#include "autoware_auto_planning_msgs/msg/path.hpp"
-#include "autoware_auto_planning_msgs/msg/path_point.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
-#include "geometry_msgs/msg/point.hpp"
-#include "geometry_msgs/msg/pose.hpp"
-#include "geometry_msgs/msg/twist.hpp"
-#include "nav_msgs/msg/odometry.hpp"
-#include "std_msgs/msg/header.hpp"
-#include "tier4_debug_msgs/msg/string_stamped.hpp"
-#include "visualization_msgs/msg/marker_array.hpp"
-
-namespace path_sampler
-{
-// std_msgs
-using std_msgs::msg::Header;
-// planning
-using autoware_auto_planning_msgs::msg::Path;
-using autoware_auto_planning_msgs::msg::PathPoint;
-using autoware_auto_planning_msgs::msg::Trajectory;
-using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-// perception
-using autoware_auto_perception_msgs::msg::PredictedObjects;
-// navigation
-using nav_msgs::msg::Odometry;
-// visualization
-using visualization_msgs::msg::Marker;
-using visualization_msgs::msg::MarkerArray;
-// debug
-using tier4_debug_msgs::msg::StringStamped;
-} // namespace path_sampler
-
-#endif // PATH_SAMPLER__TYPE_ALIAS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp
deleted file mode 100644
index b063ba3cc3e5f..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp
+++ /dev/null
@@ -1,29 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_
-#define SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-
-namespace sampler_common::constraints
-{
-/// @brief Calculate the footprint of the given path
-/// @param path sequence of pose used to build the footprint
-/// @param constraints input constraint object containing vehicle footprint offsets
-/// @return the polygon footprint of the path
-MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constraints);
-} // namespace sampler_common::constraints
-
-#endif // SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp
deleted file mode 100644
index aa9c82cd9e966..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp
+++ /dev/null
@@ -1,26 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_
-#define SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-
-namespace sampler_common::constraints
-{
-/// @brief Check if the path satisfies the hard constraints
-MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints);
-} // namespace sampler_common::constraints
-
-#endif // SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp
deleted file mode 100644
index e8318ab1e1023..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/map_constraints.hpp
+++ /dev/null
@@ -1,60 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_
-#define SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-
-#include <boost/geometry/algorithms/within.hpp>
-
-#include <vector>
-
-namespace sampler_common::constraints
-{
-struct MapConstraints
-{
- MapConstraints(
- const Point2d & ego_pose, const lanelet::LaneletMapConstPtr & map_ptr,
- const std::vector<lanelet::Ids> route_ids)
- {
- }
- /// @brief check the given path and return the corresponding cost
- /// \return cost of the path. If negative then the path is invalid.
- double check(const Path & path)
- {
- double cost = 0.0;
- for (const auto & p : path.points) {
- bool drivable = false;
- for (size_t i = 0; i < drivable_polygons.size(); ++i) {
- if (boost::geometry::within(p, drivable_polygons[i])) {
- drivable = true;
- cost += polygon_costs[i];
- break;
- }
- }
- if (!drivable) {
- cost = -1;
- break;
- }
- }
- return cost;
- }
-
- MultiPolygon2d drivable_polygons;
- std::vector<double> polygon_costs;
-};
-} // namespace sampler_common::constraints
-
-#endif // SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp
deleted file mode 100644
index 4184362497425..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp
+++ /dev/null
@@ -1,35 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_
-#define SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
-
-namespace sampler_common::constraints
-{
-/// @brief calculate the curvature cost of the given path
-void calculateCurvatureCost(Path & path, const Constraints & constraints);
-/// @brief calculate the length cost of the given path
-void calculateLengthCost(Path & path, const Constraints & constraints);
-/// @brief calculate the lateral deviation cost at the end of the given path
-void calculateLateralDeviationCost(
- Path & path, const Constraints & constraints, const transform::Spline2D & reference);
-/// @brief calculate the overall cost of the given path
-void calculateCost(
- Path & path, const Constraints & constraints, const transform::Spline2D & reference);
-} // namespace sampler_common::constraints
-
-#endif // SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp
deleted file mode 100644
index 1cb14fdf56198..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp
+++ /dev/null
@@ -1,352 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef SAMPLER_COMMON__STRUCTURES_HPP_
-#define SAMPLER_COMMON__STRUCTURES_HPP_
-
-#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
-
-#include <eigen3/Eigen/Core>
-#include <interpolation/linear_interpolation.hpp>
-
-#include <boost/geometry/core/cs.hpp>
-#include <boost/geometry/geometries/multi_polygon.hpp>
-#include <boost/geometry/geometries/point_xy.hpp>
-#include <boost/geometry/geometries/polygon.hpp>
-
-#include <algorithm>
-#include <iostream>
-#include <memory>
-#include <numeric>
-#include <string>
-#include <vector>
-
-namespace sampler_common
-{
-using tier4_autoware_utils::LinearRing2d;
-using tier4_autoware_utils::MultiPoint2d;
-using tier4_autoware_utils::MultiPolygon2d;
-using tier4_autoware_utils::Point2d;
-using tier4_autoware_utils::Polygon2d;
-
-/// @brief data about constraint check results of a given path
-struct ConstraintResults
-{
- bool collision = true;
- bool curvature = true;
- bool drivable_area = true;
-
- [[nodiscard]] bool isValid() const { return collision && curvature && drivable_area; }
-
- void clear() { collision = curvature = drivable_area = true; }
-};
-struct FrenetPoint
-{
- FrenetPoint(double s_, double d_) : s(s_), d(d_) {}
- double s = 0.0;
- double d = 0.0;
-};
-
-struct State
-{
- Point2d pose{};
- FrenetPoint frenet{0.0, 0.0};
- double curvature{};
- double heading{};
-};
-
-struct Configuration : State
-{
- double velocity{};
- double acceleration{};
-};
-
-/// @brief Path
-struct Path
-{
- std::vector<Point2d> points{};
- std::vector<double> curvatures{};
- std::vector<double> yaws{};
- std::vector<double> lengths{};
- ConstraintResults constraint_results{};
- double cost{};
- std::string tag{}; // string tag used for debugging
-
- Path() = default;
- virtual ~Path() = default;
-
- virtual void clear()
- {
- points.clear();
- curvatures.clear();
- yaws.clear();
- lengths.clear();
- constraint_results.clear();
- tag = "";
- cost = 0.0;
- }
-
- virtual void reserve(const size_t size)
- {
- points.reserve(size);
- curvatures.reserve(size);
- yaws.reserve(size);
- lengths.reserve(size);
- }
-
- [[nodiscard]] Path extend(const Path & path) const
- {
- Path extended_path;
- auto offset = 0l;
- auto length_offset = 0.0;
- if (!points.empty() && !path.points.empty()) {
- if (
- points.back().x() == path.points.front().x() &&
- points.back().y() == path.points.front().y()) {
- offset = 1l;
- }
- length_offset = std::hypot(
- path.points[offset].x() - points.back().x(), path.points[offset].y() - points.back().y());
- }
- const auto ext = [&](auto & dest, const auto & first, const auto & second) {
- dest.insert(dest.end(), first.begin(), first.end());
- dest.insert(dest.end(), std::next(second.begin(), offset), second.end());
- };
- ext(extended_path.points, points, path.points);
- ext(extended_path.curvatures, curvatures, path.curvatures);
- ext(extended_path.yaws, yaws, path.yaws);
- extended_path.lengths.insert(extended_path.lengths.end(), lengths.begin(), lengths.end());
- const auto last_base_length = lengths.empty() ? 0.0 : lengths.back() + length_offset;
- for (size_t i = offset; i < path.lengths.size(); ++i)
- extended_path.lengths.push_back(last_base_length + path.lengths[i]);
- extended_path.cost = path.cost;
- extended_path.constraint_results = path.constraint_results;
- extended_path.tag = path.tag;
- return extended_path;
- }
-
- // Return a pointer to allow overriding classes to return the appropriate type
- // Without pointer we are stuck with returning a Path
- [[nodiscard]] virtual Path * subset(const size_t from_idx, const size_t to_idx) const
- {
- auto * subpath = new Path();
- subpath->reserve(to_idx - from_idx);
-
- const auto copy_subset = [&](const auto & from, auto & to) {
- to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx));
- };
- copy_subset(points, subpath->points);
- copy_subset(curvatures, subpath->curvatures);
- copy_subset(yaws, subpath->yaws);
- copy_subset(lengths, subpath->lengths);
- return subpath;
- };
-};
-
-struct Trajectory : Path
-{
- std::vector<double> longitudinal_velocities{};
- std::vector<double> longitudinal_accelerations{};
- std::vector<double> lateral_velocities{};
- std::vector<double> lateral_accelerations{};
- std::vector<double> jerks{};
- std::vector<double> times{};
-
- Trajectory() = default;
- ~Trajectory() override = default;
- explicit Trajectory(const Path & path) : Path(path) {}
-
- void clear() override
- {
- Path::clear();
- longitudinal_velocities.clear();
- longitudinal_accelerations.clear();
- lateral_velocities.clear();
- lateral_accelerations.clear();
- jerks.clear();
- times.clear();
- }
-
- void reserve(const size_t size) override
- {
- Path::reserve(size);
- longitudinal_velocities.reserve(size);
- longitudinal_accelerations.reserve(size);
- lateral_velocities.reserve(size);
- lateral_accelerations.reserve(size);
- jerks.reserve(size);
- times.reserve(size);
- }
-
- [[nodiscard]] Trajectory extend(const Trajectory & traj) const
- {
- Trajectory extended_traj(Path::extend(traj));
- auto offset = 0l;
- auto time_offset = 0.0;
- // TODO(Maxime): remove these checks
- if (!points.empty() && !traj.points.empty()) {
- if (
- points.back().x() == traj.points.front().x() &&
- points.back().y() == traj.points.front().y())
- offset = 1l;
- const auto ds = std::hypot(
- traj.points[offset].x() - points.back().x(), traj.points[offset].y() - points.back().y());
- const auto v = std::max(
- std::max(longitudinal_velocities.back(), traj.longitudinal_velocities[offset]), 0.1);
- time_offset = std::abs(ds / v);
- }
- const auto ext = [&](auto & dest, const auto & first, const auto & second) {
- dest.insert(dest.end(), first.begin(), first.end());
- dest.insert(dest.end(), std::next(second.begin(), offset), second.end());
- };
- ext(
- extended_traj.longitudinal_velocities, longitudinal_velocities, traj.longitudinal_velocities);
- ext(
- extended_traj.longitudinal_accelerations, longitudinal_accelerations,
- traj.longitudinal_accelerations);
- ext(extended_traj.lateral_velocities, lateral_velocities, traj.lateral_velocities);
- ext(extended_traj.lateral_accelerations, lateral_accelerations, traj.lateral_accelerations);
- ext(extended_traj.jerks, jerks, traj.jerks);
- extended_traj.times.insert(extended_traj.times.end(), times.begin(), times.end());
- const auto last_base_time = times.empty() ? 0.0 : times.back() + time_offset;
- for (size_t i = offset; i < traj.times.size(); ++i)
- extended_traj.times.push_back(last_base_time + traj.times[i]);
- return extended_traj;
- }
-
- [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override
- {
- auto * sub_trajectory = new Trajectory(*Path::subset(from_idx, to_idx));
-
- const auto copy_subset = [&](const auto & from, auto & to) {
- to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx));
- };
-
- copy_subset(longitudinal_velocities, sub_trajectory->longitudinal_velocities);
- copy_subset(longitudinal_accelerations, sub_trajectory->longitudinal_accelerations);
- copy_subset(lateral_velocities, sub_trajectory->lateral_velocities);
- copy_subset(lateral_accelerations, sub_trajectory->lateral_accelerations);
- copy_subset(jerks, sub_trajectory->jerks);
- copy_subset(times, sub_trajectory->times);
- return sub_trajectory;
- }
-
- [[nodiscard]] Trajectory resample(const double fixed_interval) const
- {
- Trajectory t;
- if (lengths.size() < 2 || fixed_interval <= 0.0) return *this;
-
- const auto new_size =
- static_cast<size_t>((lengths.back() - lengths.front()) / fixed_interval) + 1;
- t.times.reserve(new_size);
- t.lengths.reserve(new_size);
- for (auto i = 0lu; i < new_size; ++i)
- t.lengths.push_back(lengths.front() + static_cast<double>(i) * fixed_interval);
- t.times = interpolation::lerp(lengths, times, t.lengths);
- std::vector<double> xs;
- std::vector<double> ys;
- xs.reserve(points.size());
- ys.reserve(points.size());
- for (const auto & p : points) {
- xs.push_back(p.x());
- ys.push_back(p.y());
- }
- const auto new_xs = interpolation::lerp(times, xs, t.times);
- const auto new_ys = interpolation::lerp(times, ys, t.times);
- for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]);
- t.curvatures = interpolation::lerp(times, curvatures, t.times);
- t.jerks = interpolation::lerp(times, jerks, t.times);
- t.yaws = interpolation::lerp(times, yaws, t.times);
- t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times);
- t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times);
- t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times);
- t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times);
- t.constraint_results = constraint_results;
- t.cost = cost;
- return t;
- }
-
- [[nodiscard]] Trajectory resampleTimeFromZero(const double fixed_interval) const
- {
- Trajectory t;
- if (times.size() < 2 || times.back() < 0.0 || fixed_interval <= 0.0) return *this;
-
- const auto min_time = 0;
- const auto max_time = times.back();
- const auto new_size = static_cast<size_t>((max_time - min_time) / fixed_interval) + 1;
- t.times.reserve(new_size);
- t.lengths.reserve(new_size);
- for (auto i = 0lu; i < new_size; ++i)
- t.times.push_back(static_cast<double>(i) * fixed_interval);
- t.lengths = interpolation::lerp(times, lengths, t.times);
- std::vector<double> xs;
- std::vector<double> ys;
- xs.reserve(points.size());
- ys.reserve(points.size());
- for (const auto & p : points) {
- xs.push_back(p.x());
- ys.push_back(p.y());
- }
- const auto new_xs = interpolation::lerp(times, xs, t.times);
- const auto new_ys = interpolation::lerp(times, ys, t.times);
- for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]);
- t.curvatures = interpolation::lerp(times, curvatures, t.times);
- t.jerks = interpolation::lerp(times, jerks, t.times);
- t.yaws = interpolation::lerp(times, yaws, t.times);
- t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times);
- t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times);
- t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times);
- t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times);
- t.constraint_results = constraint_results;
- t.cost = cost;
- return t;
- }
-};
-
-struct DynamicObstacle
-{
- std::vector<Polygon2d> footprint_per_time;
- double time_step; // [s] time step between each footprint
-};
-
-struct Constraints
-{
- struct
- {
- double lateral_deviation_weight;
- double length_weight;
- double curvature_weight;
- } soft{};
- struct
- {
- double min_curvature;
- double max_curvature;
- } hard{};
- LinearRing2d ego_footprint;
- double ego_width;
- double ego_length;
- MultiPolygon2d obstacle_polygons;
- MultiPolygon2d drivable_polygons;
- std::vector<DynamicObstacle> dynamic_obstacles;
-};
-
-struct ReusableTrajectory
-{
- Trajectory trajectory; // base trajectory
- Configuration planning_configuration; // planning configuration at the end of the trajectory
-};
-
-} // namespace sampler_common
-
-#endif // SAMPLER_COMMON__STRUCTURES_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp
deleted file mode 100644
index 7110a3828ad01..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp
+++ /dev/null
@@ -1,85 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_
-#define SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-
-#include <vector>
-
-namespace sampler_common::transform
-{
-using sampler_common::FrenetPoint;
-
-class Spline
-{
- std::vector<double> a_{};
- std::vector<double> b_{};
- std::vector<double> c_{};
- std::vector<double> d_{};
- std::vector<double> h_{};
-
-public:
- Spline() = default;
- Spline(const std::vector<double> & base_index, const std::vector<double> & base_value);
- explicit Spline(const std::vector<Point2d> & points);
- bool interpolate(
- const std::vector<double> & base_index, const std::vector<double> & base_value,
- const std::vector<double> & return_index, std::vector<double> & return_value);
- [[nodiscard]] double value(const double query, const std::vector<double> & base_index) const;
- [[nodiscard]] double velocity(const double query, const std::vector<double> & base_index) const;
- [[nodiscard]] double acceleration(
- const double query, const std::vector<double> & base_index) const;
-
-private:
- void generateSpline(
- const std::vector<double> & base_index, const std::vector<double> & base_value);
- [[nodiscard]] static bool isIncrease(const std::vector<double> & x);
- [[nodiscard]] static bool isValidInput(
- const std::vector<double> & base_index, const std::vector<double> & base_value,
- const std::vector<double> & return_index);
- [[nodiscard]] std::vector<double> solveLinearSystem(
- const double omega, const size_t max_iter) const;
- [[nodiscard]] static bool isConvergeL1(
- const std::vector<double> & r1, const std::vector<double> & r2, const double converge_range);
-};
-
-class Spline2D
-{
- std::vector<double> s_{};
- Spline x_spline_{};
- Spline y_spline_{};
-
- std::vector<Point2d> original_points_{};
-
-public:
- Spline2D() = default;
- Spline2D(const std::vector<double> & x, const std::vector<double> & y);
- [[nodiscard]] FrenetPoint frenet_naive(const Point2d & p, const double precision = 0.01) const;
- [[nodiscard]] FrenetPoint frenet(const Point2d & p, const double precision = 0.01) const;
- [[nodiscard]] Point2d cartesian(const double s) const;
- [[nodiscard]] Point2d cartesian(const FrenetPoint & fp) const;
- [[nodiscard]] double curvature(const double s) const;
- [[nodiscard]] double yaw(const double s) const;
- [[nodiscard]] double firstS() const { return s_.empty() ? 0.0 : s_.front(); }
- [[nodiscard]] double lastS() const { return s_.empty() ? 0.0 : s_.back(); }
-
-private:
- static std::vector<double> arcLength(
- const std::vector<double> & x, const std::vector<double> & y);
-};
-} // namespace sampler_common::transform
-
-#endif // SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp
deleted file mode 100644
index c4cac93b605aa..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/geometry_utils.hpp
+++ /dev/null
@@ -1,56 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_
-#define PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
-#include "eigen3/Eigen/Core"
-#include "interpolation/linear_interpolation.hpp"
-#include "interpolation/spline_interpolation.hpp"
-#include "interpolation/spline_interpolation_points_2d.hpp"
-#include "motion_utils/trajectory/trajectory.hpp"
-#include "vehicle_info_util/vehicle_info_util.hpp"
-
-#include "autoware_auto_planning_msgs/msg/path_point.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-
-#include "boost/optional/optional_fwd.hpp"
-
-#include <algorithm>
-#include <limits>
-#include <memory>
-#include <string>
-#include <vector>
-
-namespace path_sampler
-{
-namespace geometry_utils
-{
-template <typename T1, typename T2>
-bool isSamePoint(const T1 & t1, const T2 & t2)
-{
- const auto p1 = tier4_autoware_utils::getPoint(t1);
- const auto p2 = tier4_autoware_utils::getPoint(t2);
-
- constexpr double epsilon = 1e-6;
- if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) {
- return false;
- }
- return true;
-}
-} // namespace geometry_utils
-} // namespace path_sampler
-#endif // PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp
deleted file mode 100644
index 77072c9045133..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/path_sampler_utils/trajectory_utils.hpp
+++ /dev/null
@@ -1,200 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_
-#define PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_
-
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/common_structs.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/path_sampler/type_alias.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-#include "eigen3/Eigen/Core"
-#include "interpolation/linear_interpolation.hpp"
-#include "interpolation/spline_interpolation.hpp"
-#include "interpolation/spline_interpolation_points_2d.hpp"
-#include "motion_utils/trajectory/trajectory.hpp"
-
-#include "autoware_auto_planning_msgs/msg/path_point.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-
-#include <algorithm>
-#include <limits>
-#include <memory>
-#include <string>
-#include <vector>
-
-namespace path_sampler
-{
-namespace trajectory_utils
-{
-template <typename T>
-std::optional<size_t> getPointIndexAfter(
- const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx,
- const double offset)
-{
- if (points.empty()) {
- return std::nullopt;
- }
-
- double sum_length =
- -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos);
-
- // search forward
- if (sum_length < offset) {
- for (size_t i = target_seg_idx + 1; i < points.size(); ++i) {
- sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
- if (offset < sum_length) {
- return i - 1;
- }
- }
-
- return std::nullopt;
- }
-
- // search backward
- for (size_t i = target_seg_idx; 0 < i;
- --i) { // NOTE: use size_t since i is always positive value
- sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
- if (sum_length < offset) {
- return i - 1;
- }
- }
-
- return 0;
-}
-
-template <typename T>
-TrajectoryPoint convertToTrajectoryPoint(const T & point)
-{
- TrajectoryPoint traj_point;
- traj_point.pose = tier4_autoware_utils::getPose(point);
- traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps;
- traj_point.lateral_velocity_mps = point.lateral_velocity_mps;
- traj_point.heading_rate_rps = point.heading_rate_rps;
- return traj_point;
-}
-
-template <typename T>
-std::vector<TrajectoryPoint> convertToTrajectoryPoints(const std::vector<T> & points)
-{
- std::vector<TrajectoryPoint> traj_points;
- for (const auto & point : points) {
- const auto traj_point = convertToTrajectoryPoint(point);
- traj_points.push_back(traj_point);
- }
- return traj_points;
-}
-
-inline std::vector<TrajectoryPoint> convertToTrajectoryPoints(const sampler_common::Path & path)
-{
- std::vector<TrajectoryPoint> traj_points;
- for (auto i = 0UL; i < path.points.size(); ++i) {
- TrajectoryPoint p;
- p.pose.position.x = path.points[i].x();
- p.pose.position.y = path.points[i].y();
- auto quat = tf2::Quaternion();
- quat.setRPY(0.0, 0.0, path.yaws[i]);
- p.pose.orientation.w = quat.w();
- p.pose.orientation.x = quat.x();
- p.pose.orientation.y = quat.y();
- p.pose.orientation.z = quat.z();
- p.longitudinal_velocity_mps = 0.0;
- p.lateral_velocity_mps = 0.0;
- p.heading_rate_rps = 0.0;
- traj_points.push_back(p);
- }
- return traj_points;
-}
-
-void compensateLastPose(
- const PathPoint & last_path_point, std::vector<TrajectoryPoint> & traj_points,
- const double delta_dist_threshold, const double delta_yaw_threshold);
-
-template <class T>
-size_t findEgoIndex(
- const std::vector<T> & points, const geometry_msgs::msg::Pose & ego_pose,
- const EgoNearestParam & ego_nearest_param)
-{
- return motion_utils::findFirstNearestIndexWithSoftConstraints(
- points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold);
-}
-
-template <class T>
-size_t findEgoSegmentIndex(
- const std::vector<T> & points, const geometry_msgs::msg::Pose & ego_pose,
- const EgoNearestParam & ego_nearest_param)
-{
- return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
- points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold);
-}
-
-Trajectory createTrajectory(
- const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points);
-
-std::vector<TrajectoryPoint> resampleTrajectoryPoints(
- const std::vector<TrajectoryPoint> traj_points, const double interval);
-
-std::vector<TrajectoryPoint> resampleTrajectoryPointsWithoutStopPoint(
- const std::vector<TrajectoryPoint> traj_points, const double interval);
-
-template <typename T>
-std::optional<size_t> updateFrontPointForFix(
- std::vector<T> & points, std::vector<T> & points_for_fix, const double delta_arc_length,
- const EgoNearestParam & ego_nearest_param)
-{
- // calculate front point to insert in points as a fixed point
- const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex(
- points_for_fix, tier4_autoware_utils::getPose(points.front()), ego_nearest_param);
- const size_t front_point_idx_for_fix = front_seg_idx_for_fix;
- const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix);
-
- // check if the points_for_fix is longer in front than points
- const double lon_offset_to_prev_front =
- motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position);
- if (0 < lon_offset_to_prev_front) {
- RCLCPP_WARN(
- rclcpp::get_logger("path_sampler.trajectory_utils"),
- "Fixed point will not be inserted due to the error during calculation.");
- return std::nullopt;
- }
-
- const double dist = tier4_autoware_utils::calcDistance2d(points.front(), front_fix_point);
-
- // check if deviation is not too large
- constexpr double max_lat_error = 3.0;
- if (max_lat_error < dist) {
- RCLCPP_WARN(
- rclcpp::get_logger("path_sampler.trajectory_utils"),
- "New Fixed point is too far from points %f [m]", dist);
- }
-
- // update pose
- if (dist < delta_arc_length) {
- // only pose is updated
- points.front() = front_fix_point;
- } else {
- // add new front point
- T new_front_point;
- new_front_point = front_fix_point;
- points.insert(points.begin(), new_front_point);
- }
-
- return front_point_idx_for_fix;
-}
-
-void insertStopPoint(
- std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & input_stop_pose,
- const size_t stop_seg_idx);
-} // namespace trajectory_utils
-} // namespace path_sampler
-#endif // PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp
deleted file mode 100644
index 92598948ca1e4..0000000000000
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier.cpp
+++ /dev/null
@@ -1,120 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier.hpp"
-
-#include <iostream>
-
-namespace bezier_sampler
-{
-Bezier::Bezier(Eigen::Matrix<double, 6, 2> control_points)
-: control_points_(std::move(control_points))
-{
-}
-Bezier::Bezier(const std::vector<Eigen::Vector2d> & control_points)
-{
- if (control_points.size() != 6) {
- std::cerr << "Trying to initialize a quintic bezier curve with " << control_points.size()
- << " (!= 6) control points." << std::endl;
- } else {
- control_points_ << control_points[0], control_points[1], control_points[2], control_points[3],
- control_points[4], control_points[5];
- }
-}
-
-const Eigen::Matrix<double, 6, 2> & Bezier::getControlPoints() const
-{
- return control_points_;
-}
-
-Eigen::Vector2d Bezier::value(const double t) const
-{
- Eigen::Vector2d point = {0.0, 0.0};
- // sum( binomial(i in 5) * (1 - t)^(5-i) * t^i * control_points_[i] )
- point += std::pow((1 - t), 5) * control_points_.row(0);
- point += 5 * std::pow((1 - t), 4) * t * control_points_.row(1);
- point += 10 * std::pow((1 - t), 3) * t * t * control_points_.row(2);
- point += 10 * std::pow((1 - t), 2) * t * t * t * control_points_.row(3);
- point += 5 * (1 - t) * t * t * t * t * control_points_.row(4);
- point += t * t * t * t * t * control_points_.row(5);
- return point;
-}
-
-Eigen::Vector2d Bezier::valueM(const double t) const
-{
- Eigen::Matrix<double, 1, 6> ts;
- ts << 1, t, t * t, t * t * t, t * t * t * t, t * t * t * t * t;
- return ts * quintic_bezier_coefficients * control_points_;
-}
-
-std::vector<Eigen::Vector2d> Bezier::cartesian(const int nb_points) const
-{
- std::vector<Eigen::Vector2d> points;
- points.reserve(nb_points);
- const double step = 1.0 / (nb_points - 1);
- for (double t = 0.0; t <= 1.0; t += step) points.push_back(valueM(t));
- return points;
-}
-
-std::vector<Eigen::Vector2d> Bezier::cartesian(const double resolution) const
-{
- std::vector<Eigen::Vector2d> points;
- points.reserve(static_cast<int>(1 / resolution));
- for (double t = 0.0; t <= 1.0; t += resolution) points.push_back(valueM(t));
- return points;
-}
-
-std::vector<Eigen::Vector3d> Bezier::cartesianWithHeading(const int nb_points) const
-{
- std::vector<Eigen::Vector3d> points;
- points.reserve(nb_points);
- const double step = 1.0 / (nb_points - 1);
- for (double t = 0.0; t <= 1.0; t += step) {
- Eigen::Vector2d point = valueM(t);
- points.emplace_back(point.x(), point.y(), heading(t));
- }
- return points;
-}
-
-Eigen::Vector2d Bezier::velocity(const double t) const
-{
- Eigen::Matrix<double, 1, 5> ts;
- ts << 1, t, t * t, t * t * t, t * t * t * t;
- return ts * quintic_bezier_velocity_coefficients * control_points_;
-}
-
-Eigen::Vector2d Bezier::acceleration(const double t) const
-{
- Eigen::Matrix<double, 1, 4> ts;
- ts << 1, t, t * t, t * t * t;
- return ts * quintic_bezier_acceleration_coefficients * control_points_;
-}
-
-double Bezier::curvature(const double t) const
-{
- const Eigen::Vector2d vel = velocity(t);
- const Eigen::Vector2d accel = acceleration(t);
- double curvature = std::numeric_limits<double>::infinity();
- const double denominator = std::pow(vel.x() * vel.x() + vel.y() * vel.y(), 3.0 / 2.0);
- if (denominator != 0) curvature = (vel.x() * accel.y() - accel.x() * vel.y()) / denominator;
- return curvature;
-}
-
-double Bezier::heading(const double t) const
-{
- const Eigen::Vector2d vel = velocity(t);
- return std::atan2(vel.y(), vel.x());
-}
-
-} // namespace bezier_sampler
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp
deleted file mode 100644
index 617ee4d7f1d79..0000000000000
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/bezier_sampler/bezier_sampling.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "behavior_path_planner/scene_module/sampling_planner/bezier_sampler/bezier_sampling.hpp"
-
-namespace bezier_sampler
-{
-std::vector<Bezier> sample(
- const sampler_common::State & initial, const sampler_common::State & final,
- const SamplingParameters & params)
-{
- std::vector<Bezier> curves;
- curves.reserve(params.nb_t * params.nb_t * params.nb_k);
-
- double distance_initial_to_final = std::sqrt(
- (initial.pose.x() - final.pose.x()) * (initial.pose.x() - final.pose.x()) +
- (initial.pose.y() - final.pose.y()) * (initial.pose.y() - final.pose.y()));
- // Tangent vectors
- const Eigen::Vector2d initial_tangent_unit(std::cos(initial.heading), std::sin(initial.heading));
- const Eigen::Vector2d final_tangent_unit(std::cos(final.heading), std::sin(final.heading));
- // Unit vectors
- const Eigen::Vector2d initial_normal_unit = initial_tangent_unit.unitOrthogonal();
- const Eigen::Vector2d final_normal_unit = final_tangent_unit.unitOrthogonal();
-
- double step_t = (params.mt_max - params.mt_min) / (params.nb_t - 1);
- double step_k = (params.mk_max - params.mk_min) / (params.nb_k - 1);
- for (double m_initial = params.mt_min; m_initial <= params.mt_max; m_initial += step_t) {
- double initial_tangent_length = m_initial * distance_initial_to_final;
- for (double m_final = params.mt_min; m_final <= params.mt_max; m_final += step_t) {
- double final_tangent_length = m_final * distance_initial_to_final;
- for (double k = params.mk_min; k <= params.mk_max; k += step_k) {
- double acceleration_length = k * distance_initial_to_final;
- Eigen::Vector2d initial_acceleration =
- acceleration_length * initial_tangent_unit +
- initial.curvature * initial_tangent_length * initial_tangent_length * initial_normal_unit;
- Eigen::Vector2d final_acceleration =
- acceleration_length * final_tangent_unit +
- final.curvature * final_tangent_length * final_tangent_length * final_normal_unit;
- curves.push_back(generate(
- {initial.pose.x(), initial.pose.y()}, {final.pose.x(), final.pose.y()},
- initial_tangent_unit * initial_tangent_length, initial_acceleration,
- final_tangent_unit * final_tangent_length, final_acceleration));
- }
- }
- }
- return curves;
-}
-Bezier generate(
- const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose,
- const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration,
- const Eigen::Vector2d & final_velocity, const Eigen::Vector2d & final_acceleration)
-{
- Eigen::Matrix<double, 6, 2> control_points;
- // P0 and P5 correspond to the initial and final configurations
- control_points.row(0) = initial_pose;
- control_points.row(5) = final_pose;
- // P1 and P4 depend on P0, P5, and the initial/final velocities
- control_points.row(1) = control_points.row(0) + (1.0 / 5.0) * initial_velocity.transpose();
- control_points.row(4) = control_points.row(5) - (1.0 / 5.0) * final_velocity.transpose();
- // P2 and P3 depend on P1, P4, and the initial/final accelerations
- control_points.row(2) = 2 * control_points.row(1) - control_points.row(0) +
- (1.0 / 20.0) * initial_acceleration.transpose();
- control_points.row(3) = 2 * control_points.row(4) - control_points.row(5) -
- (1.0 / 20.0) * final_acceleration.transpose();
- return Bezier(control_points);
-}
-} // namespace bezier_sampler
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp
deleted file mode 100644
index 24415692e34b5..0000000000000
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/frenet_planner.cpp
+++ /dev/null
@@ -1,228 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "frenet_planner/frenet_planner.hpp"
-
-#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/conversions.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
-
-#include <eigen3/Eigen/Eigen>
-#include <helper_functions/angle_utils.hpp>
-
-#include <algorithm>
-#include <cmath>
-#include <iostream>
-
-namespace frenet_planner
-{
-std::vector<Trajectory> generateTrajectories(
- const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
- const SamplingParameters & sampling_parameters)
-{
- std::vector<Trajectory> trajectories;
- trajectories.reserve(sampling_parameters.parameters.size());
- for (const auto & parameter : sampling_parameters.parameters) {
- auto trajectory = generateCandidate(
- initial_state, parameter.target_state, parameter.target_duration,
- sampling_parameters.resolution);
- trajectory.sampling_parameter = parameter;
- calculateCartesian(reference_spline, trajectory);
- std::stringstream ss;
- ss << parameter;
- trajectory.tag = ss.str();
- trajectories.push_back(trajectory);
- }
- return trajectories;
-}
-
-std::vector<Trajectory> generateLowVelocityTrajectories(
- const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
- const SamplingParameters & sampling_parameters)
-{
- std::vector<Trajectory> trajectories;
- trajectories.reserve(sampling_parameters.parameters.size());
- for (const auto & parameter : sampling_parameters.parameters) {
- auto trajectory = generateLowVelocityCandidate(
- initial_state, parameter.target_state, parameter.target_duration,
- sampling_parameters.resolution);
- calculateCartesian(reference_spline, trajectory);
- std::stringstream ss;
- ss << parameter;
- trajectory.tag = ss.str();
- trajectories.push_back(trajectory);
- }
- return trajectories;
-}
-
-std::vector<Path> generatePaths(
- const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state,
- const SamplingParameters & sampling_parameters)
-{
- std::vector<Path> candidates;
- candidates.reserve(sampling_parameters.parameters.size());
- for (const auto parameter : sampling_parameters.parameters) {
- auto candidate =
- generateCandidate(initial_state, parameter.target_state, sampling_parameters.resolution);
- calculateCartesian(reference_spline, candidate);
- candidates.push_back(candidate);
- }
- return candidates;
-}
-
-Trajectory generateCandidate(
- const FrenetState & initial_state, const FrenetState & target_state, const double duration,
- const double time_resolution)
-{
- Trajectory trajectory;
- trajectory.longitudinal_polynomial = Polynomial(
- initial_state.position.s, initial_state.longitudinal_velocity,
- initial_state.longitudinal_acceleration, target_state.position.s,
- target_state.longitudinal_velocity, target_state.longitudinal_acceleration, duration);
- trajectory.lateral_polynomial = Polynomial(
- initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration,
- target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration,
- duration);
- for (double t = 0.0; t <= duration; t += time_resolution) {
- trajectory.times.push_back(t);
- trajectory.frenet_points.emplace_back(
- trajectory.longitudinal_polynomial->position(t), trajectory.lateral_polynomial->position(t));
- }
- return trajectory;
-}
-
-Trajectory generateLowVelocityCandidate(
- const FrenetState & initial_state, const FrenetState & target_state, const double duration,
- const double time_resolution)
-{
- Trajectory trajectory;
- trajectory.longitudinal_polynomial = Polynomial(
- initial_state.position.s, initial_state.longitudinal_velocity,
- initial_state.longitudinal_acceleration, target_state.position.s,
- target_state.longitudinal_velocity, target_state.longitudinal_acceleration, duration);
- const auto delta_s = target_state.position.s - initial_state.position.s;
- trajectory.lateral_polynomial = Polynomial(
- initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration,
- target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration,
- delta_s);
- for (double t = 0.0; t <= duration; t += time_resolution) {
- trajectory.times.push_back(t);
- const auto s = trajectory.longitudinal_polynomial->position(t);
- const auto ds = s - initial_state.position.s;
- const auto d = trajectory.lateral_polynomial->position(ds);
- trajectory.frenet_points.emplace_back(s, d);
- }
- return trajectory;
-}
-
-Path generateCandidate(
- const FrenetState & initial_state, const FrenetState & target_state, const double s_resolution)
-{
- const auto delta_s = target_state.position.s - initial_state.position.s;
- Path path;
- path.lateral_polynomial = Polynomial(
- initial_state.position.d, initial_state.lateral_velocity, initial_state.lateral_acceleration,
- target_state.position.d, target_state.lateral_velocity, target_state.lateral_acceleration,
- delta_s);
- for (double s = s_resolution; s <= delta_s; s += s_resolution) {
- path.frenet_points.emplace_back(
- initial_state.position.s + s, path.lateral_polynomial->position(s));
- }
- return path;
-}
-
-void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path)
-{
- if (!path.frenet_points.empty()) {
- path.points.reserve(path.frenet_points.size());
- path.yaws.reserve(path.frenet_points.size());
- path.lengths.reserve(path.frenet_points.size());
- path.curvatures.reserve(path.frenet_points.size());
- // Calculate cartesian positions
- for (const auto & fp : path.frenet_points) {
- path.points.push_back(reference.cartesian(fp));
- }
- // TODO(Maxime CLEMENT): more precise calculations are proposed in Appendix I of the paper:
- // Optimal path Generation for Dynamic Street Scenarios in a Frenet Frame (Werling2010)
- // Calculate cartesian yaw and interval values
- path.lengths.push_back(0.0);
- for (auto it = path.points.begin(); it != std::prev(path.points.end()); ++it) {
- const auto dx = std::next(it)->x() - it->x();
- const auto dy = std::next(it)->y() - it->y();
- path.yaws.push_back(std::atan2(dy, dx));
- path.lengths.push_back(path.lengths.back() + std::hypot(dx, dy));
- }
- path.yaws.push_back(path.yaws.back());
- // Calculate curvatures
- for (size_t i = 1; i < path.yaws.size(); ++i) {
- const auto dyaw =
- autoware::common::helper_functions::wrap_angle(path.yaws[i] - path.yaws[i - 1]);
- path.curvatures.push_back(dyaw / (path.lengths[i - 1], path.lengths[i]));
- }
- path.curvatures.push_back(path.curvatures.back());
- }
-}
-void calculateCartesian(
- const sampler_common::transform::Spline2D & reference, Trajectory & trajectory)
-{
- if (!trajectory.frenet_points.empty()) {
- trajectory.points.reserve(trajectory.frenet_points.size());
- // Calculate cartesian positions
- for (const auto & fp : trajectory.frenet_points)
- trajectory.points.push_back(reference.cartesian(fp));
- calculateLengthsAndYaws(trajectory);
- std::vector<double> d_yaws;
- d_yaws.reserve(trajectory.yaws.size());
- for (size_t i = 0; i + 1 < trajectory.yaws.size(); ++i)
- d_yaws.push_back(autoware::common::helper_functions::wrap_angle(
- trajectory.yaws[i + 1] - trajectory.yaws[i]));
- d_yaws.push_back(0.0);
- // Calculate curvatures
- for (size_t i = 1; i < trajectory.yaws.size(); ++i) {
- const auto curvature = trajectory.lengths[i] == trajectory.lengths[i - 1]
- ? 0.0
- : d_yaws[i] / (trajectory.lengths[i] - trajectory.lengths[i - 1]);
- trajectory.curvatures.push_back(curvature);
- }
- const auto last_curvature = trajectory.curvatures.empty() ? 0.0 : trajectory.curvatures.back();
- trajectory.curvatures.push_back(last_curvature);
- // Calculate velocities, accelerations, jerk
- for (size_t i = 0; i < trajectory.times.size(); ++i) {
- const auto time = trajectory.times[i];
- const auto s_vel = trajectory.longitudinal_polynomial->velocity(time);
- const auto s_acc = trajectory.longitudinal_polynomial->acceleration(time);
- const auto d_vel = trajectory.lateral_polynomial->velocity(time);
- const auto d_acc = trajectory.lateral_polynomial->acceleration(time);
- Eigen::Rotation2D rotation(d_yaws[i]);
- Eigen::Vector2d vel_vector{s_vel, d_vel};
- Eigen::Vector2d acc_vector{s_acc, d_acc};
- const auto vel = rotation * vel_vector;
- const auto acc = rotation * acc_vector;
- trajectory.longitudinal_velocities.push_back(vel.x());
- trajectory.lateral_velocities.push_back(vel.y());
- trajectory.longitudinal_accelerations.push_back(acc.x());
- trajectory.lateral_accelerations.push_back(acc.y());
- trajectory.jerks.push_back(
- trajectory.longitudinal_polynomial->jerk(time) + trajectory.lateral_polynomial->jerk(time));
- }
- if (trajectory.longitudinal_accelerations.empty()) {
- trajectory.longitudinal_accelerations.push_back(0.0);
- trajectory.lateral_accelerations.push_back(0.0);
- }
- }
-}
-
-} // namespace frenet_planner
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp
deleted file mode 100644
index 845a421fc3769..0000000000000
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/frenet_planner/polynomials.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "behavior_path_planner/scene_module/sampling_planner/frenet_planner/polynomials.hpp"
-
-#include <eigen3/Eigen/Eigen>
-
-namespace frenet_planner
-{
-// @brief Create a polynomial connecting the given initial and target configuration over the given
-// duration
-Polynomial::Polynomial(
- const double x0, const double x0v, const double x0a, const double xT, const double xTv,
- const double xTa, const double T)
-{
- d_ = x0a / 2;
- e_ = x0v;
- f_ = x0;
-
- const double T2 = T * T;
- const double T3 = T2 * T;
- const double T4 = T3 * T;
- const double T5 = T4 * T;
-
- Eigen::Matrix3d A;
- A << T5, T4, T3, 5 * T4, 4 * T3, 3 * T2, 20 * T3, 12 * T2, 6 * T;
- Eigen::Vector3d B;
- B << xT - x0 - x0v * T - 0.5 * x0a * T2, xTv - x0v - x0a * T, xTa - x0a;
-
- const Eigen::Vector3d X = A.colPivHouseholderQr().solve(B);
- a_ = X(0);
- b_ = X(1);
- c_ = X(2);
-}
-
-double Polynomial::position(const double t) const
-{
- const double t2 = t * t;
- const double t3 = t2 * t;
- const double t4 = t3 * t;
- const double t5 = t4 * t;
- return a_ * t5 + b_ * t4 + c_ * t3 + d_ * t2 + e_ * t + f_;
-}
-
-double Polynomial::velocity(const double t) const
-{
- const double t2 = t * t;
- const double t3 = t2 * t;
- const double t4 = t3 * t;
- return 5 * a_ * t4 + 4 * b_ * t3 + 3 * c_ * t2 + 2 * d_ * t + e_;
-}
-
-double Polynomial::acceleration(const double t) const
-{
- const double t2 = t * t;
- const double t3 = t2 * t;
- return 20 * a_ * t3 + 12 * b_ * t2 + 6 * c_ * t + 2 * d_;
-}
-
-double Polynomial::jerk(const double t) const
-{
- const double t2 = t * t;
- return 60 * a_ * t2 + 24 * b_ * t + 6 * c_;
-}
-} // namespace frenet_planner
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/README.md b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/README.md
deleted file mode 100644
index 4bae0bf5d6d7d..0000000000000
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/README.md
+++ /dev/null
@@ -1,6 +0,0 @@
-# Sampler Common
-
-Common functions for sampling based planners.
-This includes classes for representing paths and trajectories,
-hard and soft constraints,
-conversion between cartesian and frenet frames, ...
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/footprint.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/footprint.cpp
deleted file mode 100644
index 1a9df044c39d7..0000000000000
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/footprint.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp"
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-
-#include <eigen3/Eigen/Core>
-
-#include <boost/geometry/algorithms/append.hpp>
-#include <boost/geometry/algorithms/correct.hpp>
-
-#include <algorithm>
-#include <cmath>
-#include <vector>
-
-namespace sampler_common::constraints
-{
-
-namespace
-{
-const auto to_eigen = [](const Point2d & p) { return Eigen::Vector2d(p.x(), p.y()); };
-} // namespace
-
-MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constraints)
-{
- MultiPoint2d footprint;
-
- footprint.reserve(path.points.size() * 4);
- for (auto i = 0UL; i < path.points.size(); ++i) {
- const Eigen::Vector2d p = to_eigen(path.points[i]);
- const double heading = path.yaws[i];
- Eigen::Matrix2d rotation;
- rotation << std::cos(heading), -std::sin(heading), std::sin(heading), std::cos(heading);
- for (const auto & fp : constraints.ego_footprint) {
- const Eigen::Vector2d fp_point = p + rotation * fp;
- footprint.emplace_back(fp_point.x(), fp_point.y());
- }
- }
- return footprint;
-}
-} // namespace sampler_common::constraints
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.cpp
deleted file mode 100644
index 54e39a9b04fd9..0000000000000
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/hard_constraint.hpp"
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/footprint.hpp"
-
-#include <boost/geometry/algorithms/within.hpp>
-
-#include <vector>
-
-namespace sampler_common::constraints
-{
-bool satisfyMinMax(const std::vector<double> & values, const double min, const double max)
-{
- for (const auto value : values) {
- if (value < min || value > max) return false;
- }
- return true;
-}
-
-bool has_collision(const MultiPoint2d & footprint, const MultiPolygon2d & obstacles)
-{
- for (const auto & o : obstacles)
- for (const auto & p : footprint)
- if (boost::geometry::within(p, o)) return true;
- return false;
-}
-
-MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints)
-{
- const auto footprint = buildFootprintPoints(path, constraints);
- if (!footprint.empty()) {
- if (!boost::geometry::within(footprint, constraints.drivable_polygons)) {
- path.constraint_results.drivable_area = false;
- }
- }
- path.constraint_results.collision = !has_collision(footprint, constraints.obstacle_polygons);
- if (!satisfyMinMax(
- path.curvatures, constraints.hard.min_curvature, constraints.hard.max_curvature)) {
- path.constraint_results.curvature = false;
- }
- return footprint;
-}
-} // namespace sampler_common::constraints
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.cpp
deleted file mode 100644
index 175072bcde7da..0000000000000
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/constraints/soft_constraint.hpp"
-
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/structures.hpp"
-#include "behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp"
-
-#include <numeric>
-
-namespace sampler_common::constraints
-{
-void calculateCurvatureCost(Path & path, const Constraints & constraints)
-{
- double curvature_sum = 0.0;
- for (const auto curvature : path.curvatures) {
- curvature_sum += std::abs(curvature);
- }
- path.cost +=
- constraints.soft.curvature_weight * curvature_sum / static_cast<double>(path.curvatures.size());
-}
-void calculateLengthCost(Path & path, const Constraints & constraints)
-{
- if (!path.lengths.empty()) path.cost -= constraints.soft.length_weight * path.lengths.back();
-}
-
-void calculateLateralDeviationCost(
- Path & path, const Constraints & constraints, const transform::Spline2D & reference)
-{
- const auto fp = reference.frenet(path.points.back());
- const double lateral_deviation = std::abs(fp.d);
- path.cost += constraints.soft.lateral_deviation_weight * lateral_deviation;
-}
-
-void calculateCost(
- Path & path, const Constraints & constraints, const transform::Spline2D & reference)
-{
- if (path.points.empty()) return;
- calculateCurvatureCost(path, constraints);
- calculateLengthCost(path, constraints);
- calculateLateralDeviationCost(path, constraints, reference);
-}
-} // namespace sampler_common::constraints
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/transform/spline_transform.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/transform/spline_transform.cpp
deleted file mode 100644
index c3e06e4a170b9..0000000000000
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampler_common/transform/spline_transform.cpp
+++ /dev/null
@@ -1,315 +0,0 @@
-// Copyright 2023 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <behavior_path_planner/scene_module/sampling_planner/sampler_common/transform/spline_transform.hpp>
-
-#include <algorithm>
-#include <cmath>
-#include <iostream>
-#include <limits>
-
-namespace sampler_common::transform
-{
-Spline::Spline(const std::vector<double> & base_index, const std::vector<double> & base_value)
-{
- generateSpline(base_index, base_value);
-}
-
-Spline::Spline(const std::vector<Point2d> & points)
-{
- std::vector<double> xs;
- std::vector<double> ys;
- xs.reserve(points.size());
- ys.reserve(points.size());
- for (const auto & p : points) {
- xs.push_back(p.x());
- ys.push_back(p.y());
- }
- generateSpline(xs, ys);
-}
-
-void Spline::generateSpline(
- const std::vector<double> & base_index, const std::vector<double> & base_value)
-{
- const size_t N = base_value.size();
-
- a_.clear();
- b_.clear();
- c_.clear();
- d_.clear();
- h_.clear();
-
- a_ = base_value;
-
- for (size_t i = 0; i + 1 < N; ++i) {
- h_.push_back(base_index[i + 1] - base_index[i]);
- }
-
- c_ = solveLinearSystem(1.8, 100);
-
- for (size_t i = 0; i < N - 1; i++) {
- d_.push_back((c_[i + 1] - c_[i]) / (3.0 * h_[i]));
- b_.push_back((a_[i + 1] - a_[i]) / h_[i] - h_[i] * (2.0 * c_[i] + c_[i + 1]) / 3.0);
- }
-
- d_.push_back(0.0);
- b_.push_back(0.0);
-}
-
-double Spline::value(const double query, const std::vector<double> & base_index) const
-{
- const auto ub = std::upper_bound(base_index.begin(), base_index.end(), query);
- const size_t i = std::distance(base_index.begin(), ub) - 1;
- const double ds = query - base_index[i];
- return a_[i] + (b_[i] + (c_[i] + d_[i] * ds) * ds) * ds;
-}
-
-double Spline::velocity(const double query, const std::vector<double> & base_index) const
-{
- const auto lb = std::lower_bound(base_index.begin(), base_index.end(), query);
- const size_t i = std::distance(base_index.begin(), lb) - 1;
- const auto delta = query - base_index[i];
- return b_[i] + 2.0 * c_[i] * delta + 3.0 * d_[i] * delta * delta;
-}
-
-double Spline::acceleration(const double query, const std::vector<double> & base_index) const
-{
- const auto lb = std::lower_bound(base_index.begin(), base_index.end(), query);
- const size_t i = std::distance(base_index.begin(), lb) - 1;
- return 2.0 * c_[i] + 6.0 * d_[i] * (query - base_index[i]);
-}
-
-bool Spline::isIncrease(const std::vector<double> & x)
-{
- for (int i = 0; i < static_cast<int>(x.size()) - 1; ++i) {
- if (x[i] > x[i + 1]) return false;
- }
- return true;
-}
-
-bool Spline::isValidInput(
- const std::vector<double> & base_index, const std::vector<double> & base_value,
- const std::vector<double> & return_index)
-{
- if (base_index.empty() || base_value.empty() || return_index.empty()) {
- std::cerr << "bad index : some vector is empty. base_index: " << base_index.size()
- << ", base_value: " << base_value.size() << ", return_index: " << return_index.size()
- << std::endl;
- return false;
- }
- if (!isIncrease(base_index)) {
- std::cerr << "bad index : base_index is not monotonically increasing. base_index = ["
- << base_index.front() << ", " << base_index.back() << "]" << std::endl;
- return false;
- }
- if (!isIncrease(return_index)) {
- std::cerr << "bad index : base_index is not monotonically increasing. return_index = ["
- << return_index.front() << ", " << return_index.back() << "]" << std::endl;
- return false;
- }
- if (return_index.front() < base_index.front()) {
- std::cerr << "bad index : return_index.front() < base_index.front()" << std::endl;
- return false;
- }
- if (base_index.back() < return_index.back()) {
- std::cerr << "bad index : base_index.back() < return_index.back()" << std::endl;
- return false;
- }
- if (base_index.size() != base_value.size()) {
- std::cerr << "bad index : base_index.size() != base_value.size()" << std::endl;
- return false;
- }
-
- return true;
-}
-
-std::vector<double> Spline::solveLinearSystem(const double omega, const size_t max_iter) const
-{
- // solve A * ans = rhs by SOR method
- constexpr double converge_range = 0.00001;
- std::vector<double> ans(a_.size(), 1.0);
- std::vector<double> ans_next(a_.size(), 0.0);
- size_t num_iter = 0;
-
- while (!isConvergeL1(ans, ans_next, converge_range) && num_iter <= max_iter) {
- ans = ans_next;
- for (size_t i = 1; i < a_.size() - 1; ++i) {
- const double rhs = 3.0 / h_[i] * (a_[i + 1] - a_[i]) - 3.0 / h_[i - 1] * (a_[i] - a_[i - 1]);
- ans_next[i] += omega / (2.0 * (h_[i - 1] + h_[i])) *
- (rhs - (h_[i - 1] * ans_next[i - 1] + 2.0 * (h_[i - 1] + h_[i]) * ans[i] +
- h_[i] * ans[i + 1]));
- }
- ++num_iter;
- }
-
- if (num_iter > max_iter) std::cerr << "[Spline::solveLinearSystem] unconverged!\n";
- return ans_next;
-}
-
-bool Spline::isConvergeL1(
- const std::vector<double> & r1, const std::vector<double> & r2, const double converge_range)
-{
- double d = 0.0;
- for (size_t i = 0; i < r1.size(); ++i) {
- d += std::fabs(r1.at(i) - r2.at(i));
- }
- return d < converge_range;
-}
-
-/*
- * 2D Spline
- */
-
-Spline2D::Spline2D(const std::vector<double> & x, const std::vector<double> & y)
-: s_(arcLength(x, y)), x_spline_{s_, x}, y_spline_{s_, y}
-{
- original_points_.reserve(x.size());
- for (size_t i = 0; i < x.size(); ++i) {
- original_points_.emplace_back(x[i], y[i]);
- }
-}
-
-// @brief Calculate the distances of the points along the path
-// @details approximation using linear segments
-std::vector<double> Spline2D::arcLength(
- const std::vector<double> & x, const std::vector<double> & y)
-{
- std::vector<double> s = {0.0};
- s.reserve(x.size() - 1);
- for (size_t i = 0; i < x.size() - 1; ++i) {
- const double ds = std::hypot((x[i + 1] - x[i]), (y[i + 1] - y[i]));
- s.push_back(s.back() + ds);
- }
- return s;
-}
-
-// @brief Convert the given point to the Frenet frame of this spline
-// @details sample points along the splines and return the closest one
-FrenetPoint Spline2D::frenet_naive(const Point2d & p, double precision) const
-{
- double closest_d = std::numeric_limits<double>::max();
- double arc_length = std::numeric_limits<double>::min();
- for (double s = s_.front(); s < s_.back(); s += precision) {
- const double x_s = x_spline_.value(s, s_);
- const double y_s = y_spline_.value(s, s_);
- const double d = std::hypot(p.x() - x_s, p.y() - y_s);
- if (d <= closest_d) // also accept equality to return the largest possible arc length
- {
- closest_d = d;
- arc_length = s;
- }
- }
- // check sign of d
- const double x0 = x_spline_.value(arc_length, s_);
- const double y0 = y_spline_.value(arc_length, s_);
- const double x1 = x_spline_.value(arc_length + precision, s_);
- const double y1 = y_spline_.value(arc_length + precision, s_);
- if ((x1 - x0) * (p.y() - y0) - (y1 - y0) * (p.x() - x0) < 0) {
- closest_d *= -1.0;
- }
- return {arc_length, closest_d};
-}
-
-// @brief Convert the given point to the Frenet frame of this spline
-// @details find closest point in the lookup table
-// @warning can fail if the original points are not smooth or if some points are very far apart
-FrenetPoint Spline2D::frenet(const Point2d & p, const double precision) const
-{
- const auto distance = [&](const Point2d & p2) {
- return std::hypot(p.x() - p2.x(), p.y() - p2.y());
- };
- size_t min_i{};
- auto min_dist = std::numeric_limits<double>::max();
- for (size_t i = 0; i < original_points_.size(); ++i) {
- const auto dist = distance(original_points_[i]);
- if (dist <= min_dist) {
- min_dist = dist;
- min_i = i;
- }
- }
- auto lb_i = min_i == 0 ? min_i : min_i - 1;
- auto ub_i = min_i + 1 == original_points_.size() ? min_i : min_i + 1;
- auto best_s = s_[min_i];
- // real closest s is either in interval [lb_i:min_i] or interval [min_i:ub]
- // continue exploring the interval whose middle point is closest to the input point
- std::vector<double> s_interval = {s_[lb_i], {}, s_[min_i], {}, s_[ub_i]};
- std::vector<double> d_interval = {
- distance(original_points_[lb_i]),
- {},
- distance(original_points_[min_i]),
- {},
- distance(original_points_[ub_i])};
- while (s_interval[4] - s_interval[0] > precision) {
- s_interval[1] = s_interval[0] + (s_interval[2] - s_interval[0]) / 2;
- s_interval[3] = s_interval[2] + (s_interval[4] - s_interval[2]) / 2;
- d_interval[1] =
- distance({x_spline_.value(s_interval[1], s_), y_spline_.value(s_interval[1], s_)});
- d_interval[3] =
- distance({x_spline_.value(s_interval[3], s_), y_spline_.value(s_interval[3], s_)});
-
- for (auto i = 0; i < 5; ++i) {
- if (d_interval[i] <= min_dist) {
- min_dist = d_interval[i];
- min_i = i;
- }
- }
-
- best_s = s_interval[min_i];
- lb_i = min_i == 0 ? min_i : min_i - 1;
- ub_i = min_i == 4 ? min_i : min_i + 1;
- s_interval = {s_interval[lb_i], {}, s_interval[min_i], {}, s_interval[ub_i]};
- d_interval = {d_interval[lb_i], {}, d_interval[min_i], {}, d_interval[ub_i]};
- }
- // check sign of d
- const double x0 = x_spline_.value(best_s, s_);
- const double y0 = y_spline_.value(best_s, s_);
- const double x1 = x_spline_.value(best_s + precision, s_);
- const double y1 = y_spline_.value(best_s + precision, s_);
- if ((x1 - x0) * (p.y() - y0) - (y1 - y0) * (p.x() - x0) < 0) {
- min_dist *= -1.0;
- }
- return {best_s, min_dist};
-}
-
-Point2d Spline2D::cartesian(const double s) const
-{
- return {x_spline_.value(s, s_), y_spline_.value(s, s_)};
-}
-
-Point2d Spline2D::cartesian(const FrenetPoint & fp) const
-{
- const auto heading = yaw(fp.s);
- const auto x = x_spline_.value(fp.s, s_);
- const auto y = y_spline_.value(fp.s, s_);
- return {x + fp.d * std::cos(heading + M_PI_2), y + fp.d * std::sin(heading + M_PI_2)};
-}
-
-double Spline2D::curvature(const double s) const
-{
- // TODO(Maxime CLEMENT) search for s in s_ here and pass index
- const double x_vel = x_spline_.velocity(s, s_);
- const double y_vel = y_spline_.velocity(s, s_);
- const double x_acc = x_spline_.acceleration(s, s_);
- const double y_acc = y_spline_.acceleration(s, s_);
- return (y_acc * x_vel - x_acc * y_vel) / std::pow(x_vel * x_vel + y_vel * y_vel, 3.0 / 2.0);
-}
-
-double Spline2D::yaw(const double s) const
-{
- const double x_vel = x_spline_.velocity(s, s_);
- const double y_vel = y_spline_.velocity(s, s_);
- return std::atan2(y_vel, x_vel);
-}
-
-} // namespace sampler_common::transform
From ee2ccc32a9fb9d4208d9c01b30ff698c6e6d4748 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 27 Sep 2023 18:09:23 +0900
Subject: [PATCH 012/115] add dependencies for bezier and frenet planners
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../scene_module/sampling_planner/sampling_planner_module.hpp | 1 +
planning/behavior_path_planner/package.xml | 3 +++
2 files changed, 4 insertions(+)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 848ad15ada3b7..1209dd828464b 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -20,6 +20,7 @@
#include "behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp"
#include "behavior_path_planner/utils/sampling_planner/util.hpp"
#include "behavior_path_planner/utils/utils.hpp"
+#include "bezier_sampler/bezier_sampling.hpp"
#include <rclcpp/rclcpp.hpp>
diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml
index 2bc73325f96d5..b0e3708deb14a 100644
--- a/planning/behavior_path_planner/package.xml
+++ b/planning/behavior_path_planner/package.xml
@@ -66,6 +66,9 @@
<depend>tier4_planning_msgs</depend>
<depend>vehicle_info_util</depend>
<depend>visualization_msgs</depend>
+ <depend>bezier_sampler</depend>
+ <depend>frenet_planner</depend>
+
<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
From 73db8e607ab37102c64be09426b11243273ef910 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 28 Sep 2023 18:26:46 +0900
Subject: [PATCH 013/115] [WIP] Made a simple implementation of behavior
planning
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 14 +++++
planning/behavior_path_planner/package.xml | 1 +
.../sampling_planner_module.cpp | 58 ++++++++++++++++++-
3 files changed, 71 insertions(+), 2 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 1209dd828464b..584f86f42efb1 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -17,18 +17,28 @@
#include "behavior_path_planner/marker_utils/utils.hpp"
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/utils/path_utils.hpp"
#include "behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp"
#include "behavior_path_planner/utils/sampling_planner/util.hpp"
#include "behavior_path_planner/utils/utils.hpp"
#include "bezier_sampler/bezier_sampling.hpp"
+#include "path_sampler/common_structs.hpp"
+#include "path_sampler/node.hpp"
+#include "path_sampler/parameters.hpp"
+#include "path_sampler/type_alias.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sampler_common/transform/spline_transform.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
#include <rclcpp/rclcpp.hpp>
+#include <sampler_common/structures.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <tier4_planning_msgs/msg/lateral_offset.hpp>
#include <algorithm>
#include <memory>
+#include <optional>
#include <string>
#include <unordered_map>
#include <utility>
@@ -48,6 +58,7 @@ class SamplingPlannerModule : public SceneModuleInterface
bool isExecutionReady() const override;
BehaviorModuleOutput plan() override;
CandidateOutput planCandidate() const override;
+ void updateData() override;
void updateModuleParams(const std::any & parameters) override
{
@@ -67,7 +78,10 @@ class SamplingPlannerModule : public SceneModuleInterface
bool canTransitIdleToRunningState() override { return false; }
// member
+ Parameters params_;
+ path_sampler::PlannerData planner_data;
std::shared_ptr<SamplingPlannerParameters> parameters_;
+ vehicle_info_util::VehicleInfo vehicle_info_{};
};
} // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml
index b0e3708deb14a..04c607df0af6c 100644
--- a/planning/behavior_path_planner/package.xml
+++ b/planning/behavior_path_planner/package.xml
@@ -68,6 +68,7 @@
<depend>visualization_msgs</depend>
<depend>bezier_sampler</depend>
<depend>frenet_planner</depend>
+ <depend>path_sampler</depend>
<test_depend>ament_cmake_ros</test_depend>
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index f56e9f0580e33..4ea8c21ce05c2 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -23,13 +23,41 @@ using motion_utils::findNearestSegmentIndex;
using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
using tier4_autoware_utils::getPoint;
+using utils::toPath;
SamplingPlannerModule::SamplingPlannerModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<SamplingPlannerParameters> & parameters,
- const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map)
+ const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}
{
+ // parameters
+ params_.constraints.hard.max_curvature = 0.1;
+ params_.constraints.hard.min_curvature = -0.1;
+ params_.constraints.soft.lateral_deviation_weight = 1.0;
+ params_.constraints.soft.length_weight = 1.0;
+ params_.constraints.soft.curvature_weight = 1.0;
+ params_.sampling.enable_frenet = true;
+ params_.sampling.enable_bezier = false;
+ params_.sampling.resolution = 0.5;
+ params_.sampling.previous_path_reuse_points_nb = 2;
+ params_.sampling.target_lengths = {10.0, 20.0};
+ params_.sampling.target_lateral_positions = {-2.0, -1.0, 0.0, 1.0, 2.0};
+ params_.sampling.nb_target_lateral_positions = 2;
+ params_.sampling.frenet.target_lateral_velocities = {-0.1, 0.0, 0.1};
+ params_.sampling.frenet.target_lateral_accelerations = {0.0};
+ // params_.sampling.bezier.nb_k = declare_parameter<int>("sampling.bezier.nb_k");
+ // params_.sampling.bezier.mk_min = declare_parameter<double>("sampling.bezier.mk_min");
+ // params_.sampling.bezier.mk_max = declare_parameter<double>("sampling.bezier.mk_max");
+ // params_.sampling.bezier.nb_t = declare_parameter<int>("sampling.bezier.nb_t");
+ // params_.sampling.bezier.mt_min = declare_parameter<double>("sampling.bezier.mt_min");
+ // params_.sampling.bezier.mt_max = declare_parameter<double>("sampling.bezier.mt_max");
+ params_.preprocessing.force_zero_deviation = true;
+ params_.preprocessing.force_zero_heading = true;
+ params_.preprocessing.smooth_reference = false;
+ params_.constraints.ego_footprint = vehicle_info_.createFootprint();
+ params_.constraints.ego_width = vehicle_info_.vehicle_width_m;
+ params_.constraints.ego_length = vehicle_info_.vehicle_length_m;
}
bool SamplingPlannerModule::isExecutionRequested() const
@@ -44,7 +72,29 @@ bool SamplingPlannerModule::isExecutionReady() const
BehaviorModuleOutput SamplingPlannerModule::plan()
{
- return {};
+ const auto reference_path = planner_data_->reference_path;
+ if (reference_path->points.empty()) {
+ RCLCPP_WARN(getLogger(), "reference path is empty.");
+ return {};
+ }
+ const auto ego_closest_path_index = planner_data_->findEgoIndex(reference_path->points);
+ resetPathCandidate();
+ resetPathReference();
+ // const auto path = toPath(*reference_path);
+ // size_t end_idx = utils::getIdxByArclength(
+ // *reference_path, ego_closest_path_index, params_.sampling.target_lengths[0]);
+
+ auto clipped_path = *reference_path;
+
+ utils::clipPathLength(
+ clipped_path, ego_closest_path_index, params_.sampling.target_lengths[0], 0);
+
+ BehaviorModuleOutput output;
+
+ output.reference_path = getPreviousModuleOutput().reference_path;
+ output.path = std::make_shared<PathWithLaneId>(clipped_path);
+
+ return output;
}
CandidateOutput SamplingPlannerModule::planCandidate() const
@@ -52,4 +102,8 @@ CandidateOutput SamplingPlannerModule::planCandidate() const
return {};
}
+void SamplingPlannerModule::updateData()
+{
+}
+
} // namespace behavior_path_planner
From c45536ee50e4a52008da88a77fa6255d9df138e3 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 28 Sep 2023 18:35:48 +0900
Subject: [PATCH 014/115] [WIP] added comments on making drivable area
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner/sampling_planner_module.cpp | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 4ea8c21ce05c2..a4df5f8f6bf4f 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -92,6 +92,14 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
BehaviorModuleOutput output;
output.reference_path = getPreviousModuleOutput().reference_path;
+
+ // const auto & dp = planner_data_->drivable_area_expansion_parameters;
+ // const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_);
+ // const auto shorten_lanes = utils::cutOverlappedLanes(clipped_path, drivable_lanes);
+ // const auto expanded_lanes =
+ // utils::expandLanelets(shorten_lanes, left_offset, right_offset,
+ // dp.drivable_area_types_to_skip);
+
output.path = std::make_shared<PathWithLaneId>(clipped_path);
return output;
From 7ab791690790a755dcff33338c4c34b4b641e2a4 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Fri, 29 Sep 2023 09:14:08 +0900
Subject: [PATCH 015/115] Just adding functions to test
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.cpp | 37 +++++++++++++++----
1 file changed, 29 insertions(+), 8 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index a4df5f8f6bf4f..16cc7a2bda9be 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -62,6 +62,10 @@ SamplingPlannerModule::SamplingPlannerModule(
bool SamplingPlannerModule::isExecutionRequested() const
{
+ if (planner_data_->reference_path->points.empty()) {
+ RCLCPP_WARN(getLogger(), "reference path is empty.");
+ return false;
+ }
return true;
}
@@ -78,8 +82,9 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
return {};
}
const auto ego_closest_path_index = planner_data_->findEgoIndex(reference_path->points);
- resetPathCandidate();
- resetPathReference();
+ RCLCPP_INFO(getLogger(), "ego_closest_path_index %ld", ego_closest_path_index);
+ // resetPathCandidate();
+ // resetPathReference();
// const auto path = toPath(*reference_path);
// size_t end_idx = utils::getIdxByArclength(
// *reference_path, ego_closest_path_index, params_.sampling.target_lengths[0]);
@@ -92,14 +97,30 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
BehaviorModuleOutput output;
output.reference_path = getPreviousModuleOutput().reference_path;
+ // output.reference_path = reference_path;
+
+ const auto & route_handler = planner_data_->route_handler;
+ const auto reference_pose = planner_data_->self_odometry->pose.pose;
+ const auto & p = planner_data_->parameters;
+
+ lanelet::ConstLanelet current_lane;
+ if (!route_handler->getClosestLaneletWithinRoute(reference_pose, ¤t_lane)) {
+ RCLCPP_ERROR_THROTTLE(
+ getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
+ }
+
+ // For current_lanes with desired length
+ auto current_lanelets_ = route_handler->getLaneletSequence(
+ current_lane, reference_pose, p.backward_path_length, p.forward_path_length);
- // const auto & dp = planner_data_->drivable_area_expansion_parameters;
- // const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_);
- // const auto shorten_lanes = utils::cutOverlappedLanes(clipped_path, drivable_lanes);
- // const auto expanded_lanes =
- // utils::expandLanelets(shorten_lanes, left_offset, right_offset,
- // dp.drivable_area_types_to_skip);
+ const auto & dp = planner_data_->drivable_area_expansion_parameters;
+ const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_);
+ const auto shorten_lanes = utils::cutOverlappedLanes(clipped_path, drivable_lanes);
+ const auto expanded_lanes =
+ utils::expandLanelets(shorten_lanes, 2.0, 2.0, dp.drivable_area_types_to_skip);
+ output.drivable_area_info.drivable_lanes = expanded_lanes;
+ output.drivable_area_info.is_already_expanded = true;
output.path = std::make_shared<PathWithLaneId>(clipped_path);
return output;
From b1eb340893bb43db4aecbed0a42ef57046ff6881 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 29 Sep 2023 16:31:41 +0900
Subject: [PATCH 016/115] [WIP] Implement Frenet Planner
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 39 +++-
.../sampling_planner_parameters.hpp | 42 +++-
.../sampling_planner_module.cpp | 183 ++++++++++++++----
3 files changed, 207 insertions(+), 57 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 584f86f42efb1..02114b751f3ac 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -22,30 +22,44 @@
#include "behavior_path_planner/utils/sampling_planner/util.hpp"
#include "behavior_path_planner/utils/utils.hpp"
#include "bezier_sampler/bezier_sampling.hpp"
-#include "path_sampler/common_structs.hpp"
-#include "path_sampler/node.hpp"
-#include "path_sampler/parameters.hpp"
-#include "path_sampler/type_alias.hpp"
+#include "frenet_planner/frenet_planner.hpp"
#include "rclcpp/rclcpp.hpp"
+#include "sampler_common/structures.hpp"
#include "sampler_common/transform/spline_transform.hpp"
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+#include "tier4_autoware_utils/ros/update_param.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"
#include <rclcpp/rclcpp.hpp>
#include <sampler_common/structures.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <tier4_planning_msgs/msg/lateral_offset.hpp>
#include <algorithm>
+#include <any>
#include <memory>
#include <optional>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
-
namespace behavior_path_planner
{
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+struct SamplingPlannerData
+{
+ // input
+ std::vector<TrajectoryPoint> traj_points; // converted from the input path
+ std::vector<geometry_msgs::msg::Point> left_bound;
+ std::vector<geometry_msgs::msg::Point> right_bound;
+
+ // ego
+ geometry_msgs::msg::Pose ego_pose;
+ double ego_vel{};
+};
class SamplingPlannerModule : public SceneModuleInterface
{
public:
@@ -71,16 +85,29 @@ class SamplingPlannerModule : public SceneModuleInterface
}
private:
+ SamplingPlannerData createPlannerData(const PlanResult & path);
+
+ PlanResult generatePath();
+
bool canTransitSuccessState() override { return false; }
bool canTransitFailureState() override { return false; }
bool canTransitIdleToRunningState() override { return false; }
+ template <typename T>
+ std::vector<TrajectoryPoint> convertToTrajectoryPoints(const std::vector<T> & points);
+ template <typename T>
+ TrajectoryPoint convertToTrajectoryPoint(const T & point);
+
+ frenet_planner::SamplingParameters prepareSamplingParameters(
+ const sampler_common::State & initial_state,
+ const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
+
// member
Parameters params_;
- path_sampler::PlannerData planner_data;
std::shared_ptr<SamplingPlannerParameters> parameters_;
+
vehicle_info_util::VehicleInfo vehicle_info_{};
};
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
index c6e71c06b028f..41c0d9cc8d163 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2023 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -12,20 +12,46 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__SAMPLING_PLANNER_PARAMETERS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__SAMPLING_PLANNER_PARAMETERS_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__PARAMETERS_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__PARAMETERS_HPP_
-#include <memory>
-#include <string>
-#include <vector>
+#include "bezier_sampler/bezier_sampling.hpp"
+#include "sampler_common/structures.hpp"
+#include <vector>
namespace behavior_path_planner
{
-
struct SamplingPlannerParameters
{
double dummy_parameter{0.0};
};
+struct Parameters
+{
+ sampler_common::Constraints constraints;
+ struct
+ {
+ bool enable_frenet{};
+ bool enable_bezier{};
+ double resolution{};
+ int previous_path_reuse_points_nb{};
+ std::vector<double> target_lengths{};
+ std::vector<double> target_lateral_positions{};
+ int nb_target_lateral_positions{};
+ struct
+ {
+ std::vector<double> target_lateral_velocities{};
+ std::vector<double> target_lateral_accelerations{};
+ } frenet;
+ bezier_sampler::SamplingParameters bezier{};
+ } sampling;
+
+ struct
+ {
+ bool force_zero_deviation{};
+ bool force_zero_heading{};
+ bool smooth_reference{};
+ } preprocessing{};
+};
} // namespace behavior_path_planner
-#endif // BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__SAMPLING_PLANNER_PARAMETERS_HPP_
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__PARAMETERS_HPP_
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 16cc7a2bda9be..4ae5720245171 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -62,10 +62,15 @@ SamplingPlannerModule::SamplingPlannerModule(
bool SamplingPlannerModule::isExecutionRequested() const
{
- if (planner_data_->reference_path->points.empty()) {
+ if (getPreviousModuleOutput().reference_path->points.empty()) {
RCLCPP_WARN(getLogger(), "reference path is empty.");
return false;
}
+
+ if (!motion_utils::isDrivingForward(getPreviousModuleOutput().reference_path->points)) {
+ RCLCPP_WARN(getLogger(), "Backward path is NOT supported. Just converting path to trajectory");
+ return false;
+ }
return true;
}
@@ -74,56 +79,91 @@ bool SamplingPlannerModule::isExecutionReady() const
return true;
}
+template <typename T>
+TrajectoryPoint SamplingPlannerModule::convertToTrajectoryPoint(const T & point)
+{
+ TrajectoryPoint traj_point;
+ traj_point.pose = tier4_autoware_utils::getPose(point);
+ traj_point.longitudinal_velocity_mps = 0;
+ traj_point.lateral_velocity_mps = 0;
+ traj_point.heading_rate_rps = 0;
+ return traj_point;
+}
+
+template <typename T>
+std::vector<TrajectoryPoint> SamplingPlannerModule::convertToTrajectoryPoints(
+ const std::vector<T> & points)
+{
+ std::vector<TrajectoryPoint> traj_points;
+ for (const auto & point : points) {
+ const auto traj_point = convertToTrajectoryPoint(point);
+ traj_points.push_back(traj_point);
+ }
+ return traj_points;
+}
+
+SamplingPlannerData SamplingPlannerModule::createPlannerData(const PlanResult & path)
+{
+ // create planner data
+
+ SamplingPlannerData data;
+ // planner_data.header = path.header;
+ auto points = path->points;
+ data.traj_points = convertToTrajectoryPoints(points);
+ data.left_bound = path->left_bound;
+ data.right_bound = path->right_bound;
+ data.ego_pose = planner_data_->self_odometry->pose.pose;
+ data.ego_vel = 0;
+ // data.ego_vel = ego_state_ptr_->twist.twist.linear.x;
+ return data;
+}
+
BehaviorModuleOutput SamplingPlannerModule::plan()
{
- const auto reference_path = planner_data_->reference_path;
- if (reference_path->points.empty()) {
- RCLCPP_WARN(getLogger(), "reference path is empty.");
+ // const auto refPath = getPreviousModuleOutput().reference_path;
+ const auto path_ptr = getPreviousModuleOutput().reference_path;
+ if (path_ptr->points.empty()) {
return {};
}
- const auto ego_closest_path_index = planner_data_->findEgoIndex(reference_path->points);
+ const auto ego_closest_path_index = planner_data_->findEgoIndex(path_ptr->points);
RCLCPP_INFO(getLogger(), "ego_closest_path_index %ld", ego_closest_path_index);
// resetPathCandidate();
// resetPathReference();
- // const auto path = toPath(*reference_path);
- // size_t end_idx = utils::getIdxByArclength(
- // *reference_path, ego_closest_path_index, params_.sampling.target_lengths[0]);
-
- auto clipped_path = *reference_path;
+ // [[maybe_unused]] const auto path = toPath(*path_ptr);
+ auto reference_spline = [&]() -> sampler_common::transform::Spline2D {
+ std::vector<double> x;
+ std::vector<double> y;
+ x.reserve(path_ptr->points.size());
+ y.reserve(path_ptr->points.size());
+ for (const auto & point : path_ptr->points) {
+ x.push_back(point.point.pose.position.x);
+ y.push_back(point.point.pose.position.y);
+ }
+ return {x, y};
+ }();
+
+ const auto pose = planner_data_->self_odometry->pose.pose;
+ sampler_common::State initial_state;
+ Point2d initial_state_pose{pose.position.x, pose.position.y};
+ const auto rpy =
+ tier4_autoware_utils::getRPY(planner_data_->self_odometry->pose.pose.orientation);
+ initial_state.pose = initial_state_pose;
+ initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y});
+ initial_state.heading = rpy.z;
+
+ frenet_planner::SamplingParameters sampling_parameters =
+ prepareSamplingParameters(initial_state, reference_spline, params_);
+
+ frenet_planner::FrenetState frenet_initial_state;
+ frenet_initial_state.position = initial_state.frenet;
+
+ auto frenet_paths =
+ frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
+ // After path generation we do pruning we then select the optimal path based on obstacles and
+ // drivable area and copy to the expected output and finally copy the velocities from the ref path
+ RCLCPP_INFO(getLogger(), "Generated paths %ld", frenet_paths.size());
- utils::clipPathLength(
- clipped_path, ego_closest_path_index, params_.sampling.target_lengths[0], 0);
-
- BehaviorModuleOutput output;
-
- output.reference_path = getPreviousModuleOutput().reference_path;
- // output.reference_path = reference_path;
-
- const auto & route_handler = planner_data_->route_handler;
- const auto reference_pose = planner_data_->self_odometry->pose.pose;
- const auto & p = planner_data_->parameters;
-
- lanelet::ConstLanelet current_lane;
- if (!route_handler->getClosestLaneletWithinRoute(reference_pose, ¤t_lane)) {
- RCLCPP_ERROR_THROTTLE(
- getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
- }
-
- // For current_lanes with desired length
- auto current_lanelets_ = route_handler->getLaneletSequence(
- current_lane, reference_pose, p.backward_path_length, p.forward_path_length);
-
- const auto & dp = planner_data_->drivable_area_expansion_parameters;
- const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_);
- const auto shorten_lanes = utils::cutOverlappedLanes(clipped_path, drivable_lanes);
- const auto expanded_lanes =
- utils::expandLanelets(shorten_lanes, 2.0, 2.0, dp.drivable_area_types_to_skip);
-
- output.drivable_area_info.drivable_lanes = expanded_lanes;
- output.drivable_area_info.is_already_expanded = true;
- output.path = std::make_shared<PathWithLaneId>(clipped_path);
-
- return output;
+ return {};
}
CandidateOutput SamplingPlannerModule::planCandidate() const
@@ -135,4 +175,61 @@ void SamplingPlannerModule::updateData()
{
}
+frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters(
+ const sampler_common::State & initial_state,
+ const sampler_common::transform::Spline2D & path_spline, const Parameters & params)
+{
+ // calculate target lateral positions
+ std::vector<double> target_lateral_positions;
+ if (params.sampling.nb_target_lateral_positions > 1) {
+ target_lateral_positions = {0.0, initial_state.frenet.d};
+ double min_d = 0.0;
+ double max_d = 0.0;
+ double min_d_s = std::numeric_limits<double>::max();
+ double max_d_s = std::numeric_limits<double>::max();
+ for (const auto & drivable_poly : params.constraints.drivable_polygons) {
+ for (const auto & p : drivable_poly.outer()) {
+ const auto frenet_coordinates = path_spline.frenet(p);
+ const auto d_s = std::abs(frenet_coordinates.s - initial_state.frenet.s);
+ if (d_s < min_d_s && frenet_coordinates.d < 0.0) {
+ min_d_s = d_s;
+ min_d = frenet_coordinates.d;
+ }
+ if (d_s < max_d_s && frenet_coordinates.d > 0.0) {
+ max_d_s = d_s;
+ max_d = frenet_coordinates.d;
+ }
+ }
+ }
+ min_d += params.constraints.ego_width / 2.0;
+ max_d -= params.constraints.ego_width / 2.0;
+ if (min_d < max_d) {
+ for (auto r = 0.0; r <= 1.0; r += 1.0 / (params.sampling.nb_target_lateral_positions - 1))
+ target_lateral_positions.push_back(interpolation::lerp(min_d, max_d, r));
+ }
+ } else {
+ target_lateral_positions = params.sampling.target_lateral_positions;
+ }
+ frenet_planner::SamplingParameters sampling_parameters;
+ sampling_parameters.resolution = params.sampling.resolution;
+ const auto max_s = path_spline.lastS();
+ frenet_planner::SamplingParameter p;
+ for (const auto target_length : params.sampling.target_lengths) {
+ p.target_state.position.s =
+ std::min(max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length));
+ for (const auto target_lateral_position : target_lateral_positions) {
+ p.target_state.position.d = target_lateral_position;
+ for (const auto target_lat_vel : params.sampling.frenet.target_lateral_velocities) {
+ p.target_state.lateral_velocity = target_lat_vel;
+ for (const auto target_lat_acc : params.sampling.frenet.target_lateral_accelerations) {
+ p.target_state.lateral_acceleration = target_lat_acc;
+ sampling_parameters.parameters.push_back(p);
+ }
+ }
+ }
+ if (p.target_state.position.s == max_s) break;
+ }
+ return sampling_parameters;
+}
+
} // namespace behavior_path_planner
From 3ce2eb149e454545e4d0dd1a07ad4b8b3a732dab Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 29 Sep 2023 17:01:05 +0900
Subject: [PATCH 017/115] eliminate unused code
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 5 ----
.../sampling_planner_module.cpp | 24 -------------------
2 files changed, 29 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 02114b751f3ac..de12064bb6f3a 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -95,11 +95,6 @@ class SamplingPlannerModule : public SceneModuleInterface
bool canTransitIdleToRunningState() override { return false; }
- template <typename T>
- std::vector<TrajectoryPoint> convertToTrajectoryPoints(const std::vector<T> & points);
- template <typename T>
- TrajectoryPoint convertToTrajectoryPoint(const T & point);
-
frenet_planner::SamplingParameters prepareSamplingParameters(
const sampler_common::State & initial_state,
const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 4ae5720245171..54b9ce1508679 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -79,29 +79,6 @@ bool SamplingPlannerModule::isExecutionReady() const
return true;
}
-template <typename T>
-TrajectoryPoint SamplingPlannerModule::convertToTrajectoryPoint(const T & point)
-{
- TrajectoryPoint traj_point;
- traj_point.pose = tier4_autoware_utils::getPose(point);
- traj_point.longitudinal_velocity_mps = 0;
- traj_point.lateral_velocity_mps = 0;
- traj_point.heading_rate_rps = 0;
- return traj_point;
-}
-
-template <typename T>
-std::vector<TrajectoryPoint> SamplingPlannerModule::convertToTrajectoryPoints(
- const std::vector<T> & points)
-{
- std::vector<TrajectoryPoint> traj_points;
- for (const auto & point : points) {
- const auto traj_point = convertToTrajectoryPoint(point);
- traj_points.push_back(traj_point);
- }
- return traj_points;
-}
-
SamplingPlannerData SamplingPlannerModule::createPlannerData(const PlanResult & path)
{
// create planner data
@@ -109,7 +86,6 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData(const PlanResult &
SamplingPlannerData data;
// planner_data.header = path.header;
auto points = path->points;
- data.traj_points = convertToTrajectoryPoints(points);
data.left_bound = path->left_bound;
data.right_bound = path->right_bound;
data.ego_pose = planner_data_->self_odometry->pose.pose;
From 69b23d75850abcf3c576e971ea52adcd6fbbce30 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 3 Oct 2023 17:37:09 +0900
Subject: [PATCH 018/115] WIP add debug marker generation
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 15 ++-
.../sampling_planner_module.cpp | 109 +++++++++++++++++-
2 files changed, 120 insertions(+), 4 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index de12064bb6f3a..684cc36920f34 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -24,6 +24,8 @@
#include "bezier_sampler/bezier_sampling.hpp"
#include "frenet_planner/frenet_planner.hpp"
#include "rclcpp/rclcpp.hpp"
+#include "sampler_common/constraints/hard_constraint.hpp"
+#include "sampler_common/constraints/soft_constraint.hpp"
#include "sampler_common/structures.hpp"
#include "sampler_common/transform/spline_transform.hpp"
#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
@@ -32,7 +34,6 @@
#include "vehicle_info_util/vehicle_info_util.hpp"
#include <rclcpp/rclcpp.hpp>
-#include <sampler_common/structures.hpp>
#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -60,6 +61,14 @@ struct SamplingPlannerData
geometry_msgs::msg::Pose ego_pose;
double ego_vel{};
};
+
+struct SamplingPlannerDebugData
+{
+ std::vector<sampler_common::Path> sampled_candidates{};
+ size_t previous_sampled_candidates_nb = 0UL;
+ std::vector<tier4_autoware_utils::Polygon2d> obstacles{};
+ std::vector<tier4_autoware_utils::MultiPoint2d> footprints{};
+};
class SamplingPlannerModule : public SceneModuleInterface
{
public:
@@ -84,6 +93,8 @@ class SamplingPlannerModule : public SceneModuleInterface
{
}
+ SamplingPlannerDebugData debug_data_;
+
private:
SamplingPlannerData createPlannerData(const PlanResult & path);
@@ -95,6 +106,8 @@ class SamplingPlannerModule : public SceneModuleInterface
bool canTransitIdleToRunningState() override { return false; }
+ void updateDebugMarkers();
+
frenet_planner::SamplingParameters prepareSamplingParameters(
const sampler_common::State & initial_state,
const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 54b9ce1508679..285cf05a8cdc1 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -63,7 +63,6 @@ SamplingPlannerModule::SamplingPlannerModule(
bool SamplingPlannerModule::isExecutionRequested() const
{
if (getPreviousModuleOutput().reference_path->points.empty()) {
- RCLCPP_WARN(getLogger(), "reference path is empty.");
return false;
}
@@ -82,7 +81,6 @@ bool SamplingPlannerModule::isExecutionReady() const
SamplingPlannerData SamplingPlannerModule::createPlannerData(const PlanResult & path)
{
// create planner data
-
SamplingPlannerData data;
// planner_data.header = path.header;
auto points = path->points;
@@ -139,7 +137,112 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
// drivable area and copy to the expected output and finally copy the velocities from the ref path
RCLCPP_INFO(getLogger(), "Generated paths %ld", frenet_paths.size());
- return {};
+ debug_data_.footprints.clear();
+ for (auto & path : frenet_paths) {
+ const auto footprint =
+ sampler_common::constraints::checkHardConstraints(path, params_.constraints);
+ debug_data_.footprints.push_back(footprint);
+ sampler_common::constraints::calculateCost(path, params_.constraints, reference_spline);
+ }
+ std::vector<sampler_common::Path> candidate_paths;
+ const auto move_to_paths = [&candidate_paths](auto & paths_to_move) {
+ candidate_paths.insert(
+ candidate_paths.end(), std::make_move_iterator(paths_to_move.begin()),
+ std::make_move_iterator(paths_to_move.end()));
+ };
+
+ move_to_paths(frenet_paths);
+ debug_data_.previous_sampled_candidates_nb = debug_data_.sampled_candidates.size();
+ debug_data_.sampled_candidates = candidate_paths;
+ debug_data_.obstacles = params_.constraints.obstacle_polygons;
+ // updateDebugMarkers();
+
+ auto p = getPreviousModuleOutput().reference_path;
+ BehaviorModuleOutput out;
+ out.path = p;
+ out.reference_path = p;
+ return out;
+}
+
+void SamplingPlannerModule::updateDebugMarkers()
+{
+ const auto header = planner_data_->route_handler->getRouteHeader();
+ visualization_msgs::msg::Marker m;
+ m.header.frame_id = "map";
+ m.header.stamp = header.stamp;
+ m.action = m.ADD;
+ m.id = 0UL;
+ m.type = m.LINE_STRIP;
+ m.color.a = 1.0;
+ m.scale.x = 0.02;
+ m.ns = "candidates";
+ for (const auto & c : debug_data_.sampled_candidates) {
+ m.points.clear();
+ for (const auto & p : c.points)
+ m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ if (c.constraint_results.isValid()) {
+ m.color.g = 1.0;
+ m.color.r = 0.0;
+ } else {
+ m.color.r = 1.0;
+ m.color.g = 0.0;
+ }
+ debug_marker_.markers.push_back(m);
+ info_marker_.markers.push_back(m);
+ ++m.id;
+ }
+ m.ns = "footprint";
+ m.id = 0UL;
+ m.type = m.POINTS;
+ m.points.clear();
+ m.color.r = 1.0;
+ m.color.g = 0.0;
+ m.color.b = 1.0;
+ m.scale.y = 0.02;
+ if (!debug_data_.footprints.empty()) {
+ m.action = m.ADD;
+ for (const auto & p : debug_data_.footprints[debug_data_.footprints.size()])
+ m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ } else {
+ m.action = m.DELETE;
+ }
+ m.ns = "debug_path";
+ m.id = 0UL;
+ m.type = m.POINTS;
+ m.points.clear();
+ m.color.g = 1.0;
+ m.color.b = 0.0;
+ m.scale.y = 0.04;
+ if (!debug_data_.sampled_candidates.empty()) {
+ m.action = m.ADD;
+ for (const auto & p :
+ debug_data_.sampled_candidates[debug_data_.sampled_candidates.size()].points)
+ m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ } else {
+ m.action = m.DELETE;
+ }
+ debug_marker_.markers.push_back(m);
+ info_marker_.markers.push_back(m);
+ m.type = m.LINE_STRIP;
+ m.ns = "obstacles";
+ m.id = 0UL;
+ m.color.r = 1.0;
+ m.color.g = 0.0;
+ m.color.b = 0.0;
+ for (const auto & obs : debug_data_.obstacles) {
+ m.points.clear();
+ for (const auto & p : obs.outer())
+ m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ debug_marker_.markers.push_back(m);
+ info_marker_.markers.push_back(m);
+ ++m.id;
+ }
+ m.action = m.DELETE;
+ m.ns = "candidates";
+ for (m.id = debug_data_.sampled_candidates.size();
+ static_cast<size_t>(m.id) < debug_data_.previous_sampled_candidates_nb; ++m.id)
+ debug_marker_.markers.push_back(m);
+ info_marker_.markers.push_back(m);
}
CandidateOutput SamplingPlannerModule::planCandidate() const
From cfe4fca373e51a6080ad5ed4b1e2eb80d83c0f68 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 3 Oct 2023 17:58:44 +0900
Subject: [PATCH 019/115] Comment out for debugging
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.cpp | 157 +++++++++---------
1 file changed, 79 insertions(+), 78 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 285cf05a8cdc1..4d9677fc05aa8 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -155,7 +155,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
debug_data_.previous_sampled_candidates_nb = debug_data_.sampled_candidates.size();
debug_data_.sampled_candidates = candidate_paths;
debug_data_.obstacles = params_.constraints.obstacle_polygons;
- // updateDebugMarkers();
+ updateDebugMarkers();
auto p = getPreviousModuleOutput().reference_path;
BehaviorModuleOutput out;
@@ -166,83 +166,84 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
void SamplingPlannerModule::updateDebugMarkers()
{
- const auto header = planner_data_->route_handler->getRouteHeader();
- visualization_msgs::msg::Marker m;
- m.header.frame_id = "map";
- m.header.stamp = header.stamp;
- m.action = m.ADD;
- m.id = 0UL;
- m.type = m.LINE_STRIP;
- m.color.a = 1.0;
- m.scale.x = 0.02;
- m.ns = "candidates";
- for (const auto & c : debug_data_.sampled_candidates) {
- m.points.clear();
- for (const auto & p : c.points)
- m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
- if (c.constraint_results.isValid()) {
- m.color.g = 1.0;
- m.color.r = 0.0;
- } else {
- m.color.r = 1.0;
- m.color.g = 0.0;
- }
- debug_marker_.markers.push_back(m);
- info_marker_.markers.push_back(m);
- ++m.id;
- }
- m.ns = "footprint";
- m.id = 0UL;
- m.type = m.POINTS;
- m.points.clear();
- m.color.r = 1.0;
- m.color.g = 0.0;
- m.color.b = 1.0;
- m.scale.y = 0.02;
- if (!debug_data_.footprints.empty()) {
- m.action = m.ADD;
- for (const auto & p : debug_data_.footprints[debug_data_.footprints.size()])
- m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
- } else {
- m.action = m.DELETE;
- }
- m.ns = "debug_path";
- m.id = 0UL;
- m.type = m.POINTS;
- m.points.clear();
- m.color.g = 1.0;
- m.color.b = 0.0;
- m.scale.y = 0.04;
- if (!debug_data_.sampled_candidates.empty()) {
- m.action = m.ADD;
- for (const auto & p :
- debug_data_.sampled_candidates[debug_data_.sampled_candidates.size()].points)
- m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
- } else {
- m.action = m.DELETE;
- }
- debug_marker_.markers.push_back(m);
- info_marker_.markers.push_back(m);
- m.type = m.LINE_STRIP;
- m.ns = "obstacles";
- m.id = 0UL;
- m.color.r = 1.0;
- m.color.g = 0.0;
- m.color.b = 0.0;
- for (const auto & obs : debug_data_.obstacles) {
- m.points.clear();
- for (const auto & p : obs.outer())
- m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
- debug_marker_.markers.push_back(m);
- info_marker_.markers.push_back(m);
- ++m.id;
- }
- m.action = m.DELETE;
- m.ns = "candidates";
- for (m.id = debug_data_.sampled_candidates.size();
- static_cast<size_t>(m.id) < debug_data_.previous_sampled_candidates_nb; ++m.id)
- debug_marker_.markers.push_back(m);
- info_marker_.markers.push_back(m);
+ // const auto header = planner_data_->route_handler->getRouteHeader();
+ // visualization_msgs::msg::Marker m;
+ // m.header.frame_id = "map";
+ // m.header.stamp = header.stamp;
+ // m.action = m.ADD;
+ // m.id = 0UL;
+ // m.type = m.LINE_STRIP;
+ // m.color.a = 1.0;
+ // m.scale.x = 0.02;
+ // m.ns = "candidates";
+ // for (const auto & c : debug_data_.sampled_candidates) {
+ // m.points.clear();
+ // for (const auto & p : c.points)
+ // m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ // if (c.constraint_results.isValid()) {
+ // m.color.g = 1.0;
+ // m.color.r = 0.0;
+ // } else {
+ // m.color.r = 1.0;
+ // m.color.g = 0.0;
+ // }
+ // debug_marker_.markers.push_back(m);
+ // info_marker_.markers.push_back(m);
+ // ++m.id;
+ // }
+ // m.ns = "footprint";
+ // m.id = 0UL;
+ // m.type = m.POINTS;
+ // m.points.clear();
+ // m.color.r = 1.0;
+ // m.color.g = 0.0;
+ // m.color.b = 1.0;
+ // m.scale.y = 0.02;
+ // if (!debug_data_.footprints.empty()) {
+ // m.action = m.ADD;
+ // for (const auto & p : debug_data_.footprints[debug_data_.footprints.size()])
+ // m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ // } else {
+ // m.action = m.DELETE;
+ // }
+ // m.ns = "debug_path";
+ // m.id = 0UL;
+ // m.type = m.POINTS;
+ // m.points.clear();
+ // m.color.g = 1.0;
+ // m.color.b = 0.0;
+ // m.scale.y = 0.04;
+ // if (!debug_data_.sampled_candidates.empty()) {
+ // m.action = m.ADD;
+ // for (const auto & p :
+ // debug_data_.sampled_candidates[debug_data_.sampled_candidates.size()].points)
+ // m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ // } else {
+ // m.action = m.DELETE;
+ // }
+ // debug_marker_.markers.push_back(m);
+ // info_marker_.markers.push_back(m);
+ // m.type = m.LINE_STRIP;
+ // m.ns = "obstacles";
+ // m.id = 0UL;
+ // m.color.r = 1.0;
+ // m.color.g = 0.0;
+ // m.color.b = 0.0;
+ // for (const auto & obs : debug_data_.obstacles) {
+ // m.points.clear();
+ // for (const auto & p : obs.outer())
+ // m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ // debug_marker_.markers.push_back(m);
+ // info_marker_.markers.push_back(m);
+ // ++m.id;
+ // }
+ // m.action = m.DELETE;
+ // m.ns = "candidates";
+ // for (m.id = debug_data_.sampled_candidates.size();
+ // static_cast<size_t>(m.id) < debug_data_.previous_sampled_candidates_nb; ++m.id) {
+ // debug_marker_.markers.push_back(m);
+ // info_marker_.markers.push_back(m);
+ // }
}
CandidateOutput SamplingPlannerModule::planCandidate() const
From 4f99495e0e255ced28137278648d71961e529989 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Fri, 6 Oct 2023 09:12:18 +0900
Subject: [PATCH 020/115] return prev drivable area (temp)
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.cpp | 51 ++++++++++---------
1 file changed, 26 insertions(+), 25 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 4d9677fc05aa8..1886d3165e5d1 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -161,36 +161,37 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
BehaviorModuleOutput out;
out.path = p;
out.reference_path = p;
+ out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
return out;
}
void SamplingPlannerModule::updateDebugMarkers()
{
- // const auto header = planner_data_->route_handler->getRouteHeader();
- // visualization_msgs::msg::Marker m;
- // m.header.frame_id = "map";
- // m.header.stamp = header.stamp;
- // m.action = m.ADD;
- // m.id = 0UL;
- // m.type = m.LINE_STRIP;
- // m.color.a = 1.0;
- // m.scale.x = 0.02;
- // m.ns = "candidates";
- // for (const auto & c : debug_data_.sampled_candidates) {
- // m.points.clear();
- // for (const auto & p : c.points)
- // m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
- // if (c.constraint_results.isValid()) {
- // m.color.g = 1.0;
- // m.color.r = 0.0;
- // } else {
- // m.color.r = 1.0;
- // m.color.g = 0.0;
- // }
- // debug_marker_.markers.push_back(m);
- // info_marker_.markers.push_back(m);
- // ++m.id;
- // }
+ const auto header = planner_data_->route_handler->getRouteHeader();
+ visualization_msgs::msg::Marker m;
+ m.header.frame_id = "map";
+ m.header.stamp = header.stamp;
+ m.action = m.ADD;
+ m.id = 0UL;
+ m.type = m.LINE_STRIP;
+ m.color.a = 1.0;
+ m.scale.x = 0.02;
+ m.ns = "candidates";
+ for (const auto & c : debug_data_.sampled_candidates) {
+ m.points.clear();
+ for (const auto & p : c.points)
+ m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ if (c.constraint_results.isValid()) {
+ m.color.g = 1.0;
+ m.color.r = 0.0;
+ } else {
+ m.color.r = 1.0;
+ m.color.g = 0.0;
+ }
+ debug_marker_.markers.push_back(m);
+ info_marker_.markers.push_back(m);
+ ++m.id;
+ }
// m.ns = "footprint";
// m.id = 0UL;
// m.type = m.POINTS;
From aed6cbb20ddbc0c5224d48fff8e3da194702796b Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 7 Nov 2023 17:12:42 +0900
Subject: [PATCH 021/115] fixes to compile after rebase
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../scene_module/sampling_planner/sampling_planner_module.cpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 1886d3165e5d1..ecb9fdf20e2b3 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -23,6 +23,7 @@ using motion_utils::findNearestSegmentIndex;
using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
using tier4_autoware_utils::getPoint;
+using tier4_autoware_utils::Point2d;
using utils::toPath;
SamplingPlannerModule::SamplingPlannerModule(
From e37b183402954165fdce8e9a21cc1b1aafc36fc2 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 8 Nov 2023 18:13:35 +0900
Subject: [PATCH 022/115] WIP update sampling planner param structure to equal
behav planner
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner.param.yaml | 153 ++----------------
.../sampling_planner_module.hpp | 43 ++++-
.../sampling_planner_parameters.hpp | 34 +++-
.../scene_module/sampling_planner/manager.cpp | 26 +++
.../sampling_planner_module.cpp | 61 +++----
5 files changed, 130 insertions(+), 187 deletions(-)
diff --git a/planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml b/planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml
index 6760ec787bb03..53c531c4b04a4 100644
--- a/planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml
+++ b/planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml
@@ -1,139 +1,20 @@
/**:
ros__parameters:
start_planner:
- th_arrived_distance: 1.0
- th_stopped_velocity: 0.01
- th_stopped_time: 1.0
- collision_check_margin: 1.0
- collision_check_distance_from_end: 1.0
- th_moving_object_velocity: 1.0
- th_distance_to_middle_of_the_road: 0.1
- center_line_path_interval: 1.0
- # shift pull out
- enable_shift_pull_out: true
- check_shift_path_lane_departure: false
- minimum_shift_pull_out_distance: 0.0
- deceleration_interval: 15.0
- lateral_jerk: 0.5
- lateral_acceleration_sampling_num: 3
- minimum_lateral_acc: 0.15
- maximum_lateral_acc: 0.5
- maximum_curvature: 0.07
- # geometric pull out
- enable_geometric_pull_out: true
- divide_pull_out_path: false
- geometric_pull_out_velocity: 1.0
- arc_path_interval: 1.0
- lane_departure_margin: 0.2
- backward_velocity: -1.0
- pull_out_max_steer_angle: 0.26 # 15deg
- # search start pose backward
- enable_back: true
- search_priority: "efficient_path" # "efficient_path" or "short_back_distance"
- max_back_distance: 30.0
- backward_search_resolution: 2.0
- backward_path_update_duration: 3.0
- ignore_distance_from_lane_end: 15.0
- # turns signal
- th_turn_signal_on_lateral_offset: 1.0
- intersection_search_length: 30.0
- length_ratio_for_turn_signal_deactivation_near_intersection: 0.5
- # freespace planner
- freespace_planner:
- enable_freespace_planner: true
- end_pose_search_start_distance: 20.0
- end_pose_search_end_distance: 30.0
- end_pose_search_interval: 2.0
- freespace_planner_algorithm: "astar" # options: astar, rrtstar
- velocity: 1.0
- vehicle_shape_margin: 1.0
- time_limit: 3000.0
- minimum_turning_radius: 5.0
- maximum_turning_radius: 5.0
- turning_radius_size: 1
- # search configs
- search_configs:
- theta_size: 144
- angle_goal_range: 6.0
- curve_weight: 1.2
- reverse_weight: 1.0
- lateral_goal_range: 0.5
- longitudinal_goal_range: 2.0
- # costmap configs
- costmap_configs:
- obstacle_threshold: 30
- # -- A* search Configurations --
- astar:
- only_behind_solutions: false
- use_back: false
- distance_heuristic_weight: 1.0
- # -- RRT* search Configurations --
- rrtstar:
- enable_update: true
- use_informed_sampling: true
- max_planning_time: 150.0
- neighbor_radius: 8.0
- margin: 1.0
-
- stop_condition:
- maximum_deceleration_for_stop: 1.0
- maximum_jerk_for_stop: 1.0
- path_safety_check:
- # EgoPredictedPath
- ego_predicted_path:
- min_velocity: 0.0
- acceleration: 1.0
- max_velocity: 1.0
- time_horizon_for_front_object: 10.0
- time_horizon_for_rear_object: 10.0
- time_resolution: 0.5
- delay_until_departure: 1.0
- # For target object filtering
- target_filtering:
- safety_check_time_horizon: 5.0
- safety_check_time_resolution: 1.0
- # detection range
- object_check_forward_distance: 10.0
- object_check_backward_distance: 100.0
- ignore_object_velocity_threshold: 0.0
- # ObjectTypesToCheck
- object_types_to_check:
- check_car: true
- check_truck: true
- check_bus: true
- check_trailer: true
- check_bicycle: true
- check_motorcycle: true
- check_pedestrian: true
- check_unknown: false
- # ObjectLaneConfiguration
- object_lane_configuration:
- check_current_lane: true
- check_right_side_lane: true
- check_left_side_lane: true
- check_shoulder_lane: true
- check_other_lane: false
- include_opposite_lane: false
- invert_opposite_lane: false
- check_all_predicted_path: true
- use_all_predicted_path: true
- use_predicted_path_outside_lanelet: false
-
- # For safety check
- safety_check_params:
- # safety check configuration
- enable_safety_check: true
- # collision check parameters
- check_all_predicted_path: true
- publish_debug_marker: false
- rss_params:
- rear_vehicle_reaction_time: 1.0
- rear_vehicle_safety_time_margin: 1.0
- lateral_distance_max_threshold: 1.0
- longitudinal_distance_min_threshold: 1.0
- longitudinal_velocity_delta_time: 1.0
- # hysteresis factor to expand/shrink polygon
- hysteresis_factor_expand_rate: 1.0
- # temporary
- backward_path_length: 30.0
- forward_path_length: 100.0
+ max_curvature: 0.1
+ min_curvature: -0.1
+ lateral_deviation_weight: 1.0
+ length_weight: 1.0
+ curvature_weight: 1.0
+ enable_frenet: true
+ enable_bezier: false
+ resolution: 0.5
+ previous_path_reuse_points_nb: 2
+ nb_target_lateral_positions: {10.0, 20.0}
+ target_lengths: {-2.0, -1.0, 0.0, 1.0, 2.0}
+ target_lateral_positions: 2
+ target_lateral_velocities: {-0.1, 0.0, 0.1}
+ target_lateral_accelerations: {0.0}
+ force_zero_deviation: true
+ force_zero_heading: true
+ smooth_reference: false
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 684cc36920f34..664d6115bc161 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -85,7 +85,40 @@ class SamplingPlannerModule : public SceneModuleInterface
void updateModuleParams(const std::any & parameters) override
{
- parameters_ = std::any_cast<std::shared_ptr<SamplingPlannerParameters>>(parameters);
+ std::shared_ptr<SamplingPlannerParameters> user_params_ =
+ std::any_cast<std::shared_ptr<SamplingPlannerParameters>>(parameters);
+
+ internal_params_->constraints.hard.max_curvature = user_params_->max_curvature;
+ internal_params_->constraints.hard.min_curvature = user_params_->min_curvature;
+ internal_params_->constraints.soft.lateral_deviation_weight =
+ user_params_->lateral_deviation_weight;
+ internal_params_->constraints.soft.length_weight = user_params_->length_weight;
+ internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
+ internal_params_->sampling.enable_frenet = user_params_->enable_frenet;
+ internal_params_->sampling.enable_bezier = user_params_->enable_bezier;
+ internal_params_->sampling.resolution = user_params_->resolution;
+ internal_params_->sampling.previous_path_reuse_points_nb =
+ user_params_->previous_path_reuse_points_nb;
+ internal_params_->sampling.target_lengths = user_params_->target_lengths;
+ internal_params_->sampling.target_lateral_positions = user_params_->target_lateral_positions;
+ internal_params_->sampling.nb_target_lateral_positions =
+ user_params_->nb_target_lateral_positions;
+ internal_params_->sampling.frenet.target_lateral_velocities =
+ user_params_->target_lateral_velocities;
+ internal_params_->sampling.frenet.target_lateral_accelerations =
+ user_params_->target_lateral_accelerations;
+ // internal_params_->sampling.bezier.nb_k = user_params_->nb_k;
+ // internal_params_->sampling.bezier.mk_min = user_params_->mk_min;
+ // internal_params_->sampling.bezier.mk_max = user_params_->mk_max;
+ // internal_params_->sampling.bezier.nb_t = user_params_->nb_t;
+ // internal_params_->sampling.bezier.mt_min = user_params_->mt_min;
+ // internal_params_->sampling.bezier.mt_max = user_params_->mt_max;
+ internal_params_->preprocessing.force_zero_deviation = user_params_->force_zero_deviation;
+ internal_params_->preprocessing.force_zero_heading = user_params_->force_zero_heading;
+ internal_params_->preprocessing.smooth_reference = user_params_->smooth_reference;
+ internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint();
+ internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
+ internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
}
void acceptVisitor(
@@ -110,12 +143,12 @@ class SamplingPlannerModule : public SceneModuleInterface
frenet_planner::SamplingParameters prepareSamplingParameters(
const sampler_common::State & initial_state,
- const sampler_common::transform::Spline2D & path_spline, const Parameters & params);
+ const sampler_common::transform::Spline2D & path_spline,
+ const SamplingPlannerInternalParameters & internal_params_);
// member
- Parameters params_;
- std::shared_ptr<SamplingPlannerParameters> parameters_;
-
+ // std::shared_ptr<SamplingPlannerParameters> params_;
+ std::shared_ptr<SamplingPlannerInternalParameters> internal_params_;
vehicle_info_util::VehicleInfo vehicle_info_{};
};
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
index 41c0d9cc8d163..d1a0980c66fec 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
@@ -21,12 +21,40 @@
#include <vector>
namespace behavior_path_planner
{
+using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::MultiPoint2d;
+using tier4_autoware_utils::MultiPolygon2d;
+using tier4_autoware_utils::Point2d;
+using tier4_autoware_utils::Polygon2d;
+
struct SamplingPlannerParameters
-{
- double dummy_parameter{0.0};
+{ // constraints.hard
+ double max_curvature;
+ double min_curvature;
+ // constraints.soft
+ double lateral_deviation_weight;
+ double length_weight;
+ double curvature_weight;
+
+ // sampling
+ bool enable_frenet;
+ bool enable_bezier;
+ double resolution;
+ int previous_path_reuse_points_nb;
+ int nb_target_lateral_positions;
+
+ std::vector<double> target_lengths;
+ std::vector<double> target_lateral_positions;
+ // frenet
+ std::vector<double> target_lateral_velocities;
+ std::vector<double> target_lateral_accelerations;
+
+ bool force_zero_deviation;
+ bool force_zero_heading;
+ bool smooth_reference;
};
-struct Parameters
+struct SamplingPlannerInternalParameters
{
sampler_common::Constraints constraints;
struct
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
index 5deb93d60ce34..352fcee4351c0 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
@@ -29,6 +29,32 @@ SamplingPlannerModuleManager::SamplingPlannerModuleManager(
rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config)
: SceneModuleManagerInterface(node, name, config, {})
{
+ SamplingPlannerParameters p{};
+
+ p.max_curvature = node->declare_parameter<double>(name + ".max_curvature");
+ p.min_curvature = node->declare_parameter<double>(name + ".min_curvature");
+ p.lateral_deviation_weight = node->declare_parameter<double>(name + ".lateral_deviation_weight");
+ p.length_weight = node->declare_parameter<double>(name + ".length_weight");
+ p.curvature_weight = node->declare_parameter<double>(name + ".curvature_weight");
+ p.enable_frenet = node->declare_parameter<bool>(name + ".enable_frenet");
+ p.enable_bezier = node->declare_parameter<bool>(name + ".enable_bezier");
+ p.resolution = node->declare_parameter<double>(name + ".resolution");
+ p.previous_path_reuse_points_nb =
+ node->declare_parameter<int>(name + ".previous_path_reuse_points_nb");
+ p.nb_target_lateral_positions =
+ node->declare_parameter<int>(name + ".nb_target_lateral_positions");
+ p.target_lengths = node->declare_parameter<std::vector<double>>(name + ".target_lengths");
+ p.target_lateral_positions =
+ node->declare_parameter<std::vector<double>>(name + ".target_lateral_positions");
+ p.target_lateral_velocities =
+ node->declare_parameter<std::vector<double>>(name + ".target_lateral_velocities");
+ p.target_lateral_accelerations =
+ node->declare_parameter<std::vector<double>>(name + ".target_lateral_accelerations");
+ p.force_zero_deviation = node->declare_parameter<bool>(name + ".force_zero_deviation");
+ p.force_zero_heading = node->declare_parameter<bool>(name + ".force_zero_heading");
+ p.smooth_reference = node->declare_parameter<bool>(name + ".smooth_reference");
+
+ parameters_ = std::make_shared<SamplingPlannerParameters>(p);
}
} // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index ecb9fdf20e2b3..160d2d886af9e 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -30,35 +30,9 @@ SamplingPlannerModule::SamplingPlannerModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<SamplingPlannerParameters> & parameters,
const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}
+: SceneModuleInterface{name, node, rtc_interface_ptr_map}
{
- // parameters
- params_.constraints.hard.max_curvature = 0.1;
- params_.constraints.hard.min_curvature = -0.1;
- params_.constraints.soft.lateral_deviation_weight = 1.0;
- params_.constraints.soft.length_weight = 1.0;
- params_.constraints.soft.curvature_weight = 1.0;
- params_.sampling.enable_frenet = true;
- params_.sampling.enable_bezier = false;
- params_.sampling.resolution = 0.5;
- params_.sampling.previous_path_reuse_points_nb = 2;
- params_.sampling.target_lengths = {10.0, 20.0};
- params_.sampling.target_lateral_positions = {-2.0, -1.0, 0.0, 1.0, 2.0};
- params_.sampling.nb_target_lateral_positions = 2;
- params_.sampling.frenet.target_lateral_velocities = {-0.1, 0.0, 0.1};
- params_.sampling.frenet.target_lateral_accelerations = {0.0};
- // params_.sampling.bezier.nb_k = declare_parameter<int>("sampling.bezier.nb_k");
- // params_.sampling.bezier.mk_min = declare_parameter<double>("sampling.bezier.mk_min");
- // params_.sampling.bezier.mk_max = declare_parameter<double>("sampling.bezier.mk_max");
- // params_.sampling.bezier.nb_t = declare_parameter<int>("sampling.bezier.nb_t");
- // params_.sampling.bezier.mt_min = declare_parameter<double>("sampling.bezier.mt_min");
- // params_.sampling.bezier.mt_max = declare_parameter<double>("sampling.bezier.mt_max");
- params_.preprocessing.force_zero_deviation = true;
- params_.preprocessing.force_zero_heading = true;
- params_.preprocessing.smooth_reference = false;
- params_.constraints.ego_footprint = vehicle_info_.createFootprint();
- params_.constraints.ego_width = vehicle_info_.vehicle_width_m;
- params_.constraints.ego_length = vehicle_info_.vehicle_length_m;
+ updateModuleParams(parameters);
}
bool SamplingPlannerModule::isExecutionRequested() const
@@ -127,7 +101,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
initial_state.heading = rpy.z;
frenet_planner::SamplingParameters sampling_parameters =
- prepareSamplingParameters(initial_state, reference_spline, params_);
+ prepareSamplingParameters(initial_state, reference_spline, *params_);
frenet_planner::FrenetState frenet_initial_state;
frenet_initial_state.position = initial_state.frenet;
@@ -141,9 +115,9 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
debug_data_.footprints.clear();
for (auto & path : frenet_paths) {
const auto footprint =
- sampler_common::constraints::checkHardConstraints(path, params_.constraints);
+ sampler_common::constraints::checkHardConstraints(path, params_->constraints);
debug_data_.footprints.push_back(footprint);
- sampler_common::constraints::calculateCost(path, params_.constraints, reference_spline);
+ sampler_common::constraints::calculateCost(path, params_->constraints, reference_spline);
}
std::vector<sampler_common::Path> candidate_paths;
const auto move_to_paths = [&candidate_paths](auto & paths_to_move) {
@@ -155,7 +129,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
move_to_paths(frenet_paths);
debug_data_.previous_sampled_candidates_nb = debug_data_.sampled_candidates.size();
debug_data_.sampled_candidates = candidate_paths;
- debug_data_.obstacles = params_.constraints.obstacle_polygons;
+ debug_data_.obstacles = params_->obstacle_polygons;
updateDebugMarkers();
auto p = getPreviousModuleOutput().reference_path;
@@ -259,17 +233,18 @@ void SamplingPlannerModule::updateData()
frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters(
const sampler_common::State & initial_state,
- const sampler_common::transform::Spline2D & path_spline, const Parameters & params)
+ const sampler_common::transform::Spline2D & path_spline,
+ const SamplingPlannerParameters & params_)
{
// calculate target lateral positions
std::vector<double> target_lateral_positions;
- if (params.sampling.nb_target_lateral_positions > 1) {
+ if (params_.nb_target_lateral_positions > 1) {
target_lateral_positions = {0.0, initial_state.frenet.d};
double min_d = 0.0;
double max_d = 0.0;
double min_d_s = std::numeric_limits<double>::max();
double max_d_s = std::numeric_limits<double>::max();
- for (const auto & drivable_poly : params.constraints.drivable_polygons) {
+ for (const auto & drivable_poly : params_.drivable_polygons) {
for (const auto & p : drivable_poly.outer()) {
const auto frenet_coordinates = path_spline.frenet(p);
const auto d_s = std::abs(frenet_coordinates.s - initial_state.frenet.s);
@@ -283,27 +258,27 @@ frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParamet
}
}
}
- min_d += params.constraints.ego_width / 2.0;
- max_d -= params.constraints.ego_width / 2.0;
+ min_d += params_.ego_width / 2.0;
+ max_d -= params_.ego_width / 2.0;
if (min_d < max_d) {
- for (auto r = 0.0; r <= 1.0; r += 1.0 / (params.sampling.nb_target_lateral_positions - 1))
+ for (auto r = 0.0; r <= 1.0; r += 1.0 / (params_.nb_target_lateral_positions - 1))
target_lateral_positions.push_back(interpolation::lerp(min_d, max_d, r));
}
} else {
- target_lateral_positions = params.sampling.target_lateral_positions;
+ target_lateral_positions = params_.target_lateral_positions;
}
frenet_planner::SamplingParameters sampling_parameters;
- sampling_parameters.resolution = params.sampling.resolution;
+ sampling_parameters.resolution = params_.resolution;
const auto max_s = path_spline.lastS();
frenet_planner::SamplingParameter p;
- for (const auto target_length : params.sampling.target_lengths) {
+ for (const auto target_length : params_.target_lengths) {
p.target_state.position.s =
std::min(max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length));
for (const auto target_lateral_position : target_lateral_positions) {
p.target_state.position.d = target_lateral_position;
- for (const auto target_lat_vel : params.sampling.frenet.target_lateral_velocities) {
+ for (const auto target_lat_vel : params_.target_lateral_velocities) {
p.target_state.lateral_velocity = target_lat_vel;
- for (const auto target_lat_acc : params.sampling.frenet.target_lateral_accelerations) {
+ for (const auto target_lat_acc : params_.target_lateral_accelerations) {
p.target_state.lateral_acceleration = target_lat_acc;
sampling_parameters.parameters.push_back(p);
}
From 0b381de40952387e572f3c83a3a3da8e8ee24722 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 9 Nov 2023 12:13:13 +0900
Subject: [PATCH 023/115] Updated param handling
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../behavior_planning.launch.xml | 1 +
.../sampling_planner_module.hpp | 11 +++-
.../scene_module/sampling_planner/manager.cpp | 57 +++++++++++--------
3 files changed, 42 insertions(+), 27 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index a297697234ef2..835dd4ff76cf8 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -201,6 +201,7 @@
<param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
<param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
<param from="$(var behavior_path_planner_dynamic_avoidance_module_param_path)"/>
+ <param from="$(var behavior_path_planner_sampling_planner_param_path)"/>
<param from="$(var behavior_path_planner_lane_change_module_param_path)"/>
<param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
<param from="$(var behavior_path_planner_start_planner_module_param_path)"/>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 664d6115bc161..9ad2077af47da 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -88,12 +88,17 @@ class SamplingPlannerModule : public SceneModuleInterface
std::shared_ptr<SamplingPlannerParameters> user_params_ =
std::any_cast<std::shared_ptr<SamplingPlannerParameters>>(parameters);
+ // Constraints
internal_params_->constraints.hard.max_curvature = user_params_->max_curvature;
internal_params_->constraints.hard.min_curvature = user_params_->min_curvature;
internal_params_->constraints.soft.lateral_deviation_weight =
user_params_->lateral_deviation_weight;
internal_params_->constraints.soft.length_weight = user_params_->length_weight;
internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
+ internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint();
+ internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
+ internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
+ // Sampling
internal_params_->sampling.enable_frenet = user_params_->enable_frenet;
internal_params_->sampling.enable_bezier = user_params_->enable_bezier;
internal_params_->sampling.resolution = user_params_->resolution;
@@ -103,6 +108,7 @@ class SamplingPlannerModule : public SceneModuleInterface
internal_params_->sampling.target_lateral_positions = user_params_->target_lateral_positions;
internal_params_->sampling.nb_target_lateral_positions =
user_params_->nb_target_lateral_positions;
+
internal_params_->sampling.frenet.target_lateral_velocities =
user_params_->target_lateral_velocities;
internal_params_->sampling.frenet.target_lateral_accelerations =
@@ -113,12 +119,11 @@ class SamplingPlannerModule : public SceneModuleInterface
// internal_params_->sampling.bezier.nb_t = user_params_->nb_t;
// internal_params_->sampling.bezier.mt_min = user_params_->mt_min;
// internal_params_->sampling.bezier.mt_max = user_params_->mt_max;
+
+ // Preprocessing
internal_params_->preprocessing.force_zero_deviation = user_params_->force_zero_deviation;
internal_params_->preprocessing.force_zero_heading = user_params_->force_zero_heading;
internal_params_->preprocessing.smooth_reference = user_params_->smooth_reference;
- internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint();
- internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
- internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
}
void acceptVisitor(
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
index 352fcee4351c0..9319dfc2b72d1 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
@@ -30,30 +30,39 @@ SamplingPlannerModuleManager::SamplingPlannerModuleManager(
: SceneModuleManagerInterface(node, name, config, {})
{
SamplingPlannerParameters p{};
-
- p.max_curvature = node->declare_parameter<double>(name + ".max_curvature");
- p.min_curvature = node->declare_parameter<double>(name + ".min_curvature");
- p.lateral_deviation_weight = node->declare_parameter<double>(name + ".lateral_deviation_weight");
- p.length_weight = node->declare_parameter<double>(name + ".length_weight");
- p.curvature_weight = node->declare_parameter<double>(name + ".curvature_weight");
- p.enable_frenet = node->declare_parameter<bool>(name + ".enable_frenet");
- p.enable_bezier = node->declare_parameter<bool>(name + ".enable_bezier");
- p.resolution = node->declare_parameter<double>(name + ".resolution");
- p.previous_path_reuse_points_nb =
- node->declare_parameter<int>(name + ".previous_path_reuse_points_nb");
- p.nb_target_lateral_positions =
- node->declare_parameter<int>(name + ".nb_target_lateral_positions");
- p.target_lengths = node->declare_parameter<std::vector<double>>(name + ".target_lengths");
- p.target_lateral_positions =
- node->declare_parameter<std::vector<double>>(name + ".target_lateral_positions");
- p.target_lateral_velocities =
- node->declare_parameter<std::vector<double>>(name + ".target_lateral_velocities");
- p.target_lateral_accelerations =
- node->declare_parameter<std::vector<double>>(name + ".target_lateral_accelerations");
- p.force_zero_deviation = node->declare_parameter<bool>(name + ".force_zero_deviation");
- p.force_zero_heading = node->declare_parameter<bool>(name + ".force_zero_heading");
- p.smooth_reference = node->declare_parameter<bool>(name + ".smooth_reference");
-
+ {
+ std::string ns{"constraints.hard"};
+ p.max_curvature = node->declare_parameter<double>(ns + ".max_curvature");
+ p.min_curvature = node->declare_parameter<double>(ns + ".min_curvature");
+ ns = std::string{"constraints.soft"};
+ p.lateral_deviation_weight = node->declare_parameter<double>(ns + ".lateral_deviation_weight");
+ p.length_weight = node->declare_parameter<double>(ns + ".length_weight");
+ p.curvature_weight = node->declare_parameter<double>(ns + ".curvature_weight");
+ }
+ {
+ std::string ns{"sampling"};
+ p.enable_frenet = node->declare_parameter<bool>(ns + ".enable_frenet");
+ p.enable_bezier = node->declare_parameter<bool>(ns + ".enable_bezier");
+ p.resolution = node->declare_parameter<double>(ns + ".resolution");
+ p.previous_path_reuse_points_nb =
+ node->declare_parameter<int>(ns + ".previous_path_reuse_points_nb");
+ p.nb_target_lateral_positions =
+ node->declare_parameter<int>(ns + ".nb_target_lateral_positions");
+ p.target_lengths = node->declare_parameter<std::vector<double>>(ns + ".target_lengths");
+ p.target_lateral_positions =
+ node->declare_parameter<std::vector<double>>(ns + ".target_lateral_positions");
+ ns += ".frenet";
+ p.target_lateral_velocities =
+ node->declare_parameter<std::vector<double>>(ns + ".target_lateral_velocities");
+ p.target_lateral_accelerations =
+ node->declare_parameter<std::vector<double>>(ns + ".target_lateral_accelerations");
+ }
+ {
+ std::string ns{"preprocessing"};
+ p.force_zero_deviation = node->declare_parameter<bool>(ns + ".force_zero_initial_deviation");
+ p.force_zero_heading = node->declare_parameter<bool>(ns + ".force_zero_initial_heading");
+ p.smooth_reference = node->declare_parameter<bool>(ns + ".smooth_reference_trajectory");
+ }
parameters_ = std::make_shared<SamplingPlannerParameters>(p);
}
From 77d60acc68034f579ead8c0128d25b5f562088a6 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 9 Nov 2023 12:32:52 +0900
Subject: [PATCH 024/115] changed names of internal_variable to match changes
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.cpp | 33 ++++++++++---------
1 file changed, 18 insertions(+), 15 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 160d2d886af9e..f095bc8bc1d8f 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -32,6 +32,8 @@ SamplingPlannerModule::SamplingPlannerModule(
const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
: SceneModuleInterface{name, node, rtc_interface_ptr_map}
{
+ internal_params_ =
+ std::shared_ptr<SamplingPlannerInternalParameters>(new SamplingPlannerInternalParameters{});
updateModuleParams(parameters);
}
@@ -101,7 +103,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
initial_state.heading = rpy.z;
frenet_planner::SamplingParameters sampling_parameters =
- prepareSamplingParameters(initial_state, reference_spline, *params_);
+ prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
frenet_planner::FrenetState frenet_initial_state;
frenet_initial_state.position = initial_state.frenet;
@@ -115,9 +117,10 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
debug_data_.footprints.clear();
for (auto & path : frenet_paths) {
const auto footprint =
- sampler_common::constraints::checkHardConstraints(path, params_->constraints);
+ sampler_common::constraints::checkHardConstraints(path, internal_params_->constraints);
debug_data_.footprints.push_back(footprint);
- sampler_common::constraints::calculateCost(path, params_->constraints, reference_spline);
+ sampler_common::constraints::calculateCost(
+ path, internal_params_->constraints, reference_spline);
}
std::vector<sampler_common::Path> candidate_paths;
const auto move_to_paths = [&candidate_paths](auto & paths_to_move) {
@@ -129,7 +132,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
move_to_paths(frenet_paths);
debug_data_.previous_sampled_candidates_nb = debug_data_.sampled_candidates.size();
debug_data_.sampled_candidates = candidate_paths;
- debug_data_.obstacles = params_->obstacle_polygons;
+ debug_data_.obstacles = internal_params_->constraints.obstacle_polygons;
updateDebugMarkers();
auto p = getPreviousModuleOutput().reference_path;
@@ -234,17 +237,17 @@ void SamplingPlannerModule::updateData()
frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters(
const sampler_common::State & initial_state,
const sampler_common::transform::Spline2D & path_spline,
- const SamplingPlannerParameters & params_)
+ const SamplingPlannerInternalParameters & params_)
{
// calculate target lateral positions
std::vector<double> target_lateral_positions;
- if (params_.nb_target_lateral_positions > 1) {
+ if (params_.sampling.nb_target_lateral_positions > 1) {
target_lateral_positions = {0.0, initial_state.frenet.d};
double min_d = 0.0;
double max_d = 0.0;
double min_d_s = std::numeric_limits<double>::max();
double max_d_s = std::numeric_limits<double>::max();
- for (const auto & drivable_poly : params_.drivable_polygons) {
+ for (const auto & drivable_poly : params_.constraints.drivable_polygons) {
for (const auto & p : drivable_poly.outer()) {
const auto frenet_coordinates = path_spline.frenet(p);
const auto d_s = std::abs(frenet_coordinates.s - initial_state.frenet.s);
@@ -258,27 +261,27 @@ frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParamet
}
}
}
- min_d += params_.ego_width / 2.0;
- max_d -= params_.ego_width / 2.0;
+ min_d += params_.constraints.ego_width / 2.0;
+ max_d -= params_.constraints.ego_width / 2.0;
if (min_d < max_d) {
- for (auto r = 0.0; r <= 1.0; r += 1.0 / (params_.nb_target_lateral_positions - 1))
+ for (auto r = 0.0; r <= 1.0; r += 1.0 / (params_.sampling.nb_target_lateral_positions - 1))
target_lateral_positions.push_back(interpolation::lerp(min_d, max_d, r));
}
} else {
- target_lateral_positions = params_.target_lateral_positions;
+ target_lateral_positions = params_.sampling.target_lateral_positions;
}
frenet_planner::SamplingParameters sampling_parameters;
- sampling_parameters.resolution = params_.resolution;
+ sampling_parameters.resolution = params_.sampling.resolution;
const auto max_s = path_spline.lastS();
frenet_planner::SamplingParameter p;
- for (const auto target_length : params_.target_lengths) {
+ for (const auto target_length : params_.sampling.target_lengths) {
p.target_state.position.s =
std::min(max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length));
for (const auto target_lateral_position : target_lateral_positions) {
p.target_state.position.d = target_lateral_position;
- for (const auto target_lat_vel : params_.target_lateral_velocities) {
+ for (const auto target_lat_vel : params_.sampling.frenet.target_lateral_velocities) {
p.target_state.lateral_velocity = target_lat_vel;
- for (const auto target_lat_acc : params_.target_lateral_accelerations) {
+ for (const auto target_lat_acc : params_.sampling.frenet.target_lateral_accelerations) {
p.target_state.lateral_acceleration = target_lat_acc;
sampling_parameters.parameters.push_back(p);
}
From 0a873eb66fd11b0555535c175b66960990011618 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 9 Nov 2023 14:33:59 +0900
Subject: [PATCH 025/115] partially solve markers not clearing
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../scene_module/sampling_planner/manager.hpp | 2 +-
.../scene_module/sampling_planner/manager.cpp | 20 +++++++++++++++++++
.../sampling_planner_module.cpp | 8 +++++---
3 files changed, 26 insertions(+), 4 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp
index e805593dcff31..98c675a271e8f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp
@@ -42,7 +42,7 @@ class SamplingPlannerModuleManager : public SceneModuleManagerInterface
}
void updateModuleParams(
- [[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) override{};
+ [[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) override;
private:
std::shared_ptr<SamplingPlannerParameters> parameters_;
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
index 9319dfc2b72d1..c94544afb3383 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
@@ -66,4 +66,24 @@ SamplingPlannerModuleManager::SamplingPlannerModuleManager(
parameters_ = std::make_shared<SamplingPlannerParameters>(p);
}
+void SamplingPlannerModuleManager::updateModuleParams(
+ [[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters)
+{
+ using tier4_autoware_utils::updateParam;
+
+ auto & p = parameters_;
+
+ [[maybe_unused]] std::string ns = name_ + ".";
+
+ std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) {
+ if (!observer.expired()) {
+ const auto sampling_planner_ptr =
+ std::dynamic_pointer_cast<SamplingPlannerModule>(observer.lock());
+ if (sampling_planner_ptr) {
+ sampling_planner_ptr->updateModuleParams(p);
+ }
+ }
+ });
+}
+
} // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index f095bc8bc1d8f..3dc7428bab0c3 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -76,8 +76,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
if (path_ptr->points.empty()) {
return {};
}
- const auto ego_closest_path_index = planner_data_->findEgoIndex(path_ptr->points);
- RCLCPP_INFO(getLogger(), "ego_closest_path_index %ld", ego_closest_path_index);
+ // const auto ego_closest_path_index = planner_data_->findEgoIndex(path_ptr->points);
+ // RCLCPP_INFO(getLogger(), "ego_closest_path_index %ld", ego_closest_path_index);
// resetPathCandidate();
// resetPathReference();
// [[maybe_unused]] const auto path = toPath(*path_ptr);
@@ -112,7 +112,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
// After path generation we do pruning we then select the optimal path based on obstacles and
// drivable area and copy to the expected output and finally copy the velocities from the ref path
- RCLCPP_INFO(getLogger(), "Generated paths %ld", frenet_paths.size());
+ // RCLCPP_INFO(getLogger(), "Generated paths %ld", frenet_paths.size());
debug_data_.footprints.clear();
for (auto & path : frenet_paths) {
@@ -145,6 +145,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
void SamplingPlannerModule::updateDebugMarkers()
{
+ debug_marker_.markers.clear();
+
const auto header = planner_data_->route_handler->getRouteHeader();
visualization_msgs::msg::Marker m;
m.header.frame_id = "map";
From fbe64d76f6f38e49d47ab9c5858c6d6c7f25ba8c Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 14 Nov 2023 15:56:09 +0900
Subject: [PATCH 026/115] add param update functionality
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../scene_module/sampling_planner/manager.cpp | 41 ++++++++++++++++++-
1 file changed, 40 insertions(+), 1 deletion(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
index c94544afb3383..845c38f62077e 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
@@ -73,7 +73,46 @@ void SamplingPlannerModuleManager::updateModuleParams(
auto & p = parameters_;
- [[maybe_unused]] std::string ns = name_ + ".";
+ [[maybe_unused]] const std::string ns = name_ + ".";
+
+ {
+ std::string ns{"constraints.hard"};
+ updateParam<double>(parameters, ns + ".max_curvature", p->max_curvature);
+ updateParam<double>(parameters, ns + ".min_curvature", p->min_curvature);
+ ns = std::string{"constraints.soft"};
+ updateParam<double>(parameters, ns + ".lateral_deviation_weight", p->lateral_deviation_weight);
+ updateParam<double>(parameters, ns + ".length_weight", p->length_weight);
+ updateParam<double>(parameters, ns + ".curvature_weight", p->curvature_weight);
+ }
+ {
+ std::string ns{"sampling"};
+ updateParam<bool>(parameters, ns + ".enable_frenet", p->enable_frenet);
+ updateParam<bool>(parameters, ns + ".enable_bezier", p->enable_bezier);
+ updateParam<double>(parameters, ns + ".resolution", p->resolution);
+
+ updateParam<int>(
+ parameters, ns + ".previous_path_reuse_points_nb", p->previous_path_reuse_points_nb);
+
+ updateParam<int>(
+ parameters, ns + ".nb_target_lateral_positions", p->nb_target_lateral_positions);
+ updateParam<std::vector<double>>(parameters, ns + ".target_lengths", p->target_lengths);
+
+ bool updated = updateParam<std::vector<double>>(
+ parameters, ns + ".target_lateral_positions", p->target_lateral_positions);
+
+ ns += ".frenet";
+ updateParam<std::vector<double>>(
+ parameters, ns + ".target_lateral_velocities", p->target_lateral_velocities);
+
+ updateParam<std::vector<double>>(
+ parameters, ns + ".target_lateral_accelerations", p->target_lateral_accelerations);
+ }
+ {
+ std::string ns{"preprocessing"};
+ updateParam<bool>(parameters, ns + ".force_zero_initial_deviation", p->force_zero_deviation);
+ updateParam<bool>(parameters, ns + ".force_zero_initial_heading", p->force_zero_heading);
+ updateParam<bool>(parameters, ns + ".smooth_reference_trajectory", p->smooth_reference);
+ }
std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) {
if (!observer.expired()) {
From 439c4a0b39259f877dddd825f57e7caf9e536b8f Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 15 Nov 2023 09:14:46 +0900
Subject: [PATCH 027/115] WIP transform frenet to pathwithlaneid
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 4 ++
.../sampling_planner_module.cpp | 69 +++++++++++++++++--
2 files changed, 67 insertions(+), 6 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 9ad2077af47da..12c00e83a66ea 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -23,6 +23,7 @@
#include "behavior_path_planner/utils/utils.hpp"
#include "bezier_sampler/bezier_sampling.hpp"
#include "frenet_planner/frenet_planner.hpp"
+#include "lanelet2_extension/utility/utilities.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sampler_common/constraints/hard_constraint.hpp"
#include "sampler_common/constraints/soft_constraint.hpp"
@@ -151,6 +152,9 @@ class SamplingPlannerModule : public SceneModuleInterface
const sampler_common::transform::Spline2D & path_spline,
const SamplingPlannerInternalParameters & internal_params_);
+ PathWithLaneId convertFrenetPathToPathWithLaneId(
+ const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
+ const double velocity);
// member
// std::shared_ptr<SamplingPlannerParameters> params_;
std::shared_ptr<SamplingPlannerInternalParameters> internal_params_;
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 3dc7428bab0c3..cdb765b0bd63e 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -69,6 +69,55 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData(const PlanResult &
return data;
}
+PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneId(
+ const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
+ const double velocity)
+{
+ auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion {
+ tf2::Quaternion quaternion_tf2;
+ quaternion_tf2.setRPY(roll, pitch, yaw);
+ return quaternion_tf2;
+ };
+
+ // auto copy_point_information = []()
+
+ PathWithLaneId path;
+ const auto header = planner_data_->route_handler->getRouteHeader();
+ for (size_t i = 0; i < frenet_path.points.size(); ++i) {
+ const auto & frenet_path_point_position = frenet_path.points.at(i);
+ const auto & frenet_path_point_yaw = frenet_path.yaws.at(i);
+ const auto & frenet_path_point_velocity = frenet_path.points.;
+
+ PathPointWithLaneId point{};
+ point.point.pose.position.x = frenet_path_point_position.x();
+ point.point.pose.position.y = frenet_path_point_position.z();
+ point.point.pose.position.z = 0.0;
+
+ auto yaw_as_quaternion = quaternion_from_rpy(0.0, 0.0, frenet_path_point_yaw);
+ point.point.pose.orientation.w = yaw_as_quaternion.getW();
+ point.point.pose.orientation.x = yaw_as_quaternion.getX();
+ point.point.pose.orientation.y = yaw_as_quaternion.getY();
+ point.point.pose.orientation.z = yaw_as_quaternion.getZ();
+
+ // put the lane that contain waypoints in lane_ids.
+ bool is_in_lanes = false;
+ for (const auto & lane : lanelets) {
+ if (lanelet::utils::isInLanelet(point.point.pose, lane)) {
+ point.lane_ids.push_back(lane.id());
+ is_in_lanes = true;
+ }
+ }
+ // If none of them corresponds, assign the previous lane_ids.
+ if (!is_in_lanes && i > 0) {
+ point.lane_ids = path.points.at(i - 1).lane_ids;
+ }
+
+ point.point.longitudinal_velocity_mps = velocity;
+ path.points.push_back(point);
+ }
+ return path;
+}
+
BehaviorModuleOutput SamplingPlannerModule::plan()
{
// const auto refPath = getPreviousModuleOutput().reference_path;
@@ -135,12 +184,20 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
debug_data_.obstacles = internal_params_->constraints.obstacle_polygons;
updateDebugMarkers();
- auto p = getPreviousModuleOutput().reference_path;
- BehaviorModuleOutput out;
- out.path = p;
- out.reference_path = p;
- out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
- return out;
+ if (frenet_paths.size() < 1) {
+ auto p = getPreviousModuleOutput().reference_path;
+ BehaviorModuleOutput out;
+ out.path = p;
+ out.reference_path = p;
+ out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
+ return out;
+ }
+
+ const double max_length = *std::max_element(
+ internal_params_->sampling.target_lengths.begin(),
+ internal_params_->sampling.target_lengths.end());
+ const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_, 0, max_length, false);
+ auto out_path = convertFrenetPathToPathWithLaneId(frenet_paths[0], road_lanes);
}
void SamplingPlannerModule::updateDebugMarkers()
From 315adae4a18f4a8f68d21ac87ca5d71fa8c9ec08 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 15 Nov 2023 12:23:43 +0900
Subject: [PATCH 028/115] set frenet path as output
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 2 +-
.../scene_module/sampling_planner/manager.cpp | 2 +-
.../sampling_planner_module.cpp | 17 ++++++++++++-----
3 files changed, 14 insertions(+), 7 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 12c00e83a66ea..833b3b337a872 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -152,7 +152,7 @@ class SamplingPlannerModule : public SceneModuleInterface
const sampler_common::transform::Spline2D & path_spline,
const SamplingPlannerInternalParameters & internal_params_);
- PathWithLaneId convertFrenetPathToPathWithLaneId(
+ PathWithLaneId convertFrenetPathToPlanResult(
const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
const double velocity);
// member
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
index 845c38f62077e..4b047d6447474 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
@@ -97,7 +97,7 @@ void SamplingPlannerModuleManager::updateModuleParams(
parameters, ns + ".nb_target_lateral_positions", p->nb_target_lateral_positions);
updateParam<std::vector<double>>(parameters, ns + ".target_lengths", p->target_lengths);
- bool updated = updateParam<std::vector<double>>(
+ updateParam<std::vector<double>>(
parameters, ns + ".target_lateral_positions", p->target_lateral_positions);
ns += ".frenet";
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index cdb765b0bd63e..c4a2ab0ede9fb 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -69,7 +69,7 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData(const PlanResult &
return data;
}
-PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneId(
+PathWithLaneId SamplingPlannerModule::convertFrenetPathToPlanResult(
const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
const double velocity)
{
@@ -83,14 +83,14 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneId(
PathWithLaneId path;
const auto header = planner_data_->route_handler->getRouteHeader();
+
for (size_t i = 0; i < frenet_path.points.size(); ++i) {
const auto & frenet_path_point_position = frenet_path.points.at(i);
const auto & frenet_path_point_yaw = frenet_path.yaws.at(i);
- const auto & frenet_path_point_velocity = frenet_path.points.;
-
+ // const auto & frenet_path_point_velocity = frenet_path.points.;
PathPointWithLaneId point{};
point.point.pose.position.x = frenet_path_point_position.x();
- point.point.pose.position.y = frenet_path_point_position.z();
+ point.point.pose.position.y = frenet_path_point_position.y();
point.point.pose.position.z = 0.0;
auto yaw_as_quaternion = quaternion_from_rpy(0.0, 0.0, frenet_path_point_yaw);
@@ -193,11 +193,18 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
return out;
}
+ BehaviorModuleOutput out;
+ const double velocity = 0.5;
const double max_length = *std::max_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_, 0, max_length, false);
- auto out_path = convertFrenetPathToPathWithLaneId(frenet_paths[0], road_lanes);
+ auto out_path = convertFrenetPathToPlanResult(frenet_paths[0], road_lanes, velocity);
+ out.path = std::make_shared<PathWithLaneId>(out_path);
+ auto p = getPreviousModuleOutput().reference_path;
+ out.reference_path = p;
+ out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
+ return out;
}
void SamplingPlannerModule::updateDebugMarkers()
From 41d1583e62a1744874a4c37624b2b3f60c9ecfaf Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 15 Nov 2023 18:13:46 +0900
Subject: [PATCH 029/115] Added pruning to select the best frenet path
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 9 ++-
.../sampling_planner_module.cpp | 67 ++++++++++++++++---
2 files changed, 66 insertions(+), 10 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 833b3b337a872..a2dd877300ee0 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -30,6 +30,7 @@
#include "sampler_common/structures.hpp"
#include "sampler_common/transform/spline_transform.hpp"
#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"
#include "tier4_autoware_utils/system/stop_watch.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"
@@ -147,12 +148,18 @@ class SamplingPlannerModule : public SceneModuleInterface
void updateDebugMarkers();
+ void prepareConstraints(
+ sampler_common::Constraints & constraints,
+ const PredictedObjects::ConstSharedPtr & predicted_objects,
+ const std::vector<geometry_msgs::msg::Point> & left_bound,
+ const std::vector<geometry_msgs::msg::Point> & right_bound);
+
frenet_planner::SamplingParameters prepareSamplingParameters(
const sampler_common::State & initial_state,
const sampler_common::transform::Spline2D & path_spline,
const SamplingPlannerInternalParameters & internal_params_);
- PathWithLaneId convertFrenetPathToPlanResult(
+ PathWithLaneId convertFrenetPathToPathWithLaneID(
const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
const double velocity);
// member
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index c4a2ab0ede9fb..46b178f329397 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -69,7 +69,7 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData(const PlanResult &
return data;
}
-PathWithLaneId SamplingPlannerModule::convertFrenetPathToPlanResult(
+PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
const double velocity)
{
@@ -118,6 +118,36 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPlanResult(
return path;
}
+void SamplingPlannerModule::prepareConstraints(
+ sampler_common::Constraints & constraints,
+ const PredictedObjects::ConstSharedPtr & predicted_objects,
+ const std::vector<geometry_msgs::msg::Point> & left_bound,
+ const std::vector<geometry_msgs::msg::Point> & right_bound)
+{
+ constraints.obstacle_polygons = sampler_common::MultiPolygon2d();
+ for (const auto & o : predicted_objects->objects)
+ if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) // TODO(Maxime): param
+ constraints.obstacle_polygons.push_back(tier4_autoware_utils::toPolygon2d(o));
+
+ constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented
+
+ // TODO(Maxime): directly use lines instead of polygon
+
+ sampler_common::Polygon2d drivable_area_polygon;
+ for (const auto & p : right_bound) {
+ drivable_area_polygon.outer().emplace_back(p.x, p.y);
+ }
+ for (auto it = left_bound.rbegin(); it != left_bound.rend(); ++it)
+ drivable_area_polygon.outer().emplace_back(it->x, it->y);
+
+ if (drivable_area_polygon.outer().size() < 1) {
+ return;
+ }
+ drivable_area_polygon.outer().push_back(drivable_area_polygon.outer().front());
+
+ constraints.drivable_polygons = {drivable_area_polygon};
+}
+
BehaviorModuleOutput SamplingPlannerModule::plan()
{
// const auto refPath = getPreviousModuleOutput().reference_path;
@@ -154,14 +184,17 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::SamplingParameters sampling_parameters =
prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
+ const auto sampling_planner_data = createPlannerData(planner_data_->prev_output_path);
+
+ prepareConstraints(
+ internal_params_->constraints, planner_data_->dynamic_object, sampling_planner_data.left_bound,
+ sampling_planner_data.right_bound);
+
frenet_planner::FrenetState frenet_initial_state;
frenet_initial_state.position = initial_state.frenet;
auto frenet_paths =
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
- // After path generation we do pruning we then select the optimal path based on obstacles and
- // drivable area and copy to the expected output and finally copy the velocities from the ref path
- // RCLCPP_INFO(getLogger(), "Generated paths %ld", frenet_paths.size());
debug_data_.footprints.clear();
for (auto & path : frenet_paths) {
@@ -184,9 +217,23 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
debug_data_.obstacles = internal_params_->constraints.obstacle_polygons;
updateDebugMarkers();
- if (frenet_paths.size() < 1) {
- auto p = getPreviousModuleOutput().reference_path;
+ const auto best_path_idx = [](const auto & paths) {
+ auto min_cost = std::numeric_limits<double>::max();
+ size_t best_path_idx = 0;
+ for (auto i = 0LU; i < paths.size(); ++i) {
+ if (paths[i].constraint_results.isValid() && paths[i].cost < min_cost) {
+ best_path_idx = i;
+ min_cost = paths[i].cost;
+ }
+ }
+ return min_cost < std::numeric_limits<double>::max() ? std::optional<size_t>(best_path_idx)
+ : std::nullopt;
+ };
+ const auto selected_path_idx = best_path_idx(frenet_paths);
+
+ if (!selected_path_idx) {
BehaviorModuleOutput out;
+ const auto p = getPreviousModuleOutput().reference_path;
out.path = p;
out.reference_path = p;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
@@ -194,14 +241,16 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}
BehaviorModuleOutput out;
- const double velocity = 0.5;
+ const double velocity = 1.5;
const double max_length = *std::max_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_, 0, max_length, false);
- auto out_path = convertFrenetPathToPlanResult(frenet_paths[0], road_lanes, velocity);
+
+ const auto best_path = frenet_paths[*selected_path_idx];
+ const auto out_path = convertFrenetPathToPathWithLaneID(best_path, road_lanes, velocity);
out.path = std::make_shared<PathWithLaneId>(out_path);
- auto p = getPreviousModuleOutput().reference_path;
+ const auto p = getPreviousModuleOutput().reference_path;
out.reference_path = p;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
return out;
From 2b096abb6a1b96394798e312be547e0d7e4af189 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 16 Nov 2023 11:21:54 +0900
Subject: [PATCH 030/115] Initialize vehicle info
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../behavior_planning.launch.xml | 15 +++++++
.../sampling_planner_module.cpp | 39 +++++++++++--------
2 files changed, 38 insertions(+), 16 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 835dd4ff76cf8..e83c6ab8a13a8 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -9,6 +9,7 @@
<arg name="launch_avoidance_module" default="true"/>
<arg name="launch_avoidance_by_lane_change_module" default="true"/>
<arg name="launch_dynamic_avoidance_module" default="true"/>
+ <arg name="launch_sampling_planner_module" default="true"/>
<arg name="launch_lane_change_right_module" default="true"/>
<arg name="launch_lane_change_left_module" default="true"/>
<arg name="launch_external_request_lane_change_right_module" default="true"/>
@@ -192,6 +193,20 @@
<remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
<!-- params -->
+<<<<<<< HEAD
+=======
+ <param name="avoidance.enable_module" value="$(var launch_avoidance_module)"/>
+ <param name="avoidance_by_lc.enable_module" value="$(var launch_avoidance_by_lane_change_module)"/>
+ <param name="dynamic_avoidance.enable_module" value="$(var launch_dynamic_avoidance_module)"/>
+ <param name="sampling_planner.enable_module" value="$(var launch_sampling_planner_module)"/>
+ <param name="lane_change_right.enable_module" value="$(var launch_lane_change_right_module)"/>
+ <param name="lane_change_left.enable_module" value="$(var launch_lane_change_left_module)"/>
+ <param name="external_request_lane_change_left.enable_module" value="$(var launch_external_request_lane_change_left_module)"/>
+ <param name="external_request_lane_change_right.enable_module" value="$(var launch_external_request_lane_change_right_module)"/>
+ <param name="goal_planner.enable_module" value="$(var launch_goal_planner_module)"/>
+ <param name="start_planner.enable_module" value="$(var launch_start_planner_module)"/>
+ <param name="side_shift.enable_module" value="$(var launch_side_shift_module)"/>
+>>>>>>> 95ef8878ac (Initialize vehicle info)
<param from="$(var common_param_path)"/>
<param from="$(var vehicle_param_file)"/>
<param from="$(var nearest_search_param_path)"/>
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 46b178f329397..6872fcdb36b90 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -30,7 +30,8 @@ SamplingPlannerModule::SamplingPlannerModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<SamplingPlannerParameters> & parameters,
const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map}
+: SceneModuleInterface{name, node, rtc_interface_ptr_map},
+ vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}
{
internal_params_ =
std::shared_ptr<SamplingPlannerInternalParameters>(new SamplingPlannerInternalParameters{});
@@ -285,21 +286,27 @@ void SamplingPlannerModule::updateDebugMarkers()
info_marker_.markers.push_back(m);
++m.id;
}
- // m.ns = "footprint";
- // m.id = 0UL;
- // m.type = m.POINTS;
- // m.points.clear();
- // m.color.r = 1.0;
- // m.color.g = 0.0;
- // m.color.b = 1.0;
- // m.scale.y = 0.02;
- // if (!debug_data_.footprints.empty()) {
- // m.action = m.ADD;
- // for (const auto & p : debug_data_.footprints[debug_data_.footprints.size()])
- // m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
- // } else {
- // m.action = m.DELETE;
- // }
+ m.ns = "footprint";
+ m.id = 0UL;
+ m.type = m.POINTS;
+ m.points.clear();
+ m.color.a = 1.0;
+ m.color.r = 1.0;
+ m.color.g = 0.0;
+ m.color.b = 1.0;
+ m.scale.y = 0.2;
+ if (!debug_data_.footprints.empty()) {
+ m.action = m.ADD;
+ for (const auto & p : debug_data_.footprints[0]) {
+ m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ }
+
+ } else {
+ m.action = m.DELETE;
+ }
+ debug_marker_.markers.push_back(m);
+ info_marker_.markers.push_back(m);
+ ++m.id;
// m.ns = "debug_path";
// m.id = 0UL;
// m.type = m.POINTS;
From 14ccb9c51527fd803ea78c166803b0cc883f5006 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 16 Nov 2023 14:30:53 +0900
Subject: [PATCH 031/115] calculate properly right and left bound for drivable
area check
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 4 ++-
.../sampling_planner_module.cpp | 34 ++++++++++++++++---
2 files changed, 32 insertions(+), 6 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index a2dd877300ee0..0d895717c7dfb 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -136,7 +136,9 @@ class SamplingPlannerModule : public SceneModuleInterface
SamplingPlannerDebugData debug_data_;
private:
- SamplingPlannerData createPlannerData(const PlanResult & path);
+ SamplingPlannerData createPlannerData(
+ const PlanResult & path, const std::vector<geometry_msgs::msg::Point> & left_bound,
+ const std::vector<geometry_msgs::msg::Point> & right_bound);
PlanResult generatePath();
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 6872fcdb36b90..ca07f853c8c7e 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -56,16 +56,18 @@ bool SamplingPlannerModule::isExecutionReady() const
return true;
}
-SamplingPlannerData SamplingPlannerModule::createPlannerData(const PlanResult & path)
+SamplingPlannerData SamplingPlannerModule::createPlannerData(
+ const PlanResult & path, const std::vector<geometry_msgs::msg::Point> & left_bound,
+ const std::vector<geometry_msgs::msg::Point> & right_bound)
{
// create planner data
SamplingPlannerData data;
// planner_data.header = path.header;
auto points = path->points;
- data.left_bound = path->left_bound;
- data.right_bound = path->right_bound;
+ data.left_bound = left_bound;
+ data.right_bound = right_bound;
data.ego_pose = planner_data_->self_odometry->pose.pose;
- data.ego_vel = 0;
+ data.ego_vel = planner_data_->self_odometry->twist.twist.linear.x;
// data.ego_vel = ego_state_ptr_->twist.twist.linear.x;
return data;
}
@@ -185,7 +187,14 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::SamplingParameters sampling_parameters =
prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
- const auto sampling_planner_data = createPlannerData(planner_data_->prev_output_path);
+ const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes;
+ const auto left_bound =
+ (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, true));
+ const auto right_bound =
+ (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, false));
+
+ const auto sampling_planner_data =
+ createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
prepareConstraints(
internal_params_->constraints, planner_data_->dynamic_object, sampling_planner_data.left_bound,
@@ -198,13 +207,28 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
debug_data_.footprints.clear();
+ bool checks_drivable = false;
+ bool checks_collision = false;
+ bool checks_curvature = false;
+
for (auto & path : frenet_paths) {
const auto footprint =
sampler_common::constraints::checkHardConstraints(path, internal_params_->constraints);
debug_data_.footprints.push_back(footprint);
sampler_common::constraints::calculateCost(
path, internal_params_->constraints, reference_spline);
+ checks_drivable = checks_drivable || path.constraint_results.drivable_area;
+ checks_collision = checks_collision || path.constraint_results.collision;
+ checks_curvature = checks_curvature || path.constraint_results.curvature;
}
+ std::string pass_driveable = checks_drivable ? "PASSED drivable \n" : " NOT PASSED drivable\n";
+ std::cerr << pass_driveable;
+
+ std::string pass_collsion = checks_collision ? "PASSED collision \n" : " NOT PASSED collided\n";
+ std::cerr << pass_collsion;
+ std::string pass_curvature = checks_curvature ? "PASSED curves\n" : " NOT PASSED curves\n";
+ std::cerr << pass_curvature;
+
std::vector<sampler_common::Path> candidate_paths;
const auto move_to_paths = [&candidate_paths](auto & paths_to_move) {
candidate_paths.insert(
From b384a01d6fdd83eade9760098d43abbebe105f50 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 16 Nov 2023 15:16:07 +0900
Subject: [PATCH 032/115] remove debug prints and increase vehicle margin,
should be param
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner/sampling_planner_module.hpp | 2 +-
.../sampling_planner/sampling_planner_module.cpp | 15 ---------------
2 files changed, 1 insertion(+), 16 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 0d895717c7dfb..011bfdcaa9e10 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -97,7 +97,7 @@ class SamplingPlannerModule : public SceneModuleInterface
user_params_->lateral_deviation_weight;
internal_params_->constraints.soft.length_weight = user_params_->length_weight;
internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
- internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint();
+ internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.5);
internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
// Sampling
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index ca07f853c8c7e..7b13ee853f31f 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -207,28 +207,13 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
debug_data_.footprints.clear();
- bool checks_drivable = false;
- bool checks_collision = false;
- bool checks_curvature = false;
-
for (auto & path : frenet_paths) {
const auto footprint =
sampler_common::constraints::checkHardConstraints(path, internal_params_->constraints);
debug_data_.footprints.push_back(footprint);
sampler_common::constraints::calculateCost(
path, internal_params_->constraints, reference_spline);
- checks_drivable = checks_drivable || path.constraint_results.drivable_area;
- checks_collision = checks_collision || path.constraint_results.collision;
- checks_curvature = checks_curvature || path.constraint_results.curvature;
}
- std::string pass_driveable = checks_drivable ? "PASSED drivable \n" : " NOT PASSED drivable\n";
- std::cerr << pass_driveable;
-
- std::string pass_collsion = checks_collision ? "PASSED collision \n" : " NOT PASSED collided\n";
- std::cerr << pass_collsion;
- std::string pass_curvature = checks_curvature ? "PASSED curves\n" : " NOT PASSED curves\n";
- std::cerr << pass_curvature;
-
std::vector<sampler_common::Path> candidate_paths;
const auto move_to_paths = [&candidate_paths](auto & paths_to_move) {
candidate_paths.insert(
From 5d5f5d64639304077404312c12dae73e83220459 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 20 Nov 2023 17:12:43 +0900
Subject: [PATCH 033/115] param changes for driving in small lanes
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../scene_module/sampling_planner/sampling_planner_module.hpp | 2 +-
.../scene_module/sampling_planner/sampling_planner_module.cpp | 2 ++
2 files changed, 3 insertions(+), 1 deletion(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 011bfdcaa9e10..93595f8c8490e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -97,7 +97,7 @@ class SamplingPlannerModule : public SceneModuleInterface
user_params_->lateral_deviation_weight;
internal_params_->constraints.soft.length_weight = user_params_->length_weight;
internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
- internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.5);
+ internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.0);
internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
// Sampling
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 7b13ee853f31f..c182c605fe915 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -207,9 +207,11 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
debug_data_.footprints.clear();
+
for (auto & path : frenet_paths) {
const auto footprint =
sampler_common::constraints::checkHardConstraints(path, internal_params_->constraints);
+ path.constraint_results.curvature = true;
debug_data_.footprints.push_back(footprint);
sampler_common::constraints::calculateCost(
path, internal_params_->constraints, reference_spline);
From c040f96be28db329be071ba183906e555da384d1 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 22 Nov 2023 09:12:11 +0900
Subject: [PATCH 034/115] WIP add drivable area expansion from LC
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 2 ++
.../sampling_planner_module.cpp | 23 +++++++++++++++++++
2 files changed, 25 insertions(+)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 93595f8c8490e..24474943657f0 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -164,6 +164,8 @@ class SamplingPlannerModule : public SceneModuleInterface
PathWithLaneId convertFrenetPathToPathWithLaneID(
const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
const double velocity);
+
+ void extendOutputDrivableArea(BehaviorModuleOutput & output);
// member
// std::shared_ptr<SamplingPlannerParameters> params_;
std::shared_ptr<SamplingPlannerInternalParameters> internal_params_;
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index c182c605fe915..ebc9432b1119b 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -358,6 +358,29 @@ void SamplingPlannerModule::updateDebugMarkers()
// }
}
+lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
+{
+ return utils::getCurrentLanesFromPath(prev_module_path_, planner_data_);
+}
+
+void SamplingPlannerModule::extendOutputDrivableArea(BehaviorModuleOutput & output)
+{
+ const auto & dp = planner_data_->drivable_area_expansion_parameters;
+
+ const auto drivable_lanes = utils::lane_change::generateDrivableLanes(
+ *planner_data_->route_handler, status_.current_lanes, status_.target_lanes);
+ const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes);
+ const auto expanded_lanes = utils::expandLanelets(
+ shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
+ dp.drivable_area_types_to_skip);
+
+ // for new architecture
+ DrivableAreaInfo current_drivable_area_info;
+ current_drivable_area_info.drivable_lanes = expanded_lanes;
+ output.drivable_area_info =
+ utils::combineDrivableAreaInfo(current_drivable_area_info, prev_drivable_area_info_);
+}
+
CandidateOutput SamplingPlannerModule::planCandidate() const
{
return {};
From 937caa287bddcf1b3381b35c4839c489a54c2786 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 22 Nov 2023 16:58:28 +0900
Subject: [PATCH 035/115] add drivable area expansion
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 11 +-
.../sampling_planner_module.cpp | 207 ++++++++++++++++--
2 files changed, 198 insertions(+), 20 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 24474943657f0..001e6f18cef68 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -97,7 +97,7 @@ class SamplingPlannerModule : public SceneModuleInterface
user_params_->lateral_deviation_weight;
internal_params_->constraints.soft.length_weight = user_params_->length_weight;
internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
- internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.0);
+ internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.5);
internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
// Sampling
@@ -165,11 +165,18 @@ class SamplingPlannerModule : public SceneModuleInterface
const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
const double velocity);
- void extendOutputDrivableArea(BehaviorModuleOutput & output);
// member
// std::shared_ptr<SamplingPlannerParameters> params_;
std::shared_ptr<SamplingPlannerInternalParameters> internal_params_;
vehicle_info_util::VehicleInfo vehicle_info_{};
+
+ // move to utils
+
+ void extendOutputDrivableArea(BehaviorModuleOutput & output);
+ bool isEndPointsConnected(
+ const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane);
+ DrivableLanes generateExpandDrivableLanes(
+ const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data);
};
} // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index ebc9432b1119b..54ac06b5a40b7 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -187,7 +187,17 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::SamplingParameters sampling_parameters =
prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
- const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes;
+ // const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes;
+
+ const auto prev_module_path = getPreviousModuleOutput().path;
+ const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+
+ std::vector<DrivableLanes> drivable_lanes{};
+ // expand drivable lanes
+ std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
+ drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
+ });
+
const auto left_bound =
(utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, true));
const auto right_bound =
@@ -265,6 +275,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
const auto p = getPreviousModuleOutput().reference_path;
out.reference_path = p;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
+ extendOutputDrivableArea(out);
return out;
}
@@ -358,27 +369,35 @@ void SamplingPlannerModule::updateDebugMarkers()
// }
}
-lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
-{
- return utils::getCurrentLanesFromPath(prev_module_path_, planner_data_);
-}
+// lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
+// {
+// return utils::getCurrentLanesFromPath(prev_module_path_, planner_data_);
+// }
void SamplingPlannerModule::extendOutputDrivableArea(BehaviorModuleOutput & output)
{
- const auto & dp = planner_data_->drivable_area_expansion_parameters;
-
- const auto drivable_lanes = utils::lane_change::generateDrivableLanes(
- *planner_data_->route_handler, status_.current_lanes, status_.target_lanes);
- const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes);
- const auto expanded_lanes = utils::expandLanelets(
- shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
- dp.drivable_area_types_to_skip);
-
- // for new architecture
+ const auto prev_module_path = getPreviousModuleOutput().path;
+ const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+
+ std::vector<DrivableLanes> drivable_lanes{};
+ // expand drivable lanes
+ std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
+ drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
+ });
+
+ // // const auto drivable_lanes = utils::lane_change::generateDrivableLanes(
+ // // *planner_data_->route_handler, current_lanes, status_.target_lanes);
+ // const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(current_lanes);
+ // const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes);
+ // const auto expanded_lanes = utils::expandLanelets(
+ // shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
+ // dp.drivable_area_types_to_skip);
+
+ // // for new architecture
DrivableAreaInfo current_drivable_area_info;
- current_drivable_area_info.drivable_lanes = expanded_lanes;
- output.drivable_area_info =
- utils::combineDrivableAreaInfo(current_drivable_area_info, prev_drivable_area_info_);
+ current_drivable_area_info.drivable_lanes = drivable_lanes;
+ output.drivable_area_info = utils::combineDrivableAreaInfo(
+ current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
}
CandidateOutput SamplingPlannerModule::planCandidate() const
@@ -390,6 +409,158 @@ void SamplingPlannerModule::updateData()
{
}
+// utils
+
+template <typename T>
+void pushUniqueVector(T & base_vector, const T & additional_vector)
+{
+ base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
+}
+
+bool SamplingPlannerModule::isEndPointsConnected(
+ const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane)
+{
+ const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint();
+ const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint();
+
+ constexpr double epsilon = 1e-5;
+ return (right_back_point_2d - left_back_point_2d).norm() < epsilon;
+}
+
+DrivableLanes SamplingPlannerModule::generateExpandDrivableLanes(
+ const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data)
+{
+ const auto & route_handler = planner_data->route_handler;
+
+ DrivableLanes current_drivable_lanes;
+ current_drivable_lanes.left_lane = lanelet;
+ current_drivable_lanes.right_lane = lanelet;
+
+ // 1. get left/right side lanes
+ const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
+ const auto all_left_lanelets =
+ route_handler->getAllLeftSharedLinestringLanelets(target_lane, true, true);
+ if (!all_left_lanelets.empty()) {
+ current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet
+ pushUniqueVector(
+ current_drivable_lanes.middle_lanes,
+ lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1));
+ }
+ };
+ const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
+ const auto all_right_lanelets =
+ route_handler->getAllRightSharedLinestringLanelets(target_lane, true, true);
+ if (!all_right_lanelets.empty()) {
+ current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet
+ pushUniqueVector(
+ current_drivable_lanes.middle_lanes,
+ lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1));
+ }
+ };
+
+ update_left_lanelets(lanelet);
+ update_right_lanelets(lanelet);
+
+ // 2.1 when there are multiple lanes whose previous lanelet is the same
+ const auto get_next_lanes_from_same_previous_lane =
+ [&route_handler](const lanelet::ConstLanelet & lane) {
+ // get previous lane, and return false if previous lane does not exist
+ lanelet::ConstLanelets prev_lanes;
+ if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) {
+ return lanelet::ConstLanelets{};
+ }
+
+ lanelet::ConstLanelets next_lanes;
+ for (const auto & prev_lane : prev_lanes) {
+ const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane);
+ pushUniqueVector(next_lanes, next_lanes_from_prev);
+ }
+ return next_lanes;
+ };
+
+ const auto next_lanes_for_right =
+ get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane);
+ const auto next_lanes_for_left =
+ get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane);
+
+ // 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line
+ // of the original lane
+ const auto update_drivable_lanes =
+ [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) {
+ for (const auto & next_lane : next_lanes) {
+ const auto & edge_lane =
+ is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane;
+ if (next_lane.id() == edge_lane.id()) {
+ continue;
+ }
+
+ const auto & left_lane = is_left ? next_lane : edge_lane;
+ const auto & right_lane = is_left ? edge_lane : next_lane;
+ if (!isEndPointsConnected(left_lane, right_lane)) {
+ continue;
+ }
+
+ if (is_left) {
+ current_drivable_lanes.left_lane = next_lane;
+ } else {
+ current_drivable_lanes.right_lane = next_lane;
+ }
+
+ const auto & middle_lanes = current_drivable_lanes.middle_lanes;
+ const auto has_same_lane = std::any_of(
+ middle_lanes.begin(), middle_lanes.end(),
+ [&edge_lane](const auto & lane) { return lane.id() == edge_lane.id(); });
+
+ if (!has_same_lane) {
+ if (is_left) {
+ if (current_drivable_lanes.right_lane.id() != edge_lane.id()) {
+ current_drivable_lanes.middle_lanes.push_back(edge_lane);
+ }
+ } else {
+ if (current_drivable_lanes.left_lane.id() != edge_lane.id()) {
+ current_drivable_lanes.middle_lanes.push_back(edge_lane);
+ }
+ }
+ }
+
+ return true;
+ }
+ return false;
+ };
+
+ const auto expand_drivable_area_recursively =
+ [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) {
+ // NOTE: set max search num to avoid infinity loop for drivable area expansion
+ constexpr size_t max_recursive_search_num = 3;
+ for (size_t i = 0; i < max_recursive_search_num; ++i) {
+ const bool is_update_kept = update_drivable_lanes(next_lanes, is_left);
+ if (!is_update_kept) {
+ break;
+ }
+ if (i == max_recursive_search_num - 1) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
+ "Drivable area expansion reaches max iteration.");
+ }
+ }
+ };
+ expand_drivable_area_recursively(next_lanes_for_right, false);
+ expand_drivable_area_recursively(next_lanes_for_left, true);
+
+ // 3. update again for new left/right lanes
+ update_left_lanelets(current_drivable_lanes.left_lane);
+ update_right_lanelets(current_drivable_lanes.right_lane);
+
+ // 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes.
+ if (
+ current_drivable_lanes.left_lane.id() != lanelet.id() &&
+ current_drivable_lanes.right_lane.id() != lanelet.id()) {
+ current_drivable_lanes.middle_lanes.push_back(lanelet);
+ }
+
+ return current_drivable_lanes;
+}
+
frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters(
const sampler_common::State & initial_state,
const sampler_common::transform::Spline2D & path_spline,
From 5d93c2ea8271ab72feb3ecef92f8360801849fc1 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 22 Nov 2023 18:37:15 +0900
Subject: [PATCH 036/115] add driveable area
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner/sampling_planner_module.cpp | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 54ac06b5a40b7..a5788f91e75fa 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -105,6 +105,10 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
// put the lane that contain waypoints in lane_ids.
bool is_in_lanes = false;
for (const auto & lane : lanelets) {
+ const auto closest_point =
+ lanelet::utils::getClosestCenterPose(lane, point.point.pose.position);
+ point.point.pose.position.z = closest_point.position.z;
+
if (lanelet::utils::isInLanelet(point.point.pose, lane)) {
point.lane_ids.push_back(lane.id());
is_in_lanes = true;
@@ -176,6 +180,9 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}();
const auto pose = planner_data_->self_odometry->pose.pose;
+ lanelet::ConstLanelet closest_lanelet;
+ planner_data_->route_handler->getClosestLaneletWithinRoute(pose, &closest_lanelet);
+
sampler_common::State initial_state;
Point2d initial_state_pose{pose.position.x, pose.position.y};
const auto rpy =
From ca7b5ee25b1e2fd1f9c111876d9cc2c32e89f5bc Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Fri, 24 Nov 2023 08:56:55 +0900
Subject: [PATCH 037/115] Make the points on the path have the same z as goal
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.cpp | 22 ++++++++++++++-----
1 file changed, 16 insertions(+), 6 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index a5788f91e75fa..011557cc16456 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -105,13 +105,10 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
// put the lane that contain waypoints in lane_ids.
bool is_in_lanes = false;
for (const auto & lane : lanelets) {
- const auto closest_point =
- lanelet::utils::getClosestCenterPose(lane, point.point.pose.position);
- point.point.pose.position.z = closest_point.position.z;
-
if (lanelet::utils::isInLanelet(point.point.pose, lane)) {
point.lane_ids.push_back(lane.id());
is_in_lanes = true;
+ std::cerr << "IS IN LANES\n";
}
}
// If none of them corresponds, assign the previous lane_ids.
@@ -274,10 +271,23 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
const double max_length = *std::max_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
- const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_, 0, max_length, false);
+ const auto road_lanes =
+ utils::getExtendedCurrentLanes(planner_data_, max_length, max_length, false);
const auto best_path = frenet_paths[*selected_path_idx];
- const auto out_path = convertFrenetPathToPathWithLaneID(best_path, road_lanes, velocity);
+ auto out_path = convertFrenetPathToPathWithLaneID(best_path, road_lanes, velocity);
+ const auto goal_pose = planner_data_->route_handler->getGoalPose();
+ for (auto & p : out_path.points) {
+ // lanelet::ConstLanelet closest_lanelet{};
+ // if (!planner_data_->route_handler->getClosestLaneletWithinRoute(
+ // p.point.pose, &closest_lanelet)) {
+ // continue;
+ // }
+ // const auto lane_pose =
+ // lanelet::utils::getClosestCenterPose(closest_lanelet, p.point.pose.position);
+ p.point.pose.position.z = goal_pose.position.z;
+ }
+
out.path = std::make_shared<PathWithLaneId>(out_path);
const auto p = getPreviousModuleOutput().reference_path;
out.reference_path = p;
From 2dea7c737fa2d4957bccea2f97c4d1ae33436c8d Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Fri, 24 Nov 2023 09:23:00 +0900
Subject: [PATCH 038/115] remove print, changes
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner/sampling_planner_module.hpp | 2 +-
.../sampling_planner/sampling_planner_module.cpp | 10 +---------
2 files changed, 2 insertions(+), 10 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 001e6f18cef68..8ad7ef0a1bb4c 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -97,7 +97,7 @@ class SamplingPlannerModule : public SceneModuleInterface
user_params_->lateral_deviation_weight;
internal_params_->constraints.soft.length_weight = user_params_->length_weight;
internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
- internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.5);
+ internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.25);
internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
// Sampling
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 011557cc16456..8f11cef192f7e 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -108,7 +108,6 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
if (lanelet::utils::isInLanelet(point.point.pose, lane)) {
point.lane_ids.push_back(lane.id());
is_in_lanes = true;
- std::cerr << "IS IN LANES\n";
}
}
// If none of them corresponds, assign the previous lane_ids.
@@ -259,7 +258,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
if (!selected_path_idx) {
BehaviorModuleOutput out;
- const auto p = getPreviousModuleOutput().reference_path;
+ const auto p = getPreviousModuleOutput().path;
out.path = p;
out.reference_path = p;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
@@ -278,13 +277,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
auto out_path = convertFrenetPathToPathWithLaneID(best_path, road_lanes, velocity);
const auto goal_pose = planner_data_->route_handler->getGoalPose();
for (auto & p : out_path.points) {
- // lanelet::ConstLanelet closest_lanelet{};
- // if (!planner_data_->route_handler->getClosestLaneletWithinRoute(
- // p.point.pose, &closest_lanelet)) {
- // continue;
- // }
- // const auto lane_pose =
- // lanelet::utils::getClosestCenterPose(closest_lanelet, p.point.pose.position);
p.point.pose.position.z = goal_pose.position.z;
}
From c40bc01ea765f17b70b029caed7a04fc4de69e16 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 24 Nov 2023 11:59:03 +0900
Subject: [PATCH 039/115] WIP add prev sampling path to calculation
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner/sampling_planner_module.hpp | 2 +-
.../sampling_planner/sampling_planner_module.cpp | 7 +++++++
2 files changed, 8 insertions(+), 1 deletion(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 8ad7ef0a1bb4c..92f10de023a3a 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -169,7 +169,7 @@ class SamplingPlannerModule : public SceneModuleInterface
// std::shared_ptr<SamplingPlannerParameters> params_;
std::shared_ptr<SamplingPlannerInternalParameters> internal_params_;
vehicle_info_util::VehicleInfo vehicle_info_{};
-
+ std::optional<frenet_planner::Path> prev_sampling_path_ = std::nullopt;
// move to utils
void extendOutputDrivableArea(BehaviorModuleOutput & output);
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 8f11cef192f7e..e80a70561ce0e 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -221,6 +221,11 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
debug_data_.footprints.clear();
+ if (prev_sampling_path_) {
+ const auto prev_path = prev_sampling_path_.value();
+ frenet_paths.push_back(prev_path);
+ }
+
for (auto & path : frenet_paths) {
const auto footprint =
sampler_common::constraints::checkHardConstraints(path, internal_params_->constraints);
@@ -237,6 +242,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
};
move_to_paths(frenet_paths);
+
debug_data_.previous_sampled_candidates_nb = debug_data_.sampled_candidates.size();
debug_data_.sampled_candidates = candidate_paths;
debug_data_.obstacles = internal_params_->constraints.obstacle_polygons;
@@ -274,6 +280,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
utils::getExtendedCurrentLanes(planner_data_, max_length, max_length, false);
const auto best_path = frenet_paths[*selected_path_idx];
+ prev_sampling_path_ = best_path;
auto out_path = convertFrenetPathToPathWithLaneID(best_path, road_lanes, velocity);
const auto goal_pose = planner_data_->route_handler->getGoalPose();
for (auto & p : out_path.points) {
From ce930d00e2efd35b9225f6d2883010a76272cceb Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 27 Nov 2023 17:57:08 +0900
Subject: [PATCH 040/115] WIP constraints handler
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner/constraints_checker.hpp | 114 ++++++++++++++++++
.../sampling_planner_module.hpp | 3 +
.../utils/sampling_planner/util.hpp | 55 +++++++++
.../sampling_planner_module.cpp | 18 ++-
.../include/sampler_common/structures.hpp | 1 +
5 files changed, 188 insertions(+), 3 deletions(-)
create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/constraints_checker.hpp
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/constraints_checker.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/constraints_checker.hpp
new file mode 100644
index 0000000000000..056f0745f317c
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/constraints_checker.hpp
@@ -0,0 +1,114 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__CONSTRAINTS_CHECKER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__CONSTRAINTS_CHECKER_HPP_
+
+#include "sampler_common/structures.hpp"
+
+#include <any>
+#include <functional>
+#include <vector>
+
+using SoftConstraintsFunctionVector = std::vector<std::function<double>(
+ const sampler_common::Path &, const sampler_common::Constraints &)>;
+
+using HardConstraintsFunctionVector = std::vector<std::function<bool>(
+ const sampler_common::Path &, const sampler_common::Constraints &)>;
+
+double checkSoftConstraints(
+ const sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ const SoftConstraintsFunctionVector & soft_constraints, std::vector<double> & constraints_results)
+{
+ std::vector<double> constraints_evaluations(soft_constraints.size(), false);
+ double constraints_evaluation = 0.0;
+ for (const auto & f : soft_constraints) {
+ constraints_evaluation += f(path, constraints);
+ }
+ return constraints_evaluation;
+}
+
+bool checkHardConstraints(
+ const sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ const HardConstraintsFunctionVector & hard_constraints, std::vector<double> & constraints_results)
+{
+ std::vector<double> constraints_evaluations(hard_constraints.size(), false);
+ bool constraints_evaluation = true;
+ for (const auto & f : hard_constraints) {
+ constraints_evaluation &= f(path, constraints);
+ }
+ return constraints_evaluation;
+}
+
+// template <typename T>
+// class ConstraintsChecker
+// {
+// private:
+// /* data */
+// public:
+// ConstraintsChecker(ConstraintsFunctionVector constraints_functions)
+// : constraints_functions_(constraints_functions){};
+// ~ConstraintsChecker(){};
+// ConstraintsFunctionVector<T> constraints_functions_;
+
+// T checkConstraints(
+// const sampler_common::Path & path, const sampler_common::Constraints & constraints,
+// std::vector<double> & constraints_results) = 0;
+
+// template <>
+// double checkConstraints(
+// const sampler_common::Path & path, const sampler_common::Constraints & constraints,
+// std::vector<double> & constraints_results)
+// {
+// std::vector<double> constraints_evaluations(constraints_functions_.size(), false);
+// double constraints_evaluation = 0.0;
+// for (const auto & f : constraints_functions_) {
+// constraints_evaluation += f(path, constraints);
+// }
+// return constraints_evaluation;
+// }
+
+// template <>
+// bool checkConstraints(
+// const sampler_common::Path & path, const sampler_common::Constraints & constraints,
+// std::vector<bool> & constraints_results)
+// {
+// std::vector<bool> constraints_evaluations(constraints_functions_.size(), false);
+// bool all_constraints_passed = true;
+// for (const auto & f : constraints_functions_) {
+// all_constraints_passed &= f(path, constraints);
+// }
+// return all_constraints_passed;
+// }
+// };
+
+// class HardConstraintsChecker : public ConstraintsChecker
+// {
+// private:
+// /* data */
+// public:
+// HardConstraintsChecker(/* args */){};
+// ~HardConstraintsChecker(){};
+// };
+
+// class SoftConstraintsChecker : public ConstraintsChecker
+// {
+// private:
+// /* data */
+// public:
+// SoftConstraintsChecker(/* args */){};
+// ~SoftConstraintsChecker(){};
+// };
+
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__CONSTRAINTS_CHECKER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 92f10de023a3a..a8cc742f21462 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -25,6 +25,7 @@
#include "frenet_planner/frenet_planner.hpp"
#include "lanelet2_extension/utility/utilities.hpp"
#include "rclcpp/rclcpp.hpp"
+#include "sampler_common/constraints/footprint.hpp"
#include "sampler_common/constraints/hard_constraint.hpp"
#include "sampler_common/constraints/soft_constraint.hpp"
#include "sampler_common/structures.hpp"
@@ -134,6 +135,8 @@ class SamplingPlannerModule : public SceneModuleInterface
}
SamplingPlannerDebugData debug_data_;
+ behavior_path_planner::HardConstraintsFunctionVector hard_constraints_;
+ behavior_path_planner::SoftConstraintsFunctionVector soft_constraints_;
private:
SamplingPlannerData createPlannerData(
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index 230cbaa90ce88..d779ef746b733 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -14,10 +14,65 @@
#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
#define BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
+#include "sampler_common/structures.hpp"
+
+#include <any>
+#include <functional>
+#include <vector>
namespace behavior_path_planner
{
+struct HardConstraintsInput
+{
+};
+
+struct HardConstraintsOutput
+{
+ bool constraints_satisfied;
+ MultiPoint2d footprint;
+};
+
+using SoftConstraintsFunctionVector = std::vector<std::function<double>(
+ const sampler_common::Path &, const sampler_common::Constraints &)>;
+
+using HardConstraintsFunctionVector = std::vector<std::function<bool>(
+ const sampler_common::Path &, const sampler_common::Constraints &,
+ std::optional<MultiPoint2d> &)>;
+
+double checkSoftConstraints2(
+ const sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ const SoftConstraintsFunctionVector & soft_constraints, std::vector<double> & constraints_results)
+{
+ constraints_results.clear();
+ double constraints_evaluation = 0.0;
+ for (auto f : soft_constraints) {
+ const auto value = f(path, constraints);
+ constraints_results.push_back(value);
+ constraints_evaluation += value;
+ }
+ return constraints_evaluation;
+}
+
+void checkHardConstraints2(
+ const sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints,
+ std::vector<bool> & constraints_results)
+{
+ constraints_results.resize(hard_constraints.size());
+ bool constraints_passed = true;
+ int idx = 0;
+
+ for (const auto & f : hard_constraints) {
+ const bool constraint_check = f(path, constraints, footprint);
+ constraints_passed &= constraint_check;
+ constraints_results[idx] = constraint_check;
+ ++idx;
+ }
+ path.constraints_satisfied = constraints_passed;
+ return;
+}
+
} // namespace behavior_path_planner
#endif // BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index e80a70561ce0e..784e5305d98f4 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -36,6 +36,18 @@ SamplingPlannerModule::SamplingPlannerModule(
internal_params_ =
std::shared_ptr<SamplingPlannerInternalParameters>(new SamplingPlannerInternalParameters{});
updateModuleParams(parameters);
+
+ hard_constraints_.emplace_back(
+ [](const sampler_common::Path &, const sampler_common::Constraints &) -> bool {
+ const auto footprint = sampler_common::constraints::buildFootprintPoints(path, constraints);
+ if (!footprint.empty()) {
+ if (!boost::geometry::within(footprint, constraints.drivable_polygons)) {
+ path.constraint_results.drivable_area = false;
+ }
+ }
+ path.constraint_results.collision = !has_collision(footprint, constraints.obstacle_polygons);
+ return path.constraint_results.collision && path.constraint_results.drivable_area;
+ });
}
bool SamplingPlannerModule::isExecutionRequested() const
@@ -227,8 +239,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}
for (auto & path : frenet_paths) {
- const auto footprint =
- sampler_common::constraints::checkHardConstraints(path, internal_params_->constraints);
+ const auto footprint = buildFootprintPoints(path, internal_params_->constraints);
+ checkHardConstraints2(path, internal_params_->constraints, footprint);
path.constraint_results.curvature = true;
debug_data_.footprints.push_back(footprint);
sampler_common::constraints::calculateCost(
@@ -252,7 +264,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
auto min_cost = std::numeric_limits<double>::max();
size_t best_path_idx = 0;
for (auto i = 0LU; i < paths.size(); ++i) {
- if (paths[i].constraint_results.isValid() && paths[i].cost < min_cost) {
+ if (paths[i].constraints_satisfied && paths[i].cost < min_cost) {
best_path_idx = i;
min_cost = paths[i].cost;
}
diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
index 1cb14fdf56198..4bcb5f6a61079 100644
--- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
+++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
@@ -79,6 +79,7 @@ struct Path
std::vector<double> curvatures{};
std::vector<double> yaws{};
std::vector<double> lengths{};
+ bool constraints_satisfied;
ConstraintResults constraint_results{};
double cost{};
std::string tag{}; // string tag used for debugging
From d65665973ae67406bc1dba846ff3e635ef6b85bc Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Tue, 28 Nov 2023 16:58:41 +0900
Subject: [PATCH 041/115] Add modifiable hard constraints checking function
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../utils/sampling_planner/util.hpp | 20 ++++++++--------
.../sampling_planner_module.cpp | 23 ++++++++++++-------
.../constraints/hard_constraint.hpp | 3 +++
3 files changed, 28 insertions(+), 18 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index d779ef746b733..bd969ee196b39 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -33,20 +33,19 @@ struct HardConstraintsOutput
MultiPoint2d footprint;
};
-using SoftConstraintsFunctionVector = std::vector<std::function<double>(
- const sampler_common::Path &, const sampler_common::Constraints &)>;
+using SoftConstraintsFunctionVector = std::vector<
+ const std::function<double(const sampler_common::Path &, const sampler_common::Constraints &)>>;
-using HardConstraintsFunctionVector = std::vector<std::function<bool>(
- const sampler_common::Path &, const sampler_common::Constraints &,
- std::optional<MultiPoint2d> &)>;
+using HardConstraintsFunctionVector = std::vector<std::function<bool(
+ sampler_common::Path &, const sampler_common::Constraints &, const MultiPoint2d &)>>;
-double checkSoftConstraints2(
+inline double evaluateSoftConstraints(
const sampler_common::Path & path, const sampler_common::Constraints & constraints,
const SoftConstraintsFunctionVector & soft_constraints, std::vector<double> & constraints_results)
{
constraints_results.clear();
double constraints_evaluation = 0.0;
- for (auto f : soft_constraints) {
+ for (const auto & f : soft_constraints) {
const auto value = f(path, constraints);
constraints_results.push_back(value);
constraints_evaluation += value;
@@ -54,21 +53,22 @@ double checkSoftConstraints2(
return constraints_evaluation;
}
-void checkHardConstraints2(
- const sampler_common::Path & path, const sampler_common::Constraints & constraints,
+inline void evaluateHardConstraints(
+ sampler_common::Path & path, const sampler_common::Constraints & constraints,
const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints,
std::vector<bool> & constraints_results)
{
+ constraints_results.clear();
constraints_results.resize(hard_constraints.size());
bool constraints_passed = true;
int idx = 0;
-
for (const auto & f : hard_constraints) {
const bool constraint_check = f(path, constraints, footprint);
constraints_passed &= constraint_check;
constraints_results[idx] = constraint_check;
++idx;
}
+
path.constraints_satisfied = constraints_passed;
return;
}
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 784e5305d98f4..f9a373118a506 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -14,6 +14,8 @@
#include "behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp"
+#include <boost/geometry/algorithms/within.hpp>
+
namespace behavior_path_planner
{
using geometry_msgs::msg::Point;
@@ -38,14 +40,16 @@ SamplingPlannerModule::SamplingPlannerModule(
updateModuleParams(parameters);
hard_constraints_.emplace_back(
- [](const sampler_common::Path &, const sampler_common::Constraints &) -> bool {
- const auto footprint = sampler_common::constraints::buildFootprintPoints(path, constraints);
+ [](
+ sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ const MultiPoint2d & footprint) -> bool {
if (!footprint.empty()) {
- if (!boost::geometry::within(footprint, constraints.drivable_polygons)) {
- path.constraint_results.drivable_area = false;
- }
+ path.constraint_results.drivable_area =
+ boost::geometry::within(footprint, constraints.drivable_polygons);
}
- path.constraint_results.collision = !has_collision(footprint, constraints.obstacle_polygons);
+
+ path.constraint_results.collision =
+ !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons);
return path.constraint_results.collision && path.constraint_results.drivable_area;
});
}
@@ -239,8 +243,11 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}
for (auto & path : frenet_paths) {
- const auto footprint = buildFootprintPoints(path, internal_params_->constraints);
- checkHardConstraints2(path, internal_params_->constraints, footprint);
+ std::vector<bool> constraints_results;
+ const auto footprint =
+ sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints);
+ behavior_path_planner::evaluateHardConstraints(
+ path, internal_params_->constraints, footprint, hard_constraints_, constraints_results);
path.constraint_results.curvature = true;
debug_data_.footprints.push_back(footprint);
sampler_common::constraints::calculateCost(
diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp
index 475aae4aeea18..d5fd66618e79f 100644
--- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp
+++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp
@@ -21,6 +21,9 @@ namespace sampler_common::constraints
{
/// @brief Check if the path satisfies the hard constraints
MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints);
+bool has_collision(const MultiPoint2d & footprint, const MultiPolygon2d & obstacles);
+bool satisfyMinMax(const std::vector<double> & values, const double min, const double max);
+
} // namespace sampler_common::constraints
#endif // SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_
From 5157cb4211cf31005c8360c6f11d4055bd871f26 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Tue, 28 Nov 2023 17:43:36 +0900
Subject: [PATCH 042/115] Add modifiable soft constraints checking function
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../utils/sampling_planner/util.hpp | 27 +++++++-------
.../sampling_planner_module.cpp | 37 ++++++++++++++++---
2 files changed, 44 insertions(+), 20 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index bd969ee196b39..57dae1055cf3e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -22,35 +22,34 @@
namespace behavior_path_planner
{
+using geometry_msgs::msg::Pose;
-struct HardConstraintsInput
+struct SoftConstraintsInputs
{
+ Pose goal_pose;
};
-struct HardConstraintsOutput
-{
- bool constraints_satisfied;
- MultiPoint2d footprint;
-};
-
-using SoftConstraintsFunctionVector = std::vector<
- const std::function<double(const sampler_common::Path &, const sampler_common::Constraints &)>>;
+using SoftConstraintsFunctionVector = std::vector<std::function<double(
+ sampler_common::Path &, const sampler_common::Constraints &, const SoftConstraintsInputs &)>>;
using HardConstraintsFunctionVector = std::vector<std::function<bool(
sampler_common::Path &, const sampler_common::Constraints &, const MultiPoint2d &)>>;
-inline double evaluateSoftConstraints(
- const sampler_common::Path & path, const sampler_common::Constraints & constraints,
- const SoftConstraintsFunctionVector & soft_constraints, std::vector<double> & constraints_results)
+inline void evaluateSoftConstraints(
+ sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ const SoftConstraintsFunctionVector & soft_constraints, const SoftConstraintsInputs & input_data,
+ std::vector<double> & constraints_results)
{
constraints_results.clear();
+ constraints_results.resize(soft_constraints.size());
double constraints_evaluation = 0.0;
for (const auto & f : soft_constraints) {
- const auto value = f(path, constraints);
+ const auto value = f(path, constraints, input_data);
constraints_results.push_back(value);
constraints_evaluation += value;
}
- return constraints_evaluation;
+ path.cost = constraints_evaluation;
+ return;
}
inline void evaluateHardConstraints(
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index f9a373118a506..8ec4eb19ec3da 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -52,6 +52,25 @@ SamplingPlannerModule::SamplingPlannerModule(
!sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons);
return path.constraint_results.collision && path.constraint_results.drivable_area;
});
+
+ // soft_constraints_.emplace_back(
+ // [](
+ // sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
+ // return (path.lengths.empty()) ? 0.0 : -constraints.soft.length_weight *
+ // path.lengths.back();
+ // });
+
+ soft_constraints_.emplace_back(
+ [](
+ sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
+ [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
+ const auto & goal_pose = input_data.goal_pose;
+ const auto & last_point = path.points.back();
+ const double distance =
+ std::hypot(goal_pose.position.x - last_point.x(), goal_pose.position.y - last_point.y());
+ return distance;
+ });
}
bool SamplingPlannerModule::isExecutionRequested() const
@@ -242,16 +261,23 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_paths.push_back(prev_path);
}
+ SoftConstraintsInputs soft_constraints_input;
+ std::vector<double> soft_constraints_results;
+ soft_constraints_input.goal_pose = planner_data_->route_handler->getGoalPose();
+
for (auto & path : frenet_paths) {
- std::vector<bool> constraints_results;
+ std::vector<bool> hard_constraints_results;
const auto footprint =
sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints);
behavior_path_planner::evaluateHardConstraints(
- path, internal_params_->constraints, footprint, hard_constraints_, constraints_results);
+ path, internal_params_->constraints, footprint, hard_constraints_, hard_constraints_results);
path.constraint_results.curvature = true;
debug_data_.footprints.push_back(footprint);
- sampler_common::constraints::calculateCost(
- path, internal_params_->constraints, reference_spline);
+ evaluateSoftConstraints(
+ path, internal_params_->constraints, soft_constraints_, soft_constraints_input,
+ soft_constraints_results);
+ // sampler_common::constraints::calculateCost(
+ // path, internal_params_->constraints, reference_spline);
}
std::vector<sampler_common::Path> candidate_paths;
const auto move_to_paths = [&candidate_paths](auto & paths_to_move) {
@@ -301,9 +327,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
const auto best_path = frenet_paths[*selected_path_idx];
prev_sampling_path_ = best_path;
auto out_path = convertFrenetPathToPathWithLaneID(best_path, road_lanes, velocity);
- const auto goal_pose = planner_data_->route_handler->getGoalPose();
for (auto & p : out_path.points) {
- p.point.pose.position.z = goal_pose.position.z;
+ p.point.pose.position.z = soft_constraints_input.goal_pose.position.z;
}
out.path = std::make_shared<PathWithLaneId>(out_path);
From f1755b07bb0d1a9c93dec97755960f550df12fb0 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Tue, 28 Nov 2023 18:16:15 +0900
Subject: [PATCH 043/115] Add costs for distance to goal and curvature
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.cpp | 26 ++++++++++++++++---
1 file changed, 23 insertions(+), 3 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 8ec4eb19ec3da..2c3408b804083 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -61,15 +61,35 @@ SamplingPlannerModule::SamplingPlannerModule(
// path.lengths.back();
// });
+ // TODO Daniel: Normalize costs to max 1, min 0
+ // TODO Daniel: Maybe add a soft cost for average distance to centerline?
+ // TODO Daniel: Think of methods to prevent chattering
+ // TODO Daniel: Investigate crash when goal is reached
+ // Distance to goal
soft_constraints_.emplace_back(
[](
sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
+ if (path.points.empty()) return 0.0;
const auto & goal_pose = input_data.goal_pose;
const auto & last_point = path.points.back();
const double distance =
std::hypot(goal_pose.position.x - last_point.x(), goal_pose.position.y - last_point.y());
- return distance;
+ return 100.0 * distance;
+ });
+
+ // Curvature cost
+ soft_constraints_.emplace_back(
+ [](
+ sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
+ [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
+ if (path.curvatures.empty()) return std::numeric_limits<double>::max();
+ double curvature_sum = 0.0;
+ for (const auto curvature : path.curvatures) {
+ curvature_sum += std::abs(curvature);
+ }
+ return constraints.soft.curvature_weight * curvature_sum /
+ static_cast<double>(path.curvatures.size());
});
}
@@ -227,8 +247,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
// const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes;
- const auto prev_module_path = getPreviousModuleOutput().path;
- const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ const auto & prev_module_path = getPreviousModuleOutput().path;
+ const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
std::vector<DrivableLanes> drivable_lanes{};
// expand drivable lanes
From ac78e75a22f83b36febf0e9e16d364220ee7ff66 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Tue, 28 Nov 2023 18:18:42 +0900
Subject: [PATCH 044/115] take out todo-> solved
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../scene_module/sampling_planner/sampling_planner_module.cpp | 1 -
1 file changed, 1 deletion(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 2c3408b804083..456684de9b5f6 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -64,7 +64,6 @@ SamplingPlannerModule::SamplingPlannerModule(
// TODO Daniel: Normalize costs to max 1, min 0
// TODO Daniel: Maybe add a soft cost for average distance to centerline?
// TODO Daniel: Think of methods to prevent chattering
- // TODO Daniel: Investigate crash when goal is reached
// Distance to goal
soft_constraints_.emplace_back(
[](
From 51635a4e68377759e6d9234d49a437f02b1c0818 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 29 Nov 2023 13:33:50 +0900
Subject: [PATCH 045/115] Added normalized constraints with ref path speed
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 3 +-
.../utils/sampling_planner/util.hpp | 3 +
.../sampling_planner_module.cpp | 57 ++++++++++++++-----
3 files changed, 49 insertions(+), 14 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index a8cc742f21462..f7e197ccd49c3 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -24,6 +24,7 @@
#include "bezier_sampler/bezier_sampling.hpp"
#include "frenet_planner/frenet_planner.hpp"
#include "lanelet2_extension/utility/utilities.hpp"
+#include "motion_utils/trajectory/path_with_lane_id.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sampler_common/constraints/footprint.hpp"
#include "sampler_common/constraints/hard_constraint.hpp"
@@ -98,7 +99,7 @@ class SamplingPlannerModule : public SceneModuleInterface
user_params_->lateral_deviation_weight;
internal_params_->constraints.soft.length_weight = user_params_->length_weight;
internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
- internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.25);
+ internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.75);
internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
// Sampling
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index 57dae1055cf3e..3e747f0bfb86b 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -18,6 +18,7 @@
#include <any>
#include <functional>
+#include <optional>
#include <vector>
namespace behavior_path_planner
@@ -27,6 +28,8 @@ using geometry_msgs::msg::Pose;
struct SoftConstraintsInputs
{
Pose goal_pose;
+ Pose ego_pose;
+ std::optional<sampler_common::Path> prev_path;
};
using SoftConstraintsFunctionVector = std::vector<std::function<double(
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 456684de9b5f6..46cbcc3c76a8d 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -53,6 +53,18 @@ SamplingPlannerModule::SamplingPlannerModule(
return path.constraint_results.collision && path.constraint_results.drivable_area;
});
+ hard_constraints_.emplace_back(
+ [](
+ sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ [[maybe_unused]] const MultiPoint2d & footprint) -> bool {
+ const bool curvatures_satisfied =
+ std::all_of(path.curvatures.begin(), path.curvatures.end(), [&](const auto & v) -> bool {
+ return (v > constraints.hard.min_curvature) && (v < constraints.hard.max_curvature);
+ });
+ path.constraint_results.curvature = curvatures_satisfied;
+ return curvatures_satisfied;
+ });
+
// soft_constraints_.emplace_back(
// [](
// sampler_common::Path & path, const sampler_common::Constraints & constraints,
@@ -66,15 +78,20 @@ SamplingPlannerModule::SamplingPlannerModule(
// TODO Daniel: Think of methods to prevent chattering
// Distance to goal
soft_constraints_.emplace_back(
- [](
+ [&](
sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.points.empty()) return 0.0;
const auto & goal_pose = input_data.goal_pose;
+ const auto & ego_pose = input_data.ego_pose;
const auto & last_point = path.points.back();
const double distance =
std::hypot(goal_pose.position.x - last_point.x(), goal_pose.position.y - last_point.y());
- return 100.0 * distance;
+ const double distance_ego_to_pose = std::hypot(
+ goal_pose.position.x - ego_pose.position.x, goal_pose.position.y - ego_pose.position.y);
+ const double epsilon = 0.001;
+ return (distance_ego_to_pose > epsilon) ? distance / distance_ego_to_pose : 0.0;
+ // return 100.0 * distance;
});
// Curvature cost
@@ -87,8 +104,12 @@ SamplingPlannerModule::SamplingPlannerModule(
for (const auto curvature : path.curvatures) {
curvature_sum += std::abs(curvature);
}
- return constraints.soft.curvature_weight * curvature_sum /
- static_cast<double>(path.curvatures.size());
+ const auto curvature_average = curvature_sum / static_cast<double>(path.curvatures.size());
+ const auto max_curvature =
+ (curvature_average > 0.0) ? constraints.hard.max_curvature : constraints.hard.min_curvature;
+ return curvature_average / max_curvature;
+ // return constraints.soft.curvature_weight * curvature_sum /
+ // static_cast<double>(path.curvatures.size());
});
}
@@ -140,6 +161,7 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
PathWithLaneId path;
const auto header = planner_data_->route_handler->getRouteHeader();
+ const auto reference_path_ptr = getPreviousModuleOutput().reference_path;
for (size_t i = 0; i < frenet_path.points.size(); ++i) {
const auto & frenet_path_point_position = frenet_path.points.at(i);
@@ -168,8 +190,16 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
if (!is_in_lanes && i > 0) {
point.lane_ids = path.points.at(i - 1).lane_ids;
}
+ if (reference_path_ptr) {
+ const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+ reference_path_ptr->points, point.point.pose);
+ const auto & closest_point = reference_path_ptr->points[idx];
+ point.point.longitudinal_velocity_mps = closest_point.point.longitudinal_velocity_mps;
+ point.point.lateral_velocity_mps = closest_point.point.lateral_velocity_mps;
- point.point.longitudinal_velocity_mps = velocity;
+ } else {
+ point.point.longitudinal_velocity_mps = velocity;
+ }
path.points.push_back(point);
}
return path;
@@ -208,21 +238,21 @@ void SamplingPlannerModule::prepareConstraints(
BehaviorModuleOutput SamplingPlannerModule::plan()
{
// const auto refPath = getPreviousModuleOutput().reference_path;
- const auto path_ptr = getPreviousModuleOutput().reference_path;
- if (path_ptr->points.empty()) {
+ const auto reference_path_ptr = getPreviousModuleOutput().reference_path;
+ if (reference_path_ptr->points.empty()) {
return {};
}
- // const auto ego_closest_path_index = planner_data_->findEgoIndex(path_ptr->points);
+ // const auto ego_closest_path_index = planner_data_->findEgoIndex(reference_path_ptr->points);
// RCLCPP_INFO(getLogger(), "ego_closest_path_index %ld", ego_closest_path_index);
// resetPathCandidate();
// resetPathReference();
- // [[maybe_unused]] const auto path = toPath(*path_ptr);
+ // [[maybe_unused]] const auto path = toPath(*reference_path_ptr);
auto reference_spline = [&]() -> sampler_common::transform::Spline2D {
std::vector<double> x;
std::vector<double> y;
- x.reserve(path_ptr->points.size());
- y.reserve(path_ptr->points.size());
- for (const auto & point : path_ptr->points) {
+ x.reserve(reference_path_ptr->points.size());
+ y.reserve(reference_path_ptr->points.size());
+ for (const auto & point : reference_path_ptr->points) {
x.push_back(point.point.pose.position.x);
y.push_back(point.point.pose.position.y);
}
@@ -281,8 +311,9 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}
SoftConstraintsInputs soft_constraints_input;
- std::vector<double> soft_constraints_results;
soft_constraints_input.goal_pose = planner_data_->route_handler->getGoalPose();
+ soft_constraints_input.ego_pose = pose;
+ std::vector<double> soft_constraints_results;
for (auto & path : frenet_paths) {
std::vector<bool> hard_constraints_results;
From 21fc93f3bb020382e919acc346180e099e40e873 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 30 Nov 2023 09:44:46 +0900
Subject: [PATCH 046/115] (WIP)isExecution requested update to not execute
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner/sampling_planner_module.cpp | 13 ++++++++++---
1 file changed, 10 insertions(+), 3 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 46cbcc3c76a8d..1046bc8c0df9c 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -73,9 +73,9 @@ SamplingPlannerModule::SamplingPlannerModule(
// path.lengths.back();
// });
- // TODO Daniel: Normalize costs to max 1, min 0
- // TODO Daniel: Maybe add a soft cost for average distance to centerline?
- // TODO Daniel: Think of methods to prevent chattering
+ // TODO(Daniel): Normalize costs to max 1, min 0
+ // TODO(Daniel): Maybe add a soft cost for average distance to centerline?
+ // TODO(Daniel): Think of methods to prevent chattering
// Distance to goal
soft_constraints_.emplace_back(
[&](
@@ -123,6 +123,13 @@ bool SamplingPlannerModule::isExecutionRequested() const
RCLCPP_WARN(getLogger(), "Backward path is NOT supported. Just converting path to trajectory");
return false;
}
+
+ const auto & goal_pose = planner_data_->route_handler->getGoalPose();
+ const auto & ego_pose = planner_data_->self_odometry->pose.pose;
+ const double distance_to_goal = std::hypot(
+ ego_pose.position.x - goal_pose.position.x, ego_pose.position.y - goal_pose.position.y);
+ if (distance_to_goal < 8.0) return false;
+
return true;
}
From 389beb55056d20fcd93b15df0994011422b764ac Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 30 Nov 2023 10:08:18 +0900
Subject: [PATCH 047/115] refactor: move getInitialState to utils
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../utils/sampling_planner/util.hpp | 14 +++++++++++
.../sampling_planner_module.cpp | 23 ++++++++++---------
2 files changed, 26 insertions(+), 11 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index 3e747f0bfb86b..b8b04f1e70b3e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -15,6 +15,7 @@
#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
#define BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
#include "sampler_common/structures.hpp"
+#include "sampler_common/transform/spline_transform.hpp"
#include <any>
#include <functional>
@@ -75,6 +76,19 @@ inline void evaluateHardConstraints(
return;
}
+inline sampler_common::State getInitialState(
+ const geometry_msgs::msg::Pose & pose,
+ const sampler_common::transform::Spline2D & reference_spline)
+{
+ sampler_common::State initial_state;
+ Point2d initial_state_pose{pose.position.x, pose.position.y};
+ const auto rpy = tier4_autoware_utils::getRPY(pose.orientation);
+ initial_state.pose = initial_state_pose;
+ initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y});
+ initial_state.heading = rpy.z;
+ return initial_state;
+}
+
} // namespace behavior_path_planner
#endif // BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 1046bc8c0df9c..1b0da610aa017 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -266,17 +266,18 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
return {x, y};
}();
- const auto pose = planner_data_->self_odometry->pose.pose;
- lanelet::ConstLanelet closest_lanelet;
- planner_data_->route_handler->getClosestLaneletWithinRoute(pose, &closest_lanelet);
-
- sampler_common::State initial_state;
- Point2d initial_state_pose{pose.position.x, pose.position.y};
- const auto rpy =
- tier4_autoware_utils::getRPY(planner_data_->self_odometry->pose.pose.orientation);
- initial_state.pose = initial_state_pose;
- initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y});
- initial_state.heading = rpy.z;
+ const auto & pose = planner_data_->self_odometry->pose.pose;
+ // lanelet::ConstLanelet closest_lanelet;
+ // planner_data_->route_handler->getClosestLaneletWithinRoute(pose, &closest_lanelet);
+
+ sampler_common::State initial_state =
+ behavior_path_planner::getInitialState(pose, reference_spline);
+ // Point2d initial_state_pose{pose.position.x, pose.position.y};
+ // const auto rpy =
+ // tier4_autoware_utils::getRPY(planner_data_->self_odometry->pose.pose.orientation);
+ // initial_state.pose = initial_state_pose;
+ // initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y});
+ // initial_state.heading = rpy.z;
frenet_planner::SamplingParameters sampling_parameters =
prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
From 4eaf28dadf0b45470cac5c3e864ead4c5f9af952 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 30 Nov 2023 10:46:16 +0900
Subject: [PATCH 048/115] refactor: move some functions to utils, get rid of
velocity req in generate pathwithlaneid
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 2 +-
.../sampling_planner_module.cpp | 107 +++++++-----------
2 files changed, 43 insertions(+), 66 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index f7e197ccd49c3..239a28a9f8de3 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -167,7 +167,7 @@ class SamplingPlannerModule : public SceneModuleInterface
PathWithLaneId convertFrenetPathToPathWithLaneID(
const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
- const double velocity);
+ const double path_z);
// member
// std::shared_ptr<SamplingPlannerParameters> params_;
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 1b0da610aa017..657933db11d16 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -156,7 +156,7 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData(
PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
- const double velocity)
+ const double path_z)
{
auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion {
tf2::Quaternion quaternion_tf2;
@@ -177,7 +177,7 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
PathPointWithLaneId point{};
point.point.pose.position.x = frenet_path_point_position.x();
point.point.pose.position.y = frenet_path_point_position.y();
- point.point.pose.position.z = 0.0;
+ point.point.pose.position.z = path_z;
auto yaw_as_quaternion = quaternion_from_rpy(0.0, 0.0, frenet_path_point_yaw);
point.point.pose.orientation.w = yaw_as_quaternion.getW();
@@ -203,9 +203,6 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
const auto & closest_point = reference_path_ptr->points[idx];
point.point.longitudinal_velocity_mps = closest_point.point.longitudinal_velocity_mps;
point.point.lateral_velocity_mps = closest_point.point.lateral_velocity_mps;
-
- } else {
- point.point.longitudinal_velocity_mps = velocity;
}
path.points.push_back(point);
}
@@ -244,16 +241,10 @@ void SamplingPlannerModule::prepareConstraints(
BehaviorModuleOutput SamplingPlannerModule::plan()
{
- // const auto refPath = getPreviousModuleOutput().reference_path;
const auto reference_path_ptr = getPreviousModuleOutput().reference_path;
if (reference_path_ptr->points.empty()) {
return {};
}
- // const auto ego_closest_path_index = planner_data_->findEgoIndex(reference_path_ptr->points);
- // RCLCPP_INFO(getLogger(), "ego_closest_path_index %ld", ego_closest_path_index);
- // resetPathCandidate();
- // resetPathReference();
- // [[maybe_unused]] const auto path = toPath(*reference_path_ptr);
auto reference_spline = [&]() -> sampler_common::transform::Spline2D {
std::vector<double> x;
std::vector<double> y;
@@ -266,53 +257,44 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
return {x, y};
}();
- const auto & pose = planner_data_->self_odometry->pose.pose;
- // lanelet::ConstLanelet closest_lanelet;
- // planner_data_->route_handler->getClosestLaneletWithinRoute(pose, &closest_lanelet);
-
- sampler_common::State initial_state =
- behavior_path_planner::getInitialState(pose, reference_spline);
- // Point2d initial_state_pose{pose.position.x, pose.position.y};
- // const auto rpy =
- // tier4_autoware_utils::getRPY(planner_data_->self_odometry->pose.pose.orientation);
- // initial_state.pose = initial_state_pose;
- // initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y});
- // initial_state.heading = rpy.z;
-
- frenet_planner::SamplingParameters sampling_parameters =
- prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
-
- // const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes;
-
- const auto & prev_module_path = getPreviousModuleOutput().path;
- const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ frenet_planner::FrenetState frenet_initial_state;
+ frenet_planner::SamplingParameters sampling_parameters;
+ {
+ const auto & pose = planner_data_->self_odometry->pose.pose;
+ sampler_common::State initial_state =
+ behavior_path_planner::getInitialState(pose, reference_spline);
+ sampling_parameters =
+ prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
+ frenet_initial_state.position = initial_state.frenet;
+ }
std::vector<DrivableLanes> drivable_lanes{};
- // expand drivable lanes
- std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
- drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
- });
-
- const auto left_bound =
- (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, true));
- const auto right_bound =
- (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, false));
+ {
+ const auto & prev_module_path = getPreviousModuleOutput().path;
+ const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ // expand drivable lanes
+ std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
+ drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
+ });
+ }
- const auto sampling_planner_data =
- createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
+ {
+ const auto left_bound =
+ (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, true));
+ const auto right_bound =
+ (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, false));
- prepareConstraints(
- internal_params_->constraints, planner_data_->dynamic_object, sampling_planner_data.left_bound,
- sampling_planner_data.right_bound);
+ const auto sampling_planner_data =
+ createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
- frenet_planner::FrenetState frenet_initial_state;
- frenet_initial_state.position = initial_state.frenet;
+ prepareConstraints(
+ internal_params_->constraints, planner_data_->dynamic_object,
+ sampling_planner_data.left_bound, sampling_planner_data.right_bound);
+ }
auto frenet_paths =
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
- debug_data_.footprints.clear();
-
if (prev_sampling_path_) {
const auto prev_path = prev_sampling_path_.value();
frenet_paths.push_back(prev_path);
@@ -320,11 +302,12 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
SoftConstraintsInputs soft_constraints_input;
soft_constraints_input.goal_pose = planner_data_->route_handler->getGoalPose();
- soft_constraints_input.ego_pose = pose;
- std::vector<double> soft_constraints_results;
+ soft_constraints_input.ego_pose = planner_data_->self_odometry->pose.pose;
+ debug_data_.footprints.clear();
for (auto & path : frenet_paths) {
std::vector<bool> hard_constraints_results;
+ std::vector<double> soft_constraints_results;
const auto footprint =
sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints);
behavior_path_planner::evaluateHardConstraints(
@@ -334,9 +317,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
evaluateSoftConstraints(
path, internal_params_->constraints, soft_constraints_, soft_constraints_input,
soft_constraints_results);
- // sampler_common::constraints::calculateCost(
- // path, internal_params_->constraints, reference_spline);
}
+
std::vector<sampler_common::Path> candidate_paths;
const auto move_to_paths = [&candidate_paths](auto & paths_to_move) {
candidate_paths.insert(
@@ -345,7 +327,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
};
move_to_paths(frenet_paths);
-
debug_data_.previous_sampled_candidates_nb = debug_data_.sampled_candidates.size();
debug_data_.sampled_candidates = candidate_paths;
debug_data_.obstacles = internal_params_->constraints.obstacle_polygons;
@@ -374,24 +355,20 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
return out;
}
- BehaviorModuleOutput out;
- const double velocity = 1.5;
+ const auto best_path = frenet_paths[*selected_path_idx];
+ prev_sampling_path_ = best_path;
+
const double max_length = *std::max_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
const auto road_lanes =
utils::getExtendedCurrentLanes(planner_data_, max_length, max_length, false);
+ auto out_path = convertFrenetPathToPathWithLaneID(
+ best_path, road_lanes, soft_constraints_input.goal_pose.position.z);
- const auto best_path = frenet_paths[*selected_path_idx];
- prev_sampling_path_ = best_path;
- auto out_path = convertFrenetPathToPathWithLaneID(best_path, road_lanes, velocity);
- for (auto & p : out_path.points) {
- p.point.pose.position.z = soft_constraints_input.goal_pose.position.z;
- }
-
+ BehaviorModuleOutput out;
out.path = std::make_shared<PathWithLaneId>(out_path);
- const auto p = getPreviousModuleOutput().reference_path;
- out.reference_path = p;
+ out.reference_path = reference_path_ptr;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
extendOutputDrivableArea(out);
return out;
From f5046ac978ba720fc0f7a78555f75a59b0f85957 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 30 Nov 2023 12:18:53 +0900
Subject: [PATCH 049/115] made curvature soft constraint depend on distance to
goal
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 8 +-
.../sampling_planner_module.cpp | 89 ++++++++++---------
2 files changed, 52 insertions(+), 45 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 239a28a9f8de3..71a51eab4ca45 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -33,15 +33,13 @@
#include "sampler_common/transform/spline_transform.hpp"
#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
+#include "tier4_autoware_utils/math/constants.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"
#include "tier4_autoware_utils/system/stop_watch.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"
-#include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
-
-#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <tier4_planning_msgs/msg/lateral_offset.hpp>
+#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
+#include "tier4_planning_msgs/msg/lateral_offset.hpp"
#include <algorithm>
#include <any>
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 657933db11d16..c5ac474ddc8bb 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -65,17 +65,22 @@ SamplingPlannerModule::SamplingPlannerModule(
return curvatures_satisfied;
});
+ // TODO(Daniel): Maybe add a soft cost for average distance to centerline?
+ // TODO(Daniel): Think of methods to prevent chattering
+
+ // Yaw difference
// soft_constraints_.emplace_back(
- // [](
- // sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ // [&](
+ // sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints &
+ // constraints,
// [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
- // return (path.lengths.empty()) ? 0.0 : -constraints.soft.length_weight *
- // path.lengths.back();
+ // if (path.points.empty()) return 0.0;
+ // const auto & goal_pose_yaw =
+ // tier4_autoware_utils::getRPY(input_data.goal_pose.orientation).z; const auto &
+ // last_point_yaw = path.yaws.back(); const double angle_difference = std::abs(last_point_yaw
+ // - goal_pose_yaw); return angle_difference / (3.141519 / 4.0);
// });
- // TODO(Daniel): Normalize costs to max 1, min 0
- // TODO(Daniel): Maybe add a soft cost for average distance to centerline?
- // TODO(Daniel): Think of methods to prevent chattering
// Distance to goal
soft_constraints_.emplace_back(
[&](
@@ -85,13 +90,23 @@ SamplingPlannerModule::SamplingPlannerModule(
const auto & goal_pose = input_data.goal_pose;
const auto & ego_pose = input_data.ego_pose;
const auto & last_point = path.points.back();
- const double distance =
- std::hypot(goal_pose.position.x - last_point.x(), goal_pose.position.y - last_point.y());
- const double distance_ego_to_pose = std::hypot(
+
+ const double distance_ego_to_goal_pose = std::hypot(
goal_pose.position.x - ego_pose.position.x, goal_pose.position.y - ego_pose.position.y);
- const double epsilon = 0.001;
- return (distance_ego_to_pose > epsilon) ? distance / distance_ego_to_pose : 0.0;
- // return 100.0 * distance;
+ const double distance_path_to_goal =
+ std::hypot(goal_pose.position.x - last_point.x(), goal_pose.position.y - last_point.y());
+ constexpr double epsilon = 0.001;
+ if (distance_ego_to_goal_pose < epsilon) return 0.0;
+ const double max_target_length = *std::max_element(
+ internal_params_->sampling.target_lengths.begin(),
+ internal_params_->sampling.target_lengths.end());
+ if (distance_ego_to_goal_pose > max_target_length)
+ return distance_path_to_goal / distance_ego_to_goal_pose;
+ constexpr double pi = 3.141519;
+ const auto & goal_pose_yaw = tier4_autoware_utils::getRPY(input_data.goal_pose.orientation).z;
+ const auto & last_point_yaw = path.yaws.back();
+ const double angle_difference = std::abs(last_point_yaw - goal_pose_yaw);
+ return (distance_path_to_goal / distance_ego_to_goal_pose) + (angle_difference / (pi / 4.0));
});
// Curvature cost
@@ -294,7 +309,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
auto frenet_paths =
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
-
if (prev_sampling_path_) {
const auto prev_path = prev_sampling_path_.value();
frenet_paths.push_back(prev_path);
@@ -441,34 +455,29 @@ void SamplingPlannerModule::updateDebugMarkers()
// }
// debug_marker_.markers.push_back(m);
// info_marker_.markers.push_back(m);
- // m.type = m.LINE_STRIP;
- // m.ns = "obstacles";
- // m.id = 0UL;
- // m.color.r = 1.0;
- // m.color.g = 0.0;
- // m.color.b = 0.0;
- // for (const auto & obs : debug_data_.obstacles) {
- // m.points.clear();
- // for (const auto & p : obs.outer())
- // m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
- // debug_marker_.markers.push_back(m);
- // info_marker_.markers.push_back(m);
- // ++m.id;
- // }
- // m.action = m.DELETE;
- // m.ns = "candidates";
- // for (m.id = debug_data_.sampled_candidates.size();
- // static_cast<size_t>(m.id) < debug_data_.previous_sampled_candidates_nb; ++m.id) {
- // debug_marker_.markers.push_back(m);
- // info_marker_.markers.push_back(m);
- // }
+ m.type = m.LINE_STRIP;
+ m.ns = "obstacles";
+ m.id = 0UL;
+ m.color.r = 1.0;
+ m.color.g = 0.0;
+ m.color.b = 0.0;
+ for (const auto & obs : debug_data_.obstacles) {
+ m.points.clear();
+ for (const auto & p : obs.outer())
+ m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
+ debug_marker_.markers.push_back(m);
+ info_marker_.markers.push_back(m);
+ ++m.id;
+ }
+ m.action = m.DELETE;
+ m.ns = "candidates";
+ for (m.id = debug_data_.sampled_candidates.size();
+ static_cast<size_t>(m.id) < debug_data_.previous_sampled_candidates_nb; ++m.id) {
+ debug_marker_.markers.push_back(m);
+ info_marker_.markers.push_back(m);
+ }
}
-// lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
-// {
-// return utils::getCurrentLanesFromPath(prev_module_path_, planner_data_);
-// }
-
void SamplingPlannerModule::extendOutputDrivableArea(BehaviorModuleOutput & output)
{
const auto prev_module_path = getPreviousModuleOutput().path;
From 727e2f8e5c27aaf294b3b9bd4ba0548c4b7f0d14 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 30 Nov 2023 15:40:45 +0900
Subject: [PATCH 050/115] Add prev path extension
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.cpp | 66 +++++++++++++++----
1 file changed, 55 insertions(+), 11 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index c5ac474ddc8bb..6a4505d550f82 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -274,14 +274,13 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::FrenetState frenet_initial_state;
frenet_planner::SamplingParameters sampling_parameters;
- {
- const auto & pose = planner_data_->self_odometry->pose.pose;
- sampler_common::State initial_state =
- behavior_path_planner::getInitialState(pose, reference_spline);
- sampling_parameters =
- prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
- frenet_initial_state.position = initial_state.frenet;
- }
+
+ const auto & pose = planner_data_->self_odometry->pose.pose;
+ sampler_common::State initial_state =
+ behavior_path_planner::getInitialState(pose, reference_spline);
+ sampling_parameters =
+ prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
+ frenet_initial_state.position = initial_state.frenet;
std::vector<DrivableLanes> drivable_lanes{};
{
@@ -309,9 +308,54 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
auto frenet_paths =
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
- if (prev_sampling_path_) {
- const auto prev_path = prev_sampling_path_.value();
- frenet_paths.push_back(prev_path);
+ // if (prev_sampling_path_) {
+ // const auto prev_path = prev_sampling_path_.value();
+ // frenet_paths.push_back(prev_path);
+ // }
+
+ // EXTEND prev path
+ if (prev_sampling_path_ && prev_sampling_path_->lengths.size() > 1) {
+ // Update previous path
+ frenet_planner::Path prev_path_frenet = prev_sampling_path_.value();
+ frenet_paths.push_back(prev_path_frenet);
+
+ double x = prev_path_frenet.points.back().x();
+ double x_pose = pose.position.x;
+ if (std::abs(x - x_pose) < 90.0) {
+ // sampler_common::State reuse_state;
+ // reuse_state.curvature = reused_path->curvatures.back();
+ // reuse_state.pose = reused_path->points.back();
+ // reuse_state.heading = reused_path->yaws.back();
+ // reuse_state.frenet = reference_spline.frenet(reuse_state.pose);
+ auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion {
+ tf2::Quaternion quaternion_tf2;
+ quaternion_tf2.setRPY(roll, pitch, yaw);
+ return quaternion_tf2;
+ };
+
+ geometry_msgs::msg::Pose future_pose;
+ future_pose.position.x = prev_path_frenet.points.back().x();
+ future_pose.position.y = prev_path_frenet.points.back().y();
+ future_pose.position.z = pose.position.z;
+
+ const auto future_pose_quaternion =
+ quaternion_from_rpy(0.0, 0.0, prev_path_frenet.yaws.back());
+ future_pose.orientation.w = future_pose_quaternion.w();
+ future_pose.orientation.x = future_pose_quaternion.x();
+ future_pose.orientation.y = future_pose_quaternion.y();
+ future_pose.orientation.z = future_pose_quaternion.z();
+
+ sampler_common::State future_state =
+ behavior_path_planner::getInitialState(future_pose, reference_spline);
+ frenet_planner::FrenetState frenet_reuse_state;
+ frenet_reuse_state.position = prev_path_frenet.frenet_points.back();
+
+ frenet_planner::SamplingParameters extension_sampling_parameters =
+ prepareSamplingParameters(future_state, reference_spline, *internal_params_);
+ auto extension_frenet_paths = frenet_planner::generatePaths(
+ reference_spline, frenet_reuse_state, extension_sampling_parameters);
+ for (auto & p : extension_frenet_paths) frenet_paths.push_back(prev_path_frenet.extend(p));
+ }
}
SoftConstraintsInputs soft_constraints_input;
From d066a0a6f96ea01cdcc206d312db3f889cdf35cc Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Thu, 30 Nov 2023 18:41:51 +0900
Subject: [PATCH 051/115] add calculation of initial lateral velocity and
acceleration
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
.../sampling_planner_module.cpp | 23 ++++++++++++++++++-
1 file changed, 22 insertions(+), 1 deletion(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 6a4505d550f82..f479a248ffadc 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -280,7 +280,28 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
behavior_path_planner::getInitialState(pose, reference_spline);
sampling_parameters =
prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
- frenet_initial_state.position = initial_state.frenet;
+ {
+ frenet_initial_state.position = initial_state.frenet;
+ const auto frenet_yaw = initial_state.heading - reference_spline.yaw(initial_state.frenet.s);
+ const auto path_curvature = reference_spline.curvature(initial_state.frenet.s);
+ constexpr auto delta_s = 0.001;
+ frenet_initial_state.lateral_velocity =
+ (1 - path_curvature * initial_state.frenet.d) * std::tan(frenet_yaw);
+ const auto path_curvature_deriv =
+ (reference_spline.curvature(initial_state.frenet.s + delta_s) - path_curvature) / delta_s;
+ const auto cos_yaw = std::cos(frenet_yaw);
+ if (cos_yaw == 0.0) {
+ frenet_initial_state.lateral_acceleration = 0.0;
+ } else {
+ frenet_initial_state.lateral_acceleration =
+ -(path_curvature_deriv * initial_state.frenet.d +
+ path_curvature * frenet_initial_state.lateral_velocity) *
+ std::tan(frenet_yaw) +
+ ((1 - path_curvature * initial_state.frenet.d) / (cos_yaw * cos_yaw)) *
+ (initial_state.curvature * ((1 - path_curvature * initial_state.frenet.d) / cos_yaw) -
+ path_curvature);
+ }
+ }
std::vector<DrivableLanes> drivable_lanes{};
{
From 3c4b0e926852721942304f05c7f86947f21e9011 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Fri, 1 Dec 2023 09:14:22 +0900
Subject: [PATCH 052/115] add calculation of initial lateral velocity and
acceleration to path extension
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner/sampling_planner_module.cpp | 14 ++++++++++++--
1 file changed, 12 insertions(+), 2 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index f479a248ffadc..ee4cf258328b8 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -280,6 +280,12 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
behavior_path_planner::getInitialState(pose, reference_spline);
sampling_parameters =
prepareSamplingParameters(initial_state, reference_spline, *internal_params_);
+
+ auto set_frenet_state = [](
+ const sampler_common::State & initial_state,
+ const sampler_common::transform::Spline2D & reference_spline,
+ frenet_planner::FrenetState & frenet_initial_state)
+
{
frenet_initial_state.position = initial_state.frenet;
const auto frenet_yaw = initial_state.heading - reference_spline.yaw(initial_state.frenet.s);
@@ -301,7 +307,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
(initial_state.curvature * ((1 - path_curvature * initial_state.frenet.d) / cos_yaw) -
path_curvature);
}
- }
+ };
+ set_frenet_state(initial_state, reference_spline, frenet_initial_state);
std::vector<DrivableLanes> drivable_lanes{};
{
@@ -369,7 +376,10 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
sampler_common::State future_state =
behavior_path_planner::getInitialState(future_pose, reference_spline);
frenet_planner::FrenetState frenet_reuse_state;
- frenet_reuse_state.position = prev_path_frenet.frenet_points.back();
+
+ set_frenet_state(future_state, reference_spline, frenet_reuse_state);
+
+ // frenet_reuse_state.position = prev_path_frenet.frenet_points.back();
frenet_planner::SamplingParameters extension_sampling_parameters =
prepareSamplingParameters(future_state, reference_spline, *internal_params_);
From 31992bde92ad65e5ba3d7c95bdfa9ee7fea8c6ae Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Mon, 4 Dec 2023 09:17:11 +0900
Subject: [PATCH 053/115] WIP Add poses to path to get centerline distance and
other stuff
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../src/frenet_planner/frenet_planner.cpp | 23 ++++++++++++++++++-
.../include/sampler_common/structures.hpp | 2 ++
2 files changed, 24 insertions(+), 1 deletion(-)
diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
index a4ff1db77e8a1..a5c90e5c4af34 100644
--- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
+++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
@@ -22,6 +22,8 @@
#include <sampler_common/structures.hpp>
#include <sampler_common/transform/spline_transform.hpp>
+#include "autoware_auto_planning_msgs/msg/path.hpp"
+
#include <algorithm>
#include <cmath>
#include <iostream>
@@ -145,11 +147,17 @@ Path generateCandidate(
void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path)
{
+ auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion {
+ tf2::Quaternion quaternion_tf2;
+ quaternion_tf2.setRPY(roll, pitch, yaw);
+ return quaternion_tf2;
+ };
if (!path.frenet_points.empty()) {
path.points.reserve(path.frenet_points.size());
path.yaws.reserve(path.frenet_points.size());
path.lengths.reserve(path.frenet_points.size());
path.curvatures.reserve(path.frenet_points.size());
+ path.poses.reserve(path.frenet_points.size());
// Calculate cartesian positions
for (const auto & fp : path.frenet_points) {
path.points.push_back(reference.cartesian(fp));
@@ -161,8 +169,21 @@ void calculateCartesian(const sampler_common::transform::Spline2D & reference, P
for (auto it = path.points.begin(); it != std::prev(path.points.end()); ++it) {
const auto dx = std::next(it)->x() - it->x();
const auto dy = std::next(it)->y() - it->y();
- path.yaws.push_back(std::atan2(dy, dx));
+ const auto yaw = std::atan2(dy, dx);
+ path.yaws.push_back(yaw);
path.lengths.push_back(path.lengths.back() + std::hypot(dx, dy));
+
+ geometry_msgs::msg::Pose pose;
+ pose.position.x = it->x();
+ pose.position.y = it->y();
+ pose.position.z = 0.0;
+
+ const auto pose_quaternion = quaternion_from_rpy(0.0, 0.0, yaw);
+ pose.orientation.w = pose_quaternion.w();
+ pose.orientation.x = pose_quaternion.x();
+ pose.orientation.y = pose_quaternion.y();
+ pose.orientation.z = pose_quaternion.z();
+ path.poses.push_back(pose);
}
path.yaws.push_back(path.yaws.back());
// Calculate curvatures
diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
index 4bcb5f6a61079..b83039a6240cd 100644
--- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
+++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
@@ -79,6 +79,8 @@ struct Path
std::vector<double> curvatures{};
std::vector<double> yaws{};
std::vector<double> lengths{};
+ std::vector<geometry_msgs::msg::Pose> poses{};
+
bool constraints_satisfied;
ConstraintResults constraint_results{};
double cost{};
From 8466bc8a53e924d0d22130245795a56a96de5540 Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Fri, 1 Dec 2023 13:45:34 +0900
Subject: [PATCH 054/115] clear info_marker_ to prevent performance issues
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
.../scene_module/sampling_planner/sampling_planner_module.cpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index ee4cf258328b8..1a7577514ad4d 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -466,6 +466,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
void SamplingPlannerModule::updateDebugMarkers()
{
debug_marker_.markers.clear();
+ info_marker_.markers.clear();
const auto header = planner_data_->route_handler->getRouteHeader();
visualization_msgs::msg::Marker m;
From 349d1164eeb636dae442c7a7854497cdf9afbff3 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 4 Dec 2023 13:49:16 +0900
Subject: [PATCH 055/115] solve dependency issues
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../frenet_planner/src/frenet_planner/frenet_planner.cpp | 3 +++
.../sampler_common/include/sampler_common/structures.hpp | 2 ++
2 files changed, 5 insertions(+)
diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
index a5c90e5c4af34..45cf380a63020 100644
--- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
+++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
@@ -23,6 +23,9 @@
#include <sampler_common/transform/spline_transform.hpp>
#include "autoware_auto_planning_msgs/msg/path.hpp"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <tf2/utils.h>
#include <algorithm>
#include <cmath>
diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
index b83039a6240cd..dab1c58afeccd 100644
--- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
+++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
@@ -20,6 +20,8 @@
#include <eigen3/Eigen/Core>
#include <interpolation/linear_interpolation.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
From be59647ff16ddd5c13926d145e9b1c0fb5f73e69 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 4 Dec 2023 13:56:53 +0900
Subject: [PATCH 056/115] Add cost to avg. distance to centerline
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../utils/sampling_planner/util.hpp | 1 +
.../sampling_planner_module.cpp | 19 +++++++++++++++++++
2 files changed, 20 insertions(+)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index b8b04f1e70b3e..924d7f52a5f83 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -31,6 +31,7 @@ struct SoftConstraintsInputs
Pose goal_pose;
Pose ego_pose;
std::optional<sampler_common::Path> prev_path;
+ lanelet::ConstLanelets current_lanes;
};
using SoftConstraintsFunctionVector = std::vector<std::function<double(
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 1a7577514ad4d..e1f0a16fc5cb2 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -109,6 +109,24 @@ SamplingPlannerModule::SamplingPlannerModule(
return (distance_path_to_goal / distance_ego_to_goal_pose) + (angle_difference / (pi / 4.0));
});
+ // Avg. distance to centerline
+ soft_constraints_.emplace_back(
+ [&](
+ [[maybe_unused]] sampler_common::Path & path,
+ [[maybe_unused]] const sampler_common::Constraints & constraints,
+ [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
+ if (path.poses.empty()) return 0.0;
+ double distance_average{0.0};
+ for (const auto pose : path.poses) {
+ const double lateral_distance_to_center_lane =
+ lanelet::utils::getArcCoordinates(input_data.current_lanes, pose).distance;
+ distance_average += lateral_distance_to_center_lane;
+ }
+ distance_average = distance_average / path.poses.size();
+ const double acceptable_width = constraints.ego_width / 2.0;
+ return distance_average / acceptable_width;
+ });
+
// Curvature cost
soft_constraints_.emplace_back(
[](
@@ -392,6 +410,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
SoftConstraintsInputs soft_constraints_input;
soft_constraints_input.goal_pose = planner_data_->route_handler->getGoalPose();
soft_constraints_input.ego_pose = planner_data_->self_odometry->pose.pose;
+ soft_constraints_input.current_lanes = utils::getCurrentLanes(planner_data_);
debug_data_.footprints.clear();
for (auto & path : frenet_paths) {
From 6491e15934742dccd7395d28df346fc5ddec5627 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 4 Dec 2023 18:32:33 +0900
Subject: [PATCH 057/115] added arc lenght based extension limit
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../utils/sampling_planner/util.hpp | 6 +-
.../sampling_planner_module.cpp | 155 ++++++++++++------
.../include/sampler_common/structures.hpp | 3 +
3 files changed, 114 insertions(+), 50 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index 924d7f52a5f83..e4cca98464857 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -30,6 +30,8 @@ struct SoftConstraintsInputs
{
Pose goal_pose;
Pose ego_pose;
+ behavior_path_planner::PlanResult reference_path;
+ behavior_path_planner::PlanResult prev_module_path;
std::optional<sampler_common::Path> prev_path;
lanelet::ConstLanelets current_lanes;
};
@@ -46,7 +48,6 @@ inline void evaluateSoftConstraints(
std::vector<double> & constraints_results)
{
constraints_results.clear();
- constraints_results.resize(soft_constraints.size());
double constraints_evaluation = 0.0;
for (const auto & f : soft_constraints) {
const auto value = f(path, constraints, input_data);
@@ -63,13 +64,12 @@ inline void evaluateHardConstraints(
std::vector<bool> & constraints_results)
{
constraints_results.clear();
- constraints_results.resize(hard_constraints.size());
bool constraints_passed = true;
int idx = 0;
for (const auto & f : hard_constraints) {
const bool constraint_check = f(path, constraints, footprint);
constraints_passed &= constraint_check;
- constraints_results[idx] = constraint_check;
+ constraints_results.push_back(constraint_check);
++idx;
}
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index e1f0a16fc5cb2..1238850a7962e 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -87,47 +87,90 @@ SamplingPlannerModule::SamplingPlannerModule(
sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.points.empty()) return 0.0;
+ if (!std::any_of(
+ input_data.current_lanes.begin(), input_data.current_lanes.end(),
+ [&](const auto & lane) {
+ return lanelet::utils::isInLanelet(input_data.goal_pose, lane);
+ }))
+ return 0.0;
+ if (path.poses.empty()) return 0.0;
const auto & goal_pose = input_data.goal_pose;
const auto & ego_pose = input_data.ego_pose;
- const auto & last_point = path.points.back();
+ const auto & path_last_pose = path.poses.back();
+
+ const auto ego_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes, ego_pose);
+ const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes, goal_pose);
+ const auto path_last_arc =
+ lanelet::utils::getArcCoordinates(input_data.current_lanes, path_last_pose);
+
+ const double distance_ego_to_goal_pose = std::abs(ego_arc.length - goal_arc.length);
- const double distance_ego_to_goal_pose = std::hypot(
- goal_pose.position.x - ego_pose.position.x, goal_pose.position.y - ego_pose.position.y);
- const double distance_path_to_goal =
- std::hypot(goal_pose.position.x - last_point.x(), goal_pose.position.y - last_point.y());
constexpr double epsilon = 0.001;
if (distance_ego_to_goal_pose < epsilon) return 0.0;
+ // const double distance_ego_to_goal_pose = std::hypot(
+ // goal_pose.position.x - ego_pose.position.x, goal_pose.position.y - ego_pose.position.y);
+ // const double distance_path_to_goal =
+ // std::hypot(goal_pose.position.x - last_point.x(), goal_pose.position.y - last_point.y());
+ // constexpr double epsilon = 0.001;
+ // if (distance_ego_to_goal_pose < epsilon) return 0.0;
+
+ // if (distance_path_to_goal < max_target_length)
+ // return std::abs(path_last_arc.distance - goal_arc.distance);
+ const double length_path_to_goal = std::abs(path_last_arc.length - goal_arc.length);
+ // if (distance_ego_to_goal_pose > max_target_length)
+ // return length_path_to_goal / distance_ego_to_goal_pose;
+ const double lateral_distance_to_goal = std::abs(path_last_arc.distance - goal_arc.distance);
+
+ const double max_target_distance = *std::max_element(
+ internal_params_->sampling.target_lateral_positions.begin(),
+ internal_params_->sampling.target_lateral_positions.end());
+
const double max_target_length = *std::max_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
- if (distance_ego_to_goal_pose > max_target_length)
- return distance_path_to_goal / distance_ego_to_goal_pose;
- constexpr double pi = 3.141519;
- const auto & goal_pose_yaw = tier4_autoware_utils::getRPY(input_data.goal_pose.orientation).z;
- const auto & last_point_yaw = path.yaws.back();
- const double angle_difference = std::abs(last_point_yaw - goal_pose_yaw);
- return (distance_path_to_goal / distance_ego_to_goal_pose) + (angle_difference / (pi / 4.0));
+
+ const double reference_max_distance = std::hypot(max_target_length, max_target_distance);
+
+ return (length_path_to_goal / reference_max_distance) +
+ (lateral_distance_to_goal /
+ reference_max_distance); // + (angle_difference / (pi / 4.0));
});
// Avg. distance to centerline
- soft_constraints_.emplace_back(
- [&](
- [[maybe_unused]] sampler_common::Path & path,
- [[maybe_unused]] const sampler_common::Constraints & constraints,
- [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
- if (path.poses.empty()) return 0.0;
- double distance_average{0.0};
- for (const auto pose : path.poses) {
- const double lateral_distance_to_center_lane =
- lanelet::utils::getArcCoordinates(input_data.current_lanes, pose).distance;
- distance_average += lateral_distance_to_center_lane;
- }
- distance_average = distance_average / path.poses.size();
- const double acceptable_width = constraints.ego_width / 2.0;
- return distance_average / acceptable_width;
- });
+ // soft_constraints_.emplace_back(
+ // [&](
+ // [[maybe_unused]] sampler_common::Path & path,
+ // [[maybe_unused]] const sampler_common::Constraints & constraints,
+ // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
+ // if (path.poses.empty()) return 0.0;
+ // if (!std::any_of(
+ // input_data.current_lanes.begin(), input_data.current_lanes.end(),
+ // [&](const auto & lane) {
+ // return lanelet::utils::isInLanelet(input_data.goal_pose, lane);
+ // }))
+ // return 0.0;
+
+ // const auto & goal_pose = input_data.goal_pose;
+ // const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes,
+ // goal_pose); const double min_target_length = *std::min_element(
+ // internal_params_->sampling.target_lengths.begin(),
+ // internal_params_->sampling.target_lengths.end());
+
+ // double distance_average{0.0};
+ // for (const auto pose : path.poses) {
+ // const auto & path_point_arc =
+ // lanelet::utils::getArcCoordinates(input_data.current_lanes, pose);
+ // const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
+ // const double lateral_distance_to_center_lane =
+ // (distance_point_to_goal < min_target_length) ? 0.0 : std::abs(path_point_arc.distance);
+ // distance_average += lateral_distance_to_center_lane;
+ // }
+ // distance_average = distance_average / path.poses.size();
+ // const double acceptable_width = constraints.ego_width / 2.0;
+ // return distance_average / acceptable_width;
+ // });
- // Curvature cost
+ // // Curvature cost
soft_constraints_.emplace_back(
[](
sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
@@ -140,7 +183,7 @@ SamplingPlannerModule::SamplingPlannerModule(
const auto curvature_average = curvature_sum / static_cast<double>(path.curvatures.size());
const auto max_curvature =
(curvature_average > 0.0) ? constraints.hard.max_curvature : constraints.hard.min_curvature;
- return curvature_average / max_curvature;
+ return 100.0 * curvature_average / max_curvature;
// return constraints.soft.curvature_weight * curvature_sum /
// static_cast<double>(path.curvatures.size());
});
@@ -289,7 +332,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}
return {x, y};
}();
-
frenet_planner::FrenetState frenet_initial_state;
frenet_planner::SamplingParameters sampling_parameters;
@@ -329,14 +371,13 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
set_frenet_state(initial_state, reference_spline, frenet_initial_state);
std::vector<DrivableLanes> drivable_lanes{};
- {
- const auto & prev_module_path = getPreviousModuleOutput().path;
- const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
- // expand drivable lanes
- std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
- drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
- });
- }
+
+ const auto & prev_module_path = getPreviousModuleOutput().path;
+ const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ // expand drivable lanes
+ std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
+ drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
+ });
{
const auto left_bound =
@@ -365,9 +406,18 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::Path prev_path_frenet = prev_sampling_path_.value();
frenet_paths.push_back(prev_path_frenet);
- double x = prev_path_frenet.points.back().x();
- double x_pose = pose.position.x;
- if (std::abs(x - x_pose) < 90.0) {
+ geometry_msgs::msg::Pose future_pose = prev_path_frenet.poses.back();
+ const double length_path = lanelet::utils::getArcCoordinates(current_lanes, future_pose).length;
+ const auto goal_pose = planner_data_->route_handler->getGoalPose();
+ const double length_goal = lanelet::utils::getArcCoordinates(current_lanes, goal_pose).length;
+
+ const double min_target_length = *std::min_element(
+ internal_params_->sampling.target_lengths.begin(),
+ internal_params_->sampling.target_lengths.end());
+
+ // double x = prev_path_frenet.points.back().x();
+ // double x_pose = pose.position.x;
+ if (std::abs(length_goal - length_path) > min_target_length) {
// sampler_common::State reuse_state;
// reuse_state.curvature = reused_path->curvatures.back();
// reuse_state.pose = reused_path->points.back();
@@ -390,7 +440,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
future_pose.orientation.x = future_pose_quaternion.x();
future_pose.orientation.y = future_pose_quaternion.y();
future_pose.orientation.z = future_pose_quaternion.z();
-
sampler_common::State future_state =
behavior_path_planner::getInitialState(future_pose, reference_spline);
frenet_planner::FrenetState frenet_reuse_state;
@@ -410,9 +459,13 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
SoftConstraintsInputs soft_constraints_input;
soft_constraints_input.goal_pose = planner_data_->route_handler->getGoalPose();
soft_constraints_input.ego_pose = planner_data_->self_odometry->pose.pose;
- soft_constraints_input.current_lanes = utils::getCurrentLanes(planner_data_);
+ soft_constraints_input.current_lanes = current_lanes;
+ soft_constraints_input.reference_path = reference_path_ptr;
+ soft_constraints_input.prev_module_path = prev_module_path;
debug_data_.footprints.clear();
+ std::vector<std::vector<bool>> hard_constraints_results_full;
+ std::vector<std::vector<double>> soft_constraints_results_full;
for (auto & path : frenet_paths) {
std::vector<bool> hard_constraints_results;
std::vector<double> soft_constraints_results;
@@ -425,6 +478,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
evaluateSoftConstraints(
path, internal_params_->constraints, soft_constraints_, soft_constraints_input,
soft_constraints_results);
+ soft_constraints_results_full.push_back(soft_constraints_results);
}
std::vector<sampler_common::Path> candidate_paths;
@@ -456,14 +510,21 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
if (!selected_path_idx) {
BehaviorModuleOutput out;
- const auto p = getPreviousModuleOutput().path;
- out.path = p;
- out.reference_path = p;
+ out.path = getPreviousModuleOutput().path;
+ out.reference_path = getPreviousModuleOutput().reference_path;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
return out;
}
const auto best_path = frenet_paths[*selected_path_idx];
+
+ std::cerr << "Soft constraints results of best: ";
+ for (const auto result : soft_constraints_results_full[*selected_path_idx])
+ std::cerr << result << ",";
+ std::cerr << "\n";
+
+ std::cerr << "Poses " << best_path.poses.size() << "\n";
+
prev_sampling_path_ = best_path;
const double max_length = *std::max_element(
diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
index dab1c58afeccd..edc85b5ad8665 100644
--- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
+++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
@@ -97,6 +97,7 @@ struct Path
curvatures.clear();
yaws.clear();
lengths.clear();
+ poses.clear();
constraint_results.clear();
tag = "";
cost = 0.0;
@@ -108,6 +109,7 @@ struct Path
curvatures.reserve(size);
yaws.reserve(size);
lengths.reserve(size);
+ poses.reserve(size);
}
[[nodiscard]] Path extend(const Path & path) const
@@ -131,6 +133,7 @@ struct Path
ext(extended_path.points, points, path.points);
ext(extended_path.curvatures, curvatures, path.curvatures);
ext(extended_path.yaws, yaws, path.yaws);
+ ext(extended_path.poses, poses, path.poses);
extended_path.lengths.insert(extended_path.lengths.end(), lengths.begin(), lengths.end());
const auto last_base_length = lengths.empty() ? 0.0 : lengths.back() + length_offset;
for (size_t i = offset; i < path.lengths.size(); ++i)
From edea1bcb7efb3ad6a18a47f769b8296fcc0fc1ca Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 5 Dec 2023 11:56:00 +0900
Subject: [PATCH 058/115] Add running and success conditions, add dist to soft
const
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 29 ++-
.../sampling_planner_module.cpp | 168 ++++++++++++------
2 files changed, 136 insertions(+), 61 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 71a51eab4ca45..8f64e482dd29f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -140,15 +140,31 @@ class SamplingPlannerModule : public SceneModuleInterface
private:
SamplingPlannerData createPlannerData(
const PlanResult & path, const std::vector<geometry_msgs::msg::Point> & left_bound,
- const std::vector<geometry_msgs::msg::Point> & right_bound);
+ const std::vector<geometry_msgs::msg::Point> & right_bound) const;
PlanResult generatePath();
- bool canTransitSuccessState() override { return false; }
+ bool canTransitSuccessState() override
+ {
+ const auto ego_pose = planner_data_->self_odometry->pose.pose;
+ const auto prev_module_path = getPreviousModuleOutput().path;
+ const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ const auto ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose);
+ const auto goal_pose = planner_data_->route_handler->getGoalPose();
+ const auto goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose);
+ const double length_to_goal = std::abs(goal_arc.length - ego_arc.length);
+ const double min_target_length = *std::min_element(
+ internal_params_->sampling.target_lengths.begin(),
+ internal_params_->sampling.target_lengths.end());
+ return isReferencePathSafe() &&
+ ((std::abs(ego_arc.distance) < 1.5) || (length_to_goal < min_target_length));
+ }
bool canTransitFailureState() override { return false; }
- bool canTransitIdleToRunningState() override { return false; }
+ bool canTransitIdleToRunningState() override { return prev_sampling_path_.has_value(); }
+
+ bool isReferencePathSafe() const;
void updateDebugMarkers();
@@ -156,7 +172,7 @@ class SamplingPlannerModule : public SceneModuleInterface
sampler_common::Constraints & constraints,
const PredictedObjects::ConstSharedPtr & predicted_objects,
const std::vector<geometry_msgs::msg::Point> & left_bound,
- const std::vector<geometry_msgs::msg::Point> & right_bound);
+ const std::vector<geometry_msgs::msg::Point> & right_bound) const;
frenet_planner::SamplingParameters prepareSamplingParameters(
const sampler_common::State & initial_state,
@@ -176,9 +192,10 @@ class SamplingPlannerModule : public SceneModuleInterface
void extendOutputDrivableArea(BehaviorModuleOutput & output);
bool isEndPointsConnected(
- const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane);
+ const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) const;
DrivableLanes generateExpandDrivableLanes(
- const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data);
+ const lanelet::ConstLanelet & lanelet,
+ const std::shared_ptr<const PlannerData> & planner_data) const;
};
} // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 1238850a7962e..aa7b7438b65ff 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -57,6 +57,7 @@ SamplingPlannerModule::SamplingPlannerModule(
[](
sampler_common::Path & path, const sampler_common::Constraints & constraints,
[[maybe_unused]] const MultiPoint2d & footprint) -> bool {
+ if (path.curvatures.empty()) return true;
const bool curvatures_satisfied =
std::all_of(path.curvatures.begin(), path.curvatures.end(), [&](const auto & v) -> bool {
return (v > constraints.hard.min_curvature) && (v < constraints.hard.max_curvature);
@@ -107,18 +108,7 @@ SamplingPlannerModule::SamplingPlannerModule(
constexpr double epsilon = 0.001;
if (distance_ego_to_goal_pose < epsilon) return 0.0;
- // const double distance_ego_to_goal_pose = std::hypot(
- // goal_pose.position.x - ego_pose.position.x, goal_pose.position.y - ego_pose.position.y);
- // const double distance_path_to_goal =
- // std::hypot(goal_pose.position.x - last_point.x(), goal_pose.position.y - last_point.y());
- // constexpr double epsilon = 0.001;
- // if (distance_ego_to_goal_pose < epsilon) return 0.0;
-
- // if (distance_path_to_goal < max_target_length)
- // return std::abs(path_last_arc.distance - goal_arc.distance);
const double length_path_to_goal = std::abs(path_last_arc.length - goal_arc.length);
- // if (distance_ego_to_goal_pose > max_target_length)
- // return length_path_to_goal / distance_ego_to_goal_pose;
const double lateral_distance_to_goal = std::abs(path_last_arc.distance - goal_arc.distance);
const double max_target_distance = *std::max_element(
@@ -136,39 +126,46 @@ SamplingPlannerModule::SamplingPlannerModule(
reference_max_distance); // + (angle_difference / (pi / 4.0));
});
- // Avg. distance to centerline
- // soft_constraints_.emplace_back(
- // [&](
- // [[maybe_unused]] sampler_common::Path & path,
- // [[maybe_unused]] const sampler_common::Constraints & constraints,
- // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
- // if (path.poses.empty()) return 0.0;
- // if (!std::any_of(
- // input_data.current_lanes.begin(), input_data.current_lanes.end(),
- // [&](const auto & lane) {
- // return lanelet::utils::isInLanelet(input_data.goal_pose, lane);
- // }))
- // return 0.0;
-
- // const auto & goal_pose = input_data.goal_pose;
- // const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes,
- // goal_pose); const double min_target_length = *std::min_element(
- // internal_params_->sampling.target_lengths.begin(),
- // internal_params_->sampling.target_lengths.end());
-
- // double distance_average{0.0};
- // for (const auto pose : path.poses) {
- // const auto & path_point_arc =
- // lanelet::utils::getArcCoordinates(input_data.current_lanes, pose);
- // const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
- // const double lateral_distance_to_center_lane =
- // (distance_point_to_goal < min_target_length) ? 0.0 : std::abs(path_point_arc.distance);
- // distance_average += lateral_distance_to_center_lane;
- // }
- // distance_average = distance_average / path.poses.size();
- // const double acceptable_width = constraints.ego_width / 2.0;
- // return distance_average / acceptable_width;
- // });
+ // Distance to centerline
+ soft_constraints_.emplace_back(
+ [&](
+ [[maybe_unused]] sampler_common::Path & path,
+ [[maybe_unused]] const sampler_common::Constraints & constraints,
+ [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
+ if (path.poses.empty()) return 0.0;
+ if (!std::any_of(
+ input_data.current_lanes.begin(), input_data.current_lanes.end(),
+ [&](const auto & lane) {
+ return lanelet::utils::isInLanelet(input_data.goal_pose, lane);
+ }))
+ return 0.0;
+
+ const auto & goal_pose = input_data.goal_pose;
+ const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes, goal_pose);
+ const double min_target_length = *std::min_element(
+ internal_params_->sampling.target_lengths.begin(),
+ internal_params_->sampling.target_lengths.end());
+
+ // double distance_average{0.0};
+ // for (const auto pose : path.poses) {
+ // const auto & path_point_arc =
+ // lanelet::utils::getArcCoordinates(input_data.current_lanes, pose);
+ // const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
+ // const double lateral_distance_to_center_lane =
+ // (distance_point_to_goal < min_target_length) ? 0.0 : std::abs(path_point_arc.distance);
+ // distance_average += lateral_distance_to_center_lane;
+ // }
+ const auto & path_point_arc =
+ lanelet::utils::getArcCoordinates(input_data.current_lanes, path.poses.back());
+ const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
+ const double lateral_distance_to_center_lane =
+ (distance_point_to_goal < min_target_length) ? 0.0 : std::abs(path_point_arc.distance);
+ // distance_average += lateral_distance_to_center_lane;
+ // distance_average = distance_average / path.poses.size();
+ const double acceptable_width = constraints.ego_width / 2.0;
+ // return distance_average / acceptable_width;
+ return lateral_distance_to_center_lane / acceptable_width;
+ });
// // Curvature cost
soft_constraints_.emplace_back(
@@ -183,7 +180,7 @@ SamplingPlannerModule::SamplingPlannerModule(
const auto curvature_average = curvature_sum / static_cast<double>(path.curvatures.size());
const auto max_curvature =
(curvature_average > 0.0) ? constraints.hard.max_curvature : constraints.hard.min_curvature;
- return 100.0 * curvature_average / max_curvature;
+ return 1000.0 * curvature_average / max_curvature;
// return constraints.soft.curvature_weight * curvature_sum /
// static_cast<double>(path.curvatures.size());
});
@@ -200,13 +197,73 @@ bool SamplingPlannerModule::isExecutionRequested() const
return false;
}
- const auto & goal_pose = planner_data_->route_handler->getGoalPose();
- const auto & ego_pose = planner_data_->self_odometry->pose.pose;
- const double distance_to_goal = std::hypot(
- ego_pose.position.x - goal_pose.position.x, ego_pose.position.y - goal_pose.position.y);
- if (distance_to_goal < 8.0) return false;
+ return !isReferencePathSafe();
+}
- return true;
+bool SamplingPlannerModule::isReferencePathSafe() const
+{
+ std::vector<DrivableLanes> drivable_lanes{};
+
+ const auto & prev_module_path = getPreviousModuleOutput().path;
+ const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ // expand drivable lanes
+ std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
+ auto d = generateExpandDrivableLanes(lanelet, planner_data_);
+ drivable_lanes.push_back(d);
+ });
+
+ {
+ const auto left_bound =
+ (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, true));
+ const auto right_bound =
+ (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, false));
+
+ const auto sampling_planner_data =
+ createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
+
+ prepareConstraints(
+ internal_params_->constraints, planner_data_->dynamic_object,
+ sampling_planner_data.left_bound, sampling_planner_data.right_bound);
+ }
+
+ std::vector<bool> hard_constraints_results;
+ auto transform_to_sampling_path = [](const PlanResult plan) {
+ sampler_common::Path path;
+ for (size_t i = 0; i < plan->points.size(); ++i) {
+ // plan->points[i].point.pose
+
+ const auto x = plan->points[i].point.pose.position.x;
+ const auto y = plan->points[i].point.pose.position.y;
+ const auto quat = plan->points[i].point.pose.orientation;
+ // tf2::Quaternion quaternion(q.x,q.y,q.z,q.w);
+ geometry_msgs::msg::Vector3 rpy;
+ tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w);
+ tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
+ // const auto z = plan->points[0].point.pose.position.z;
+ path.points.emplace_back(Point2d{x, y});
+ path.poses.emplace_back(plan->points[i].point.pose);
+ path.yaws.emplace_back(rpy.z);
+ }
+ return path;
+ };
+ sampler_common::Path reference_path =
+ transform_to_sampling_path(getPreviousModuleOutput().reference_path);
+ const auto footprint = sampler_common::constraints::buildFootprintPoints(
+ reference_path, internal_params_->constraints);
+
+ behavior_path_planner::HardConstraintsFunctionVector hard_constraints_reference_path;
+ hard_constraints_reference_path.emplace_back(
+ [](
+ sampler_common::Path & path, const sampler_common::Constraints & constraints,
+ const MultiPoint2d & footprint) -> bool {
+ path.constraint_results.collision =
+ !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons);
+ return path.constraint_results.collision;
+ });
+ behavior_path_planner::evaluateHardConstraints(
+ reference_path, internal_params_->constraints, footprint, hard_constraints_reference_path,
+ hard_constraints_results);
+ return reference_path.constraints_satisfied;
}
bool SamplingPlannerModule::isExecutionReady() const
@@ -216,7 +273,7 @@ bool SamplingPlannerModule::isExecutionReady() const
SamplingPlannerData SamplingPlannerModule::createPlannerData(
const PlanResult & path, const std::vector<geometry_msgs::msg::Point> & left_bound,
- const std::vector<geometry_msgs::msg::Point> & right_bound)
+ const std::vector<geometry_msgs::msg::Point> & right_bound) const
{
// create planner data
SamplingPlannerData data;
@@ -289,7 +346,7 @@ void SamplingPlannerModule::prepareConstraints(
sampler_common::Constraints & constraints,
const PredictedObjects::ConstSharedPtr & predicted_objects,
const std::vector<geometry_msgs::msg::Point> & left_bound,
- const std::vector<geometry_msgs::msg::Point> & right_bound)
+ const std::vector<geometry_msgs::msg::Point> & right_bound) const
{
constraints.obstacle_polygons = sampler_common::MultiPolygon2d();
for (const auto & o : predicted_objects->objects)
@@ -678,7 +735,7 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
}
bool SamplingPlannerModule::isEndPointsConnected(
- const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane)
+ const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) const
{
const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint();
const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint();
@@ -688,7 +745,8 @@ bool SamplingPlannerModule::isEndPointsConnected(
}
DrivableLanes SamplingPlannerModule::generateExpandDrivableLanes(
- const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data)
+ const lanelet::ConstLanelet & lanelet,
+ const std::shared_ptr<const PlannerData> & planner_data) const
{
const auto & route_handler = planner_data->route_handler;
From 0dfdaeb2bb06680f24525bbd79b3bd1f63107f1d Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 5 Dec 2023 14:54:10 +0900
Subject: [PATCH 059/115] update success transition
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 24 +++++++++++++++++--
.../sampling_planner_module.cpp | 10 --------
2 files changed, 22 insertions(+), 12 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 8f64e482dd29f..641e7cffa6d43 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -148,6 +148,8 @@ class SamplingPlannerModule : public SceneModuleInterface
{
const auto ego_pose = planner_data_->self_odometry->pose.pose;
const auto prev_module_path = getPreviousModuleOutput().path;
+ const auto prev_module_reference_path = getPreviousModuleOutput().reference_path;
+
const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
const auto ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose);
const auto goal_pose = planner_data_->route_handler->getGoalPose();
@@ -156,8 +158,26 @@ class SamplingPlannerModule : public SceneModuleInterface
const double min_target_length = *std::min_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
- return isReferencePathSafe() &&
- ((std::abs(ego_arc.distance) < 1.5) || (length_to_goal < min_target_length));
+ const auto nearest_index =
+ motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose);
+ double yaw_difference = 0.0;
+ if (nearest_index) {
+ auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double {
+ geometry_msgs::msg::Vector3 rpy;
+ tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w);
+ tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
+ return rpy.z;
+ };
+ const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation;
+ const double ref_path_yaw = toYaw(quat);
+ const double ego_yaw = toYaw(ego_pose.orientation);
+ yaw_difference = std::abs(ego_yaw - ref_path_yaw);
+ }
+ constexpr double pi = 3.14159;
+ const bool merged_back_to_path =
+ ((length_to_goal < min_target_length) || (std::abs(ego_arc.distance)) < 1.5) &&
+ (yaw_difference < pi / 18.0);
+ return isReferencePathSafe() && (merged_back_to_path);
}
bool canTransitFailureState() override { return false; }
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index aa7b7438b65ff..6e31a7b193c97 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -145,16 +145,6 @@ SamplingPlannerModule::SamplingPlannerModule(
const double min_target_length = *std::min_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
-
- // double distance_average{0.0};
- // for (const auto pose : path.poses) {
- // const auto & path_point_arc =
- // lanelet::utils::getArcCoordinates(input_data.current_lanes, pose);
- // const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
- // const double lateral_distance_to_center_lane =
- // (distance_point_to_goal < min_target_length) ? 0.0 : std::abs(path_point_arc.distance);
- // distance_average += lateral_distance_to_center_lane;
- // }
const auto & path_point_arc =
lanelet::utils::getArcCoordinates(input_data.current_lanes, path.poses.back());
const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
From 6810e31e3bdb9665e4c293433dfd1ed72a2da8c5 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 5 Dec 2023 17:08:26 +0900
Subject: [PATCH 060/115] Solve bug with goal not being in any current lanelet
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 3 +-
.../sampling_planner_module.cpp | 90 +++++++++++++------
2 files changed, 66 insertions(+), 27 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 641e7cffa6d43..8b5f8a3e2d9fb 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -17,6 +17,7 @@
#include "behavior_path_planner/marker_utils/utils.hpp"
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp"
#include "behavior_path_planner/utils/path_utils.hpp"
#include "behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp"
#include "behavior_path_planner/utils/sampling_planner/util.hpp"
@@ -176,7 +177,7 @@ class SamplingPlannerModule : public SceneModuleInterface
constexpr double pi = 3.14159;
const bool merged_back_to_path =
((length_to_goal < min_target_length) || (std::abs(ego_arc.distance)) < 1.5) &&
- (yaw_difference < pi / 18.0);
+ (yaw_difference < pi / 18.0); // TODO(Daniel) magic numbers
return isReferencePathSafe() && (merged_back_to_path);
}
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 6e31a7b193c97..9c16e6a2a98b5 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -88,13 +88,8 @@ SamplingPlannerModule::SamplingPlannerModule(
sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.points.empty()) return 0.0;
- if (!std::any_of(
- input_data.current_lanes.begin(), input_data.current_lanes.end(),
- [&](const auto & lane) {
- return lanelet::utils::isInLanelet(input_data.goal_pose, lane);
- }))
- return 0.0;
if (path.poses.empty()) return 0.0;
+
const auto & goal_pose = input_data.goal_pose;
const auto & ego_pose = input_data.ego_pose;
const auto & path_last_pose = path.poses.back();
@@ -133,13 +128,6 @@ SamplingPlannerModule::SamplingPlannerModule(
[[maybe_unused]] const sampler_common::Constraints & constraints,
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.poses.empty()) return 0.0;
- if (!std::any_of(
- input_data.current_lanes.begin(), input_data.current_lanes.end(),
- [&](const auto & lane) {
- return lanelet::utils::isInLanelet(input_data.goal_pose, lane);
- }))
- return 0.0;
-
const auto & goal_pose = input_data.goal_pose;
const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes, goal_pose);
const double min_target_length = *std::min_element(
@@ -193,9 +181,19 @@ bool SamplingPlannerModule::isExecutionRequested() const
bool SamplingPlannerModule::isReferencePathSafe() const
{
std::vector<DrivableLanes> drivable_lanes{};
-
const auto & prev_module_path = getPreviousModuleOutput().path;
- const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ const auto & p = planner_data_->parameters;
+ const auto ego_pose = planner_data_->self_odometry->pose.pose;
+ lanelet::ConstLanelet current_lane;
+
+ if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("behavior_path_planner").get_child("utils"),
+ "failed to find closest lanelet within route!!!");
+ return {};
+ }
+ const auto current_lanes = planner_data_->route_handler->getLaneletSequence(
+ current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
// expand drivable lanes
std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
auto d = generateExpandDrivableLanes(lanelet, planner_data_);
@@ -420,7 +418,20 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
std::vector<DrivableLanes> drivable_lanes{};
const auto & prev_module_path = getPreviousModuleOutput().path;
- const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ // const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+
+ const auto & p = planner_data_->parameters;
+ const auto ego_pose = planner_data_->self_odometry->pose.pose;
+ lanelet::ConstLanelet current_lane;
+
+ if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("behavior_path_planner").get_child("utils"),
+ "failed to find closest lanelet within route!!!");
+ return {};
+ }
+ const auto current_lanes = planner_data_->route_handler->getLaneletSequence(
+ current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
// expand drivable lanes
std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
@@ -442,10 +453,25 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
auto frenet_paths =
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
- // if (prev_sampling_path_) {
- // const auto prev_path = prev_sampling_path_.value();
- // frenet_paths.push_back(prev_path);
- // }
+
+ auto get_goal_pose = [&]() {
+ auto goal_pose = planner_data_->route_handler->getGoalPose();
+ if (!std::any_of(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) {
+ return lanelet::utils::isInLanelet(goal_pose, lane);
+ })) {
+ // std::cerr << "goal not in current lanes. Finidng farthest path point to use as goal\n";
+ for (auto it = reference_path_ptr->points.rbegin(); it < reference_path_ptr->points.rend();
+ it++) {
+ if (std::any_of(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) {
+ return lanelet::utils::isInLanelet(it->point.pose, lane);
+ })) {
+ goal_pose = it->point.pose;
+ break;
+ }
+ }
+ }
+ return goal_pose;
+ };
// EXTEND prev path
if (prev_sampling_path_ && prev_sampling_path_->lengths.size() > 1) {
@@ -455,7 +481,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
geometry_msgs::msg::Pose future_pose = prev_path_frenet.poses.back();
const double length_path = lanelet::utils::getArcCoordinates(current_lanes, future_pose).length;
- const auto goal_pose = planner_data_->route_handler->getGoalPose();
+ const auto goal_pose = get_goal_pose();
const double length_goal = lanelet::utils::getArcCoordinates(current_lanes, goal_pose).length;
const double min_target_length = *std::min_element(
@@ -464,7 +490,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
// double x = prev_path_frenet.points.back().x();
// double x_pose = pose.position.x;
- if (std::abs(length_goal - length_path) > min_target_length) {
+ if (std::abs(length_goal - length_path) > min_target_length / 2.0) {
// sampler_common::State reuse_state;
// reuse_state.curvature = reused_path->curvatures.back();
// reuse_state.pose = reused_path->points.back();
@@ -504,8 +530,9 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}
SoftConstraintsInputs soft_constraints_input;
- soft_constraints_input.goal_pose = planner_data_->route_handler->getGoalPose();
+ soft_constraints_input.goal_pose = get_goal_pose();
soft_constraints_input.ego_pose = planner_data_->self_odometry->pose.pose;
+ ;
soft_constraints_input.current_lanes = current_lanes;
soft_constraints_input.reference_path = reference_path_ptr;
soft_constraints_input.prev_module_path = prev_module_path;
@@ -580,7 +607,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
const auto road_lanes =
utils::getExtendedCurrentLanes(planner_data_, max_length, max_length, false);
auto out_path = convertFrenetPathToPathWithLaneID(
- best_path, road_lanes, soft_constraints_input.goal_pose.position.z);
+ best_path, road_lanes, planner_data_->route_handler->getGoalPose().position.z);
BehaviorModuleOutput out;
out.path = std::make_shared<PathWithLaneId>(out_path);
@@ -684,8 +711,19 @@ void SamplingPlannerModule::updateDebugMarkers()
void SamplingPlannerModule::extendOutputDrivableArea(BehaviorModuleOutput & output)
{
const auto prev_module_path = getPreviousModuleOutput().path;
- const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
-
+ // const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ const auto & p = planner_data_->parameters;
+ const auto ego_pose = planner_data_->self_odometry->pose.pose;
+ lanelet::ConstLanelet current_lane;
+
+ if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("behavior_path_planner").get_child("utils"),
+ "failed to find closest lanelet within route!!!");
+ return;
+ }
+ const auto current_lanes = planner_data_->route_handler->getLaneletSequence(
+ current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
std::vector<DrivableLanes> drivable_lanes{};
// expand drivable lanes
std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
From e7694dd6d7e434f9aa507ed1a2c3b03cee889ddc Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 5 Dec 2023 17:14:22 +0900
Subject: [PATCH 061/115] Add todo comment
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../scene_module/sampling_planner/sampling_planner_module.cpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 9c16e6a2a98b5..49f114671482b 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -68,6 +68,7 @@ SamplingPlannerModule::SamplingPlannerModule(
// TODO(Daniel): Maybe add a soft cost for average distance to centerline?
// TODO(Daniel): Think of methods to prevent chattering
+ // TODO(Daniel): Add penalty for not ending up on the same line as ref path?
// Yaw difference
// soft_constraints_.emplace_back(
From f26ad3835c212a3df9adb5435add3a16f31c6a1c Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 6 Dec 2023 09:25:37 +0900
Subject: [PATCH 062/115] Adjust to centerline cost
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 28 +++++++++----------
.../sampling_planner_module.cpp | 23 +++++++++------
2 files changed, 29 insertions(+), 22 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 8b5f8a3e2d9fb..6b34df63dfbc7 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -24,6 +24,7 @@
#include "behavior_path_planner/utils/utils.hpp"
#include "bezier_sampler/bezier_sampling.hpp"
#include "frenet_planner/frenet_planner.hpp"
+#include "lanelet2_extension/utility/query.hpp"
#include "lanelet2_extension/utility/utilities.hpp"
#include "motion_utils/trajectory/path_with_lane_id.hpp"
#include "rclcpp/rclcpp.hpp"
@@ -162,22 +163,21 @@ class SamplingPlannerModule : public SceneModuleInterface
const auto nearest_index =
motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose);
double yaw_difference = 0.0;
- if (nearest_index) {
- auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double {
- geometry_msgs::msg::Vector3 rpy;
- tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w);
- tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
- return rpy.z;
- };
- const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation;
- const double ref_path_yaw = toYaw(quat);
- const double ego_yaw = toYaw(ego_pose.orientation);
- yaw_difference = std::abs(ego_yaw - ref_path_yaw);
- }
+ if (!nearest_index) return false;
+ auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double {
+ geometry_msgs::msg::Vector3 rpy;
+ tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w);
+ tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
+ return rpy.z;
+ };
+ const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation;
+ const double ref_path_yaw = toYaw(quat);
+ const double ego_yaw = toYaw(ego_pose.orientation);
+ yaw_difference = std::abs(ego_yaw - ref_path_yaw);
constexpr double pi = 3.14159;
const bool merged_back_to_path =
- ((length_to_goal < min_target_length) || (std::abs(ego_arc.distance)) < 1.5) &&
- (yaw_difference < pi / 18.0); // TODO(Daniel) magic numbers
+ ((length_to_goal < min_target_length) || (std::abs(ego_arc.distance)) < 0.5) &&
+ (yaw_difference < pi / 36.0); // TODO(Daniel) magic numbers
return isReferencePathSafe() && (merged_back_to_path);
}
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 49f114671482b..28bc50ab3d53e 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -69,6 +69,7 @@ SamplingPlannerModule::SamplingPlannerModule(
// TODO(Daniel): Maybe add a soft cost for average distance to centerline?
// TODO(Daniel): Think of methods to prevent chattering
// TODO(Daniel): Add penalty for not ending up on the same line as ref path?
+ // TODO(Daniel): Length increasing curvature cost, increase curvature cost the longer the path is
// Yaw difference
// soft_constraints_.emplace_back(
@@ -130,17 +131,23 @@ SamplingPlannerModule::SamplingPlannerModule(
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.poses.empty()) return 0.0;
const auto & goal_pose = input_data.goal_pose;
- const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes, goal_pose);
- const double min_target_length = *std::min_element(
- internal_params_->sampling.target_lengths.begin(),
- internal_params_->sampling.target_lengths.end());
+ lanelet::ConstLanelet closest_lanelet;
+ lanelet::utils::query::getClosestLanelet(
+ input_data.current_lanes, goal_pose, &closest_lanelet);
+ lanelet::ConstLanelets closest_lanelets{closest_lanelet};
+ // const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes,
+ // goal_pose);
+ // const double min_target_length = *std::min_element(
+ // internal_params_->sampling.target_lengths.begin(),
+ // internal_params_->sampling.target_lengths.end());
const auto & path_point_arc =
- lanelet::utils::getArcCoordinates(input_data.current_lanes, path.poses.back());
- const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
- const double lateral_distance_to_center_lane =
- (distance_point_to_goal < min_target_length) ? 0.0 : std::abs(path_point_arc.distance);
+ lanelet::utils::getArcCoordinates(closest_lanelets, path.poses.back());
+ // const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
+ // const double lateral_distance_to_center_lane =
+ // (distance_point_to_goal < min_target_length) ? 0.0 : std::abs(path_point_arc.distance);
// distance_average += lateral_distance_to_center_lane;
// distance_average = distance_average / path.poses.size();
+ const double lateral_distance_to_center_lane = std::abs(path_point_arc.distance);
const double acceptable_width = constraints.ego_width / 2.0;
// return distance_average / acceptable_width;
return lateral_distance_to_center_lane / acceptable_width;
From f5a2eb69463120d640295c0236983231efcb8172 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 6 Dec 2023 13:01:20 +0900
Subject: [PATCH 063/115] update soft costs
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.cpp | 41 +++++++++++++++----
1 file changed, 34 insertions(+), 7 deletions(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 28bc50ab3d53e..d11457e6b9794 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -131,26 +131,27 @@ SamplingPlannerModule::SamplingPlannerModule(
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.poses.empty()) return 0.0;
const auto & goal_pose = input_data.goal_pose;
- lanelet::ConstLanelet closest_lanelet;
+ lanelet::ConstLanelet closest_lanelet_to_goal;
lanelet::utils::query::getClosestLanelet(
- input_data.current_lanes, goal_pose, &closest_lanelet);
- lanelet::ConstLanelets closest_lanelets{closest_lanelet};
+ input_data.current_lanes, goal_pose, &closest_lanelet_to_goal);
+ lanelet::ConstLanelets closest_lanelets_to_goal{closest_lanelet_to_goal};
// const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes,
// goal_pose);
// const double min_target_length = *std::min_element(
// internal_params_->sampling.target_lengths.begin(),
// internal_params_->sampling.target_lengths.end());
const auto & path_point_arc =
- lanelet::utils::getArcCoordinates(closest_lanelets, path.poses.back());
+ lanelet::utils::getArcCoordinates(closest_lanelets_to_goal, path.poses.back());
// const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
// const double lateral_distance_to_center_lane =
// (distance_point_to_goal < min_target_length) ? 0.0 : std::abs(path_point_arc.distance);
// distance_average += lateral_distance_to_center_lane;
// distance_average = distance_average / path.poses.size();
const double lateral_distance_to_center_lane = std::abs(path_point_arc.distance);
- const double acceptable_width = constraints.ego_width / 2.0;
+
+ // const double acceptable_width = constraints.ego_width / 2.0;
// return distance_average / acceptable_width;
- return lateral_distance_to_center_lane / acceptable_width;
+ return lateral_distance_to_center_lane;
});
// // Curvature cost
@@ -170,6 +171,26 @@ SamplingPlannerModule::SamplingPlannerModule(
// return constraints.soft.curvature_weight * curvature_sum /
// static_cast<double>(path.curvatures.size());
});
+ // Same lane as goal cost
+ // soft_constraints_.emplace_back(
+ // [](
+ // sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints &
+ // constraints,
+ // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
+ // if (path.points.empty()) return 0.0;
+ // // double curvature_sum = 0.0;
+ // // for (const auto curvature : path.curvatures) {
+ // // curvature_sum += std::abs(curvature);
+ // // }
+ // // const auto curvature_average = curvature_sum /
+ // static_cast<double>(path.curvatures.size());
+ // // const auto max_curvature =
+ // // (curvature_average > 0.0) ? constraints.hard.max_curvature :
+ // // constraints.hard.min_curvature;
+ // // return 1000.0 * curvature_average / max_curvature;
+ // // // return constraints.soft.curvature_weight * curvature_sum /
+ // // // static_cast<double>(path.curvatures.size());
+ // });
}
bool SamplingPlannerModule::isExecutionRequested() const
@@ -496,9 +517,15 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
+ const double max_target_length = *std::max_element(
+ internal_params_->sampling.target_lengths.begin(),
+ internal_params_->sampling.target_lengths.end());
+
// double x = prev_path_frenet.points.back().x();
// double x_pose = pose.position.x;
- if (std::abs(length_goal - length_path) > min_target_length / 2.0) {
+ if (
+ std::abs(length_goal - length_path) > min_target_length / 2.0 &&
+ length_path < 2.0 * max_target_length) {
// sampler_common::State reuse_state;
// reuse_state.curvature = reused_path->curvatures.back();
// reuse_state.pose = reused_path->points.back();
From 2bc08ce2075b80ea13e6b284ead05594e41be60f Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 6 Dec 2023 16:23:35 +0900
Subject: [PATCH 064/115] tuning
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 39 +++++++-
.../sampling_planner_module.cpp | 93 +++++++++++--------
2 files changed, 89 insertions(+), 43 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 6b34df63dfbc7..16ab99dd9cf18 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -148,11 +148,36 @@ class SamplingPlannerModule : public SceneModuleInterface
bool canTransitSuccessState() override
{
- const auto ego_pose = planner_data_->self_odometry->pose.pose;
- const auto prev_module_path = getPreviousModuleOutput().path;
+ std::vector<DrivableLanes> drivable_lanes{};
+ const auto & prev_module_path = getPreviousModuleOutput().path;
const auto prev_module_reference_path = getPreviousModuleOutput().reference_path;
- const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ const auto & p = planner_data_->parameters;
+ const auto ego_pose = planner_data_->self_odometry->pose.pose;
+ lanelet::ConstLanelet current_lane;
+
+ if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("behavior_path_planner").get_child("utils"),
+ "failed to find closest lanelet within route!!!");
+ return {};
+ }
+ const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
+ current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
+ // expand drivable lanes
+ std::for_each(
+ current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) {
+ drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
+ });
+
+ lanelet::ConstLanelets current_lanes;
+
+ for (auto & d : drivable_lanes) {
+ current_lanes.push_back(d.right_lane);
+ current_lanes.push_back(d.left_lane);
+ current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
+ }
+
const auto ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose);
const auto goal_pose = planner_data_->route_handler->getGoalPose();
const auto goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose);
@@ -175,9 +200,15 @@ class SamplingPlannerModule : public SceneModuleInterface
const double ego_yaw = toYaw(ego_pose.orientation);
yaw_difference = std::abs(ego_yaw - ref_path_yaw);
constexpr double pi = 3.14159;
+
+ lanelet::ConstLanelet closest_lanelet_to_ego;
+ lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
+ lanelet::ConstLanelet closest_lanelet_to_goal;
+ lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
const bool merged_back_to_path =
((length_to_goal < min_target_length) || (std::abs(ego_arc.distance)) < 0.5) &&
- (yaw_difference < pi / 36.0); // TODO(Daniel) magic numbers
+ (yaw_difference < pi / 36.0) &&
+ (closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id()); // TODO(Daniel) magic numbers
return isReferencePathSafe() && (merged_back_to_path);
}
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index d11457e6b9794..add3241871913 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -106,21 +106,23 @@ SamplingPlannerModule::SamplingPlannerModule(
constexpr double epsilon = 0.001;
if (distance_ego_to_goal_pose < epsilon) return 0.0;
const double length_path_to_goal = std::abs(path_last_arc.length - goal_arc.length);
- const double lateral_distance_to_goal = std::abs(path_last_arc.distance - goal_arc.distance);
+ // const double lateral_distance_to_goal = std::abs(path_last_arc.distance -
+ // goal_arc.distance);
- const double max_target_distance = *std::max_element(
- internal_params_->sampling.target_lateral_positions.begin(),
- internal_params_->sampling.target_lateral_positions.end());
+ // const double max_target_distance = *std::max_element(
+ // internal_params_->sampling.target_lateral_positions.begin(),
+ // internal_params_->sampling.target_lateral_positions.end());
const double max_target_length = *std::max_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
- const double reference_max_distance = std::hypot(max_target_length, max_target_distance);
+ // const double reference_max_distance = std::hypot(max_target_length, max_target_distance);
- return (length_path_to_goal / reference_max_distance) +
- (lateral_distance_to_goal /
- reference_max_distance); // + (angle_difference / (pi / 4.0));
+ // return (length_path_to_goal / reference_max_distance) +
+ // (lateral_distance_to_goal /
+ // reference_max_distance); // + (angle_difference / (pi / 4.0));
+ return length_path_to_goal / max_target_length;
});
// Distance to centerline
@@ -144,7 +146,8 @@ SamplingPlannerModule::SamplingPlannerModule(
lanelet::utils::getArcCoordinates(closest_lanelets_to_goal, path.poses.back());
// const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
// const double lateral_distance_to_center_lane =
- // (distance_point_to_goal < min_target_length) ? 0.0 : std::abs(path_point_arc.distance);
+ // (distance_point_to_goal < min_target_length) ? 0.0 :
+ std::abs(path_point_arc.distance);
// distance_average += lateral_distance_to_center_lane;
// distance_average = distance_average / path.poses.size();
const double lateral_distance_to_center_lane = std::abs(path_point_arc.distance);
@@ -167,7 +170,7 @@ SamplingPlannerModule::SamplingPlannerModule(
const auto curvature_average = curvature_sum / static_cast<double>(path.curvatures.size());
const auto max_curvature =
(curvature_average > 0.0) ? constraints.hard.max_curvature : constraints.hard.min_curvature;
- return 1000.0 * curvature_average / max_curvature;
+ return 50.0 * curvature_average / max_curvature;
// return constraints.soft.curvature_weight * curvature_sum /
// static_cast<double>(path.curvatures.size());
});
@@ -177,19 +180,16 @@ SamplingPlannerModule::SamplingPlannerModule(
// sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints &
// constraints,
// [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
- // if (path.points.empty()) return 0.0;
- // // double curvature_sum = 0.0;
- // // for (const auto curvature : path.curvatures) {
- // // curvature_sum += std::abs(curvature);
- // // }
- // // const auto curvature_average = curvature_sum /
- // static_cast<double>(path.curvatures.size());
- // // const auto max_curvature =
- // // (curvature_average > 0.0) ? constraints.hard.max_curvature :
- // // constraints.hard.min_curvature;
- // // return 1000.0 * curvature_average / max_curvature;
- // // // return constraints.soft.curvature_weight * curvature_sum /
- // // // static_cast<double>(path.curvatures.size());
+ // if (path.poses.empty()) return 0.0;
+
+ // const auto & goal_pose = input_data.goal_pose;
+ // lanelet::ConstLanelet closest_lanelet_to_goal;
+ // lanelet::utils::query::getClosestLanelet(
+ // input_data.current_lanes, goal_pose, &closest_lanelet_to_goal);
+ // lanelet::ConstLanelet closest_lanelet_to_path_end;
+ // lanelet::utils::query::getClosestLanelet(
+ // input_data.current_lanes, path.poses.back(), &closest_lanelet_to_path_end);
+ // return (closest_lanelet_to_goal.id() == closest_lanelet_to_path_end.id()) ? 0.0 : 0.5;
// });
}
@@ -221,13 +221,21 @@ bool SamplingPlannerModule::isReferencePathSafe() const
"failed to find closest lanelet within route!!!");
return {};
}
- const auto current_lanes = planner_data_->route_handler->getLaneletSequence(
+ const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
// expand drivable lanes
- std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
- auto d = generateExpandDrivableLanes(lanelet, planner_data_);
- drivable_lanes.push_back(d);
- });
+ std::for_each(
+ current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) {
+ drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
+ });
+
+ lanelet::ConstLanelets current_lanes;
+
+ for (auto & d : drivable_lanes) {
+ current_lanes.push_back(d.right_lane);
+ current_lanes.push_back(d.left_lane);
+ current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
+ }
{
const auto left_bound =
@@ -459,12 +467,21 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
"failed to find closest lanelet within route!!!");
return {};
}
- const auto current_lanes = planner_data_->route_handler->getLaneletSequence(
+ const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
// expand drivable lanes
- std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
- drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
- });
+ std::for_each(
+ current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) {
+ drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
+ });
+
+ lanelet::ConstLanelets current_lanes;
+
+ for (auto & d : drivable_lanes) {
+ current_lanes.push_back(d.right_lane);
+ current_lanes.push_back(d.left_lane);
+ current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
+ }
{
const auto left_bound =
@@ -517,15 +534,13 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
- const double max_target_length = *std::max_element(
- internal_params_->sampling.target_lengths.begin(),
- internal_params_->sampling.target_lengths.end());
+ // const double max_target_length = *std::max_element(
+ // internal_params_->sampling.target_lengths.begin(),
+ // internal_params_->sampling.target_lengths.end());
// double x = prev_path_frenet.points.back().x();
// double x_pose = pose.position.x;
- if (
- std::abs(length_goal - length_path) > min_target_length / 2.0 &&
- length_path < 2.0 * max_target_length) {
+ if (std::abs(length_goal - length_path) > min_target_length) {
// sampler_common::State reuse_state;
// reuse_state.curvature = reused_path->curvatures.back();
// reuse_state.pose = reused_path->points.back();
@@ -633,7 +648,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
std::cerr << "\n";
std::cerr << "Poses " << best_path.poses.size() << "\n";
-
+ std::cerr << "Length of best " << best_path.lengths.back() << "\n";
prev_sampling_path_ = best_path;
const double max_length = *std::max_element(
From db3c8b5282454478f08b9d60b409aba45c0ec15b Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 6 Dec 2023 18:31:05 +0900
Subject: [PATCH 065/115] add reference path change after sampling planner
Success (which might cause a LC
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../scene_module/sampling_planner/sampling_planner_module.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index add3241871913..1536c296f03ee 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -170,7 +170,7 @@ SamplingPlannerModule::SamplingPlannerModule(
const auto curvature_average = curvature_sum / static_cast<double>(path.curvatures.size());
const auto max_curvature =
(curvature_average > 0.0) ? constraints.hard.max_curvature : constraints.hard.min_curvature;
- return 50.0 * curvature_average / max_curvature;
+ return 500.0 * curvature_average / max_curvature;
// return constraints.soft.curvature_weight * curvature_sum /
// static_cast<double>(path.curvatures.size());
});
From 2bfa077aec00a8a431f94ed1c03af9f47e95eddf Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 7 Dec 2023 12:11:16 +0900
Subject: [PATCH 066/115] Added soft constraints weights as parameter to easily
tune w/ rqt
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 1 +
.../sampling_planner_parameters.hpp | 1 +
.../utils/sampling_planner/util.hpp | 22 +++++----
.../scene_module/sampling_planner/manager.cpp | 2 +
.../sampling_planner_module.cpp | 46 ++++++++-----------
.../include/sampler_common/structures.hpp | 1 +
6 files changed, 37 insertions(+), 36 deletions(-)
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
index 16ab99dd9cf18..d559a3dedd447 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
@@ -99,6 +99,7 @@ class SamplingPlannerModule : public SceneModuleInterface
user_params_->lateral_deviation_weight;
internal_params_->constraints.soft.length_weight = user_params_->length_weight;
internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
+ internal_params_->constraints.soft.weights = user_params_->weights;
internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.75);
internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
index d1a0980c66fec..56eeb0b2a3455 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
@@ -35,6 +35,7 @@ struct SamplingPlannerParameters
double lateral_deviation_weight;
double length_weight;
double curvature_weight;
+ std::vector<double> weights;
// sampling
bool enable_frenet;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index e4cca98464857..adabb33cbc60f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -19,9 +19,9 @@
#include <any>
#include <functional>
+#include <numeric>
#include <optional>
#include <vector>
-
namespace behavior_path_planner
{
using geometry_msgs::msg::Pose;
@@ -44,17 +44,21 @@ using HardConstraintsFunctionVector = std::vector<std::function<bool(
inline void evaluateSoftConstraints(
sampler_common::Path & path, const sampler_common::Constraints & constraints,
- const SoftConstraintsFunctionVector & soft_constraints, const SoftConstraintsInputs & input_data,
- std::vector<double> & constraints_results)
+ const SoftConstraintsFunctionVector & soft_constraints_functions,
+ const SoftConstraintsInputs & input_data, std::vector<double> & constraints_results)
{
constraints_results.clear();
- double constraints_evaluation = 0.0;
- for (const auto & f : soft_constraints) {
- const auto value = f(path, constraints, input_data);
- constraints_results.push_back(value);
- constraints_evaluation += value;
+ for (const auto & f : soft_constraints_functions) {
+ const auto cost = f(path, constraints, input_data);
+ constraints_results.push_back(cost);
+ }
+ if (constraints.soft.weights.size() != constraints_results.size()) {
+ path.cost = std::accumulate(constraints_results.begin(), constraints_results.end(), 0.0);
+ return;
}
- path.cost = constraints_evaluation;
+
+ path.cost = std::inner_product(
+ constraints_results.begin(), constraints_results.end(), constraints.soft.weights.begin(), 0.0);
return;
}
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
index 4b047d6447474..dd9cf62cb3eb7 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
@@ -38,6 +38,7 @@ SamplingPlannerModuleManager::SamplingPlannerModuleManager(
p.lateral_deviation_weight = node->declare_parameter<double>(ns + ".lateral_deviation_weight");
p.length_weight = node->declare_parameter<double>(ns + ".length_weight");
p.curvature_weight = node->declare_parameter<double>(ns + ".curvature_weight");
+ p.weights = node->declare_parameter<std::vector<double>>(ns + ".weights");
}
{
std::string ns{"sampling"};
@@ -83,6 +84,7 @@ void SamplingPlannerModuleManager::updateModuleParams(
updateParam<double>(parameters, ns + ".lateral_deviation_weight", p->lateral_deviation_weight);
updateParam<double>(parameters, ns + ".length_weight", p->length_weight);
updateParam<double>(parameters, ns + ".curvature_weight", p->curvature_weight);
+ updateParam<std::vector<double>>(parameters, ns + ".weights", p->weights);
}
{
std::string ns{"sampling"};
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 1536c296f03ee..558108db72fd3 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -137,23 +137,11 @@ SamplingPlannerModule::SamplingPlannerModule(
lanelet::utils::query::getClosestLanelet(
input_data.current_lanes, goal_pose, &closest_lanelet_to_goal);
lanelet::ConstLanelets closest_lanelets_to_goal{closest_lanelet_to_goal};
- // const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes,
- // goal_pose);
- // const double min_target_length = *std::min_element(
- // internal_params_->sampling.target_lengths.begin(),
- // internal_params_->sampling.target_lengths.end());
const auto & path_point_arc =
lanelet::utils::getArcCoordinates(closest_lanelets_to_goal, path.poses.back());
- // const double distance_point_to_goal = std::abs(path_point_arc.length - goal_arc.length);
- // const double lateral_distance_to_center_lane =
- // (distance_point_to_goal < min_target_length) ? 0.0 :
+
std::abs(path_point_arc.distance);
- // distance_average += lateral_distance_to_center_lane;
- // distance_average = distance_average / path.poses.size();
const double lateral_distance_to_center_lane = std::abs(path_point_arc.distance);
-
- // const double acceptable_width = constraints.ego_width / 2.0;
- // return distance_average / acceptable_width;
return lateral_distance_to_center_lane;
});
@@ -170,7 +158,7 @@ SamplingPlannerModule::SamplingPlannerModule(
const auto curvature_average = curvature_sum / static_cast<double>(path.curvatures.size());
const auto max_curvature =
(curvature_average > 0.0) ? constraints.hard.max_curvature : constraints.hard.min_curvature;
- return 500.0 * curvature_average / max_curvature;
+ return curvature_average / max_curvature;
// return constraints.soft.curvature_weight * curvature_sum /
// static_cast<double>(path.curvatures.size());
});
@@ -520,7 +508,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
};
// EXTEND prev path
- if (prev_sampling_path_ && prev_sampling_path_->lengths.size() > 1) {
+ if (prev_sampling_path_ && prev_sampling_path_->points.size() > 100000) {
// Update previous path
frenet_planner::Path prev_path_frenet = prev_sampling_path_.value();
frenet_paths.push_back(prev_path_frenet);
@@ -541,11 +529,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
// double x = prev_path_frenet.points.back().x();
// double x_pose = pose.position.x;
if (std::abs(length_goal - length_path) > min_target_length) {
- // sampler_common::State reuse_state;
- // reuse_state.curvature = reused_path->curvatures.back();
- // reuse_state.pose = reused_path->points.back();
- // reuse_state.heading = reused_path->yaws.back();
- // reuse_state.frenet = reference_spline.frenet(reuse_state.pose);
auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion {
tf2::Quaternion quaternion_tf2;
quaternion_tf2.setRPY(roll, pitch, yaw);
@@ -582,7 +565,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
SoftConstraintsInputs soft_constraints_input;
soft_constraints_input.goal_pose = get_goal_pose();
soft_constraints_input.ego_pose = planner_data_->self_odometry->pose.pose;
- ;
+
soft_constraints_input.current_lanes = current_lanes;
soft_constraints_input.reference_path = reference_path_ptr;
soft_constraints_input.prev_module_path = prev_module_path;
@@ -618,6 +601,12 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
debug_data_.obstacles = internal_params_->constraints.obstacle_polygons;
updateDebugMarkers();
+ const double max_length = *std::max_element(
+ internal_params_->sampling.target_lengths.begin(),
+ internal_params_->sampling.target_lengths.end());
+ const auto road_lanes = utils::getExtendedCurrentLanes(
+ planner_data_, max_length, max_length, false); // Do these max lengths make sense?
+
const auto best_path_idx = [](const auto & paths) {
auto min_cost = std::numeric_limits<double>::max();
size_t best_path_idx = 0;
@@ -630,11 +619,19 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
return min_cost < std::numeric_limits<double>::max() ? std::optional<size_t>(best_path_idx)
: std::nullopt;
};
+
const auto selected_path_idx = best_path_idx(frenet_paths);
if (!selected_path_idx) {
BehaviorModuleOutput out;
- out.path = getPreviousModuleOutput().path;
+ PathWithLaneId out_path = (prev_sampling_path_)
+ ? convertFrenetPathToPathWithLaneID(
+ prev_sampling_path_.value(), road_lanes,
+ planner_data_->route_handler->getGoalPose().position.z)
+ : PathWithLaneId{};
+
+ out.path = (prev_sampling_path_) ? std::make_shared<PathWithLaneId>(out_path)
+ : getPreviousModuleOutput().path;
out.reference_path = getPreviousModuleOutput().reference_path;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
return out;
@@ -651,11 +648,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
std::cerr << "Length of best " << best_path.lengths.back() << "\n";
prev_sampling_path_ = best_path;
- const double max_length = *std::max_element(
- internal_params_->sampling.target_lengths.begin(),
- internal_params_->sampling.target_lengths.end());
- const auto road_lanes =
- utils::getExtendedCurrentLanes(planner_data_, max_length, max_length, false);
auto out_path = convertFrenetPathToPathWithLaneID(
best_path, road_lanes, planner_data_->route_handler->getGoalPose().position.z);
diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
index edc85b5ad8665..e546bc7571731 100644
--- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
+++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
@@ -335,6 +335,7 @@ struct Constraints
double lateral_deviation_weight;
double length_weight;
double curvature_weight;
+ std::vector<double> weights;
} soft{};
struct
{
From 003e51b1f7b4d69169597d3f6852047ae3664434 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 8 Dec 2023 13:52:00 +0900
Subject: [PATCH 067/115] improve performance by computing arc coordinates
before soft constraints check
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../behavior_planning.launch.xml | 11 ++--
.../utils/sampling_planner/util.hpp | 3 ++
.../sampling_planner_module.cpp | 54 ++++++-------------
3 files changed, 27 insertions(+), 41 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index e83c6ab8a13a8..01a410c711435 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -175,8 +175,7 @@
/>
<let name="behavior_velocity_planner_launch_modules" value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + '$(var launch_module_list_end)'")"/>
- <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
- <composable_node pkg="behavior_path_planner" plugin="behavior_path_planner::BehaviorPathPlannerNode" name="behavior_path_planner" namespace="">
+ <node pkg="behavior_path_planner" exec="behavior_path_planner" name="behavior_path_planner" namespace="">
<!-- topic remap -->
<remap from="~/input/route" to="$(var input_route_topic_name)"/>
<remap from="~/input/vector_map" to="$(var input_vector_map_topic_name)"/>
@@ -224,8 +223,12 @@
<param from="$(var behavior_path_planner_scene_module_manager_param_path)"/>
<param from="$(var behavior_path_planner_common_param_path)"/>
<!-- composable node config -->
- <extra_arg name="use_intra_process_comms" value="false"/>
- </composable_node>
+ <!-- <extra_arg name="use_intra_process_comms" value="false"/> -->
+ </node>
+
+
+ <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
+
<composable_node pkg="behavior_velocity_planner" plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode" name="behavior_velocity_planner" namespace="">
<!-- topic remap -->
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
index adabb33cbc60f..b032f19ae7d1a 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
@@ -30,6 +30,9 @@ struct SoftConstraintsInputs
{
Pose goal_pose;
Pose ego_pose;
+ lanelet::ArcCoordinates ego_arc;
+ lanelet::ArcCoordinates goal_arc;
+ lanelet::ConstLanelets closest_lanelets_to_goal;
behavior_path_planner::PlanResult reference_path;
behavior_path_planner::PlanResult prev_module_path;
std::optional<sampler_common::Path> prev_path;
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
index 558108db72fd3..ee01fa813f7f6 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
@@ -91,37 +91,17 @@ SamplingPlannerModule::SamplingPlannerModule(
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.points.empty()) return 0.0;
if (path.poses.empty()) return 0.0;
-
- const auto & goal_pose = input_data.goal_pose;
- const auto & ego_pose = input_data.ego_pose;
- const auto & path_last_pose = path.poses.back();
-
- const auto ego_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes, ego_pose);
- const auto goal_arc = lanelet::utils::getArcCoordinates(input_data.current_lanes, goal_pose);
- const auto path_last_arc =
- lanelet::utils::getArcCoordinates(input_data.current_lanes, path_last_pose);
-
- const double distance_ego_to_goal_pose = std::abs(ego_arc.length - goal_arc.length);
+ const double distance_ego_to_goal_pose =
+ std::abs(input_data.ego_arc.length - input_data.goal_arc.length);
constexpr double epsilon = 0.001;
if (distance_ego_to_goal_pose < epsilon) return 0.0;
- const double length_path_to_goal = std::abs(path_last_arc.length - goal_arc.length);
- // const double lateral_distance_to_goal = std::abs(path_last_arc.distance -
- // goal_arc.distance);
-
- // const double max_target_distance = *std::max_element(
- // internal_params_->sampling.target_lateral_positions.begin(),
- // internal_params_->sampling.target_lateral_positions.end());
-
+ const double length_last_point_on_path = input_data.ego_arc.length + path.lengths.back();
+ const double length_path_to_goal =
+ std::abs(length_last_point_on_path - input_data.goal_arc.length);
const double max_target_length = *std::max_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
-
- // const double reference_max_distance = std::hypot(max_target_length, max_target_distance);
-
- // return (length_path_to_goal / reference_max_distance) +
- // (lateral_distance_to_goal /
- // reference_max_distance); // + (angle_difference / (pi / 4.0));
return length_path_to_goal / max_target_length;
});
@@ -132,15 +112,8 @@ SamplingPlannerModule::SamplingPlannerModule(
[[maybe_unused]] const sampler_common::Constraints & constraints,
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.poses.empty()) return 0.0;
- const auto & goal_pose = input_data.goal_pose;
- lanelet::ConstLanelet closest_lanelet_to_goal;
- lanelet::utils::query::getClosestLanelet(
- input_data.current_lanes, goal_pose, &closest_lanelet_to_goal);
- lanelet::ConstLanelets closest_lanelets_to_goal{closest_lanelet_to_goal};
const auto & path_point_arc =
- lanelet::utils::getArcCoordinates(closest_lanelets_to_goal, path.poses.back());
-
- std::abs(path_point_arc.distance);
+ lanelet::utils::getArcCoordinates(input_data.closest_lanelets_to_goal, path.poses.back());
const double lateral_distance_to_center_lane = std::abs(path_point_arc.distance);
return lateral_distance_to_center_lane;
});
@@ -162,7 +135,7 @@ SamplingPlannerModule::SamplingPlannerModule(
// return constraints.soft.curvature_weight * curvature_sum /
// static_cast<double>(path.curvatures.size());
});
- // Same lane as goal cost
+ // Same lane as goal cost Still in testing dont delete
// soft_constraints_.emplace_back(
// [](
// sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints &
@@ -508,7 +481,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
};
// EXTEND prev path
- if (prev_sampling_path_ && prev_sampling_path_->points.size() > 100000) {
+ if (prev_sampling_path_ && prev_sampling_path_->points.size()) {
// Update previous path
frenet_planner::Path prev_path_frenet = prev_sampling_path_.value();
frenet_paths.push_back(prev_path_frenet);
@@ -563,13 +536,20 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}
SoftConstraintsInputs soft_constraints_input;
- soft_constraints_input.goal_pose = get_goal_pose();
- soft_constraints_input.ego_pose = planner_data_->self_odometry->pose.pose;
+ const auto & goal_pose = get_goal_pose();
+ soft_constraints_input.goal_pose = soft_constraints_input.ego_pose =
+ planner_data_->self_odometry->pose.pose;
soft_constraints_input.current_lanes = current_lanes;
soft_constraints_input.reference_path = reference_path_ptr;
soft_constraints_input.prev_module_path = prev_module_path;
+ soft_constraints_input.ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose);
+ soft_constraints_input.goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose);
+ lanelet::ConstLanelet closest_lanelet_to_goal;
+ lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
+ soft_constraints_input.closest_lanelets_to_goal = {closest_lanelet_to_goal};
+
debug_data_.footprints.clear();
std::vector<std::vector<bool>> hard_constraints_results_full;
std::vector<std::vector<double>> soft_constraints_results_full;
From bf2ffedcea4c30968b882f9a3543003835673b08 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 12 Dec 2023 12:38:01 +0900
Subject: [PATCH 068/115] temp
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../src/behavior_path_planner_node.cpp | 7 -
.../CMakeLists.txt | 13 +
.../config}/sampling_planner.param.yaml | 0
.../constraints_checker.hpp | 0
.../manager.hpp | 54 ++++
.../sampling_planner_module.hpp | 259 ++++++++++++++++++
.../sampling_planner_parameters.hpp | 0
.../util.hpp | 0
.../include}/manager.hpp | 6 +-
.../include}/sampling_planner_module.hpp | 14 +-
.../package.xml | 36 +++
.../plugins.xml | 3 +
.../src}/manager.cpp | 9 +-
.../src}/sampling_planner_module.cpp | 8 +-
14 files changed, 385 insertions(+), 24 deletions(-)
create mode 100644 planning/behavior_path_sampling_planner_module/CMakeLists.txt
rename planning/{behavior_path_planner/config/sampling_planner => behavior_path_sampling_planner_module/config}/sampling_planner.param.yaml (100%)
rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner => behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module}/constraints_checker.hpp (100%)
create mode 100644 planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
create mode 100644 planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
rename planning/{behavior_path_planner/include/behavior_path_planner/utils/sampling_planner => behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module}/sampling_planner_parameters.hpp (100%)
rename planning/{behavior_path_planner/include/behavior_path_planner/utils/sampling_planner => behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module}/util.hpp (100%)
rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner => behavior_path_sampling_planner_module/include}/manager.hpp (85%)
rename planning/{behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner => behavior_path_sampling_planner_module/include}/sampling_planner_module.hpp (94%)
create mode 100644 planning/behavior_path_sampling_planner_module/package.xml
create mode 100644 planning/behavior_path_sampling_planner_module/plugins.xml
rename planning/{behavior_path_planner/src/scene_module/sampling_planner => behavior_path_sampling_planner_module/src}/manager.cpp (95%)
rename planning/{behavior_path_planner/src/scene_module/sampling_planner => behavior_path_sampling_planner_module/src}/sampling_planner_module.cpp (99%)
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 6f8bfff3d6522..3eb5eca5954fb 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -144,13 +144,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
path_reference_publishers_.emplace(
manager->name(), create_publisher<Path>(path_reference_name_space + manager->name(), 1));
}
- if (p.config_sampling_planner.enable_module) {
- RCLCPP_INFO(get_logger(), "The priority is %d", p.config_sampling_planner.priority);
-
- auto manager = std::make_shared<SamplingPlannerModuleManager>(
- this, "sampling_planner", p.config_sampling_planner);
- planner_manager_->registerSceneModuleManager(manager);
- }
}
m_set_param_res = this->add_on_set_parameters_callback(
diff --git a/planning/behavior_path_sampling_planner_module/CMakeLists.txt b/planning/behavior_path_sampling_planner_module/CMakeLists.txt
new file mode 100644
index 0000000000000..4d2198e7748c7
--- /dev/null
+++ b/planning/behavior_path_sampling_planner_module/CMakeLists.txt
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.14)
+project(behavior_path_sampling_planner_module)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+ src/sampling_planner_module.cpp
+ src/manager.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE config)
diff --git a/planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml b/planning/behavior_path_sampling_planner_module/config/sampling_planner.param.yaml
similarity index 100%
rename from planning/behavior_path_planner/config/sampling_planner/sampling_planner.param.yaml
rename to planning/behavior_path_sampling_planner_module/config/sampling_planner.param.yaml
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/constraints_checker.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp
similarity index 100%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/constraints_checker.hpp
rename to planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
new file mode 100644
index 0000000000000..6a4e7d35ce94c
--- /dev/null
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
@@ -0,0 +1,54 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
+
+#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
+#include "behavior_path_sampling_planner_module/sampling_planner_module.hpp"
+#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+class SamplingPlannerModuleManager : public SceneModuleManagerInterface
+{
+public:
+ SamplingPlannerModuleManager() : SceneModuleManagerInterface{"side_shift"} {}
+ void init(rclcpp::Node * node) override;
+
+ std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+ {
+ return std::make_unique<SamplingPlannerModule>(
+ name_, *node_, parameters_, rtc_interface_ptr_map_,
+ objects_of_interest_marker_interface_ptr_map_);
+ }
+
+ void updateModuleParams(
+ [[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) override;
+
+private:
+ std::shared_ptr<SamplingPlannerParameters> parameters_;
+};
+
+} // namespace behavior_path_planner
+
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
new file mode 100644
index 0000000000000..a2f6f78968d36
--- /dev/null
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -0,0 +1,259 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
+
+#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
+#include "behavior_path_planner_common/marker_utils/utils.hpp"
+#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp"
+#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
+#include "behavior_path_planner_common/utils/path_utils.hpp"
+#include "behavior_path_planner_common/utils/utils.hpp"
+#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp"
+#include "behavior_path_sampling_planner_module/util.hpp"
+#include "bezier_sampler/bezier_sampling.hpp"
+#include "frenet_planner/frenet_planner.hpp"
+#include "lanelet2_extension/utility/query.hpp"
+#include "lanelet2_extension/utility/utilities.hpp"
+#include "motion_utils/trajectory/path_with_lane_id.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sampler_common/constraints/footprint.hpp"
+#include "sampler_common/constraints/hard_constraint.hpp"
+#include "sampler_common/constraints/soft_constraint.hpp"
+#include "sampler_common/structures.hpp"
+#include "sampler_common/transform/spline_transform.hpp"
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
+#include "tier4_autoware_utils/math/constants.hpp"
+#include "tier4_autoware_utils/ros/update_param.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
+
+#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
+#include "tier4_planning_msgs/msg/lateral_offset.hpp"
+
+#include <algorithm>
+#include <any>
+#include <memory>
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+namespace behavior_path_planner
+{
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+struct SamplingPlannerData
+{
+ // input
+ std::vector<TrajectoryPoint> traj_points; // converted from the input path
+ std::vector<geometry_msgs::msg::Point> left_bound;
+ std::vector<geometry_msgs::msg::Point> right_bound;
+
+ // ego
+ geometry_msgs::msg::Pose ego_pose;
+ double ego_vel{};
+};
+
+struct SamplingPlannerDebugData
+{
+ std::vector<sampler_common::Path> sampled_candidates{};
+ size_t previous_sampled_candidates_nb = 0UL;
+ std::vector<tier4_autoware_utils::Polygon2d> obstacles{};
+ std::vector<tier4_autoware_utils::MultiPoint2d> footprints{};
+};
+class SamplingPlannerModule : public SceneModuleInterface
+{
+public:
+ SamplingPlannerModule(
+ const std::string & name, rclcpp::Node & node,
+ const std::shared_ptr<SamplingPlannerParameters> & parameters,
+ const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+ std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+ objects_of_interest_marker_interface_ptr_map);
+
+ bool isExecutionRequested() const override;
+ bool isExecutionReady() const override;
+ BehaviorModuleOutput plan() override;
+ CandidateOutput planCandidate() const override;
+ void updateData() override;
+
+ void updateModuleParams(const std::any & parameters) override
+ {
+ std::shared_ptr<SamplingPlannerParameters> user_params_ =
+ std::any_cast<std::shared_ptr<SamplingPlannerParameters>>(parameters);
+
+ // Constraints
+ internal_params_->constraints.hard.max_curvature = user_params_->max_curvature;
+ internal_params_->constraints.hard.min_curvature = user_params_->min_curvature;
+ internal_params_->constraints.soft.lateral_deviation_weight =
+ user_params_->lateral_deviation_weight;
+ internal_params_->constraints.soft.length_weight = user_params_->length_weight;
+ internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
+ internal_params_->constraints.soft.weights = user_params_->weights;
+ internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.75);
+ internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
+ internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
+ // Sampling
+ internal_params_->sampling.enable_frenet = user_params_->enable_frenet;
+ internal_params_->sampling.enable_bezier = user_params_->enable_bezier;
+ internal_params_->sampling.resolution = user_params_->resolution;
+ internal_params_->sampling.previous_path_reuse_points_nb =
+ user_params_->previous_path_reuse_points_nb;
+ internal_params_->sampling.target_lengths = user_params_->target_lengths;
+ internal_params_->sampling.target_lateral_positions = user_params_->target_lateral_positions;
+ internal_params_->sampling.nb_target_lateral_positions =
+ user_params_->nb_target_lateral_positions;
+
+ internal_params_->sampling.frenet.target_lateral_velocities =
+ user_params_->target_lateral_velocities;
+ internal_params_->sampling.frenet.target_lateral_accelerations =
+ user_params_->target_lateral_accelerations;
+ // internal_params_->sampling.bezier.nb_k = user_params_->nb_k;
+ // internal_params_->sampling.bezier.mk_min = user_params_->mk_min;
+ // internal_params_->sampling.bezier.mk_max = user_params_->mk_max;
+ // internal_params_->sampling.bezier.nb_t = user_params_->nb_t;
+ // internal_params_->sampling.bezier.mt_min = user_params_->mt_min;
+ // internal_params_->sampling.bezier.mt_max = user_params_->mt_max;
+
+ // Preprocessing
+ internal_params_->preprocessing.force_zero_deviation = user_params_->force_zero_deviation;
+ internal_params_->preprocessing.force_zero_heading = user_params_->force_zero_heading;
+ internal_params_->preprocessing.smooth_reference = user_params_->smooth_reference;
+ }
+
+ void acceptVisitor(
+ [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
+ {
+ }
+
+ SamplingPlannerDebugData debug_data_;
+ behavior_path_planner::HardConstraintsFunctionVector hard_constraints_;
+ behavior_path_planner::SoftConstraintsFunctionVector soft_constraints_;
+
+private:
+ SamplingPlannerData createPlannerData(
+ const PlanResult & path, const std::vector<geometry_msgs::msg::Point> & left_bound,
+ const std::vector<geometry_msgs::msg::Point> & right_bound) const;
+
+ PlanResult generatePath();
+
+ bool canTransitSuccessState() override
+ {
+ std::vector<DrivableLanes> drivable_lanes{};
+ const auto & prev_module_path = getPreviousModuleOutput().path;
+ const auto prev_module_reference_path = getPreviousModuleOutput().reference_path;
+
+ const auto & p = planner_data_->parameters;
+ const auto ego_pose = planner_data_->self_odometry->pose.pose;
+ lanelet::ConstLanelet current_lane;
+
+ if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("behavior_path_planner").get_child("utils"),
+ "failed to find closest lanelet within route!!!");
+ return {};
+ }
+ const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
+ current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
+ // expand drivable lanes
+ std::for_each(
+ current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) {
+ drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
+ });
+
+ lanelet::ConstLanelets current_lanes;
+
+ for (auto & d : drivable_lanes) {
+ current_lanes.push_back(d.right_lane);
+ current_lanes.push_back(d.left_lane);
+ current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
+ }
+
+ const auto ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose);
+ const auto goal_pose = planner_data_->route_handler->getGoalPose();
+ const auto goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose);
+ const double length_to_goal = std::abs(goal_arc.length - ego_arc.length);
+ const double min_target_length = *std::min_element(
+ internal_params_->sampling.target_lengths.begin(),
+ internal_params_->sampling.target_lengths.end());
+ const auto nearest_index =
+ motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose);
+ double yaw_difference = 0.0;
+ if (!nearest_index) return false;
+ auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double {
+ geometry_msgs::msg::Vector3 rpy;
+ tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w);
+ tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
+ return rpy.z;
+ };
+ const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation;
+ const double ref_path_yaw = toYaw(quat);
+ const double ego_yaw = toYaw(ego_pose.orientation);
+ yaw_difference = std::abs(ego_yaw - ref_path_yaw);
+ constexpr double pi = 3.14159;
+
+ lanelet::ConstLanelet closest_lanelet_to_ego;
+ lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
+ lanelet::ConstLanelet closest_lanelet_to_goal;
+ lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
+ const bool merged_back_to_path =
+ ((length_to_goal < min_target_length) || (std::abs(ego_arc.distance)) < 0.5) &&
+ (yaw_difference < pi / 36.0) &&
+ (closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id()); // TODO(Daniel) magic numbers
+ return isReferencePathSafe() && (merged_back_to_path);
+ }
+
+ bool canTransitFailureState() override { return false; }
+
+ bool canTransitIdleToRunningState() override { return prev_sampling_path_.has_value(); }
+
+ bool isReferencePathSafe() const;
+
+ void updateDebugMarkers();
+
+ void prepareConstraints(
+ sampler_common::Constraints & constraints,
+ const PredictedObjects::ConstSharedPtr & predicted_objects,
+ const std::vector<geometry_msgs::msg::Point> & left_bound,
+ const std::vector<geometry_msgs::msg::Point> & right_bound) const;
+
+ frenet_planner::SamplingParameters prepareSamplingParameters(
+ const sampler_common::State & initial_state,
+ const sampler_common::transform::Spline2D & path_spline,
+ const SamplingPlannerInternalParameters & internal_params_);
+
+ PathWithLaneId convertFrenetPathToPathWithLaneID(
+ const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
+ const double path_z);
+
+ // member
+ // std::shared_ptr<SamplingPlannerParameters> params_;
+ std::shared_ptr<SamplingPlannerInternalParameters> internal_params_;
+ vehicle_info_util::VehicleInfo vehicle_info_{};
+ std::optional<frenet_planner::Path> prev_sampling_path_ = std::nullopt;
+ // move to utils
+
+ void extendOutputDrivableArea(BehaviorModuleOutput & output);
+ bool isEndPointsConnected(
+ const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) const;
+ DrivableLanes generateExpandDrivableLanes(
+ const lanelet::ConstLanelet & lanelet,
+ const std::shared_ptr<const PlannerData> & planner_data) const;
+};
+
+} // namespace behavior_path_planner
+
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
similarity index 100%
rename from planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp
rename to planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
similarity index 100%
rename from planning/behavior_path_planner/include/behavior_path_planner/utils/sampling_planner/util.hpp
rename to planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp b/planning/behavior_path_sampling_planner_module/include/manager.hpp
similarity index 85%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp
rename to planning/behavior_path_sampling_planner_module/include/manager.hpp
index 98c675a271e8f..e405e7629e745 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/manager.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/manager.hpp
@@ -15,9 +15,9 @@
#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
-#include "behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp"
-#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
-#include "behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp"
+#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
+#include "behavior_path_sampling_planner_module/sampling_planner_module.hpp"
+#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp"
#include <rclcpp/rclcpp.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp
similarity index 94%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
rename to planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp
index d559a3dedd447..1c14d602f3caa 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp
@@ -15,13 +15,13 @@
#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
-#include "behavior_path_planner/marker_utils/utils.hpp"
-#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
-#include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp"
-#include "behavior_path_planner/utils/path_utils.hpp"
-#include "behavior_path_planner/utils/sampling_planner/sampling_planner_parameters.hpp"
-#include "behavior_path_planner/utils/sampling_planner/util.hpp"
-#include "behavior_path_planner/utils/utils.hpp"
+#include "behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp"
+#include "behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp"
+#include "behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp"
+#include "behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp"
+#include "behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp"
+#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp"
+#include "behavior_path_sampling_planner_module/util.hpp"
#include "bezier_sampler/bezier_sampling.hpp"
#include "frenet_planner/frenet_planner.hpp"
#include "lanelet2_extension/utility/query.hpp"
diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml
new file mode 100644
index 0000000000000..23bebe496ee31
--- /dev/null
+++ b/planning/behavior_path_sampling_planner_module/package.xml
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>behavior_path_sampling_planner_module</name>
+ <version>0.1.0</version>
+ <description>The behavior_path_sampling_planner_module package</description>
+
+ <maintainer email="daniel.sanchez@tier4.jp">Daniel Sanchez</maintainer>
+ <maintainer email="maxime.clement@tier4.jp">Maxime Clement</maintainer>
+
+
+ <license>Apache License 2.0</license>
+
+ <buildtool_depend>ament_cmake_auto</buildtool_depend>
+ <buildtool_depend>autoware_cmake</buildtool_depend>
+ <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+ <depend>behavior_path_planner</depend>
+ <depend>behavior_path_planner_common</depend>
+ <depend>sampler_common</depend>
+ <depend>motion_utils</depend>
+ <depend>pluginlib</depend>
+ <depend>rclcpp</depend>
+ <depend>rtc_interface</depend>
+ <depend>tier4_autoware_utils</depend>
+ <depend>tier4_planning_msgs</depend>
+ <depend>visualization_msgs</depend>
+
+ <test_depend>ament_cmake_ros</test_depend>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>autoware_lint_common</test_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
diff --git a/planning/behavior_path_sampling_planner_module/plugins.xml b/planning/behavior_path_sampling_planner_module/plugins.xml
new file mode 100644
index 0000000000000..04a8dbb3171c0
--- /dev/null
+++ b/planning/behavior_path_sampling_planner_module/plugins.xml
@@ -0,0 +1,3 @@
+<library path="behavior_path_sampling_planner_module">
+ <class type="behavior_path_planner::SamplingPlannerModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
+</library>
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp b/planning/behavior_path_sampling_planner_module/src/manager.cpp
similarity index 95%
rename from planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
rename to planning/behavior_path_sampling_planner_module/src/manager.cpp
index dd9cf62cb3eb7..d0c362c908894 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/manager.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/manager.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "behavior_path_planner/scene_module/sampling_planner/manager.hpp"
+#include "behavior_path_sampling_planner_module/manager.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"
@@ -25,10 +25,11 @@
namespace behavior_path_planner
{
-SamplingPlannerModuleManager::SamplingPlannerModuleManager(
- rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config)
-: SceneModuleManagerInterface(node, name, config, {})
+void SamplingPlannerModuleManager::init(rclcpp::Node * node)
{
+ // init manager interface
+ initInterface(node, {""});
+
SamplingPlannerParameters p{};
{
std::string ns{"constraints.hard"};
diff --git a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
rename to planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index ee01fa813f7f6..4244e66765e97 100644
--- a/planning/behavior_path_planner/src/scene_module/sampling_planner/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "behavior_path_planner/scene_module/sampling_planner/sampling_planner_module.hpp"
+#include "behavior_path_sampling_planner_module/sampling_planner_module.hpp"
#include <boost/geometry/algorithms/within.hpp>
@@ -31,8 +31,10 @@ using utils::toPath;
SamplingPlannerModule::SamplingPlannerModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<SamplingPlannerParameters> & parameters,
- const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map},
+ const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+ std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+ objects_of_interest_marker_interface_ptr_map)
+: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT
vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}
{
internal_params_ =
From ce8cf656e5c0df02e03cf89f1db01f4c619df180 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 12 Dec 2023 15:24:10 +0900
Subject: [PATCH 069/115] temp
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../behavior_planning.launch.xml | 33 ++++++++++---------
.../launch/behavior_path_planner.launch.xml | 2 ++
.../src/behavior_path_planner_node.cpp | 7 ++++
3 files changed, 27 insertions(+), 15 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 01a410c711435..9172e0e02e8b9 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -18,6 +18,18 @@
<arg name="launch_start_planner_module" default="true"/>
<arg name="launch_side_shift_module" default="true"/>
+ <arg name="launch_avoidance_module" default="true"/>
+ <arg name="launch_dynamic_avoidance_module" default="true"/>
+ <arg name="launch_side_shift_module" default="true"/>
+ <arg name="launch_goal_planner_module" default="true"/>
+ <arg name="launch_start_planner_module" default="true"/>
+ <arg name="launch_sampling_planner_module" default="true"/>
+ <arg name="launch_lane_change_right_module" default="true"/>
+ <arg name="launch_lane_change_left_module" default="true"/>
+ <arg name="launch_external_request_lane_change_right_module" default="true"/>
+ <arg name="launch_external_request_lane_change_left_module" default="true"/>
+ <arg name="launch_avoidance_by_lane_change_module" default="true"/>
+
<arg name="launch_crosswalk_module" default="true"/>
<arg name="launch_walkway_module" default="true"/>
<arg name="launch_traffic_light_module" default="true"/>
@@ -58,6 +70,11 @@
name="behavior_path_planner_launch_modules"
value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::StartPlannerModuleManager, '")"
if="$(var launch_start_planner_module)"
+ />
+ <let
+ name="behavior_path_planner_launch_modules"
+ value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::SamplingPlannerModuleManager, '")"
+ if="$(var launch_sampling_planner_module)"
/>
<let
name="behavior_path_planner_launch_modules"
@@ -192,20 +209,6 @@
<remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
<!-- params -->
-<<<<<<< HEAD
-=======
- <param name="avoidance.enable_module" value="$(var launch_avoidance_module)"/>
- <param name="avoidance_by_lc.enable_module" value="$(var launch_avoidance_by_lane_change_module)"/>
- <param name="dynamic_avoidance.enable_module" value="$(var launch_dynamic_avoidance_module)"/>
- <param name="sampling_planner.enable_module" value="$(var launch_sampling_planner_module)"/>
- <param name="lane_change_right.enable_module" value="$(var launch_lane_change_right_module)"/>
- <param name="lane_change_left.enable_module" value="$(var launch_lane_change_left_module)"/>
- <param name="external_request_lane_change_left.enable_module" value="$(var launch_external_request_lane_change_left_module)"/>
- <param name="external_request_lane_change_right.enable_module" value="$(var launch_external_request_lane_change_right_module)"/>
- <param name="goal_planner.enable_module" value="$(var launch_goal_planner_module)"/>
- <param name="start_planner.enable_module" value="$(var launch_start_planner_module)"/>
- <param name="side_shift.enable_module" value="$(var launch_side_shift_module)"/>
->>>>>>> 95ef8878ac (Initialize vehicle info)
<param from="$(var common_param_path)"/>
<param from="$(var vehicle_param_file)"/>
<param from="$(var nearest_search_param_path)"/>
@@ -215,7 +218,7 @@
<param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
<param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
<param from="$(var behavior_path_planner_dynamic_avoidance_module_param_path)"/>
- <param from="$(var behavior_path_planner_sampling_planner_param_path)"/>
+ <param from="$(var behavior_path_planner_sampling_planner_module_param_path)"/>
<param from="$(var behavior_path_planner_lane_change_module_param_path)"/>
<param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
<param from="$(var behavior_path_planner_start_planner_module_param_path)"/>
diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml
index 19ea0491e776f..79c590363beb5 100644
--- a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml
+++ b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml
@@ -19,6 +19,7 @@
<arg name="behavior_path_planner_lane_change_module_param_path"/>
<arg name="behavior_path_planner_goal_planner_module_param_path"/>
<arg name="behavior_path_planner_start_planner_module_param_path"/>
+ <arg name="behavior_path_planner_sampling_planner_module_param_path"/>
<arg name="behavior_path_planner_drivable_area_expansion_param_path"/>
<node pkg="behavior_path_planner" exec="behavior_path_planner" name="behavior_path_planner" output="screen">
@@ -49,6 +50,7 @@
<param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
<param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
<param from="$(var behavior_path_planner_dynamic_avoidance_module_param_path)"/>
+ <param from="$(var behavior_path_planner_sampling_planner_module_param_path)"/>
<param from="$(var behavior_path_planner_lane_change_module_param_path)"/>
<param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
<param from="$(var behavior_path_planner_start_planner_module_param_path)"/>
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 3eb5eca5954fb..2302c5f9a5aad 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -50,6 +50,7 @@ using vehicle_info_util::VehicleInfoUtil;
BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options)
: Node("behavior_path_planner", node_options)
{
+ std::cerr << "Constructing the node BehaviorPathPlannerNode0\n";
using std::placeholders::_1;
using std::chrono_literals::operator""ms;
@@ -59,6 +60,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
planner_data_->parameters = getCommonParam();
planner_data_->drivable_area_expansion_parameters.init(*this);
}
+ std::cerr << "Constructing the node BehaviorPathPlannerNode1\n";
// publisher
path_publisher_ = create_publisher<PathWithLaneId>("~/output/path", 1);
@@ -79,6 +81,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
bound_publisher_ = create_publisher<MarkerArray>("~/debug/bound", 1);
const auto qos_transient_local = rclcpp::QoS{1}.transient_local();
+ std::cerr << "Constructing the node BehaviorPathPlannerNode3\n";
+
// subscriber
velocity_subscriber_ = create_subscription<Odometry>(
"~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onOdometry, this, _1),
@@ -122,6 +126,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
createSubscriptionOptions(this));
{
+ std::cerr << "Constructing the node BehaviorPathPlannerNode4\n";
+
const std::string path_candidate_name_space = "/planning/path_candidate/";
const std::string path_reference_name_space = "/planning/path_reference/";
@@ -129,6 +135,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
const auto & p = planner_data_->parameters;
planner_manager_ = std::make_shared<PlannerManager>(*this, p.max_iteration_num, p.verbose);
+ std::cerr << "Constructing the node BehaviorPathPlannerNode5\n";
for (const auto & name : declare_parameter<std::vector<std::string>>("launch_modules")) {
// workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
From ec03bc0c3ceda91ddc5fc3804e0f3b163808ff20 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 12 Dec 2023 16:03:05 +0900
Subject: [PATCH 070/115] deleted unusused
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../behavior_planning.launch.py | 321 ------------------
1 file changed, 321 deletions(-)
delete mode 100644 launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
deleted file mode 100644
index b44678d700b96..0000000000000
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
+++ /dev/null
@@ -1,321 +0,0 @@
-# Copyright 2021-2023 TIER IV, Inc. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import launch
-from launch.actions import DeclareLaunchArgument
-from launch.actions import GroupAction
-from launch.actions import IncludeLaunchDescription
-from launch.actions import OpaqueFunction
-from launch.actions import SetLaunchConfiguration
-from launch.conditions import IfCondition
-from launch.conditions import UnlessCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
-from launch.substitutions import PythonExpression
-from launch_ros.actions import ComposableNodeContainer
-from launch_ros.descriptions import ComposableNode
-from launch_ros.substitutions import FindPackageShare
-import yaml
-
-
-def launch_setup(context, *args, **kwargs):
- # vehicle information parameter
- vehicle_param_path = LaunchConfiguration("vehicle_param_file").perform(context)
- with open(vehicle_param_path, "r") as f:
- vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-
- # common parameter
- with open(LaunchConfiguration("common_param_path").perform(context), "r") as f:
- common_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- # nearest search parameter
- with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f:
- nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-
- # behavior path planner
- with open(LaunchConfiguration("side_shift_param_path").perform(context), "r") as f:
- side_shift_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("avoidance_param_path").perform(context), "r") as f:
- avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("avoidance_by_lc_param_path").perform(context), "r") as f:
- avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("dynamic_avoidance_param_path").perform(context), "r") as f:
- dynamic_avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("sampling_planner_param_path").perform(context), "r") as f:
- sampling_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f:
- lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f:
- goal_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("start_planner_param_path").perform(context), "r") as f:
- start_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f:
- drivable_area_expansion_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("scene_module_manager_param_path").perform(context), "r") as f:
- scene_module_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f:
- behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-
- glog_component = ComposableNode(
- package="glog_component",
- plugin="GlogComponent",
- name="glog_component",
- )
-
- behavior_path_planner_component = ComposableNode(
- package="behavior_path_planner",
- plugin="behavior_path_planner::BehaviorPathPlannerNode",
- name="behavior_path_planner",
- namespace="",
- remappings=[
- ("~/input/route", LaunchConfiguration("input_route_topic_name")),
- ("~/input/vector_map", LaunchConfiguration("map_topic_name")),
- ("~/input/perception", "/perception/object_recognition/objects"),
- ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"),
- (
- "~/input/costmap",
- "/planning/scenario_planning/parking/costmap_generator/occupancy_grid",
- ),
- ("~/input/odometry", "/localization/kinematic_state"),
- ("~/input/accel", "/localization/acceleration"),
- ("~/input/scenario", "/planning/scenario_planning/scenario"),
- ("~/output/path", "path_with_lane_id"),
- ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"),
- ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"),
- ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"),
- ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
- ],
- parameters=[
- common_param,
- nearest_search_param,
- side_shift_param,
- avoidance_param,
- avoidance_by_lc_param,
- dynamic_avoidance_param,
- sampling_planner_param,
- lane_change_param,
- goal_planner_param,
- start_planner_param,
- drivable_area_expansion_param,
- scene_module_manager_param,
- behavior_path_planner_param,
- vehicle_param,
- {
- "lane_change.enable_collision_check_at_prepare_phase": LaunchConfiguration(
- "use_experimental_lane_change_function"
- ),
- "lane_change.use_predicted_path_outside_lanelet": LaunchConfiguration(
- "use_experimental_lane_change_function"
- ),
- "lane_change.use_all_predicted_path": LaunchConfiguration(
- "use_experimental_lane_change_function"
- ),
- },
- ],
- extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
- )
-
- # smoother param
- with open(
- LaunchConfiguration("motion_velocity_smoother_param_path").perform(context), "r"
- ) as f:
- motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
- with open(
- LaunchConfiguration("behavior_velocity_smoother_type_param_path").perform(context), "r"
- ) as f:
- behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-
- # behavior velocity planner
- behavior_velocity_planner_common_param_path = LaunchConfiguration(
- "behavior_velocity_planner_common_param_path"
- ).perform(context)
- behavior_velocity_planner_module_param_paths = LaunchConfiguration(
- "behavior_velocity_planner_module_param_paths"
- ).perform(context)
-
- behavior_velocity_planner_params_paths = [
- behavior_velocity_planner_common_param_path,
- *yaml.safe_load(behavior_velocity_planner_module_param_paths),
- ]
-
- behavior_velocity_planner_params = {}
- for path in behavior_velocity_planner_params_paths:
- with open(path) as f:
- behavior_velocity_planner_params.update(yaml.safe_load(f)["/**"]["ros__parameters"])
-
- behavior_velocity_planner_component = ComposableNode(
- package="behavior_velocity_planner",
- plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode",
- name="behavior_velocity_planner",
- namespace="",
- remappings=[
- ("~/input/path_with_lane_id", "path_with_lane_id"),
- ("~/input/vector_map", "/map/vector_map"),
- ("~/input/vehicle_odometry", "/localization/kinematic_state"),
- ("~/input/accel", "/localization/acceleration"),
- ("~/input/dynamic_objects", "/perception/object_recognition/objects"),
- (
- "~/input/no_ground_pointcloud",
- "/perception/obstacle_segmentation/pointcloud",
- ),
- (
- "~/input/compare_map_filtered_pointcloud",
- "compare_map_filtered/pointcloud",
- ),
- (
- "~/input/vector_map_inside_area_filtered_pointcloud",
- "vector_map_inside_area_filtered/pointcloud",
- ),
- (
- "~/input/traffic_signals",
- "/perception/traffic_light_recognition/traffic_signals",
- ),
- (
- "~/input/external_velocity_limit_mps",
- "/planning/scenario_planning/max_velocity_default",
- ),
- ("~/input/virtual_traffic_light_states", "/awapi/tmp/virtual_traffic_light_states"),
- (
- "~/input/occupancy_grid",
- "/perception/occupancy_grid_map/map",
- ),
- ("~/output/path", "path"),
- ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
- (
- "~/output/infrastructure_commands",
- "/planning/scenario_planning/status/infrastructure_commands",
- ),
- ("~/output/traffic_signal", "debug/traffic_signal"),
- ],
- parameters=[
- nearest_search_param,
- behavior_velocity_planner_params,
- vehicle_param,
- common_param,
- motion_velocity_smoother_param,
- behavior_velocity_smoother_type_param,
- ],
- extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
- )
-
- container = ComposableNodeContainer(
- name="behavior_planning_container",
- namespace="",
- package="rclcpp_components",
- executable=LaunchConfiguration("container_executable"),
- composable_node_descriptions=[
- behavior_path_planner_component,
- behavior_velocity_planner_component,
- glog_component,
- ],
- output="screen",
- )
-
- # This condition is true if run_out module is enabled and its detection method is Points
- run_out_module = "behavior_velocity_planner::RunOutModulePlugin"
- run_out_method = behavior_velocity_planner_params.get("run_out", {}).get("detection_method")
- launch_run_out = run_out_module in behavior_velocity_planner_params["launch_modules"]
- launch_run_out_with_points_method = launch_run_out and run_out_method == "Points"
-
- # load compare map for run_out module
- load_compare_map = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- [
- FindPackageShare("tier4_planning_launch"),
- "/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py",
- ],
- ),
- launch_arguments={
- "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
- "container_name": LaunchConfiguration("container_name"),
- "use_multithread": "true",
- }.items(),
- # launch compare map only when run_out module is enabled and detection method is Points
- condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))),
- )
-
- load_vector_map_inside_area_filter = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- [
- FindPackageShare("tier4_planning_launch"),
- "/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py",
- ]
- ),
- launch_arguments={
- "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"),
- "container_name": LaunchConfiguration("container_name"),
- "use_multithread": "true",
- "polygon_type": "no_obstacle_segmentation_area_for_run_out",
- }.items(),
- # launch vector map filter only when run_out module is enabled and detection method is Points
- condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))),
- )
-
- group = GroupAction(
- [
- container,
- load_compare_map,
- load_vector_map_inside_area_filter,
- ]
- )
-
- return [group]
-
-
-def generate_launch_description():
- launch_arguments = []
-
- def add_launch_arg(name: str, default_value=None, description=None):
- launch_arguments.append(
- DeclareLaunchArgument(name, default_value=default_value, description=description)
- )
-
- # vehicle parameter
- add_launch_arg("vehicle_param_file")
-
- # interface parameter
- add_launch_arg(
- "input_route_topic_name", "/planning/mission_planning/route", "input topic of route"
- )
- add_launch_arg("map_topic_name", "/map/vector_map", "input topic of map")
-
- # package parameter
- add_launch_arg("use_experimental_lane_change_function")
-
- # component
- add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication")
- add_launch_arg("use_multithread", "false", "use multithread")
-
- # for points filter of run out module
- add_launch_arg("use_pointcloud_container", "true")
- add_launch_arg("container_name", "pointcloud_container")
-
- set_container_executable = SetLaunchConfiguration(
- "container_executable",
- "component_container",
- condition=UnlessCondition(LaunchConfiguration("use_multithread")),
- )
- set_container_mt_executable = SetLaunchConfiguration(
- "container_executable",
- "component_container_mt",
- condition=IfCondition(LaunchConfiguration("use_multithread")),
- )
-
- return launch.LaunchDescription(
- launch_arguments
- + [
- set_container_executable,
- set_container_mt_executable,
- ]
- + [OpaqueFunction(function=launch_setup)]
- )
From 50a6dc375496a56d6c0de8cb56f1d3413765e548 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 12 Dec 2023 16:04:35 +0900
Subject: [PATCH 071/115] delete unused
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../behavior_path_planner/parameters.hpp | 148 ------------------
1 file changed, 148 deletions(-)
delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
deleted file mode 100644
index ba7b554a8444a..0000000000000
--- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
+++ /dev/null
@@ -1,148 +0,0 @@
-// Copyright 2021 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
-#define BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
-
-#include <interpolation/linear_interpolation.hpp>
-#include <vehicle_info_util/vehicle_info_util.hpp>
-
-#include <string>
-#include <utility>
-#include <vector>
-
-struct ModuleConfigParameters
-{
- bool enable_module{false};
- bool enable_rtc{false};
- bool enable_simultaneous_execution_as_approved_module{false};
- bool enable_simultaneous_execution_as_candidate_module{false};
- bool keep_last{false};
- uint8_t priority{0};
- uint8_t max_module_size{0};
-};
-
-struct LateralAccelerationMap
-{
- std::vector<double> base_vel;
- std::vector<double> base_min_acc;
- std::vector<double> base_max_acc;
-
- void add(const double velocity, const double min_acc, const double max_acc)
- {
- if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) {
- return;
- }
-
- size_t idx = 0;
- for (size_t i = 0; i < base_vel.size(); ++i) {
- if (velocity < base_vel.at(i)) {
- break;
- }
- idx = i + 1;
- }
-
- base_vel.insert(base_vel.begin() + idx, velocity);
- base_min_acc.insert(base_min_acc.begin() + idx, min_acc);
- base_max_acc.insert(base_max_acc.begin() + idx, max_acc);
- }
-
- std::pair<double, double> find(const double velocity) const
- {
- if (!base_vel.empty() && velocity < base_vel.front()) {
- return std::make_pair(base_min_acc.front(), base_max_acc.front());
- }
- if (!base_vel.empty() && velocity > base_vel.back()) {
- return std::make_pair(base_min_acc.back(), base_max_acc.back());
- }
-
- const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity);
- const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity);
-
- return std::make_pair(min_acc, max_acc);
- }
-};
-
-struct BehaviorPathPlannerParameters
-{
- bool verbose;
-
- ModuleConfigParameters config_avoidance;
- ModuleConfigParameters config_avoidance_by_lc;
- ModuleConfigParameters config_dynamic_avoidance;
- ModuleConfigParameters config_start_planner;
- ModuleConfigParameters config_goal_planner;
- ModuleConfigParameters config_side_shift;
- ModuleConfigParameters config_lane_change_left;
- ModuleConfigParameters config_lane_change_right;
- ModuleConfigParameters config_sampling_planner;
- ModuleConfigParameters config_ext_request_lane_change_left;
- ModuleConfigParameters config_ext_request_lane_change_right;
-
- double backward_path_length;
- double forward_path_length;
- double backward_length_buffer_for_end_of_lane;
- double backward_length_buffer_for_end_of_pull_over;
- double backward_length_buffer_for_end_of_pull_out;
-
- // common parameters
- double min_acc;
- double max_acc;
-
- // lane change parameters
- double lane_changing_lateral_jerk{0.5};
- double minimum_lane_changing_velocity{5.6};
- double lane_change_prepare_duration{4.0};
- double lane_change_finish_judge_buffer{3.0};
- LateralAccelerationMap lane_change_lat_acc_map;
-
- double minimum_pull_over_length;
- double minimum_pull_out_length;
- double drivable_area_resolution;
-
- double refine_goal_search_radius_range;
-
- double turn_signal_intersection_search_distance;
- double turn_signal_intersection_angle_threshold_deg;
- double turn_signal_search_time;
- double turn_signal_minimum_search_distance;
- double turn_signal_shift_length_threshold;
- bool turn_signal_on_swerving;
-
- bool enable_akima_spline_first;
- bool enable_cog_on_centerline;
- double input_path_interval;
- double output_path_interval;
-
- double ego_nearest_dist_threshold;
- double ego_nearest_yaw_threshold;
-
- // vehicle info
- vehicle_info_util::VehicleInfo vehicle_info;
- double wheel_base;
- double front_overhang;
- double rear_overhang;
- double vehicle_width;
- double vehicle_length;
- double wheel_tread;
- double left_over_hang;
- double right_over_hang;
- double base_link2front;
- double base_link2rear;
-
- // maximum drivable area visualization
- bool visualize_maximum_drivable_area;
-};
-
-#endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
From f47544ad4f42e7ff55109a860b96fb95e35023d6 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 12 Dec 2023 17:00:34 +0900
Subject: [PATCH 072/115] add plugin export macro
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../behavior_path_sampling_planner_module/src/manager.cpp | 5 +++++
1 file changed, 5 insertions(+)
diff --git a/planning/behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_sampling_planner_module/src/manager.cpp
index d0c362c908894..3ad13efdf56ea 100644
--- a/planning/behavior_path_sampling_planner_module/src/manager.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/manager.cpp
@@ -129,3 +129,8 @@ void SamplingPlannerModuleManager::updateModuleParams(
}
} // namespace behavior_path_planner
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(
+ behavior_path_planner::SamplingPlannerModuleManager,
+ behavior_path_planner::SceneModuleManagerInterface)
From 11ee0547100cd6bb316183e0fa282276a6be450d Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 12 Dec 2023 17:38:56 +0900
Subject: [PATCH 073/115] fix launch file
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../behavior_planning.launch.xml | 17 +++++++----------
1 file changed, 7 insertions(+), 10 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 9172e0e02e8b9..0b790c2829996 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -9,13 +9,13 @@
<arg name="launch_avoidance_module" default="true"/>
<arg name="launch_avoidance_by_lane_change_module" default="true"/>
<arg name="launch_dynamic_avoidance_module" default="true"/>
- <arg name="launch_sampling_planner_module" default="true"/>
<arg name="launch_lane_change_right_module" default="true"/>
<arg name="launch_lane_change_left_module" default="true"/>
<arg name="launch_external_request_lane_change_right_module" default="true"/>
<arg name="launch_external_request_lane_change_left_module" default="true"/>
<arg name="launch_goal_planner_module" default="true"/>
<arg name="launch_start_planner_module" default="true"/>
+ <arg name="launch_sampling_planner_module" default="true"/>
<arg name="launch_side_shift_module" default="true"/>
<arg name="launch_avoidance_module" default="true"/>
@@ -73,7 +73,7 @@
/>
<let
name="behavior_path_planner_launch_modules"
- value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::SamplingPlannerModuleManager, '")"
+ value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::StartPlannerModuleManager, '")"
if="$(var launch_sampling_planner_module)"
/>
<let
@@ -192,7 +192,8 @@
/>
<let name="behavior_velocity_planner_launch_modules" value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + '$(var launch_module_list_end)'")"/>
- <node pkg="behavior_path_planner" exec="behavior_path_planner" name="behavior_path_planner" namespace="">
+ <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" 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)"/>
<remap from="~/input/vector_map" to="$(var input_vector_map_topic_name)"/>
@@ -218,20 +219,16 @@
<param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
<param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>
<param from="$(var behavior_path_planner_dynamic_avoidance_module_param_path)"/>
- <param from="$(var behavior_path_planner_sampling_planner_module_param_path)"/>
<param from="$(var behavior_path_planner_lane_change_module_param_path)"/>
<param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
<param from="$(var behavior_path_planner_start_planner_module_param_path)"/>
+ <param from="$(var behavior_path_planner_sampling_planner_module_param_path)"/>
<param from="$(var behavior_path_planner_drivable_area_expansion_param_path)"/>
<param from="$(var behavior_path_planner_scene_module_manager_param_path)"/>
<param from="$(var behavior_path_planner_common_param_path)"/>
<!-- composable node config -->
- <!-- <extra_arg name="use_intra_process_comms" value="false"/> -->
- </node>
-
-
- <node_container pkg="rclcpp_components" exec="$(var container_type)" name="behavior_planning_container" namespace="" args="" output="screen">
-
+ <extra_arg name="use_intra_process_comms" value="false"/>
+ </composable_node>
<composable_node pkg="behavior_velocity_planner" plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode" name="behavior_velocity_planner" namespace="">
<!-- topic remap -->
From 93a29f1684cd7b7c11238c3e8beec7f186fbbc61 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 12 Dec 2023 18:49:51 +0900
Subject: [PATCH 074/115] WIP still not launching sampling planner plugin
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../behavior_planning.launch.xml | 2 +-
.../src/behavior_path_planner_node.cpp | 6 ----
.../include/manager.hpp | 7 ++++-
.../include/sampling_planner_module.hpp | 4 +++
.../package.xml | 31 +++++++++++++++++--
5 files changed, 39 insertions(+), 11 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 0b790c2829996..b276345ba1d41 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -73,7 +73,7 @@
/>
<let
name="behavior_path_planner_launch_modules"
- value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::StartPlannerModuleManager, '")"
+ value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::SamplingPlannerModuleManager, '")"
if="$(var launch_sampling_planner_module)"
/>
<let
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 2302c5f9a5aad..c51b2d975b31b 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -50,7 +50,6 @@ using vehicle_info_util::VehicleInfoUtil;
BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options)
: Node("behavior_path_planner", node_options)
{
- std::cerr << "Constructing the node BehaviorPathPlannerNode0\n";
using std::placeholders::_1;
using std::chrono_literals::operator""ms;
@@ -60,7 +59,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
planner_data_->parameters = getCommonParam();
planner_data_->drivable_area_expansion_parameters.init(*this);
}
- std::cerr << "Constructing the node BehaviorPathPlannerNode1\n";
// publisher
path_publisher_ = create_publisher<PathWithLaneId>("~/output/path", 1);
@@ -81,7 +79,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
bound_publisher_ = create_publisher<MarkerArray>("~/debug/bound", 1);
const auto qos_transient_local = rclcpp::QoS{1}.transient_local();
- std::cerr << "Constructing the node BehaviorPathPlannerNode3\n";
// subscriber
velocity_subscriber_ = create_subscription<Odometry>(
@@ -126,8 +123,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
createSubscriptionOptions(this));
{
- std::cerr << "Constructing the node BehaviorPathPlannerNode4\n";
-
const std::string path_candidate_name_space = "/planning/path_candidate/";
const std::string path_reference_name_space = "/planning/path_reference/";
@@ -135,7 +130,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
const auto & p = planner_data_->parameters;
planner_manager_ = std::make_shared<PlannerManager>(*this, p.max_iteration_num, p.verbose);
- std::cerr << "Constructing the node BehaviorPathPlannerNode5\n";
for (const auto & name : declare_parameter<std::vector<std::string>>("launch_modules")) {
// workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
diff --git a/planning/behavior_path_sampling_planner_module/include/manager.hpp b/planning/behavior_path_sampling_planner_module/include/manager.hpp
index e405e7629e745..10c105fdf14d5 100644
--- a/planning/behavior_path_sampling_planner_module/include/manager.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/manager.hpp
@@ -32,13 +32,18 @@ namespace behavior_path_planner
class SamplingPlannerModuleManager : public SceneModuleManagerInterface
{
public:
+ SamplingPlannerModuleManager() : SceneModuleManagerInterface{"sampling_planner"} {}
+
+ void init(rclcpp::Node * node) override;
+
SamplingPlannerModuleManager(
rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
{
return std::make_unique<SamplingPlannerModule>(
- name_, *node_, parameters_, rtc_interface_ptr_map_);
+ name_, *node_, parameters_, rtc_interface_ptr_map_,
+ objects_of_interest_marker_interface_ptr_map_);
}
void updateModuleParams(
diff --git a/planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp
index 1c14d602f3caa..54c441818dec0 100644
--- a/planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp
@@ -87,6 +87,10 @@ class SamplingPlannerModule : public SceneModuleInterface
CandidateOutput planCandidate() const override;
void updateData() override;
+ // void processOnEntry() override;
+ // void processOnExit() override;
+ // BehaviorModuleOutput planWaitingApproval() override;
+
void updateModuleParams(const std::any & parameters) override
{
std::shared_ptr<SamplingPlannerParameters> user_params_ =
diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml
index 23bebe496ee31..c9b64a53bee43 100644
--- a/planning/behavior_path_sampling_planner_module/package.xml
+++ b/planning/behavior_path_sampling_planner_module/package.xml
@@ -15,16 +15,41 @@
<buildtool_depend>autoware_cmake</buildtool_depend>
<buildtool_depend>eigen3_cmake_module</buildtool_depend>
- <depend>behavior_path_planner</depend>
+ <depend>autoware_adapi_v1_msgs</depend>
+ <depend>autoware_auto_perception_msgs</depend>
+ <depend>autoware_auto_planning_msgs</depend>
+ <depend>autoware_auto_tf2</depend>
+ <depend>autoware_auto_vehicle_msgs</depend>
+ <depend>autoware_perception_msgs</depend>
<depend>behavior_path_planner_common</depend>
- <depend>sampler_common</depend>
+ <depend>freespace_planning_algorithms</depend>
+ <depend>geometry_msgs</depend>
+ <depend>interpolation</depend>
+ <depend>lane_departure_checker</depend>
+ <depend>lanelet2_extension</depend>
+ <depend>libboost-dev</depend>
+ <depend>libopencv-dev</depend>
+ <depend>magic_enum</depend>
<depend>motion_utils</depend>
+ <depend>object_recognition_utils</depend>
+ <depend>planning_test_utils</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
- <depend>rtc_interface</depend>
+ <depend>rclcpp_components</depend>
+ <depend>route_handler</depend>
+ <depend>sensor_msgs</depend>
+ <depend>signal_processing</depend>
+ <depend>tf2</depend>
+ <depend>tf2_eigen</depend>
+ <depend>tf2_geometry_msgs</depend>
+ <depend>tf2_ros</depend>
<depend>tier4_autoware_utils</depend>
<depend>tier4_planning_msgs</depend>
+ <depend>vehicle_info_util</depend>
<depend>visualization_msgs</depend>
+ <depend>bezier_sampler</depend>
+ <depend>frenet_planner</depend>
+ <depend>path_sampler</depend>
<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
From d6aed0f73d8af53992ee9914d087b94c7c6286ce Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 13 Dec 2023 09:22:28 +0900
Subject: [PATCH 075/115] solve problem of plugin insertion (duplicated files)
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../manager.hpp | 2 +-
.../include/manager.hpp | 58 ----
.../include/sampling_planner_module.hpp | 260 ------------------
3 files changed, 1 insertion(+), 319 deletions(-)
delete mode 100644 planning/behavior_path_sampling_planner_module/include/manager.hpp
delete mode 100644 planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
index 6a4e7d35ce94c..ebf676a6b8649 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
@@ -32,7 +32,7 @@ namespace behavior_path_planner
class SamplingPlannerModuleManager : public SceneModuleManagerInterface
{
public:
- SamplingPlannerModuleManager() : SceneModuleManagerInterface{"side_shift"} {}
+ SamplingPlannerModuleManager() : SceneModuleManagerInterface{"sampling_planner"} {}
void init(rclcpp::Node * node) override;
std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
diff --git a/planning/behavior_path_sampling_planner_module/include/manager.hpp b/planning/behavior_path_sampling_planner_module/include/manager.hpp
deleted file mode 100644
index 10c105fdf14d5..0000000000000
--- a/planning/behavior_path_sampling_planner_module/include/manager.hpp
+++ /dev/null
@@ -1,58 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
-
-#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
-#include "behavior_path_sampling_planner_module/sampling_planner_module.hpp"
-#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp"
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <memory>
-#include <string>
-#include <unordered_map>
-#include <vector>
-
-namespace behavior_path_planner
-{
-
-class SamplingPlannerModuleManager : public SceneModuleManagerInterface
-{
-public:
- SamplingPlannerModuleManager() : SceneModuleManagerInterface{"sampling_planner"} {}
-
- void init(rclcpp::Node * node) override;
-
- SamplingPlannerModuleManager(
- rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
-
- std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
- {
- return std::make_unique<SamplingPlannerModule>(
- name_, *node_, parameters_, rtc_interface_ptr_map_,
- objects_of_interest_marker_interface_ptr_map_);
- }
-
- void updateModuleParams(
- [[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) override;
-
-private:
- std::shared_ptr<SamplingPlannerParameters> parameters_;
-};
-
-} // namespace behavior_path_planner
-
-#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
diff --git a/planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp
deleted file mode 100644
index 54c441818dec0..0000000000000
--- a/planning/behavior_path_sampling_planner_module/include/sampling_planner_module.hpp
+++ /dev/null
@@ -1,260 +0,0 @@
-// Copyright 2021 Tier IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
-
-#include "behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp"
-#include "behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp"
-#include "behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp"
-#include "behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp"
-#include "behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp"
-#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp"
-#include "behavior_path_sampling_planner_module/util.hpp"
-#include "bezier_sampler/bezier_sampling.hpp"
-#include "frenet_planner/frenet_planner.hpp"
-#include "lanelet2_extension/utility/query.hpp"
-#include "lanelet2_extension/utility/utilities.hpp"
-#include "motion_utils/trajectory/path_with_lane_id.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "sampler_common/constraints/footprint.hpp"
-#include "sampler_common/constraints/hard_constraint.hpp"
-#include "sampler_common/constraints/soft_constraint.hpp"
-#include "sampler_common/structures.hpp"
-#include "sampler_common/transform/spline_transform.hpp"
-#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
-#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
-#include "tier4_autoware_utils/math/constants.hpp"
-#include "tier4_autoware_utils/ros/update_param.hpp"
-#include "tier4_autoware_utils/system/stop_watch.hpp"
-#include "vehicle_info_util/vehicle_info_util.hpp"
-
-#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
-#include "tier4_planning_msgs/msg/lateral_offset.hpp"
-
-#include <algorithm>
-#include <any>
-#include <memory>
-#include <optional>
-#include <string>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-namespace behavior_path_planner
-{
-using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-struct SamplingPlannerData
-{
- // input
- std::vector<TrajectoryPoint> traj_points; // converted from the input path
- std::vector<geometry_msgs::msg::Point> left_bound;
- std::vector<geometry_msgs::msg::Point> right_bound;
-
- // ego
- geometry_msgs::msg::Pose ego_pose;
- double ego_vel{};
-};
-
-struct SamplingPlannerDebugData
-{
- std::vector<sampler_common::Path> sampled_candidates{};
- size_t previous_sampled_candidates_nb = 0UL;
- std::vector<tier4_autoware_utils::Polygon2d> obstacles{};
- std::vector<tier4_autoware_utils::MultiPoint2d> footprints{};
-};
-class SamplingPlannerModule : public SceneModuleInterface
-{
-public:
- SamplingPlannerModule(
- const std::string & name, rclcpp::Node & node,
- const std::shared_ptr<SamplingPlannerParameters> & parameters,
- const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
-
- bool isExecutionRequested() const override;
- bool isExecutionReady() const override;
- BehaviorModuleOutput plan() override;
- CandidateOutput planCandidate() const override;
- void updateData() override;
-
- // void processOnEntry() override;
- // void processOnExit() override;
- // BehaviorModuleOutput planWaitingApproval() override;
-
- void updateModuleParams(const std::any & parameters) override
- {
- std::shared_ptr<SamplingPlannerParameters> user_params_ =
- std::any_cast<std::shared_ptr<SamplingPlannerParameters>>(parameters);
-
- // Constraints
- internal_params_->constraints.hard.max_curvature = user_params_->max_curvature;
- internal_params_->constraints.hard.min_curvature = user_params_->min_curvature;
- internal_params_->constraints.soft.lateral_deviation_weight =
- user_params_->lateral_deviation_weight;
- internal_params_->constraints.soft.length_weight = user_params_->length_weight;
- internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
- internal_params_->constraints.soft.weights = user_params_->weights;
- internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.75);
- internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
- internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
- // Sampling
- internal_params_->sampling.enable_frenet = user_params_->enable_frenet;
- internal_params_->sampling.enable_bezier = user_params_->enable_bezier;
- internal_params_->sampling.resolution = user_params_->resolution;
- internal_params_->sampling.previous_path_reuse_points_nb =
- user_params_->previous_path_reuse_points_nb;
- internal_params_->sampling.target_lengths = user_params_->target_lengths;
- internal_params_->sampling.target_lateral_positions = user_params_->target_lateral_positions;
- internal_params_->sampling.nb_target_lateral_positions =
- user_params_->nb_target_lateral_positions;
-
- internal_params_->sampling.frenet.target_lateral_velocities =
- user_params_->target_lateral_velocities;
- internal_params_->sampling.frenet.target_lateral_accelerations =
- user_params_->target_lateral_accelerations;
- // internal_params_->sampling.bezier.nb_k = user_params_->nb_k;
- // internal_params_->sampling.bezier.mk_min = user_params_->mk_min;
- // internal_params_->sampling.bezier.mk_max = user_params_->mk_max;
- // internal_params_->sampling.bezier.nb_t = user_params_->nb_t;
- // internal_params_->sampling.bezier.mt_min = user_params_->mt_min;
- // internal_params_->sampling.bezier.mt_max = user_params_->mt_max;
-
- // Preprocessing
- internal_params_->preprocessing.force_zero_deviation = user_params_->force_zero_deviation;
- internal_params_->preprocessing.force_zero_heading = user_params_->force_zero_heading;
- internal_params_->preprocessing.smooth_reference = user_params_->smooth_reference;
- }
-
- void acceptVisitor(
- [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
- {
- }
-
- SamplingPlannerDebugData debug_data_;
- behavior_path_planner::HardConstraintsFunctionVector hard_constraints_;
- behavior_path_planner::SoftConstraintsFunctionVector soft_constraints_;
-
-private:
- SamplingPlannerData createPlannerData(
- const PlanResult & path, const std::vector<geometry_msgs::msg::Point> & left_bound,
- const std::vector<geometry_msgs::msg::Point> & right_bound) const;
-
- PlanResult generatePath();
-
- bool canTransitSuccessState() override
- {
- std::vector<DrivableLanes> drivable_lanes{};
- const auto & prev_module_path = getPreviousModuleOutput().path;
- const auto prev_module_reference_path = getPreviousModuleOutput().reference_path;
-
- const auto & p = planner_data_->parameters;
- const auto ego_pose = planner_data_->self_odometry->pose.pose;
- lanelet::ConstLanelet current_lane;
-
- if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
- RCLCPP_ERROR(
- rclcpp::get_logger("behavior_path_planner").get_child("utils"),
- "failed to find closest lanelet within route!!!");
- return {};
- }
- const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
- current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
- // expand drivable lanes
- std::for_each(
- current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) {
- drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
- });
-
- lanelet::ConstLanelets current_lanes;
-
- for (auto & d : drivable_lanes) {
- current_lanes.push_back(d.right_lane);
- current_lanes.push_back(d.left_lane);
- current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
- }
-
- const auto ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose);
- const auto goal_pose = planner_data_->route_handler->getGoalPose();
- const auto goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose);
- const double length_to_goal = std::abs(goal_arc.length - ego_arc.length);
- const double min_target_length = *std::min_element(
- internal_params_->sampling.target_lengths.begin(),
- internal_params_->sampling.target_lengths.end());
- const auto nearest_index =
- motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose);
- double yaw_difference = 0.0;
- if (!nearest_index) return false;
- auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double {
- geometry_msgs::msg::Vector3 rpy;
- tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w);
- tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
- return rpy.z;
- };
- const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation;
- const double ref_path_yaw = toYaw(quat);
- const double ego_yaw = toYaw(ego_pose.orientation);
- yaw_difference = std::abs(ego_yaw - ref_path_yaw);
- constexpr double pi = 3.14159;
-
- lanelet::ConstLanelet closest_lanelet_to_ego;
- lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
- lanelet::ConstLanelet closest_lanelet_to_goal;
- lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
- const bool merged_back_to_path =
- ((length_to_goal < min_target_length) || (std::abs(ego_arc.distance)) < 0.5) &&
- (yaw_difference < pi / 36.0) &&
- (closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id()); // TODO(Daniel) magic numbers
- return isReferencePathSafe() && (merged_back_to_path);
- }
-
- bool canTransitFailureState() override { return false; }
-
- bool canTransitIdleToRunningState() override { return prev_sampling_path_.has_value(); }
-
- bool isReferencePathSafe() const;
-
- void updateDebugMarkers();
-
- void prepareConstraints(
- sampler_common::Constraints & constraints,
- const PredictedObjects::ConstSharedPtr & predicted_objects,
- const std::vector<geometry_msgs::msg::Point> & left_bound,
- const std::vector<geometry_msgs::msg::Point> & right_bound) const;
-
- frenet_planner::SamplingParameters prepareSamplingParameters(
- const sampler_common::State & initial_state,
- const sampler_common::transform::Spline2D & path_spline,
- const SamplingPlannerInternalParameters & internal_params_);
-
- PathWithLaneId convertFrenetPathToPathWithLaneID(
- const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets,
- const double path_z);
-
- // member
- // std::shared_ptr<SamplingPlannerParameters> params_;
- std::shared_ptr<SamplingPlannerInternalParameters> internal_params_;
- vehicle_info_util::VehicleInfo vehicle_info_{};
- std::optional<frenet_planner::Path> prev_sampling_path_ = std::nullopt;
- // move to utils
-
- void extendOutputDrivableArea(BehaviorModuleOutput & output);
- bool isEndPointsConnected(
- const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) const;
- DrivableLanes generateExpandDrivableLanes(
- const lanelet::ConstLanelet & lanelet,
- const std::shared_ptr<const PlannerData> & planner_data) const;
-};
-
-} // namespace behavior_path_planner
-
-#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
From 2ab53f65f636bd14d7e796acf346bdaa1c8b07ff Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 13 Dec 2023 18:08:28 +0900
Subject: [PATCH 076/115] partly fix issue with PathwithLaneID not having
laneids at the first points
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 1 +
.../src/sampling_planner_module.cpp | 9 +++++++--
2 files changed, 8 insertions(+), 2 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index a2f6f78968d36..f29dc29bab78c 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -213,6 +213,7 @@ class SamplingPlannerModule : public SceneModuleInterface
((length_to_goal < min_target_length) || (std::abs(ego_arc.distance)) < 0.5) &&
(yaw_difference < pi / 36.0) &&
(closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id()); // TODO(Daniel) magic numbers
+ std::cerr << __func__ << " Am I reaching the end of this \n";
return isReferencePathSafe() && (merged_back_to_path);
}
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 4244e66765e97..5c93a19f2ffb0 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -72,7 +72,8 @@ SamplingPlannerModule::SamplingPlannerModule(
// TODO(Daniel): Think of methods to prevent chattering
// TODO(Daniel): Add penalty for not ending up on the same line as ref path?
// TODO(Daniel): Length increasing curvature cost, increase curvature cost the longer the path is
-
+ // TODO(Daniel): in frenet path to path with laneID transform assign the laneid to the points from
+ // end to start -> that will prevent cases were some points on the output dont have a laneID
// Yaw difference
// soft_constraints_.emplace_back(
// [&](
@@ -377,6 +378,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}
return {x, y};
}();
+
frenet_planner::FrenetState frenet_initial_state;
frenet_planner::SamplingParameters sampling_parameters;
@@ -413,6 +415,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
path_curvature);
}
};
+
set_frenet_state(initial_state, reference_spline, frenet_initial_state);
std::vector<DrivableLanes> drivable_lanes{};
@@ -631,8 +634,10 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
prev_sampling_path_ = best_path;
auto out_path = convertFrenetPathToPathWithLaneID(
- best_path, road_lanes, planner_data_->route_handler->getGoalPose().position.z);
+ best_path, current_lanes, planner_data_->route_handler->getGoalPose().position.z);
+ std::cerr << "road_lanes size " << road_lanes.size() << "\n";
+ std::cerr << "First lane ID size " << out_path.points.at(0).lane_ids.size() << "\n";
BehaviorModuleOutput out;
out.path = std::make_shared<PathWithLaneId>(out_path);
out.reference_path = reference_path_ptr;
From 28def353e270ff5de7ad8b6361203a3a3b240af0 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 20 Dec 2023 08:32:16 +0900
Subject: [PATCH 077/115] Modify PreviousOutput path since it is no longer a
shared ptr
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 6 +-
.../src/sampling_planner_module.cpp | 65 +++++++------------
2 files changed, 26 insertions(+), 45 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index f29dc29bab78c..5cb9bc2b407e3 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -153,8 +153,10 @@ class SamplingPlannerModule : public SceneModuleInterface
bool canTransitSuccessState() override
{
std::vector<DrivableLanes> drivable_lanes{};
- const auto & prev_module_path = getPreviousModuleOutput().path;
- const auto prev_module_reference_path = getPreviousModuleOutput().reference_path;
+ const auto & prev_module_path =
+ std::make_shared<PathWithLaneId>(getPreviousModuleOutput().path);
+ const auto prev_module_reference_path =
+ std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
const auto & p = planner_data_->parameters;
const auto ego_pose = planner_data_->self_odometry->pose.pose;
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 5c93a19f2ffb0..c08769e9d8ecf 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -74,7 +74,8 @@ SamplingPlannerModule::SamplingPlannerModule(
// TODO(Daniel): Length increasing curvature cost, increase curvature cost the longer the path is
// TODO(Daniel): in frenet path to path with laneID transform assign the laneid to the points from
// end to start -> that will prevent cases were some points on the output dont have a laneID
- // Yaw difference
+
+ // Yaw difference soft constraint cost
// soft_constraints_.emplace_back(
// [&](
// sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints &
@@ -115,8 +116,9 @@ SamplingPlannerModule::SamplingPlannerModule(
[[maybe_unused]] const sampler_common::Constraints & constraints,
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.poses.empty()) return 0.0;
- const auto & path_point_arc =
- lanelet::utils::getArcCoordinates(input_data.closest_lanelets_to_goal, path.poses.back());
+ const auto & last_pose = path.poses.back();
+ const auto path_point_arc =
+ lanelet::utils::getArcCoordinates(input_data.closest_lanelets_to_goal, last_pose);
const double lateral_distance_to_center_lane = std::abs(path_point_arc.distance);
return lateral_distance_to_center_lane;
});
@@ -159,11 +161,11 @@ SamplingPlannerModule::SamplingPlannerModule(
bool SamplingPlannerModule::isExecutionRequested() const
{
- if (getPreviousModuleOutput().reference_path->points.empty()) {
+ if (getPreviousModuleOutput().reference_path.points.empty()) {
return false;
}
- if (!motion_utils::isDrivingForward(getPreviousModuleOutput().reference_path->points)) {
+ if (!motion_utils::isDrivingForward(getPreviousModuleOutput().reference_path.points)) {
RCLCPP_WARN(getLogger(), "Backward path is NOT supported. Just converting path to trajectory");
return false;
}
@@ -174,7 +176,10 @@ bool SamplingPlannerModule::isExecutionRequested() const
bool SamplingPlannerModule::isReferencePathSafe() const
{
std::vector<DrivableLanes> drivable_lanes{};
- const auto & prev_module_path = getPreviousModuleOutput().path;
+ const auto & prev_module_path = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().path);
+ const auto & prev_module_reference_path =
+ std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
+
const auto & p = planner_data_->parameters;
const auto ego_pose = planner_data_->self_odometry->pose.pose;
lanelet::ConstLanelet current_lane;
@@ -235,8 +240,7 @@ bool SamplingPlannerModule::isReferencePathSafe() const
}
return path;
};
- sampler_common::Path reference_path =
- transform_to_sampling_path(getPreviousModuleOutput().reference_path);
+ sampler_common::Path reference_path = transform_to_sampling_path(prev_module_reference_path);
const auto footprint = sampler_common::constraints::buildFootprintPoints(
reference_path, internal_params_->constraints);
@@ -290,7 +294,8 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
PathWithLaneId path;
const auto header = planner_data_->route_handler->getRouteHeader();
- const auto reference_path_ptr = getPreviousModuleOutput().reference_path;
+ const auto reference_path_ptr =
+ std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
for (size_t i = 0; i < frenet_path.points.size(); ++i) {
const auto & frenet_path_point_position = frenet_path.points.at(i);
@@ -363,7 +368,8 @@ void SamplingPlannerModule::prepareConstraints(
BehaviorModuleOutput SamplingPlannerModule::plan()
{
- const auto reference_path_ptr = getPreviousModuleOutput().reference_path;
+ const auto reference_path_ptr =
+ std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
if (reference_path_ptr->points.empty()) {
return {};
}
@@ -420,8 +426,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
std::vector<DrivableLanes> drivable_lanes{};
- const auto & prev_module_path = getPreviousModuleOutput().path;
- // const auto & current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
+ const auto prev_module_path = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().path);
const auto & p = planner_data_->parameters;
const auto ego_pose = planner_data_->self_odometry->pose.pose;
@@ -500,38 +505,13 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
- // const double max_target_length = *std::max_element(
- // internal_params_->sampling.target_lengths.begin(),
- // internal_params_->sampling.target_lengths.end());
-
- // double x = prev_path_frenet.points.back().x();
- // double x_pose = pose.position.x;
if (std::abs(length_goal - length_path) > min_target_length) {
- auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion {
- tf2::Quaternion quaternion_tf2;
- quaternion_tf2.setRPY(roll, pitch, yaw);
- return quaternion_tf2;
- };
-
- geometry_msgs::msg::Pose future_pose;
- future_pose.position.x = prev_path_frenet.points.back().x();
- future_pose.position.y = prev_path_frenet.points.back().y();
- future_pose.position.z = pose.position.z;
-
- const auto future_pose_quaternion =
- quaternion_from_rpy(0.0, 0.0, prev_path_frenet.yaws.back());
- future_pose.orientation.w = future_pose_quaternion.w();
- future_pose.orientation.x = future_pose_quaternion.x();
- future_pose.orientation.y = future_pose_quaternion.y();
- future_pose.orientation.z = future_pose_quaternion.z();
+ geometry_msgs::msg::Pose future_pose = prev_path_frenet.poses.back();
sampler_common::State future_state =
behavior_path_planner::getInitialState(future_pose, reference_spline);
frenet_planner::FrenetState frenet_reuse_state;
set_frenet_state(future_state, reference_spline, frenet_reuse_state);
-
- // frenet_reuse_state.position = prev_path_frenet.frenet_points.back();
-
frenet_planner::SamplingParameters extension_sampling_parameters =
prepareSamplingParameters(future_state, reference_spline, *internal_params_);
auto extension_frenet_paths = frenet_planner::generatePaths(
@@ -615,8 +595,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
planner_data_->route_handler->getGoalPose().position.z)
: PathWithLaneId{};
- out.path = (prev_sampling_path_) ? std::make_shared<PathWithLaneId>(out_path)
- : getPreviousModuleOutput().path;
+ out.path = (prev_sampling_path_) ? out_path : getPreviousModuleOutput().path;
out.reference_path = getPreviousModuleOutput().reference_path;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
return out;
@@ -639,8 +618,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
std::cerr << "road_lanes size " << road_lanes.size() << "\n";
std::cerr << "First lane ID size " << out_path.points.at(0).lane_ids.size() << "\n";
BehaviorModuleOutput out;
- out.path = std::make_shared<PathWithLaneId>(out_path);
- out.reference_path = reference_path_ptr;
+ out.path = out_path;
+ out.reference_path = *reference_path_ptr;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
extendOutputDrivableArea(out);
return out;
@@ -739,7 +718,7 @@ void SamplingPlannerModule::updateDebugMarkers()
void SamplingPlannerModule::extendOutputDrivableArea(BehaviorModuleOutput & output)
{
- const auto prev_module_path = getPreviousModuleOutput().path;
+ const auto prev_module_path = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().path);
// const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
const auto & p = planner_data_->parameters;
const auto ego_pose = planner_data_->self_odometry->pose.pose;
From 3dca5de5e0bfc3422e08a7b94d9edb2e782ea1c7 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 27 Dec 2023 10:21:15 +0900
Subject: [PATCH 078/115] Added new change root lanelet request override
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 5cb9bc2b407e3..dec2a55a3fa1e 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -89,6 +89,7 @@ class SamplingPlannerModule : public SceneModuleInterface
BehaviorModuleOutput plan() override;
CandidateOutput planCandidate() const override;
void updateData() override;
+ bool isRootLaneletToBeUpdated() const override { return current_state_ == ModuleStatus::SUCCESS; }
void updateModuleParams(const std::any & parameters) override
{
From 3d1adb1d27f2f3f363d1e94b4a524d66373351a5 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 27 Dec 2023 14:54:02 +0900
Subject: [PATCH 079/115] WIP update collision detection to use rtree
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../util.hpp | 5 ++
.../src/sampling_planner_module.cpp | 53 ++++++++++++-------
.../include/sampler_common/structures.hpp | 6 +++
3 files changed, 45 insertions(+), 19 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
index b032f19ae7d1a..3103702788a15 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
@@ -17,6 +17,10 @@
#include "sampler_common/structures.hpp"
#include "sampler_common/transform/spline_transform.hpp"
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
+
+#include <boost/geometry/index/rtree.hpp>
+
#include <any>
#include <functional>
#include <numeric>
@@ -24,6 +28,7 @@
#include <vector>
namespace behavior_path_planner
{
+
using geometry_msgs::msg::Pose;
struct SoftConstraintsInputs
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index c08769e9d8ecf..0ea5e82bcf616 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -45,13 +45,24 @@ SamplingPlannerModule::SamplingPlannerModule(
[](
sampler_common::Path & path, const sampler_common::Constraints & constraints,
const MultiPoint2d & footprint) -> bool {
+ namespace bg = boost::geometry;
+ namespace bgi = boost::geometry::index;
+
if (!footprint.empty()) {
path.constraint_results.drivable_area =
- boost::geometry::within(footprint, constraints.drivable_polygons);
+ bg::within(footprint, constraints.drivable_polygons);
+ }
+
+ // path.constraint_results.collision =
+ // !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons);
+ for (const auto & f : footprint) {
+ const auto collision_index = constraints.rtree.qbegin(bgi::intersects(f));
+ if (collision_index != constraints.rtree.qend()) {
+ path.constraint_results.collision = false;
+ break;
+ }
}
- path.constraint_results.collision =
- !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons);
return path.constraint_results.collision && path.constraint_results.drivable_area;
});
@@ -343,9 +354,15 @@ void SamplingPlannerModule::prepareConstraints(
const std::vector<geometry_msgs::msg::Point> & right_bound) const
{
constraints.obstacle_polygons = sampler_common::MultiPolygon2d();
+ constraints.rtree.clear();
+ size_t i = 0;
for (const auto & o : predicted_objects->objects)
- if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) // TODO(Maxime): param
- constraints.obstacle_polygons.push_back(tier4_autoware_utils::toPolygon2d(o));
+ if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) {
+ const auto polygon = tier4_autoware_utils::toPolygon2d(o);
+ constraints.obstacle_polygons.push_back(polygon);
+ const auto box = boost::geometry::return_envelope<tier4_autoware_utils::Box2d>(polygon);
+ constraints.rtree.insert(std::make_pair(box, i++));
+ }
constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented
@@ -454,20 +471,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
}
- {
- const auto left_bound =
- (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, true));
- const auto right_bound =
- (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, false));
-
- const auto sampling_planner_data =
- createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
-
- prepareConstraints(
- internal_params_->constraints, planner_data_->dynamic_object,
- sampling_planner_data.left_bound, sampling_planner_data.right_bound);
- }
-
auto frenet_paths =
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
@@ -520,6 +523,18 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
}
}
+ const auto left_bound =
+ (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, true));
+ const auto right_bound =
+ (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, false));
+
+ const auto sampling_planner_data =
+ createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
+
+ prepareConstraints(
+ internal_params_->constraints, planner_data_->dynamic_object, sampling_planner_data.left_bound,
+ sampling_planner_data.right_bound);
+
SoftConstraintsInputs soft_constraints_input;
const auto & goal_pose = get_goal_pose();
soft_constraints_input.goal_pose = soft_constraints_input.ego_pose =
diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
index e546bc7571731..05c7fe47d79ef 100644
--- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
+++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp
@@ -26,12 +26,14 @@
#include <boost/geometry/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/index/rtree.hpp>
#include <algorithm>
#include <iostream>
#include <memory>
#include <numeric>
#include <string>
+#include <utility>
#include <vector>
namespace sampler_common
@@ -42,6 +44,9 @@ using tier4_autoware_utils::MultiPolygon2d;
using tier4_autoware_utils::Point2d;
using tier4_autoware_utils::Polygon2d;
+typedef std::pair<tier4_autoware_utils::Box2d, size_t> BoxIndexPair;
+typedef boost::geometry::index::rtree<BoxIndexPair, boost::geometry::index::rstar<16, 4>> Rtree;
+
/// @brief data about constraint check results of a given path
struct ConstraintResults
{
@@ -348,6 +353,7 @@ struct Constraints
MultiPolygon2d obstacle_polygons;
MultiPolygon2d drivable_polygons;
std::vector<DynamicObstacle> dynamic_obstacles;
+ Rtree rtree;
};
struct ReusableTrajectory
From 9890006b7aa302895a7ff423b8bfc039e04be1b5 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 27 Dec 2023 15:11:16 +0900
Subject: [PATCH 080/115] fix bug with index
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../src/sampling_planner_module.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 0ea5e82bcf616..daf0747f91c4a 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -356,13 +356,15 @@ void SamplingPlannerModule::prepareConstraints(
constraints.obstacle_polygons = sampler_common::MultiPolygon2d();
constraints.rtree.clear();
size_t i = 0;
- for (const auto & o : predicted_objects->objects)
+ for (const auto & o : predicted_objects->objects) {
if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) {
const auto polygon = tier4_autoware_utils::toPolygon2d(o);
constraints.obstacle_polygons.push_back(polygon);
const auto box = boost::geometry::return_envelope<tier4_autoware_utils::Box2d>(polygon);
- constraints.rtree.insert(std::make_pair(box, i++));
+ constraints.rtree.insert(std::make_pair(box, i));
}
+ i++;
+ }
constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented
From 848c47f8f611e87e0c067d45ce26c0397566a720 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 27 Dec 2023 15:24:29 +0900
Subject: [PATCH 081/115] Add rtree for collision checking
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 4 ++++
.../src/sampling_planner_module.cpp | 2 --
2 files changed, 4 insertions(+), 2 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index dec2a55a3fa1e..45a105bc754db 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -44,6 +44,10 @@
#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
#include "tier4_planning_msgs/msg/lateral_offset.hpp"
+#include <boost/geometry.hpp>
+#include <boost/geometry/algorithms/within.hpp>
+#include <boost/geometry/index/rtree.hpp>
+
#include <algorithm>
#include <any>
#include <memory>
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index daf0747f91c4a..0586c277e6851 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -14,8 +14,6 @@
#include "behavior_path_sampling_planner_module/sampling_planner_module.hpp"
-#include <boost/geometry/algorithms/within.hpp>
-
namespace behavior_path_planner
{
using geometry_msgs::msg::Point;
From ab7d10a1ffd1fbd06284439e24287e57b67a2e0e Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 27 Dec 2023 18:41:30 +0900
Subject: [PATCH 082/115] refine soft constraints use remaining length of path
max curv and normalize lateral error
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../src/sampling_planner_module.cpp | 86 +++++++++++++------
1 file changed, 58 insertions(+), 28 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 0586c277e6851..3683182ff1794 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -104,18 +104,41 @@ SamplingPlannerModule::SamplingPlannerModule(
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.points.empty()) return 0.0;
if (path.poses.empty()) return 0.0;
- const double distance_ego_to_goal_pose =
- std::abs(input_data.ego_arc.length - input_data.goal_arc.length);
-
- constexpr double epsilon = 0.001;
- if (distance_ego_to_goal_pose < epsilon) return 0.0;
- const double length_last_point_on_path = input_data.ego_arc.length + path.lengths.back();
- const double length_path_to_goal =
- std::abs(length_last_point_on_path - input_data.goal_arc.length);
+
+ const auto & ego_pose = input_data.ego_pose;
+
+ const auto closest_path_pose_itr = std::min_element(
+ path.poses.begin(), path.poses.end(), [&ego_pose](const auto & p1, const auto & p2) {
+ const auto dist1 =
+ std::hypot(p1.position.x - ego_pose.position.x, p1.position.y - ego_pose.position.y);
+ const auto dist2 =
+ std::hypot(p2.position.x - ego_pose.position.x, p2.position.y - ego_pose.position.y);
+ return dist1 < dist2;
+ });
+
+ int closest_path_pose_index = static_cast<int>(closest_path_pose_itr - path.poses.begin());
+
+ const double remaining_path_length =
+ path.lengths.back() - path.lengths.at(closest_path_pose_index);
+
const double max_target_length = *std::max_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
- return length_path_to_goal / max_target_length;
+
+ // const double distance_ego_to_goal_pose =
+ // std::abs(input_data.ego_arc.length - input_data.goal_arc.length);
+ // std::cerr << "distance_ego_to_goal_pose " << distance_ego_to_goal_pose << "\n";
+ // if (closest_lanelet_to_goal.id() == closest_lanelet_to_path_end.id()) {
+ // constexpr double epsilon = 0.001;
+ // if (distance_ego_to_goal_pose < epsilon) return 0.0;
+ // const double length_last_point_on_path = input_data.ego_arc.length + path.lengths.back();
+ // const double length_path_to_goal =
+ // std::abs(length_last_point_on_path - input_data.goal_arc.length);
+ // return max_target_length / length_path_to_goal;
+ // }
+
+ // return length_path_to_goal / max_target_length;
+ return max_target_length / remaining_path_length;
});
// Distance to centerline
@@ -129,7 +152,10 @@ SamplingPlannerModule::SamplingPlannerModule(
const auto path_point_arc =
lanelet::utils::getArcCoordinates(input_data.closest_lanelets_to_goal, last_pose);
const double lateral_distance_to_center_lane = std::abs(path_point_arc.distance);
- return lateral_distance_to_center_lane;
+ const double max_target_lateral_positions = *std::max_element(
+ internal_params_->sampling.target_lateral_positions.begin(),
+ internal_params_->sampling.target_lateral_positions.end());
+ return lateral_distance_to_center_lane / max_target_lateral_positions;
});
// // Curvature cost
@@ -138,16 +164,20 @@ SamplingPlannerModule::SamplingPlannerModule(
sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.curvatures.empty()) return std::numeric_limits<double>::max();
- double curvature_sum = 0.0;
- for (const auto curvature : path.curvatures) {
- curvature_sum += std::abs(curvature);
- }
- const auto curvature_average = curvature_sum / static_cast<double>(path.curvatures.size());
- const auto max_curvature =
- (curvature_average > 0.0) ? constraints.hard.max_curvature : constraints.hard.min_curvature;
- return curvature_average / max_curvature;
- // return constraints.soft.curvature_weight * curvature_sum /
- // static_cast<double>(path.curvatures.size());
+ // double curvature_sum = 0.0;
+ // for (const auto curvature : path.curvatures) {
+ // curvature_sum += std::abs(curvature);
+ // }
+
+ const double max_path_curvature = *std::max_element(
+ path.curvatures.begin(), path.curvatures.end(),
+ [](const auto & a, const auto & b) { return std::abs(a) < std::abs(b); });
+
+ // const auto curvature_average = curvature_sum / static_cast<double>(path.curvatures.size());
+ // const auto max_curvature =
+ // (curvature_average > 0.0) ? constraints.hard.max_curvature :
+ // constraints.hard.min_curvature;
+ return std::abs(max_path_curvature) / constraints.hard.max_curvature;
});
// Same lane as goal cost Still in testing dont delete
// soft_constraints_.emplace_back(
@@ -157,14 +187,14 @@ SamplingPlannerModule::SamplingPlannerModule(
// [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
// if (path.poses.empty()) return 0.0;
- // const auto & goal_pose = input_data.goal_pose;
- // lanelet::ConstLanelet closest_lanelet_to_goal;
- // lanelet::utils::query::getClosestLanelet(
- // input_data.current_lanes, goal_pose, &closest_lanelet_to_goal);
- // lanelet::ConstLanelet closest_lanelet_to_path_end;
- // lanelet::utils::query::getClosestLanelet(
- // input_data.current_lanes, path.poses.back(), &closest_lanelet_to_path_end);
- // return (closest_lanelet_to_goal.id() == closest_lanelet_to_path_end.id()) ? 0.0 : 0.5;
+ // const auto & goal_pose = input_data.goal_pose;
+ // lanelet::ConstLanelet closest_lanelet_to_goal;
+ // lanelet::utils::query::getClosestLanelet(
+ // input_data.current_lanes, goal_pose, &closest_lanelet_to_goal);
+ // lanelet::ConstLanelet closest_lanelet_to_path_end;
+ // lanelet::utils::query::getClosestLanelet(
+ // input_data.current_lanes, path.poses.back(), &closest_lanelet_to_path_end);
+ // return (closest_lanelet_to_goal.id() == closest_lanelet_to_path_end.id()) ? 0.0 : 0.5;
// });
}
From e69485dccee3ab51f08befcbf1eb7ca5d791c77e Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 4 Jan 2024 13:49:18 +0900
Subject: [PATCH 083/115] Add sanity check and delete unused code
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../src/sampling_planner_module.cpp | 56 +++----------------
1 file changed, 9 insertions(+), 47 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 3683182ff1794..3b7b749832043 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -51,8 +51,6 @@ SamplingPlannerModule::SamplingPlannerModule(
bg::within(footprint, constraints.drivable_polygons);
}
- // path.constraint_results.collision =
- // !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons);
for (const auto & f : footprint) {
const auto collision_index = constraints.rtree.qbegin(bgi::intersects(f));
if (collision_index != constraints.rtree.qend()) {
@@ -83,8 +81,9 @@ SamplingPlannerModule::SamplingPlannerModule(
// TODO(Daniel): Length increasing curvature cost, increase curvature cost the longer the path is
// TODO(Daniel): in frenet path to path with laneID transform assign the laneid to the points from
// end to start -> that will prevent cases were some points on the output dont have a laneID
-
- // Yaw difference soft constraint cost
+ // TODO(Daniel): Set a goal that depends on the reference path and NOT the actual goal set by the
+ // user.
+ // Yaw difference soft constraint cost -> Considering implementation
// soft_constraints_.emplace_back(
// [&](
// sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints &
@@ -107,6 +106,10 @@ SamplingPlannerModule::SamplingPlannerModule(
const auto & ego_pose = input_data.ego_pose;
+ const double max_target_length = *std::max_element(
+ internal_params_->sampling.target_lengths.begin(),
+ internal_params_->sampling.target_lengths.end());
+
const auto closest_path_pose_itr = std::min_element(
path.poses.begin(), path.poses.end(), [&ego_pose](const auto & p1, const auto & p2) {
const auto dist1 =
@@ -121,23 +124,8 @@ SamplingPlannerModule::SamplingPlannerModule(
const double remaining_path_length =
path.lengths.back() - path.lengths.at(closest_path_pose_index);
- const double max_target_length = *std::max_element(
- internal_params_->sampling.target_lengths.begin(),
- internal_params_->sampling.target_lengths.end());
-
- // const double distance_ego_to_goal_pose =
- // std::abs(input_data.ego_arc.length - input_data.goal_arc.length);
- // std::cerr << "distance_ego_to_goal_pose " << distance_ego_to_goal_pose << "\n";
- // if (closest_lanelet_to_goal.id() == closest_lanelet_to_path_end.id()) {
- // constexpr double epsilon = 0.001;
- // if (distance_ego_to_goal_pose < epsilon) return 0.0;
- // const double length_last_point_on_path = input_data.ego_arc.length + path.lengths.back();
- // const double length_path_to_goal =
- // std::abs(length_last_point_on_path - input_data.goal_arc.length);
- // return max_target_length / length_path_to_goal;
- // }
-
- // return length_path_to_goal / max_target_length;
+ constexpr double epsilon = 1E-5;
+ if (remaining_path_length < epsilon) return max_target_length;
return max_target_length / remaining_path_length;
});
@@ -164,38 +152,12 @@ SamplingPlannerModule::SamplingPlannerModule(
sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
[[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
if (path.curvatures.empty()) return std::numeric_limits<double>::max();
- // double curvature_sum = 0.0;
- // for (const auto curvature : path.curvatures) {
- // curvature_sum += std::abs(curvature);
- // }
const double max_path_curvature = *std::max_element(
path.curvatures.begin(), path.curvatures.end(),
[](const auto & a, const auto & b) { return std::abs(a) < std::abs(b); });
-
- // const auto curvature_average = curvature_sum / static_cast<double>(path.curvatures.size());
- // const auto max_curvature =
- // (curvature_average > 0.0) ? constraints.hard.max_curvature :
- // constraints.hard.min_curvature;
return std::abs(max_path_curvature) / constraints.hard.max_curvature;
});
- // Same lane as goal cost Still in testing dont delete
- // soft_constraints_.emplace_back(
- // [](
- // sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints &
- // constraints,
- // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double {
- // if (path.poses.empty()) return 0.0;
-
- // const auto & goal_pose = input_data.goal_pose;
- // lanelet::ConstLanelet closest_lanelet_to_goal;
- // lanelet::utils::query::getClosestLanelet(
- // input_data.current_lanes, goal_pose, &closest_lanelet_to_goal);
- // lanelet::ConstLanelet closest_lanelet_to_path_end;
- // lanelet::utils::query::getClosestLanelet(
- // input_data.current_lanes, path.poses.back(), &closest_lanelet_to_path_end);
- // return (closest_lanelet_to_goal.id() == closest_lanelet_to_path_end.id()) ? 0.0 : 0.5;
- // });
}
bool SamplingPlannerModule::isExecutionRequested() const
From eb321f7e6d9b667288544c8d5e3e28dee3bf2f5e Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 4 Jan 2024 15:06:46 +0900
Subject: [PATCH 084/115] change success transit function to be more accurate
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 23 +++++++++++--------
.../src/sampling_planner_module.cpp | 6 ++---
2 files changed, 16 insertions(+), 13 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 45a105bc754db..3fa874ea83658 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -196,9 +196,19 @@ class SamplingPlannerModule : public SceneModuleInterface
const double min_target_length = *std::min_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
+
+ lanelet::ConstLanelet closest_lanelet_to_ego;
+ lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
+ lanelet::ConstLanelet closest_lanelet_to_goal;
+ lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
+
+ if (
+ length_to_goal < min_target_length &&
+ closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id())
+ return isReferencePathSafe();
+
const auto nearest_index =
motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose);
- double yaw_difference = 0.0;
if (!nearest_index) return false;
auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double {
geometry_msgs::msg::Vector3 rpy;
@@ -209,18 +219,11 @@ class SamplingPlannerModule : public SceneModuleInterface
const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation;
const double ref_path_yaw = toYaw(quat);
const double ego_yaw = toYaw(ego_pose.orientation);
- yaw_difference = std::abs(ego_yaw - ref_path_yaw);
- constexpr double pi = 3.14159;
+ const double yaw_difference = std::abs(ego_yaw - ref_path_yaw);
- lanelet::ConstLanelet closest_lanelet_to_ego;
- lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
- lanelet::ConstLanelet closest_lanelet_to_goal;
- lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
const bool merged_back_to_path =
- ((length_to_goal < min_target_length) || (std::abs(ego_arc.distance)) < 0.5) &&
- (yaw_difference < pi / 36.0) &&
+ (std::abs(ego_arc.distance) < 0.25) && (yaw_difference < M_PI / 36.0) &&
(closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id()); // TODO(Daniel) magic numbers
- std::cerr << __func__ << " Am I reaching the end of this \n";
return isReferencePathSafe() && (merged_back_to_path);
}
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 3b7b749832043..d1b4fcfd7bef2 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -26,6 +26,9 @@ using tier4_autoware_utils::getPoint;
using tier4_autoware_utils::Point2d;
using utils::toPath;
+namespace bg = boost::geometry;
+namespace bgi = boost::geometry::index;
+
SamplingPlannerModule::SamplingPlannerModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<SamplingPlannerParameters> & parameters,
@@ -43,9 +46,6 @@ SamplingPlannerModule::SamplingPlannerModule(
[](
sampler_common::Path & path, const sampler_common::Constraints & constraints,
const MultiPoint2d & footprint) -> bool {
- namespace bg = boost::geometry;
- namespace bgi = boost::geometry::index;
-
if (!footprint.empty()) {
path.constraint_results.drivable_area =
bg::within(footprint, constraints.drivable_polygons);
From 10b38f9b926d9221f6de26a23d1066ccfdb62232 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 4 Jan 2024 16:00:06 +0900
Subject: [PATCH 085/115] refactor
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../sampling_planner_module.hpp | 27 ++++++++++---------
.../src/sampling_planner_module.cpp | 2 +-
2 files changed, 15 insertions(+), 14 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 3fa874ea83658..ac47b374a2987 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -165,13 +165,15 @@ class SamplingPlannerModule : public SceneModuleInterface
const auto & p = planner_data_->parameters;
const auto ego_pose = planner_data_->self_odometry->pose.pose;
+ const auto goal_pose = planner_data_->route_handler->getGoalPose();
+
lanelet::ConstLanelet current_lane;
if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
RCLCPP_ERROR(
rclcpp::get_logger("behavior_path_planner").get_child("utils"),
"failed to find closest lanelet within route!!!");
- return {};
+ return false;
}
const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
@@ -188,24 +190,23 @@ class SamplingPlannerModule : public SceneModuleInterface
current_lanes.push_back(d.left_lane);
current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
}
+ lanelet::ConstLanelet closest_lanelet_to_ego;
+ lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
+ lanelet::ConstLanelet closest_lanelet_to_goal;
+ lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
+ const bool ego_and_goal_on_same_lanelet =
+ closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id();
+
+ if (!ego_and_goal_on_same_lanelet) return false;
const auto ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose);
- const auto goal_pose = planner_data_->route_handler->getGoalPose();
const auto goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose);
const double length_to_goal = std::abs(goal_arc.length - ego_arc.length);
const double min_target_length = *std::min_element(
internal_params_->sampling.target_lengths.begin(),
internal_params_->sampling.target_lengths.end());
- lanelet::ConstLanelet closest_lanelet_to_ego;
- lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
- lanelet::ConstLanelet closest_lanelet_to_goal;
- lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
-
- if (
- length_to_goal < min_target_length &&
- closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id())
- return isReferencePathSafe();
+ if (length_to_goal < min_target_length) return isReferencePathSafe();
const auto nearest_index =
motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose);
@@ -221,9 +222,9 @@ class SamplingPlannerModule : public SceneModuleInterface
const double ego_yaw = toYaw(ego_pose.orientation);
const double yaw_difference = std::abs(ego_yaw - ref_path_yaw);
+ // TODO(Daniel) magic numbers
const bool merged_back_to_path =
- (std::abs(ego_arc.distance) < 0.25) && (yaw_difference < M_PI / 36.0) &&
- (closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id()); // TODO(Daniel) magic numbers
+ (std::abs(ego_arc.distance) < 0.25) && (yaw_difference < M_PI / 36.0);
return isReferencePathSafe() && (merged_back_to_path);
}
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index d1b4fcfd7bef2..bd20b27d019ef 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -486,7 +486,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
};
// EXTEND prev path
- if (prev_sampling_path_ && prev_sampling_path_->points.size()) {
+ if (prev_sampling_path_ && prev_sampling_path_->points.size() > 0) {
// Update previous path
frenet_planner::Path prev_path_frenet = prev_sampling_path_.value();
frenet_paths.push_back(prev_path_frenet);
From 0b72e9ec3a6e670299afdb49bc3d996a16896dd6 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Fri, 5 Jan 2024 12:57:05 +0900
Subject: [PATCH 086/115] solve bug for path.extend with 0 poses
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../frenet_planner/src/frenet_planner/frenet_planner.cpp | 2 ++
1 file changed, 2 insertions(+)
diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
index 45cf380a63020..497cbc4f7138c 100644
--- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
+++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
@@ -189,6 +189,8 @@ void calculateCartesian(const sampler_common::transform::Spline2D & reference, P
path.poses.push_back(pose);
}
path.yaws.push_back(path.yaws.back());
+ path.poses.push_back(path.poses.back());
+
// Calculate curvatures
for (size_t i = 1; i < path.yaws.size(); ++i) {
const auto dyaw =
From c59a3e4c9208afca4ac14b55fba9a21e100b532d Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Fri, 5 Jan 2024 12:58:44 +0900
Subject: [PATCH 087/115] add hard check for empty paths
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
.../src/sampling_planner_module.cpp | 24 ++++++++++++-------
1 file changed, 16 insertions(+), 8 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index bd20b27d019ef..7604c819d15cd 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -42,6 +42,14 @@ SamplingPlannerModule::SamplingPlannerModule(
std::shared_ptr<SamplingPlannerInternalParameters>(new SamplingPlannerInternalParameters{});
updateModuleParams(parameters);
+ // check if the path is empty
+ hard_constraints_.emplace_back(
+ [](
+ sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
+ [[maybe_unused]] const MultiPoint2d & footprint) -> bool {
+ return !path.points.empty() && !path.poses.empty();
+ });
+
hard_constraints_.emplace_back(
[](
sampler_common::Path & path, const sampler_common::Constraints & constraints,
@@ -66,7 +74,10 @@ SamplingPlannerModule::SamplingPlannerModule(
[](
sampler_common::Path & path, const sampler_common::Constraints & constraints,
[[maybe_unused]] const MultiPoint2d & footprint) -> bool {
- if (path.curvatures.empty()) return true;
+ if (path.curvatures.empty()) {
+ path.constraint_results.curvature = false;
+ return false;
+ }
const bool curvatures_satisfied =
std::all_of(path.curvatures.begin(), path.curvatures.end(), [&](const auto & v) -> bool {
return (v > constraints.hard.min_curvature) && (v < constraints.hard.max_curvature);
@@ -176,8 +187,8 @@ bool SamplingPlannerModule::isExecutionRequested() const
bool SamplingPlannerModule::isReferencePathSafe() const
{
+ // TODO(Daniel): Don't use reference path, use a straight path forward.
std::vector<DrivableLanes> drivable_lanes{};
- const auto & prev_module_path = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().path);
const auto & prev_module_reference_path =
std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
@@ -269,15 +280,12 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData(
const PlanResult & path, const std::vector<geometry_msgs::msg::Point> & left_bound,
const std::vector<geometry_msgs::msg::Point> & right_bound) const
{
- // create planner data
SamplingPlannerData data;
- // planner_data.header = path.header;
auto points = path->points;
data.left_bound = left_bound;
data.right_bound = right_bound;
data.ego_pose = planner_data_->self_odometry->pose.pose;
data.ego_vel = planner_data_->self_odometry->twist.twist.linear.x;
- // data.ego_vel = ego_state_ptr_->twist.twist.linear.x;
return data;
}
@@ -291,8 +299,6 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
return quaternion_tf2;
};
- // auto copy_point_information = []()
-
PathWithLaneId path;
const auto header = planner_data_->route_handler->getRouteHeader();
const auto reference_path_ptr =
@@ -511,7 +517,9 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
prepareSamplingParameters(future_state, reference_spline, *internal_params_);
auto extension_frenet_paths = frenet_planner::generatePaths(
reference_spline, frenet_reuse_state, extension_sampling_parameters);
- for (auto & p : extension_frenet_paths) frenet_paths.push_back(prev_path_frenet.extend(p));
+ for (auto & p : extension_frenet_paths) {
+ if (!p.points.empty()) frenet_paths.push_back(prev_path_frenet.extend(p));
+ }
}
}
From 18ea6fc124052dd249b9ad52f54b14ecc71595d9 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 19 Jan 2024 13:09:14 +0900
Subject: [PATCH 088/115] fix private current_state usage
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 5 ++++-
.../src/sampling_planner_module.cpp | 1 -
2 files changed, 4 insertions(+), 2 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index ac47b374a2987..c24afb2cd7733 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -93,7 +93,10 @@ class SamplingPlannerModule : public SceneModuleInterface
BehaviorModuleOutput plan() override;
CandidateOutput planCandidate() const override;
void updateData() override;
- bool isRootLaneletToBeUpdated() const override { return current_state_ == ModuleStatus::SUCCESS; }
+ bool isRootLaneletToBeUpdated() const override
+ {
+ return getCurrentStatus() == ModuleStatus::SUCCESS;
+ }
void updateModuleParams(const std::any & parameters) override
{
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 7604c819d15cd..8736b282fc9f3 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -24,7 +24,6 @@ using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
using tier4_autoware_utils::getPoint;
using tier4_autoware_utils::Point2d;
-using utils::toPath;
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
From dc1143014ff515d6c875653711b3a409a7c4ce13 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 19 Jan 2024 17:58:17 +0900
Subject: [PATCH 089/115] Add path reuse at different lenghts
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 2 +-
.../src/sampling_planner_module.cpp | 57 ++++++++++++++-----
2 files changed, 45 insertions(+), 14 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index c24afb2cd7733..4a6637fce7cd5 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -111,7 +111,7 @@ class SamplingPlannerModule : public SceneModuleInterface
internal_params_->constraints.soft.length_weight = user_params_->length_weight;
internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight;
internal_params_->constraints.soft.weights = user_params_->weights;
- internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.75);
+ internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.25);
internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m;
internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m;
// Sampling
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 8736b282fc9f3..595fa7c028eca 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -468,8 +468,13 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
}
- auto frenet_paths =
- frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
+ const bool prev_path_is_valid = prev_sampling_path_ && prev_sampling_path_->points.size() > 0;
+ std::vector<frenet_planner::Path> frenet_paths;
+
+ if (!prev_path_is_valid) {
+ frenet_paths =
+ frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
+ }
auto get_goal_pose = [&]() {
auto goal_pose = planner_data_->route_handler->getGoalPose();
@@ -491,22 +496,48 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
};
// EXTEND prev path
- if (prev_sampling_path_ && prev_sampling_path_->points.size() > 0) {
+ if (prev_path_is_valid) {
// Update previous path
frenet_planner::Path prev_path_frenet = prev_sampling_path_.value();
frenet_paths.push_back(prev_path_frenet);
- geometry_msgs::msg::Pose future_pose = prev_path_frenet.poses.back();
- const double length_path = lanelet::utils::getArcCoordinates(current_lanes, future_pose).length;
- const auto goal_pose = get_goal_pose();
- const double length_goal = lanelet::utils::getArcCoordinates(current_lanes, goal_pose).length;
+ auto get_subset = [](const frenet_planner::Path & path, size_t offset) -> frenet_planner::Path {
+ frenet_planner::Path s;
+ s.points = {path.points.begin(), path.points.begin() + offset};
+ s.curvatures = {path.curvatures.begin(), path.curvatures.begin() + offset};
+ s.yaws = {path.yaws.begin(), path.yaws.begin() + offset};
+ s.lengths = {path.lengths.begin(), path.lengths.begin() + offset};
+ s.poses = {path.poses.begin(), path.poses.begin() + offset};
+ return s;
+ };
+
+ sampler_common::State current_state;
+ current_state.pose = {ego_pose.position.x, ego_pose.position.y};
+
+ const auto closest_iter = std::min_element(
+ prev_path_frenet.points.begin(), prev_path_frenet.points.end() - 1,
+ [&](const auto & p1, const auto & p2) {
+ return boost::geometry::distance(p1, current_state.pose) <=
+ boost::geometry::distance(p2, current_state.pose);
+ });
+
+ const auto current_idx = std::distance(prev_path_frenet.points.begin(), closest_iter);
+ const double current_length = prev_path_frenet.lengths.at(current_idx);
+ const double remaining_path_length = prev_path_frenet.lengths.back() - current_length;
+ const double length_step = remaining_path_length / 3.0;
+ std::cout << "Current idx " << current_idx << "\n";
+ std::cout << "current_length " << current_length << "\n";
+ for (double reuse_length = 0.0; reuse_length <= remaining_path_length;
+ reuse_length += length_step) {
+ size_t reuse_idx;
+ for (reuse_idx = current_idx + 1; reuse_idx + 2 < prev_path_frenet.lengths.size();
+ ++reuse_idx) {
+ if (prev_path_frenet.lengths[reuse_idx] - current_length >= reuse_length) break;
+ }
- const double min_target_length = *std::min_element(
- internal_params_->sampling.target_lengths.begin(),
- internal_params_->sampling.target_lengths.end());
+ const auto reused_path = get_subset(prev_path_frenet, reuse_idx);
- if (std::abs(length_goal - length_path) > min_target_length) {
- geometry_msgs::msg::Pose future_pose = prev_path_frenet.poses.back();
+ geometry_msgs::msg::Pose future_pose = reused_path.poses.back();
sampler_common::State future_state =
behavior_path_planner::getInitialState(future_pose, reference_spline);
frenet_planner::FrenetState frenet_reuse_state;
@@ -517,7 +548,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
auto extension_frenet_paths = frenet_planner::generatePaths(
reference_spline, frenet_reuse_state, extension_sampling_parameters);
for (auto & p : extension_frenet_paths) {
- if (!p.points.empty()) frenet_paths.push_back(prev_path_frenet.extend(p));
+ if (!p.points.empty()) frenet_paths.push_back(reused_path.extend(p));
}
}
}
From 05701bb7043bd235ac242e1f044a05719717f127 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 22 Jan 2024 13:24:40 +0900
Subject: [PATCH 090/115] delete old comments use param for path reuse
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/manager.cpp | 20 ++++++---
.../src/sampling_planner_module.cpp | 43 +++----------------
2 files changed, 18 insertions(+), 45 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_sampling_planner_module/src/manager.cpp
index 3ad13efdf56ea..54bf8b2b57a22 100644
--- a/planning/behavior_path_sampling_planner_module/src/manager.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/manager.cpp
@@ -36,15 +36,18 @@ void SamplingPlannerModuleManager::init(rclcpp::Node * node)
p.max_curvature = node->declare_parameter<double>(ns + ".max_curvature");
p.min_curvature = node->declare_parameter<double>(ns + ".min_curvature");
ns = std::string{"constraints.soft"};
- p.lateral_deviation_weight = node->declare_parameter<double>(ns + ".lateral_deviation_weight");
- p.length_weight = node->declare_parameter<double>(ns + ".length_weight");
- p.curvature_weight = node->declare_parameter<double>(ns + ".curvature_weight");
+ p.lateral_deviation_weight =
+ node->declare_parameter<double>(ns + ".lateral_deviation_weight"); // [[unused]] Delete?
+ p.length_weight = node->declare_parameter<double>(ns + ".length_weight"); // [[unused]] Delete?
+ p.curvature_weight =
+ node->declare_parameter<double>(ns + ".curvature_weight"); // [[unused]] Delete?
p.weights = node->declare_parameter<std::vector<double>>(ns + ".weights");
}
{
std::string ns{"sampling"};
p.enable_frenet = node->declare_parameter<bool>(ns + ".enable_frenet");
- p.enable_bezier = node->declare_parameter<bool>(ns + ".enable_bezier");
+ p.enable_bezier = node->declare_parameter<bool>(
+ ns + ".enable_bezier"); // [[unused]] will be used in the future
p.resolution = node->declare_parameter<double>(ns + ".resolution");
p.previous_path_reuse_points_nb =
node->declare_parameter<int>(ns + ".previous_path_reuse_points_nb");
@@ -61,9 +64,12 @@ void SamplingPlannerModuleManager::init(rclcpp::Node * node)
}
{
std::string ns{"preprocessing"};
- p.force_zero_deviation = node->declare_parameter<bool>(ns + ".force_zero_initial_deviation");
- p.force_zero_heading = node->declare_parameter<bool>(ns + ".force_zero_initial_heading");
- p.smooth_reference = node->declare_parameter<bool>(ns + ".smooth_reference_trajectory");
+ p.force_zero_deviation = node->declare_parameter<bool>(
+ ns + ".force_zero_initial_deviation"); // [[unused]] will be used in the future
+ p.force_zero_heading = node->declare_parameter<bool>(
+ ns + ".force_zero_initial_heading"); // [[unused]] will be used in the future
+ p.smooth_reference = node->declare_parameter<bool>(
+ ns + ".smooth_reference_trajectory"); // [[unused]] will be used in the future
}
parameters_ = std::make_shared<SamplingPlannerParameters>(p);
}
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 595fa7c028eca..e95ba8965ac13 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -235,16 +235,12 @@ bool SamplingPlannerModule::isReferencePathSafe() const
auto transform_to_sampling_path = [](const PlanResult plan) {
sampler_common::Path path;
for (size_t i = 0; i < plan->points.size(); ++i) {
- // plan->points[i].point.pose
-
const auto x = plan->points[i].point.pose.position.x;
const auto y = plan->points[i].point.pose.position.y;
const auto quat = plan->points[i].point.pose.orientation;
- // tf2::Quaternion quaternion(q.x,q.y,q.z,q.w);
geometry_msgs::msg::Vector3 rpy;
tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w);
tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
- // const auto z = plan->points[0].point.pose.position.z;
path.points.emplace_back(Point2d{x, y});
path.poses.emplace_back(plan->points[i].point.pose);
path.yaws.emplace_back(rpy.z);
@@ -306,7 +302,6 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID(
for (size_t i = 0; i < frenet_path.points.size(); ++i) {
const auto & frenet_path_point_position = frenet_path.points.at(i);
const auto & frenet_path_point_yaw = frenet_path.yaws.at(i);
- // const auto & frenet_path_point_velocity = frenet_path.points.;
PathPointWithLaneId point{};
point.point.pose.position.x = frenet_path_point_position.x();
point.point.pose.position.y = frenet_path_point_position.y();
@@ -481,7 +476,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
if (!std::any_of(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) {
return lanelet::utils::isInLanelet(goal_pose, lane);
})) {
- // std::cerr << "goal not in current lanes. Finidng farthest path point to use as goal\n";
for (auto it = reference_path_ptr->points.rbegin(); it < reference_path_ptr->points.rend();
it++) {
if (std::any_of(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) {
@@ -495,9 +489,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
return goal_pose;
};
- // EXTEND prev path
+ // Extend prev path
if (prev_path_is_valid) {
- // Update previous path
frenet_planner::Path prev_path_frenet = prev_sampling_path_.value();
frenet_paths.push_back(prev_path_frenet);
@@ -524,9 +517,10 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
const auto current_idx = std::distance(prev_path_frenet.points.begin(), closest_iter);
const double current_length = prev_path_frenet.lengths.at(current_idx);
const double remaining_path_length = prev_path_frenet.lengths.back() - current_length;
- const double length_step = remaining_path_length / 3.0;
- std::cout << "Current idx " << current_idx << "\n";
- std::cout << "current_length " << current_length << "\n";
+ const int path_divisions = (internal_params_->sampling.previous_path_reuse_points_nb > 0)
+ ? internal_params_->sampling.previous_path_reuse_points_nb
+ : 1;
+ const double length_step = remaining_path_length / path_divisions;
for (double reuse_length = 0.0; reuse_length <= remaining_path_length;
reuse_length += length_step) {
size_t reuse_idx;
@@ -721,23 +715,6 @@ void SamplingPlannerModule::updateDebugMarkers()
debug_marker_.markers.push_back(m);
info_marker_.markers.push_back(m);
++m.id;
- // m.ns = "debug_path";
- // m.id = 0UL;
- // m.type = m.POINTS;
- // m.points.clear();
- // m.color.g = 1.0;
- // m.color.b = 0.0;
- // m.scale.y = 0.04;
- // if (!debug_data_.sampled_candidates.empty()) {
- // m.action = m.ADD;
- // for (const auto & p :
- // debug_data_.sampled_candidates[debug_data_.sampled_candidates.size()].points)
- // m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y()));
- // } else {
- // m.action = m.DELETE;
- // }
- // debug_marker_.markers.push_back(m);
- // info_marker_.markers.push_back(m);
m.type = m.LINE_STRIP;
m.ns = "obstacles";
m.id = 0UL;
@@ -764,7 +741,6 @@ void SamplingPlannerModule::updateDebugMarkers()
void SamplingPlannerModule::extendOutputDrivableArea(BehaviorModuleOutput & output)
{
const auto prev_module_path = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().path);
- // const auto current_lanes = utils::getCurrentLanesFromPath(*prev_module_path, planner_data_);
const auto & p = planner_data_->parameters;
const auto ego_pose = planner_data_->self_odometry->pose.pose;
lanelet::ConstLanelet current_lane;
@@ -782,15 +758,6 @@ void SamplingPlannerModule::extendOutputDrivableArea(BehaviorModuleOutput & outp
std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
});
-
- // // const auto drivable_lanes = utils::lane_change::generateDrivableLanes(
- // // *planner_data_->route_handler, current_lanes, status_.target_lanes);
- // const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(current_lanes);
- // const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes);
- // const auto expanded_lanes = utils::expandLanelets(
- // shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
- // dp.drivable_area_types_to_skip);
-
// // for new architecture
DrivableAreaInfo current_drivable_area_info;
current_drivable_area_info.drivable_lanes = drivable_lanes;
From 50e799c38db731c0ba6589d8dc420c4567a10d0e Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 22 Jan 2024 13:38:10 +0900
Subject: [PATCH 091/115] light refactoring
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/sampling_planner_module.cpp | 21 ++++++++-----------
1 file changed, 9 insertions(+), 12 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index e95ba8965ac13..47b2911df9a35 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -463,14 +463,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
}
- const bool prev_path_is_valid = prev_sampling_path_ && prev_sampling_path_->points.size() > 0;
- std::vector<frenet_planner::Path> frenet_paths;
-
- if (!prev_path_is_valid) {
- frenet_paths =
- frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
- }
-
auto get_goal_pose = [&]() {
auto goal_pose = planner_data_->route_handler->getGoalPose();
if (!std::any_of(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) {
@@ -489,8 +481,13 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
return goal_pose;
};
+ const bool prev_path_is_valid = prev_sampling_path_ && prev_sampling_path_->points.size() > 0;
+ const int path_divisions = internal_params_->sampling.previous_path_reuse_points_nb;
+ const bool is_extend_previous_path = path_divisions > 0;
+
+ std::vector<frenet_planner::Path> frenet_paths;
// Extend prev path
- if (prev_path_is_valid) {
+ if (prev_path_is_valid && is_extend_previous_path) {
frenet_planner::Path prev_path_frenet = prev_sampling_path_.value();
frenet_paths.push_back(prev_path_frenet);
@@ -517,9 +514,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
const auto current_idx = std::distance(prev_path_frenet.points.begin(), closest_iter);
const double current_length = prev_path_frenet.lengths.at(current_idx);
const double remaining_path_length = prev_path_frenet.lengths.back() - current_length;
- const int path_divisions = (internal_params_->sampling.previous_path_reuse_points_nb > 0)
- ? internal_params_->sampling.previous_path_reuse_points_nb
- : 1;
const double length_step = remaining_path_length / path_divisions;
for (double reuse_length = 0.0; reuse_length <= remaining_path_length;
reuse_length += length_step) {
@@ -545,6 +539,9 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
if (!p.points.empty()) frenet_paths.push_back(reused_path.extend(p));
}
}
+ } else {
+ frenet_paths =
+ frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
}
const auto left_bound =
From efc87fbd1e0e689910d13a3a63e2169d3609cdb1 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 22 Jan 2024 15:12:27 +0900
Subject: [PATCH 092/115] pre-commit changes
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../behavior_planning/behavior_planning.launch.xml | 2 +-
planning/behavior_path_planner/package.xml | 7 +++----
.../constraints_checker.hpp | 6 +++---
.../behavior_path_sampling_planner_module/manager.hpp | 6 +++---
.../sampling_planner_module.hpp | 6 +++---
.../sampling_planner_parameters.hpp | 6 +++---
.../include/behavior_path_sampling_planner_module/util.hpp | 6 +++---
planning/behavior_path_sampling_planner_module/package.xml | 7 +++----
8 files changed, 22 insertions(+), 24 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index b276345ba1d41..c5296ebeb2eca 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -71,7 +71,7 @@
value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::StartPlannerModuleManager, '")"
if="$(var launch_start_planner_module)"
/>
- <let
+ <let
name="behavior_path_planner_launch_modules"
value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::SamplingPlannerModuleManager, '")"
if="$(var launch_sampling_planner_module)"
diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml
index 04c607df0af6c..b146ba91e6a3b 100644
--- a/planning/behavior_path_planner/package.xml
+++ b/planning/behavior_path_planner/package.xml
@@ -42,7 +42,9 @@
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>behavior_path_planner_common</depend>
+ <depend>bezier_sampler</depend>
<depend>freespace_planning_algorithms</depend>
+ <depend>frenet_planner</depend>
<depend>geometry_msgs</depend>
<depend>interpolation</depend>
<depend>lane_departure_checker</depend>
@@ -52,6 +54,7 @@
<depend>magic_enum</depend>
<depend>motion_utils</depend>
<depend>object_recognition_utils</depend>
+ <depend>path_sampler</depend>
<depend>planning_test_utils</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
@@ -66,10 +69,6 @@
<depend>tier4_planning_msgs</depend>
<depend>vehicle_info_util</depend>
<depend>visualization_msgs</depend>
- <depend>bezier_sampler</depend>
- <depend>frenet_planner</depend>
- <depend>path_sampler</depend>
-
<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp
index 056f0745f317c..435d3b1064aa5 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__CONSTRAINTS_CHECKER_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__CONSTRAINTS_CHECKER_HPP_
+#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__CONSTRAINTS_CHECKER_HPP_
+#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__CONSTRAINTS_CHECKER_HPP_
#include "sampler_common/structures.hpp"
@@ -111,4 +111,4 @@ bool checkHardConstraints(
// ~SoftConstraintsChecker(){};
// };
-#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__CONSTRAINTS_CHECKER_HPP_
+#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__CONSTRAINTS_CHECKER_HPP_
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
index ebf676a6b8649..43177ded3e062 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
+#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_
+#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_
#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
#include "behavior_path_sampling_planner_module/sampling_planner_module.hpp"
@@ -51,4 +51,4 @@ class SamplingPlannerModuleManager : public SceneModuleManagerInterface
} // namespace behavior_path_planner
-#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__MANAGER_HPP_
+#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 4a6637fce7cd5..481d063e2a57e 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
+#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_
+#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
#include "behavior_path_planner_common/marker_utils/utils.hpp"
@@ -271,4 +271,4 @@ class SamplingPlannerModule : public SceneModuleInterface
} // namespace behavior_path_planner
-#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__SAMPLING_PLANNER_MODULE_HPP_
+#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
index 56eeb0b2a3455..b4f0420ad7228 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__PARAMETERS_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__PARAMETERS_HPP_
+#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_
+#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_
#include "bezier_sampler/bezier_sampling.hpp"
#include "sampler_common/structures.hpp"
@@ -83,4 +83,4 @@ struct SamplingPlannerInternalParameters
} preprocessing{};
};
} // namespace behavior_path_planner
-#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SAMPLING_PLANNER__PARAMETERS_HPP_
+#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
index 3103702788a15..056d1a3f8a7ef 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
+#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_
+#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_
#include "sampler_common/structures.hpp"
#include "sampler_common/transform/spline_transform.hpp"
@@ -104,4 +104,4 @@ inline sampler_common::State getInitialState(
} // namespace behavior_path_planner
-#endif // BEHAVIOR_PATH_PLANNER__UTILS__SAMPLING_PLANNER__UTIL_HPP_
+#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_
diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml
index c9b64a53bee43..6a40c89e2d307 100644
--- a/planning/behavior_path_sampling_planner_module/package.xml
+++ b/planning/behavior_path_sampling_planner_module/package.xml
@@ -8,7 +8,6 @@
<maintainer email="daniel.sanchez@tier4.jp">Daniel Sanchez</maintainer>
<maintainer email="maxime.clement@tier4.jp">Maxime Clement</maintainer>
-
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake_auto</buildtool_depend>
@@ -22,7 +21,9 @@
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>behavior_path_planner_common</depend>
+ <depend>bezier_sampler</depend>
<depend>freespace_planning_algorithms</depend>
+ <depend>frenet_planner</depend>
<depend>geometry_msgs</depend>
<depend>interpolation</depend>
<depend>lane_departure_checker</depend>
@@ -32,6 +33,7 @@
<depend>magic_enum</depend>
<depend>motion_utils</depend>
<depend>object_recognition_utils</depend>
+ <depend>path_sampler</depend>
<depend>planning_test_utils</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
@@ -47,9 +49,6 @@
<depend>tier4_planning_msgs</depend>
<depend>vehicle_info_util</depend>
<depend>visualization_msgs</depend>
- <depend>bezier_sampler</depend>
- <depend>frenet_planner</depend>
- <depend>path_sampler</depend>
<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
From 90716724714c631531fd03f293d5a89e29482d37 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 22 Jan 2024 15:31:05 +0900
Subject: [PATCH 093/115] pre-commit add dependency
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../include/sampler_common/constraints/hard_constraint.hpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp
index d5fd66618e79f..652167ad94e75 100644
--- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp
+++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp
@@ -17,6 +17,7 @@
#include "sampler_common/structures.hpp"
+#include <vector>
namespace sampler_common::constraints
{
/// @brief Check if the path satisfies the hard constraints
From 8e6b364ec4aff2741ee5ec261c06ef1f6c22eb5c Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 29 Jan 2024 14:10:07 +0900
Subject: [PATCH 094/115] delete unused dependencies
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
planning/behavior_path_sampling_planner_module/package.xml | 5 -----
1 file changed, 5 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml
index 6a40c89e2d307..6aee782e5eeaf 100644
--- a/planning/behavior_path_sampling_planner_module/package.xml
+++ b/planning/behavior_path_sampling_planner_module/package.xml
@@ -22,17 +22,12 @@
<depend>autoware_perception_msgs</depend>
<depend>behavior_path_planner_common</depend>
<depend>bezier_sampler</depend>
- <depend>freespace_planning_algorithms</depend>
<depend>frenet_planner</depend>
<depend>geometry_msgs</depend>
<depend>interpolation</depend>
- <depend>lane_departure_checker</depend>
<depend>lanelet2_extension</depend>
<depend>libboost-dev</depend>
- <depend>libopencv-dev</depend>
- <depend>magic_enum</depend>
<depend>motion_utils</depend>
- <depend>object_recognition_utils</depend>
<depend>path_sampler</depend>
<depend>planning_test_utils</depend>
<depend>pluginlib</depend>
From f564a430cb37c31f82685e2aeafcb677dfdc3e9d Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 29 Jan 2024 14:19:23 +0900
Subject: [PATCH 095/115] change constraints evaluation to return vectors
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../util.hpp | 19 +++++++++----------
.../src/sampling_planner_module.cpp | 16 ++++++----------
2 files changed, 15 insertions(+), 20 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
index 056d1a3f8a7ef..019bcc684b86f 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
@@ -50,32 +50,31 @@ using SoftConstraintsFunctionVector = std::vector<std::function<double(
using HardConstraintsFunctionVector = std::vector<std::function<bool(
sampler_common::Path &, const sampler_common::Constraints &, const MultiPoint2d &)>>;
-inline void evaluateSoftConstraints(
+inline std::vector<double> evaluateSoftConstraints(
sampler_common::Path & path, const sampler_common::Constraints & constraints,
const SoftConstraintsFunctionVector & soft_constraints_functions,
- const SoftConstraintsInputs & input_data, std::vector<double> & constraints_results)
+ const SoftConstraintsInputs & input_data)
{
- constraints_results.clear();
+ std::vector<double> constraints_results;
for (const auto & f : soft_constraints_functions) {
const auto cost = f(path, constraints, input_data);
constraints_results.push_back(cost);
}
if (constraints.soft.weights.size() != constraints_results.size()) {
path.cost = std::accumulate(constraints_results.begin(), constraints_results.end(), 0.0);
- return;
+ return constraints_results;
}
path.cost = std::inner_product(
constraints_results.begin(), constraints_results.end(), constraints.soft.weights.begin(), 0.0);
- return;
+ return constraints_results;
}
-inline void evaluateHardConstraints(
+inline std::vector<bool> evaluateHardConstraints(
sampler_common::Path & path, const sampler_common::Constraints & constraints,
- const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints,
- std::vector<bool> & constraints_results)
+ const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints)
{
- constraints_results.clear();
+ std::vector<bool> constraints_results;
bool constraints_passed = true;
int idx = 0;
for (const auto & f : hard_constraints) {
@@ -86,7 +85,7 @@ inline void evaluateHardConstraints(
}
path.constraints_satisfied = constraints_passed;
- return;
+ return constraints_results;
}
inline sampler_common::State getInitialState(
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 47b2911df9a35..005f0e386588c 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -260,9 +260,8 @@ bool SamplingPlannerModule::isReferencePathSafe() const
!sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons);
return path.constraint_results.collision;
});
- behavior_path_planner::evaluateHardConstraints(
- reference_path, internal_params_->constraints, footprint, hard_constraints_reference_path,
- hard_constraints_results);
+ evaluateHardConstraints(
+ reference_path, internal_params_->constraints, footprint, hard_constraints_reference_path);
return reference_path.constraints_satisfied;
}
@@ -575,17 +574,14 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
std::vector<std::vector<bool>> hard_constraints_results_full;
std::vector<std::vector<double>> soft_constraints_results_full;
for (auto & path : frenet_paths) {
- std::vector<bool> hard_constraints_results;
- std::vector<double> soft_constraints_results;
const auto footprint =
sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints);
- behavior_path_planner::evaluateHardConstraints(
- path, internal_params_->constraints, footprint, hard_constraints_, hard_constraints_results);
+ std::vector<bool> hard_constraints_results =
+ evaluateHardConstraints(path, internal_params_->constraints, footprint, hard_constraints_);
path.constraint_results.curvature = true;
debug_data_.footprints.push_back(footprint);
- evaluateSoftConstraints(
- path, internal_params_->constraints, soft_constraints_, soft_constraints_input,
- soft_constraints_results);
+ std::vector<double> soft_constraints_results = evaluateSoftConstraints(
+ path, internal_params_->constraints, soft_constraints_, soft_constraints_input);
soft_constraints_results_full.push_back(soft_constraints_results);
}
From 1d906e6e9db259af3d6eb0056d3d9a0922eb862d Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 29 Jan 2024 14:45:03 +0900
Subject: [PATCH 096/115] use tier4 autoware utils function to calc quaternion
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../include/frenet_planner/frenet_planner.hpp | 1 +
.../src/frenet_planner/frenet_planner.cpp | 12 +-----------
2 files changed, 2 insertions(+), 11 deletions(-)
diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp
index c98e6d97969d4..7d60214a52722 100644
--- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp
+++ b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp
@@ -18,6 +18,7 @@
#include "frenet_planner/structures.hpp"
#include "sampler_common/structures.hpp"
#include "sampler_common/transform/spline_transform.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
#include <optional>
#include <vector>
diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
index 497cbc4f7138c..5b395883dd65d 100644
--- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
+++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp
@@ -150,11 +150,6 @@ Path generateCandidate(
void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path)
{
- auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion {
- tf2::Quaternion quaternion_tf2;
- quaternion_tf2.setRPY(roll, pitch, yaw);
- return quaternion_tf2;
- };
if (!path.frenet_points.empty()) {
path.points.reserve(path.frenet_points.size());
path.yaws.reserve(path.frenet_points.size());
@@ -180,12 +175,7 @@ void calculateCartesian(const sampler_common::transform::Spline2D & reference, P
pose.position.x = it->x();
pose.position.y = it->y();
pose.position.z = 0.0;
-
- const auto pose_quaternion = quaternion_from_rpy(0.0, 0.0, yaw);
- pose.orientation.w = pose_quaternion.w();
- pose.orientation.x = pose_quaternion.x();
- pose.orientation.y = pose_quaternion.y();
- pose.orientation.z = pose_quaternion.z();
+ pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, yaw);
path.poses.push_back(pose);
}
path.yaws.push_back(path.yaws.back());
From 7e5b83db43a7531206c53d9a825e56f906ca3e27 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 29 Jan 2024 14:51:46 +0900
Subject: [PATCH 097/115] refactor, use autoware utils
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 4 +---
1 file changed, 1 insertion(+), 3 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 481d063e2a57e..c3a9dc55d66d8 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -215,9 +215,7 @@ class SamplingPlannerModule : public SceneModuleInterface
motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose);
if (!nearest_index) return false;
auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double {
- geometry_msgs::msg::Vector3 rpy;
- tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w);
- tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
+ const auto rpy = tier4_autoware_utils::getRPY(quat);
return rpy.z;
};
const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation;
From 7d9d6f6243e4e46407ec2b45fa672bc74a64852b Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 29 Jan 2024 15:34:07 +0900
Subject: [PATCH 098/115] update comment
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/sampling_planner_module.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 005f0e386588c..38e73ea450afe 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -106,7 +106,7 @@ SamplingPlannerModule::SamplingPlannerModule(
// - goal_pose_yaw); return angle_difference / (3.141519 / 4.0);
// });
- // Distance to goal
+ // Remaining path length
soft_constraints_.emplace_back(
[&](
sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints,
From 9917778baf3a849c33228511fd1c1e76f9973168 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 29 Jan 2024 15:34:25 +0900
Subject: [PATCH 099/115] Add documentation
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../README.md | 76 +++++++++++++++++++
1 file changed, 76 insertions(+)
create mode 100644 planning/behavior_path_sampling_planner_module/README.md
diff --git a/planning/behavior_path_sampling_planner_module/README.md b/planning/behavior_path_sampling_planner_module/README.md
new file mode 100644
index 0000000000000..5b8e961f30c8f
--- /dev/null
+++ b/planning/behavior_path_sampling_planner_module/README.md
@@ -0,0 +1,76 @@
+# Behavior Path Sampling Based Planner
+
+WARNING: This module is experimental and has not been properly tested on a real vehicle, use only on simulations.
+
+## Purpose
+
+This package implements a node that uses sampling based planning to generate a drivable trajectory for the behavior path planner. It is heavily based off the [sampling_based_planner module](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/sampling_based_planner).
+
+## Features
+
+This package is able to:
+
+- create a smooth trajectory to avoid static obstacles.
+- guarantees the generated trajectory (if any) complies with customizable hard constraints.
+- transitions to a success state after the ego vehicle merges to its goal lane.
+- re-use previously generated outputs to re-sample new alternative paths
+
+Note that the velocity is just taken over from the input path.
+
+## Algorithm
+
+Sampling based planning is decomposed into 3 successive steps:
+
+1. Sampling: candidate trajectories are generated.
+2. Pruning: invalid candidates are discarded.
+3. Selection: the best remaining valid candidate is selected.
+
+### Sampling
+
+Candidate trajectories are generated based on the current ego state and some target state.
+1 sampling algorithms is currently implemented: polynomials in the frenet frame.
+
+### Pruning
+
+The validity of each candidate trajectory is checked using a set of hard constraints.
+
+- collision: ensure no collision with static obstacles;
+- curvature: ensure smooth curvature;
+- drivable area: ensure the trajectory stays within the drivable area.
+
+### Selection
+
+Among the valid candidate trajectories, the _best_ one is determined using a set of soft constraints (i.e., objective functions).
+
+- curvature: prefer smoother trajectories;
+- length: prefer trajectories with longer remaining path length;
+- lateral deviation: prefer trajectories close to the goal.
+
+Each soft constraint is associated with a weight to allow tuning of the preferences.
+
+## Limitations
+
+The quality of the candidates generated with polynomials in frenet frame greatly depend on the reference path.
+If the reference path is not smooth, the resulting candidates will probably be un-drivable.
+
+Failure to find a valid trajectory current results in a suddenly stopping trajectory.
+
+The module has troubles generating paths that converge rapidly to the goal lanelet. Basically, after overcoming all obstacles, the module should prioritize paths that rapidly make the ego vehicle converge back to its goal lane (ie. paths with low average and final lateral deviation). However, this does not function properly at the moment.
+
+Detection of proper merging can be rough: Sometimes, the module when detects that the ego has converged on the goal lanelet and that there are no more obstacles, the planner transitions to the goal planner, but the transition is not very smooth and could cause discomfort for the user.
+
+## Future works
+Some possible improvements for this module include:
+
+-Implementing a dynamic weight tuning algorithm: Dynamically change weights depending on the scenario (ie. to prioritize more the paths with low curvature and low avg. lat. deviation after all obstacles have been avoided).
+
+-Implementing multi-objective optimization to improve computing time and possibly make a more dynamic soft constraints weight tuning. [Related publication](https://ieeexplore.ieee.org/abstract/document/10180226).
+
+-Implement bezier curves as another method to obtain samples, see the [sampling_based_planner module](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/sampling_based_planner).
+
+-Explore the possibility to replace several or other behavior path modules with the sampling based behavior path module.
+
+-Perform real-life tests of this module once it has matured and some of its limitations have been solved.
+
+## Other possibilities
+The module is currently aimed at creating paths for static obstacle avoidance. However, the nature of sampling planner allows this module to be expanded or repurposed to other tasks such as lane changes, dynamic avoidance and general reaching of a goal. It is possible, with a good dynamic/scenario dependant weight tuning to use the sampling planning approach as a replacement for the other behavior path modules, assuming good candidate pruning and good soft constraints weight tuning.
From 4bd0b0a6da66127a401b238ee5a8220268d9e88a Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 29 Jan 2024 15:35:46 +0900
Subject: [PATCH 100/115] pre-commit changes
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
planning/behavior_path_sampling_planner_module/README.md | 2 ++
1 file changed, 2 insertions(+)
diff --git a/planning/behavior_path_sampling_planner_module/README.md b/planning/behavior_path_sampling_planner_module/README.md
index 5b8e961f30c8f..601c5b4b26d01 100644
--- a/planning/behavior_path_sampling_planner_module/README.md
+++ b/planning/behavior_path_sampling_planner_module/README.md
@@ -60,6 +60,7 @@ The module has troubles generating paths that converge rapidly to the goal lanel
Detection of proper merging can be rough: Sometimes, the module when detects that the ego has converged on the goal lanelet and that there are no more obstacles, the planner transitions to the goal planner, but the transition is not very smooth and could cause discomfort for the user.
## Future works
+
Some possible improvements for this module include:
-Implementing a dynamic weight tuning algorithm: Dynamically change weights depending on the scenario (ie. to prioritize more the paths with low curvature and low avg. lat. deviation after all obstacles have been avoided).
@@ -73,4 +74,5 @@ Some possible improvements for this module include:
-Perform real-life tests of this module once it has matured and some of its limitations have been solved.
## Other possibilities
+
The module is currently aimed at creating paths for static obstacle avoidance. However, the nature of sampling planner allows this module to be expanded or repurposed to other tasks such as lane changes, dynamic avoidance and general reaching of a goal. It is possible, with a good dynamic/scenario dependant weight tuning to use the sampling planning approach as a replacement for the other behavior path modules, assuming good candidate pruning and good soft constraints weight tuning.
From 86d291a5d5431281c06124dc088df3b0a8a80234 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 31 Jan 2024 13:35:05 +0900
Subject: [PATCH 101/115] delete unused dependencies and repeated args
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../behavior_planning/behavior_planning.launch.xml | 12 ------------
.../package.xml | 4 ----
2 files changed, 16 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index c5296ebeb2eca..602bd25ad8d27 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -18,18 +18,6 @@
<arg name="launch_sampling_planner_module" default="true"/>
<arg name="launch_side_shift_module" default="true"/>
- <arg name="launch_avoidance_module" default="true"/>
- <arg name="launch_dynamic_avoidance_module" default="true"/>
- <arg name="launch_side_shift_module" default="true"/>
- <arg name="launch_goal_planner_module" default="true"/>
- <arg name="launch_start_planner_module" default="true"/>
- <arg name="launch_sampling_planner_module" default="true"/>
- <arg name="launch_lane_change_right_module" default="true"/>
- <arg name="launch_lane_change_left_module" default="true"/>
- <arg name="launch_external_request_lane_change_right_module" default="true"/>
- <arg name="launch_external_request_lane_change_left_module" default="true"/>
- <arg name="launch_avoidance_by_lane_change_module" default="true"/>
-
<arg name="launch_crosswalk_module" default="true"/>
<arg name="launch_walkway_module" default="true"/>
<arg name="launch_traffic_light_module" default="true"/>
diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml
index 6aee782e5eeaf..995f6e5a97fa7 100644
--- a/planning/behavior_path_sampling_planner_module/package.xml
+++ b/planning/behavior_path_sampling_planner_module/package.xml
@@ -11,10 +11,7 @@
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake_auto</buildtool_depend>
- <buildtool_depend>autoware_cmake</buildtool_depend>
- <buildtool_depend>eigen3_cmake_module</buildtool_depend>
- <depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_tf2</depend>
@@ -26,7 +23,6 @@
<depend>geometry_msgs</depend>
<depend>interpolation</depend>
<depend>lanelet2_extension</depend>
- <depend>libboost-dev</depend>
<depend>motion_utils</depend>
<depend>path_sampler</depend>
<depend>planning_test_utils</depend>
From 534001bbb2dd985c374de41912016491243bcf05 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 31 Jan 2024 13:36:10 +0900
Subject: [PATCH 102/115] update copyright and fix magic numbers
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 7 +++++--
.../include/behavior_path_sampling_planner_module/util.hpp | 4 +---
.../src/sampling_planner_module.cpp | 2 +-
3 files changed, 7 insertions(+), 6 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index c3a9dc55d66d8..1ac37d2735d38 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2023 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -224,8 +224,11 @@ class SamplingPlannerModule : public SceneModuleInterface
const double yaw_difference = std::abs(ego_yaw - ref_path_yaw);
// TODO(Daniel) magic numbers
+ constexpr double threshold_lat_distance_for_merging = 0.25;
+ constexpr double threshold_yaw_difference_for_merging = M_PI / 36.0; // 5 degrees
const bool merged_back_to_path =
- (std::abs(ego_arc.distance) < 0.25) && (yaw_difference < M_PI / 36.0);
+ (std::abs(ego_arc.distance) < threshold_lat_distance_to_merge) &&
+ (yaw_difference < threshold_yaw_difference_for_merging);
return isReferencePathSafe() && (merged_back_to_path);
}
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
index 019bcc684b86f..7e93200ebbef2 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2024 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -76,12 +76,10 @@ inline std::vector<bool> evaluateHardConstraints(
{
std::vector<bool> constraints_results;
bool constraints_passed = true;
- int idx = 0;
for (const auto & f : hard_constraints) {
const bool constraint_check = f(path, constraints, footprint);
constraints_passed &= constraint_check;
constraints_results.push_back(constraint_check);
- ++idx;
}
path.constraints_satisfied = constraints_passed;
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 38e73ea450afe..f6143c10a30cc 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2023 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
From 0a5ac84cfe955f5202d6bb1bcdb68514b10954c8 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 31 Jan 2024 13:59:39 +0900
Subject: [PATCH 103/115] delete unused header
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../constraints_checker.hpp | 114 ------------------
1 file changed, 114 deletions(-)
delete mode 100644 planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp
deleted file mode 100644
index 435d3b1064aa5..0000000000000
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/constraints_checker.hpp
+++ /dev/null
@@ -1,114 +0,0 @@
-// Copyright 2023 TIER IV, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__CONSTRAINTS_CHECKER_HPP_
-#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__CONSTRAINTS_CHECKER_HPP_
-
-#include "sampler_common/structures.hpp"
-
-#include <any>
-#include <functional>
-#include <vector>
-
-using SoftConstraintsFunctionVector = std::vector<std::function<double>(
- const sampler_common::Path &, const sampler_common::Constraints &)>;
-
-using HardConstraintsFunctionVector = std::vector<std::function<bool>(
- const sampler_common::Path &, const sampler_common::Constraints &)>;
-
-double checkSoftConstraints(
- const sampler_common::Path & path, const sampler_common::Constraints & constraints,
- const SoftConstraintsFunctionVector & soft_constraints, std::vector<double> & constraints_results)
-{
- std::vector<double> constraints_evaluations(soft_constraints.size(), false);
- double constraints_evaluation = 0.0;
- for (const auto & f : soft_constraints) {
- constraints_evaluation += f(path, constraints);
- }
- return constraints_evaluation;
-}
-
-bool checkHardConstraints(
- const sampler_common::Path & path, const sampler_common::Constraints & constraints,
- const HardConstraintsFunctionVector & hard_constraints, std::vector<double> & constraints_results)
-{
- std::vector<double> constraints_evaluations(hard_constraints.size(), false);
- bool constraints_evaluation = true;
- for (const auto & f : hard_constraints) {
- constraints_evaluation &= f(path, constraints);
- }
- return constraints_evaluation;
-}
-
-// template <typename T>
-// class ConstraintsChecker
-// {
-// private:
-// /* data */
-// public:
-// ConstraintsChecker(ConstraintsFunctionVector constraints_functions)
-// : constraints_functions_(constraints_functions){};
-// ~ConstraintsChecker(){};
-// ConstraintsFunctionVector<T> constraints_functions_;
-
-// T checkConstraints(
-// const sampler_common::Path & path, const sampler_common::Constraints & constraints,
-// std::vector<double> & constraints_results) = 0;
-
-// template <>
-// double checkConstraints(
-// const sampler_common::Path & path, const sampler_common::Constraints & constraints,
-// std::vector<double> & constraints_results)
-// {
-// std::vector<double> constraints_evaluations(constraints_functions_.size(), false);
-// double constraints_evaluation = 0.0;
-// for (const auto & f : constraints_functions_) {
-// constraints_evaluation += f(path, constraints);
-// }
-// return constraints_evaluation;
-// }
-
-// template <>
-// bool checkConstraints(
-// const sampler_common::Path & path, const sampler_common::Constraints & constraints,
-// std::vector<bool> & constraints_results)
-// {
-// std::vector<bool> constraints_evaluations(constraints_functions_.size(), false);
-// bool all_constraints_passed = true;
-// for (const auto & f : constraints_functions_) {
-// all_constraints_passed &= f(path, constraints);
-// }
-// return all_constraints_passed;
-// }
-// };
-
-// class HardConstraintsChecker : public ConstraintsChecker
-// {
-// private:
-// /* data */
-// public:
-// HardConstraintsChecker(/* args */){};
-// ~HardConstraintsChecker(){};
-// };
-
-// class SoftConstraintsChecker : public ConstraintsChecker
-// {
-// private:
-// /* data */
-// public:
-// SoftConstraintsChecker(/* args */){};
-// ~SoftConstraintsChecker(){};
-// };
-
-#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__CONSTRAINTS_CHECKER_HPP_
From 312fe57c11184f81b9942975f82c9a44bd50cc91 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 31 Jan 2024 13:59:56 +0900
Subject: [PATCH 104/115] refactoring
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 2 +-
.../sampling_planner_parameters.hpp | 51 ++++++++++---------
2 files changed, 29 insertions(+), 24 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 1ac37d2735d38..4097f5f286ecb 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -227,7 +227,7 @@ class SamplingPlannerModule : public SceneModuleInterface
constexpr double threshold_lat_distance_for_merging = 0.25;
constexpr double threshold_yaw_difference_for_merging = M_PI / 36.0; // 5 degrees
const bool merged_back_to_path =
- (std::abs(ego_arc.distance) < threshold_lat_distance_to_merge) &&
+ (std::abs(ego_arc.distance) < threshold_lat_distance_for_merging) &&
(yaw_difference < threshold_yaw_difference_for_merging);
return isReferencePathSafe() && (merged_back_to_path);
}
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
index b4f0420ad7228..377379d122aef 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
@@ -55,32 +55,37 @@ struct SamplingPlannerParameters
bool smooth_reference;
};
+struct Preprocessing
+{
+ bool force_zero_deviation{};
+ bool force_zero_heading{};
+ bool smooth_reference{};
+};
+
+struct Frenet
+{
+ std::vector<double> target_lateral_velocities{};
+ std::vector<double> target_lateral_accelerations{};
+};
+
+struct Sampling
+{
+ bool enable_frenet{};
+ bool enable_bezier{};
+ double resolution{};
+ int previous_path_reuse_points_nb{};
+ std::vector<double> target_lengths{};
+ std::vector<double> target_lateral_positions{};
+ int nb_target_lateral_positions{};
+ Frenet frenet;
+ bezier_sampler::SamplingParameters bezier{};
+};
+
struct SamplingPlannerInternalParameters
{
sampler_common::Constraints constraints;
- struct
- {
- bool enable_frenet{};
- bool enable_bezier{};
- double resolution{};
- int previous_path_reuse_points_nb{};
- std::vector<double> target_lengths{};
- std::vector<double> target_lateral_positions{};
- int nb_target_lateral_positions{};
- struct
- {
- std::vector<double> target_lateral_velocities{};
- std::vector<double> target_lateral_accelerations{};
- } frenet;
- bezier_sampler::SamplingParameters bezier{};
- } sampling;
-
- struct
- {
- bool force_zero_deviation{};
- bool force_zero_heading{};
- bool smooth_reference{};
- } preprocessing{};
+ Sampling sampling;
+ Preprocessing preprocessing{};
};
} // namespace behavior_path_planner
#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_
From 57487d0c0c63822ef8b3cd5586165a51baba0fc5 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 31 Jan 2024 16:02:58 +0900
Subject: [PATCH 105/115] remove unused dependency
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
planning/behavior_path_planner/package.xml | 1 -
1 file changed, 1 deletion(-)
diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml
index b146ba91e6a3b..7f50c61a8343a 100644
--- a/planning/behavior_path_planner/package.xml
+++ b/planning/behavior_path_planner/package.xml
@@ -42,7 +42,6 @@
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>behavior_path_planner_common</depend>
- <depend>bezier_sampler</depend>
<depend>freespace_planning_algorithms</depend>
<depend>frenet_planner</depend>
<depend>geometry_msgs</depend>
From 55e6039eb9d3e73027e64bcbe278cbb01a636dcd Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Thu, 1 Feb 2024 11:01:21 +0900
Subject: [PATCH 106/115] update copyright and dependency
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../behavior_path_planner/src/behavior_path_planner_node.cpp | 1 -
.../include/behavior_path_sampling_planner_module/manager.hpp | 2 +-
.../sampling_planner_module.hpp | 2 +-
.../sampling_planner_parameters.hpp | 2 +-
.../include/behavior_path_sampling_planner_module/util.hpp | 2 +-
planning/behavior_path_sampling_planner_module/package.xml | 1 +
planning/behavior_path_sampling_planner_module/src/manager.cpp | 2 +-
7 files changed, 6 insertions(+), 6 deletions(-)
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 c51b2d975b31b..3eb5eca5954fb 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -79,7 +79,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
bound_publisher_ = create_publisher<MarkerArray>("~/debug/bound", 1);
const auto qos_transient_local = rclcpp::QoS{1}.transient_local();
-
// subscriber
velocity_subscriber_ = create_subscription<Odometry>(
"~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onOdometry, this, _1),
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
index 43177ded3e062..1fc1534d915d5 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 4097f5f286ecb..8155dc2fe6a17 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
index 377379d122aef..6e00fefe89574 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
index 7e93200ebbef2..ac8ba788cb9bd 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml
index 995f6e5a97fa7..cfac87b3c557f 100644
--- a/planning/behavior_path_sampling_planner_module/package.xml
+++ b/planning/behavior_path_sampling_planner_module/package.xml
@@ -11,6 +11,7 @@
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake_auto</buildtool_depend>
+ <buildtool_depend>autoware_cmake</buildtool_depend>
<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
diff --git a/planning/behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_sampling_planner_module/src/manager.cpp
index 54bf8b2b57a22..724b87c4752e5 100644
--- a/planning/behavior_path_sampling_planner_module/src/manager.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/manager.cpp
@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
From 15042933b6de59dfec1c0484a05c1ada52451727 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Thu, 1 Feb 2024 11:01:44 +0900
Subject: [PATCH 107/115] update calcBound to work properly
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/sampling_planner_module.cpp | 19 +++++++++----------
1 file changed, 9 insertions(+), 10 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index f6143c10a30cc..85cc7fcf8009d 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -218,10 +218,10 @@ bool SamplingPlannerModule::isReferencePathSafe() const
}
{
- const auto left_bound =
- (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, true));
- const auto right_bound =
- (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, false));
+ const auto left_bound = (utils::calcBound(
+ getPreviousModuleOutput().path, planner_data_, drivable_lanes, false, false, false, true));
+ const auto right_bound = (utils::calcBound(
+ getPreviousModuleOutput().path, planner_data_, drivable_lanes, false, false, false, false));
const auto sampling_planner_data =
createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
@@ -543,11 +543,10 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
}
- const auto left_bound =
- (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, true));
- const auto right_bound =
- (utils::calcBound(planner_data_->route_handler, drivable_lanes, false, false, false));
-
+ const auto left_bound = (utils::calcBound(
+ getPreviousModuleOutput().path, planner_data_, drivable_lanes, false, false, false, true));
+ const auto right_bound = (utils::calcBound(
+ getPreviousModuleOutput().path, planner_data_, drivable_lanes, false, false, false, false));
const auto sampling_planner_data =
createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
From fc7ba1d90540d46108b8ddbf51762672f31e5fb9 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Thu, 1 Feb 2024 17:13:24 +0900
Subject: [PATCH 108/115] solve problem with drivable area
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../static_drivable_area.hpp | 2 +-
.../static_drivable_area.cpp | 10 +++-
.../sampling_planner_module.hpp | 3 +-
.../src/sampling_planner_module.cpp | 47 ++++++++-----------
4 files changed, 30 insertions(+), 32 deletions(-)
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp
index 9afa2608e1db6..0108eb87471f1 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp
@@ -82,7 +82,7 @@ std::vector<geometry_msgs::msg::Point> calcBound(
const std::vector<DrivableLanes> & drivable_lanes,
const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
const bool enable_expanding_freespace_areas, const bool is_left,
- const bool is_driving_forward = true);
+ const bool is_driving_forward = true, const bool expand_to_road_edges = false);
std::vector<geometry_msgs::msg::Point> postProcess(
const std::vector<geometry_msgs::msg::Point> & original_bound, const PathWithLaneId & path,
diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
index 18627e8396239..c160fe6c01c08 100644
--- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
+++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
@@ -1568,7 +1568,8 @@ std::vector<geometry_msgs::msg::Point> calcBound(
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
const std::vector<DrivableLanes> & drivable_lanes,
const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
- const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward)
+ const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward,
+ const bool expand_to_road_edges)
{
using motion_utils::removeOverlapPoints;
@@ -1619,8 +1620,13 @@ std::vector<geometry_msgs::msg::Point> calcBound(
enable_expanding_intersection_areas, is_left, is_driving_forward);
};
+ // If bounds should contain all drivable lanes
+ if (expand_to_road_edges) {
+ return post_process(removeOverlapPoints(to_ros_point(bound_points)), true);
+ }
+
// Step2. if there is no drivable area defined by polygon, return original drivable bound.
- if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) {
+ if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas && false) {
return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process);
}
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 8155dc2fe6a17..83b874b146fde 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -262,7 +262,8 @@ class SamplingPlannerModule : public SceneModuleInterface
std::optional<frenet_planner::Path> prev_sampling_path_ = std::nullopt;
// move to utils
- void extendOutputDrivableArea(BehaviorModuleOutput & output);
+ void extendOutputDrivableArea(
+ BehaviorModuleOutput & output, std::vector<DrivableLanes> & drivable_lanes);
bool isEndPointsConnected(
const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) const;
DrivableLanes generateExpandDrivableLanes(
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 85cc7fcf8009d..c33e5ecb1cbaf 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -218,10 +218,13 @@ bool SamplingPlannerModule::isReferencePathSafe() const
}
{
+ const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path;
const auto left_bound = (utils::calcBound(
- getPreviousModuleOutput().path, planner_data_, drivable_lanes, false, false, false, true));
+ path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true, true,
+ true));
const auto right_bound = (utils::calcBound(
- getPreviousModuleOutput().path, planner_data_, drivable_lanes, false, false, false, false));
+ path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false, true,
+ true));
const auto sampling_planner_data =
createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
@@ -432,8 +435,6 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
set_frenet_state(initial_state, reference_spline, frenet_initial_state);
- std::vector<DrivableLanes> drivable_lanes{};
-
const auto prev_module_path = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().path);
const auto & p = planner_data_->parameters;
@@ -446,9 +447,12 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
"failed to find closest lanelet within route!!!");
return {};
}
+
+ std::vector<DrivableLanes> drivable_lanes{};
const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
// expand drivable lanes
+
std::for_each(
current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) {
drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
@@ -543,10 +547,14 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters);
}
+ const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path;
const auto left_bound = (utils::calcBound(
- getPreviousModuleOutput().path, planner_data_, drivable_lanes, false, false, false, true));
+ path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true, true,
+ true));
const auto right_bound = (utils::calcBound(
- getPreviousModuleOutput().path, planner_data_, drivable_lanes, false, false, false, false));
+ path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false, true,
+ true));
+
const auto sampling_planner_data =
createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
@@ -641,10 +649,10 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
std::cerr << "Poses " << best_path.poses.size() << "\n";
std::cerr << "Length of best " << best_path.lengths.back() << "\n";
- prev_sampling_path_ = best_path;
- auto out_path = convertFrenetPathToPathWithLaneID(
+ const auto out_path = convertFrenetPathToPathWithLaneID(
best_path, current_lanes, planner_data_->route_handler->getGoalPose().position.z);
+ prev_sampling_path_ = best_path;
std::cerr << "road_lanes size " << road_lanes.size() << "\n";
std::cerr << "First lane ID size " << out_path.points.at(0).lane_ids.size() << "\n";
@@ -652,7 +660,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
out.path = out_path;
out.reference_path = *reference_path_ptr;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
- extendOutputDrivableArea(out);
+ extendOutputDrivableArea(out, drivable_lanes);
return out;
}
@@ -730,26 +738,9 @@ void SamplingPlannerModule::updateDebugMarkers()
}
}
-void SamplingPlannerModule::extendOutputDrivableArea(BehaviorModuleOutput & output)
+void SamplingPlannerModule::extendOutputDrivableArea(
+ BehaviorModuleOutput & output, std::vector<DrivableLanes> & drivable_lanes)
{
- const auto prev_module_path = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().path);
- const auto & p = planner_data_->parameters;
- const auto ego_pose = planner_data_->self_odometry->pose.pose;
- lanelet::ConstLanelet current_lane;
-
- if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
- RCLCPP_ERROR(
- rclcpp::get_logger("behavior_path_planner").get_child("utils"),
- "failed to find closest lanelet within route!!!");
- return;
- }
- const auto current_lanes = planner_data_->route_handler->getLaneletSequence(
- current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
- std::vector<DrivableLanes> drivable_lanes{};
- // expand drivable lanes
- std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lanelet) {
- drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
- });
// // for new architecture
DrivableAreaInfo current_drivable_area_info;
current_drivable_area_info.drivable_lanes = drivable_lanes;
From 24efc6fb8d4ee8bddcb46bbe8605edd27c98fd01 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Thu, 1 Feb 2024 17:16:56 +0900
Subject: [PATCH 109/115] remove forced false
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/utils/drivable_area_expansion/static_drivable_area.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
index c160fe6c01c08..4e22f8e262c6d 100644
--- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
+++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
@@ -1626,7 +1626,7 @@ std::vector<geometry_msgs::msg::Point> calcBound(
}
// Step2. if there is no drivable area defined by polygon, return original drivable bound.
- if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas && false) {
+ if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) {
return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process);
}
From 565ccddbdf5bedfac38db36e7303a71b476e2f19 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 2 Feb 2024 09:43:45 +0900
Subject: [PATCH 110/115] solve calc bound problem
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../static_drivable_area.hpp | 2 +-
.../static_drivable_area.cpp | 8 +-------
.../sampling_planner_module.hpp | 6 ++++--
.../src/sampling_planner_module.cpp | 17 +++++++----------
4 files changed, 13 insertions(+), 20 deletions(-)
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp
index 0108eb87471f1..9afa2608e1db6 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp
@@ -82,7 +82,7 @@ std::vector<geometry_msgs::msg::Point> calcBound(
const std::vector<DrivableLanes> & drivable_lanes,
const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
const bool enable_expanding_freespace_areas, const bool is_left,
- const bool is_driving_forward = true, const bool expand_to_road_edges = false);
+ const bool is_driving_forward = true);
std::vector<geometry_msgs::msg::Point> postProcess(
const std::vector<geometry_msgs::msg::Point> & original_bound, const PathWithLaneId & path,
diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
index 4e22f8e262c6d..18627e8396239 100644
--- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
+++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
@@ -1568,8 +1568,7 @@ std::vector<geometry_msgs::msg::Point> calcBound(
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
const std::vector<DrivableLanes> & drivable_lanes,
const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
- const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward,
- const bool expand_to_road_edges)
+ const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward)
{
using motion_utils::removeOverlapPoints;
@@ -1620,11 +1619,6 @@ std::vector<geometry_msgs::msg::Point> calcBound(
enable_expanding_intersection_areas, is_left, is_driving_forward);
};
- // If bounds should contain all drivable lanes
- if (expand_to_road_edges) {
- return post_process(removeOverlapPoints(to_ros_point(bound_points)), true);
- }
-
// Step2. if there is no drivable area defined by polygon, return original drivable bound.
if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) {
return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process);
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 83b874b146fde..916bbc01117b6 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -224,11 +224,13 @@ class SamplingPlannerModule : public SceneModuleInterface
const double yaw_difference = std::abs(ego_yaw - ref_path_yaw);
// TODO(Daniel) magic numbers
- constexpr double threshold_lat_distance_for_merging = 0.25;
- constexpr double threshold_yaw_difference_for_merging = M_PI / 36.0; // 5 degrees
+ constexpr double threshold_lat_distance_for_merging = 0.15;
+ constexpr double threshold_yaw_difference_for_merging = M_PI / 72.0; // 2.5 degrees
const bool merged_back_to_path =
(std::abs(ego_arc.distance) < threshold_lat_distance_for_merging) &&
(yaw_difference < threshold_yaw_difference_for_merging);
+ if (isReferencePathSafe() && (merged_back_to_path))
+ for (int i = 0; i < 10; ++i) std::cerr << "MERGED BACK!!!!!\n";
return isReferencePathSafe() && (merged_back_to_path);
}
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index c33e5ecb1cbaf..0a1fe5b0d2335 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -220,11 +220,9 @@ bool SamplingPlannerModule::isReferencePathSafe() const
{
const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path;
const auto left_bound = (utils::calcBound(
- path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true, true,
- true));
+ path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true));
const auto right_bound = (utils::calcBound(
- path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false, true,
- true));
+ path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false));
const auto sampling_planner_data =
createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
@@ -445,7 +443,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
RCLCPP_ERROR(
rclcpp::get_logger("behavior_path_planner").get_child("utils"),
"failed to find closest lanelet within route!!!");
- return {};
+ return getPreviousModuleOutput();
}
std::vector<DrivableLanes> drivable_lanes{};
@@ -549,11 +547,9 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path;
const auto left_bound = (utils::calcBound(
- path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true, true,
- true));
+ path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true));
const auto right_bound = (utils::calcBound(
- path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false, true,
- true));
+ path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false));
const auto sampling_planner_data =
createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
@@ -630,13 +626,14 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
BehaviorModuleOutput out;
PathWithLaneId out_path = (prev_sampling_path_)
? convertFrenetPathToPathWithLaneID(
- prev_sampling_path_.value(), road_lanes,
+ prev_sampling_path_.value(), current_lanes,
planner_data_->route_handler->getGoalPose().position.z)
: PathWithLaneId{};
out.path = (prev_sampling_path_) ? out_path : getPreviousModuleOutput().path;
out.reference_path = getPreviousModuleOutput().reference_path;
out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
+ extendOutputDrivableArea(out, drivable_lanes);
return out;
}
From a7faeb6e6354e3f79ceedb53890efdc59bae923e Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 2 Feb 2024 11:55:43 +0900
Subject: [PATCH 111/115] fix compatibility with updates to bpp
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 2 --
1 file changed, 2 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 916bbc01117b6..e9feb64bcfece 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -236,8 +236,6 @@ class SamplingPlannerModule : public SceneModuleInterface
bool canTransitFailureState() override { return false; }
- bool canTransitIdleToRunningState() override { return prev_sampling_path_.has_value(); }
-
bool isReferencePathSafe() const;
void updateDebugMarkers();
From 5283a033185e819d83fe5470c2100fa840af2ad2 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 2 Feb 2024 12:04:05 +0900
Subject: [PATCH 112/115] remove cerr print
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 2 --
1 file changed, 2 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index e9feb64bcfece..e2a103ea84281 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -229,8 +229,6 @@ class SamplingPlannerModule : public SceneModuleInterface
const bool merged_back_to_path =
(std::abs(ego_arc.distance) < threshold_lat_distance_for_merging) &&
(yaw_difference < threshold_yaw_difference_for_merging);
- if (isReferencePathSafe() && (merged_back_to_path))
- for (int i = 0; i < 10; ++i) std::cerr << "MERGED BACK!!!!!\n";
return isReferencePathSafe() && (merged_back_to_path);
}
From 29a6f0fc23cf86a3e0ee292bbc20032f804d61b7 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 2 Feb 2024 16:27:56 +0900
Subject: [PATCH 113/115] solve bugs when merging with lane
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 6 +--
.../src/sampling_planner_module.cpp | 38 +++++++++++++++++++
2 files changed, 40 insertions(+), 4 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index e2a103ea84281..8f99c2c04926a 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -205,11 +205,9 @@ class SamplingPlannerModule : public SceneModuleInterface
const auto ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose);
const auto goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose);
const double length_to_goal = std::abs(goal_arc.length - ego_arc.length);
- const double min_target_length = *std::min_element(
- internal_params_->sampling.target_lengths.begin(),
- internal_params_->sampling.target_lengths.end());
- if (length_to_goal < min_target_length) return isReferencePathSafe();
+ constexpr double epsilon = 1E-5;
+ if (length_to_goal < epsilon) return isReferencePathSafe();
const auto nearest_index =
motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose);
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 0a1fe5b0d2335..9bd98539b50ff 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -172,6 +172,44 @@ SamplingPlannerModule::SamplingPlannerModule(
bool SamplingPlannerModule::isExecutionRequested() const
{
+ const auto & p = planner_data_->parameters;
+ const auto ego_pose = planner_data_->self_odometry->pose.pose;
+ const auto goal_pose = planner_data_->route_handler->getGoalPose();
+
+ lanelet::ConstLanelet current_lane;
+
+ if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("behavior_path_planner").get_child("utils"),
+ "failed to find closest lanelet within route!!!");
+ return false;
+ }
+ const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
+ current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
+ // expand drivable lanes
+ std::vector<DrivableLanes> drivable_lanes{};
+
+ std::for_each(
+ current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) {
+ drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
+ });
+
+ lanelet::ConstLanelets current_lanes;
+
+ for (auto & d : drivable_lanes) {
+ current_lanes.push_back(d.right_lane);
+ current_lanes.push_back(d.left_lane);
+ current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
+ }
+ lanelet::ConstLanelet closest_lanelet_to_ego;
+ lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
+ lanelet::ConstLanelet closest_lanelet_to_goal;
+ lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
+ const bool ego_and_goal_on_same_lanelet =
+ closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id();
+
+ if (!ego_and_goal_on_same_lanelet) return false;
+
if (getPreviousModuleOutput().reference_path.points.empty()) {
return false;
}
From 83985cdcb25778b04fb4fbcd6864b0af7839306c Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 2 Feb 2024 18:04:33 +0900
Subject: [PATCH 114/115] solve issue of sbp not activating
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/sampling_planner_module.cpp | 28 -------------------
1 file changed, 28 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
index 9bd98539b50ff..73a726d8cdf7c 100644
--- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
+++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp
@@ -172,10 +172,7 @@ SamplingPlannerModule::SamplingPlannerModule(
bool SamplingPlannerModule::isExecutionRequested() const
{
- const auto & p = planner_data_->parameters;
const auto ego_pose = planner_data_->self_odometry->pose.pose;
- const auto goal_pose = planner_data_->route_handler->getGoalPose();
-
lanelet::ConstLanelet current_lane;
if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) {
@@ -184,31 +181,6 @@ bool SamplingPlannerModule::isExecutionRequested() const
"failed to find closest lanelet within route!!!");
return false;
}
- const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence(
- current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
- // expand drivable lanes
- std::vector<DrivableLanes> drivable_lanes{};
-
- std::for_each(
- current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) {
- drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_));
- });
-
- lanelet::ConstLanelets current_lanes;
-
- for (auto & d : drivable_lanes) {
- current_lanes.push_back(d.right_lane);
- current_lanes.push_back(d.left_lane);
- current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end());
- }
- lanelet::ConstLanelet closest_lanelet_to_ego;
- lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego);
- lanelet::ConstLanelet closest_lanelet_to_goal;
- lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal);
- const bool ego_and_goal_on_same_lanelet =
- closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id();
-
- if (!ego_and_goal_on_same_lanelet) return false;
if (getPreviousModuleOutput().reference_path.points.empty()) {
return false;
From 4b87d15adf7de44f316b06c896eb66defa026340 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Mon, 5 Feb 2024 14:27:38 +0900
Subject: [PATCH 115/115] remove unused commented code
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../sampling_planner_module.hpp | 6 ------
1 file changed, 6 deletions(-)
diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
index 8f99c2c04926a..f72be654d1e1e 100644
--- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
+++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp
@@ -129,12 +129,6 @@ class SamplingPlannerModule : public SceneModuleInterface
user_params_->target_lateral_velocities;
internal_params_->sampling.frenet.target_lateral_accelerations =
user_params_->target_lateral_accelerations;
- // internal_params_->sampling.bezier.nb_k = user_params_->nb_k;
- // internal_params_->sampling.bezier.mk_min = user_params_->mk_min;
- // internal_params_->sampling.bezier.mk_max = user_params_->mk_max;
- // internal_params_->sampling.bezier.nb_t = user_params_->nb_t;
- // internal_params_->sampling.bezier.mt_min = user_params_->mt_min;
- // internal_params_->sampling.bezier.mt_max = user_params_->mt_max;
// Preprocessing
internal_params_->preprocessing.force_zero_deviation = user_params_->force_zero_deviation;