Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autoware_behavior_velocity_planner_common,autoware_behavior_velocity_stop_line_module): revert pr7710 and Check next_lane_id in createTargetPoint on stop line #7896

Closed
wants to merge 3 commits into from
Closed

feat(autoware_behavior_velocity_planner_common,autoware_behavior_velocity_stop_line_module): revert pr7710 and Check next_lane_id in createTargetPoint on stop line #7896

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1daaab4
Revert "fix(autoware_behavior_velocity_planner_common): remove lane_i…
yhisaki Jul 8, 2024
915c102
lane_id to lane_ids
yhisaki Jul 8, 2024
f117c9d
add detail comment and check prev lane
yhisaki Jul 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions .../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@

// Get stop point
const auto stop_point = arc_lane_utils::createTargetPoint(
original_path, stop_line, planner_param_.stop_margin,
original_path, stop_line, {lane_id_}, planner_param_.stop_margin,
planner_data_->vehicle_info_.max_longitudinal_offset_m);
if (!stop_point) {
return true;
Expand Down Expand Up @@ -128,7 +128,7 @@
if (planner_param_.use_dead_line) {
// Use '-' for margin because it's the backward distance from stop line
const auto dead_line_point = arc_lane_utils::createTargetPoint(
original_path, stop_line, -planner_param_.dead_line_margin,
original_path, stop_line, {lane_id_}, -planner_param_.dead_line_margin,

Check warning on line 131 in planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp#L131 

Added line #L131 was not covered by tests
planner_data_->vehicle_info_.max_longitudinal_offset_m);

if (dead_line_point) {
Expand Down
2 changes: 1 addition & 1 deletion ...planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason
return true;
}
const auto stop_point = arc_lane_utils::createTargetPoint(
original_path, stop_line.value(), planner_param_.stop_margin,
original_path, stop_line.value(), {lane_id_}, planner_param_.stop_margin,
planner_data_->vehicle_info_.max_longitudinal_offset_m);
if (!stop_point) {
setSafe(true);
Expand Down
30 changes: 25 additions & 5 deletions ...er_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,10 @@
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>

#include <algorithm>
#include <cstdint>
#include <optional>
#include <utility>
#include <vector>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
Expand All @@ -31,7 +33,7 @@ namespace autoware::behavior_velocity_planner
{
namespace
{
geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p)
inline geometry_msgs::msg::Point convertToGeomPoint(const autoware::universe_utils::Point2d & p)
{
geometry_msgs::msg::Point geom_p;
geom_p.x = p.x();
Expand Down Expand Up @@ -61,9 +63,27 @@ std::optional<geometry_msgs::msg::Point> checkCollision(
template <class T>
std::optional<PathIndexWithPoint> findCollisionSegment(
const T & path, const geometry_msgs::msg::Point & stop_line_p1,
const geometry_msgs::msg::Point & stop_line_p2)
const geometry_msgs::msg::Point & stop_line_p2, const std::vector<int64_t> & target_lane_ids)
{
for (size_t i = 0; i < path.points.size() - 1; ++i) {
const auto & prev_lane_ids = path.points.at(i).lane_ids;
const auto & next_lane_ids = path.points.at(i + 1).lane_ids;

const bool is_target_lane_in_prev_lane = std::any_of(
target_lane_ids.begin(), target_lane_ids.end(), [&prev_lane_ids](size_t target_lane_id) {
return std::find(prev_lane_ids.begin(), prev_lane_ids.end(), target_lane_id) !=
prev_lane_ids.end();
});
const bool is_target_lane_in_next_lane = std::any_of(
target_lane_ids.begin(), target_lane_ids.end(), [&next_lane_ids](size_t target_lane_id) {
return std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) !=
next_lane_ids.end();
});

if (!is_target_lane_in_prev_lane && !is_target_lane_in_next_lane) {
continue;
}

const auto & p1 =
autoware::universe_utils::getPoint(path.points.at(i)); // Point before collision point
const auto & p2 =
Expand All @@ -81,12 +101,12 @@ std::optional<PathIndexWithPoint> findCollisionSegment(

template <class T>
std::optional<PathIndexWithPoint> findCollisionSegment(
const T & path, const LineString2d & stop_line)
const T & path, const LineString2d & stop_line, const std::vector<int64_t> & target_lane_ids)
{
const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0));
const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1));

return findCollisionSegment(path, stop_line_p1, stop_line_p2);
return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_ids);
}

template <class T>
Expand Down Expand Up @@ -183,7 +203,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse

std::optional<PathIndexWithPose> createTargetPoint(
const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
const double margin, const double vehicle_offset);
const std::vector<int64_t> & lane_ids, const double margin, const double vehicle_offset);

} // namespace arc_lane_utils
} // namespace autoware::behavior_velocity_planner
Expand Down
6 changes: 2 additions & 4 deletions ...ocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,6 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif

#include <algorithm>
#include <memory>
#include <utility>
#include <vector>

Expand Down Expand Up @@ -109,10 +107,10 @@

std::optional<PathIndexWithPose> createTargetPoint(
const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
const double margin, const double vehicle_offset)
const std::vector<int64_t> & lane_ids, const double margin, const double vehicle_offset)
{
// Find collision segment
const auto collision_segment = findCollisionSegment(path, stop_line);
const auto collision_segment = findCollisionSegment(path, stop_line, lane_ids);

Check warning on line 113 in planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Excess Number of Function Arguments 

createTargetPoint has 5 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
if (!collision_segment) {
// No collision
return {};
Expand Down
5 changes: 4 additions & 1 deletion ...elocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,11 +49,14 @@ TEST(findCollisionSegment, nominal)
*
**/
auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6);
for (auto & point : path.points) {
point.lane_ids.push_back(100);
}

LineString2d stop_line;
stop_line.emplace_back(Point2d(3.5, 3.0));
stop_line.emplace_back(Point2d(3.5, -3.0));
auto segment = arc_lane_utils::findCollisionSegment(path, stop_line);
auto segment = arc_lane_utils::findCollisionSegment(path, stop_line, {100});
EXPECT_EQ(segment->first, static_cast<size_t>(3));
EXPECT_DOUBLE_EQ(segment->second.x, 3.5);
EXPECT_DOUBLE_EQ(segment->second.y, 0.0);
Expand Down
22 changes: 18 additions & 4 deletions planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,12 @@
#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>

#include <algorithm>
#include <vector>
#include <lanelet2_core/Forward.h>

#include <cstdint>

namespace autoware::behavior_velocity_planner
{
namespace bg = boost::geometry;

StopLineModule::StopLineModule(
const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line,
Expand Down Expand Up @@ -51,8 +51,22 @@
stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length);

// Calculate stop pose and insert index

// Due to the resampling of PathWithLaneId, lane_id mismatches can occur at intersections near the
// ends of lanes. Therefore, the system now accepts the next and previous lane_ids in addition to
// the intended lane_id. See more details in the following link:
// https://github.com/autowarefoundation/autoware.universe/pull/7896#issue-2395067667
auto lane = this->planner_data_->route_handler_->getLaneletsFromId(lane_id_);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please add a comment since this change is not straightforward.

std::vector<int64_t> search_lane_ids;
search_lane_ids.push_back(lane_id_);
for (const auto & next_lane : this->planner_data_->route_handler_->getNextLanelets(lane)) {
search_lane_ids.push_back(next_lane.id());
}
for (const auto & prev_lane : this->planner_data_->route_handler_->getPreviousLanelets(lane)) {
search_lane_ids.push_back(prev_lane.id());
}
const auto stop_point = arc_lane_utils::createTargetPoint(
*path, stop_line, planner_param_.stop_margin,
*path, stop_line, search_lane_ids, planner_param_.stop_margin,

Check warning on line 69 in planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

StopLineModule::modifyPathVelocity increases in cyclomatic complexity from 13 to 15, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
planner_data_->vehicle_info_.max_longitudinal_offset_m);

// If no collision found, do nothing
Expand Down
2 changes: 1 addition & 1 deletion ...or_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -379,7 +379,7 @@ std::optional<size_t> VirtualTrafficLightModule::getPathIndexOfFirstEndLine()
end_line_p2.y = end_line.back().y();

const auto collision =
arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2);
arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, {lane_id_});
if (!collision) {
continue;
}
Expand Down
Loading