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(behavior_velocity_run_out_module): exclude obstacle crossing ego back line #6680

Merged
merged 20 commits into from
Mar 27, 2024
Merged

feat(behavior_velocity_run_out_module): exclude obstacle crossing ego back line #6680

Changes from 1 commit
Commits
Show all changes
20 commits
Select commit Hold shift + click to select a range
8a2ed10
add method to ignore target obstacles that cross ego cut lane
danielsanchezaran Mar 22, 2024
4c56d2e
WIP add debug support
danielsanchezaran Mar 25, 2024
15323ed
add params and finish debug marker
danielsanchezaran Mar 25, 2024
9db1a4b
change lane to line
danielsanchezaran Mar 25, 2024
97fef69
use autoware utils to get the cut line
danielsanchezaran Mar 26, 2024
6750f64
simplify code wit calcOffsetPose
danielsanchezaran Mar 26, 2024
099a2c7
Update readme and eliminate unused code
danielsanchezaran Mar 26, 2024
214dc90
update readme
danielsanchezaran Mar 26, 2024
0ea38b3
eliminate unused function
danielsanchezaran Mar 26, 2024
be22578
readme
danielsanchezaran Mar 26, 2024
ee30dd4
comments and readme
danielsanchezaran Mar 26, 2024
8da6110
eliminate unused include
danielsanchezaran Mar 26, 2024
e202b55
typo
danielsanchezaran Mar 26, 2024
306a050
rename param for consistency
danielsanchezaran Mar 26, 2024
24ff740
change lane to line for consistency
danielsanchezaran Mar 27, 2024
19fb4e6
rename for clarity, add brief
danielsanchezaran Mar 27, 2024
b678057
fix indentation
danielsanchezaran Mar 27, 2024
006a24b
update description
danielsanchezaran Mar 27, 2024
9b9efe8
lane ->line
danielsanchezaran Mar 27, 2024
63f02b7
lane -> line
danielsanchezaran Mar 27, 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
47 changes: 9 additions & 38 deletions planning/behavior_velocity_run_out_module/src/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,20 +181,15 @@ bool pathIntersectsEgoCutLine(
const double half_line_length, std::vector<geometry_msgs::msg::Point> & ego_cut_lane)
{
if (path.size() < 2) return false;
const auto ego_rpy = rpyFromQuat(ego_pose.orientation);
const double ego_yaw = ego_rpy.z;

auto getOffsetPoint = [](
const geometry_msgs::msg::Pose & ego_pose, const double half_line_length,
const double angle) {
geometry_msgs::msg::Point p;
p.x = ego_pose.position.x + half_line_length * std::cos(angle);
p.y = ego_pose.position.y + half_line_length * std::sin(angle);
return p;
};

const geometry_msgs::msg::Point p1 = getOffsetPoint(ego_pose, half_line_length, ego_yaw + M_PI_2);
const geometry_msgs::msg::Point p2 = getOffsetPoint(ego_pose, half_line_length, ego_yaw - M_PI_2);
auto getOffsetPoint =
[](const geometry_msgs::msg::Pose & ego_pose, const double half_line_length) {
const auto p = tier4_autoware_utils::calcOffsetPose(ego_pose, 0.0, half_line_length, 0.0);
return tier4_autoware_utils::createPoint(p.position.x, p.position.y, p.position.z);
Copy link
Contributor

Choose a reason for hiding this comment

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

tier4_autoware_utils::calcOffsetPose(ego_pose, 0.0, half_line_length, 0.0).position

is enough.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

@takayuki5168 Ah! right thanks for the review

};

const auto p1 = getOffsetPoint(ego_pose, half_line_length);
const auto p2 = getOffsetPoint(ego_pose, -half_line_length);

ego_cut_lane = {p1, p2};
for (size_t i = 1; i < path.size(); ++i) {
const geometry_msgs::msg::Point & p3 = path.at(i).position;
Expand All @@ -207,30 +202,6 @@ bool pathIntersectsEgoCutLine(
return false;
}

// LineString2d createLineStringFromPath(const PredictedPath & path)
// {
// LineString2d line_string;

// for (const auto & p : path.path) {
// line_string.emplace_back(Point2d{p.position.x, p.position.y});
// }
// return line_string;
// }

// LineString2d createEgoCutLine(const geometry_msgs::msg::Pose & ego_pose, const double offset)
// {
// Point2d ego_point{ego_pose.position.x, ego_pose.position.y};
// const auto ego_rpy = rpyFromQuat(ego_pose.orientation);
// const double ego_yaw = ego_rpy.z;
// const Point2d p1{
// ego_pose.position.x + offset * std::cos(ego_yaw + M_PI_2),
// ego_pose.position.y + offset * std::sin(ego_yaw + M_PI_2)};
// const Point2d p2{
// ego_pose.position.x + offset * std::cos(ego_yaw - M_PI_2),
// ego_pose.position.y + offset * std::sin(ego_yaw - M_PI_2)};
// return {p1, p2};
// }

LineString2d createLineString2d(const lanelet::BasicPolygon2d & poly)
{
LineString2d line_string;
Expand Down
Loading