Skip to content

Commit

Permalink
Merge branch 'beta/v0.29.0' into exp/beta/v0.29.0-semseg-filter
Browse files Browse the repository at this point in the history
  • Loading branch information
badai-nguyen authored Sep 10, 2024
2 parents e6063c4 + 79dc610 commit 44c5577
Show file tree
Hide file tree
Showing 82 changed files with 1,772 additions and 1,751 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -2504,6 +2504,15 @@ extern template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg:
const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
const double threshold = 0.0);

/// @brief calculate the time_from_start fields of the given trajectory points
/// @details this function assumes constant longitudinal velocity between points
/// @param trajectory trajectory for which to calculate the time_from_start
/// @param current_ego_point current ego position
/// @param min_velocity minimum velocity used for a trajectory point
void calculate_time_from_start(
std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f);

} // namespace autoware::motion_utils

#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_
17 changes: 17 additions & 0 deletions common/autoware_motion_utils/src/resample/resample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@
#include "interpolation/spline_interpolation.hpp"
#include "interpolation/zero_order_hold.hpp"

#include <cstdlib>

namespace autoware::motion_utils
{
std::vector<geometry_msgs::msg::Point> resamplePointVector(
Expand Down Expand Up @@ -601,11 +603,13 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory(
rear_wheel_angle.push_back(input_trajectory.points.front().rear_wheel_angle_rad);
time_from_start.push_back(
rclcpp::Duration(input_trajectory.points.front().time_from_start).seconds());

for (size_t i = 1; i < input_trajectory.points.size(); ++i) {
const auto & prev_pt = input_trajectory.points.at(i - 1);
const auto & curr_pt = input_trajectory.points.at(i);
const double ds =
autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position);

input_arclength.push_back(ds + input_arclength.back());
input_pose.push_back(curr_pt.pose);
v_lon.push_back(curr_pt.longitudinal_velocity_mps);
Expand All @@ -617,6 +621,19 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory(
time_from_start.push_back(rclcpp::Duration(curr_pt.time_from_start).seconds());
}

// Set Zero Velocity After Stop Point
// If the longitudinal velocity is zero, set the velocity to zero after that point.
bool stop_point_found_in_v_lon = false;
constexpr double epsilon = 1e-4;
for (size_t i = 0; i < v_lon.size(); ++i) {
if (std::abs(v_lon.at(i)) < epsilon) {
stop_point_found_in_v_lon = true;
}
if (stop_point_found_in_v_lon) {
v_lon.at(i) = 0.0;
}
}

// Interpolate
const auto lerp = [&](const auto & input) {
return interpolation::lerp(input_arclength, input, resampled_arclength);
Expand Down
23 changes: 23 additions & 0 deletions common/autoware_motion_utils/src/trajectory/trajectory.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -599,4 +599,27 @@ template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg::Trajec
const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
const double threshold);

void calculate_time_from_start(
std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
const geometry_msgs::msg::Point & current_ego_point, const float min_velocity)
{
const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point);
if (nearest_segment_idx + 1 == trajectory.size()) {
return;
}
for (auto & p : trajectory) {
p.time_from_start = rclcpp::Duration::from_seconds(0);
}
// TODO(Maxime): some points can have very low velocities which introduce huge time errors
// Temporary solution: use a minimum velocity
for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) {
const auto & from = trajectory[idx - 1];
const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps);
if (velocity != 0.0) {
auto & to = trajectory[idx];
const auto t = universe_utils::calcDistance2d(from, to) / velocity;
to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start;
}
}
}
} // namespace autoware::motion_utils
163 changes: 107 additions & 56 deletions common/autoware_motion_utils/test/src/resample/test_resample.cpp

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include "utility_functions.hpp"

#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/route_handler/route_handler.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/math/normalization.hpp>
#include <autoware/universe_utils/math/unit_conversion.hpp>
Expand All @@ -27,10 +28,13 @@
#include <lanelet2_extension/utility/utilities.hpp>
#include <lanelet2_extension/visualization/visualization.hpp>

#include <boost/geometry/algorithms/difference.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>

#include <lanelet2_core/Forward.h>
#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/BoundingBox.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_routing/Route.h>
#include <lanelet2_routing/RoutingCost.h>
#include <tf2/utils.h>

#include <limits>
Expand Down Expand Up @@ -216,7 +220,7 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
}

visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
autoware::universe_utils::LinearRing2d goal_footprint) const
autoware::universe_utils::LinearRing2d goal_footprint)
{
visualization_msgs::msg::MarkerArray msg;
auto marker = autoware::universe_utils::createDefaultMarker(
Expand Down Expand Up @@ -244,52 +248,58 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
return msg;
}

bool DefaultPlanner::check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet,
const lanelet::ConstLanelet & combined_prev_lanelet,
const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length,
const double search_margin)
lanelet::ConstLanelets next_lanelets_up_to(
const lanelet::ConstLanelet & start_lanelet, const double up_to_distance,
const route_handler::RouteHandler & route_handler)
{
// check if goal footprint is in current lane
if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) {
return true;
lanelet::ConstLanelets lanelets;
if (up_to_distance <= 0.0) {
return lanelets;
}
const auto following = route_handler_.getNextLanelets(current_lanelet);
// check if goal footprint is in between many lanelets in depth-first search manner
for (const auto & next_lane : following) {
next_lane_length += lanelet::utils::getLaneletLength2d(next_lane);
lanelet::ConstLanelets lanelets;
lanelets.push_back(combined_prev_lanelet);
for (const auto & next_lane : route_handler.getNextLanelets(start_lanelet)) {
lanelets.push_back(next_lane);
lanelet::ConstLanelet combined_lanelets =
combine_lanelets_with_shoulder(lanelets, route_handler_);

// if next lanelet length is longer than vehicle longitudinal offset
if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
// and if the goal_footprint is within the (accumulated) combined_lanelets, terminate the
// query
if (boost::geometry::within(goal_footprint, combined_lanelets.polygon2d().basicPolygon())) {
return true;
}
// if not, iteration continues to next next_lane, and this subtree is terminated
} else { // if next lanelet length is shorter than vehicle longitudinal offset, check the
// overlap with the polygon including the next_lane(s) until the additional lanes get
// longer than ego vehicle length
if (!check_goal_footprint_inside_lanes(
next_lane, combined_lanelets, goal_footprint, next_lane_length)) {
next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
continue;
} else {
return true;
}
const auto next_lanelets = next_lanelets_up_to(
next_lane, up_to_distance - lanelet::geometry::length2d(next_lane), route_handler);
lanelets.insert(lanelets.end(), next_lanelets.begin(), next_lanelets.end());
}
return lanelets;
}

bool DefaultPlanner::check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets,
const universe_utils::Polygon2d & goal_footprint) const
{
universe_utils::MultiPolygon2d ego_lanes;
universe_utils::Polygon2d poly;
for (const auto & ll : path_lanelets) {
const auto left_shoulder = route_handler_.getLeftShoulderLanelet(ll);
if (left_shoulder) {
boost::geometry::convert(left_shoulder->polygon2d().basicPolygon(), poly);
ego_lanes.push_back(poly);
}
const auto right_shoulder = route_handler_.getRightShoulderLanelet(ll);
if (right_shoulder) {
boost::geometry::convert(right_shoulder->polygon2d().basicPolygon(), poly);
ego_lanes.push_back(poly);
}
boost::geometry::convert(ll.polygon2d().basicPolygon(), poly);
ego_lanes.push_back(poly);
}
return false;
const auto next_lanelets =
next_lanelets_up_to(current_lanelet, vehicle_info_.max_longitudinal_offset_m, route_handler_);
for (const auto & ll : next_lanelets) {
boost::geometry::convert(ll.polygon2d().basicPolygon(), poly);
ego_lanes.push_back(poly);
}

// check if goal footprint is in the ego lane
universe_utils::MultiPolygon2d difference;
boost::geometry::difference(goal_footprint, ego_lanes, difference);
return boost::geometry::is_empty(difference);
}

bool DefaultPlanner::is_goal_valid(
const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets)
const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets)
{
const auto logger = node_->get_logger();

Expand Down Expand Up @@ -337,16 +347,10 @@ bool DefaultPlanner::is_goal_valid(
pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint));
const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint);

double next_lane_length = 0.0;
// combine calculated route lanelets
const lanelet::ConstLanelet combined_prev_lanelet =
combine_lanelets_with_shoulder(path_lanelets, route_handler_);

// check if goal footprint exceeds lane when the goal isn't in parking_lot
if (
param_.check_footprint_inside_lanes &&
!check_goal_footprint_inside_lanes(
closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
!check_goal_footprint_inside_lanes(closest_lanelet, path_lanelets, polygon_footprint) &&
!is_in_parking_lot(
lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()),
lanelet::utils::conversion::toLaneletPoint(goal.position))) {
Expand Down Expand Up @@ -375,11 +379,7 @@ bool DefaultPlanner::is_goal_valid(
// check if goal is in parking lot
const auto parking_lots =
lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr());
if (is_in_parking_lot(parking_lots, goal_lanelet_pt)) {
return true;
}

return false;
return is_in_parking_lot(parking_lots, goal_lanelet_pt);
}

PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
Expand Down Expand Up @@ -429,7 +429,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
return route_msg;
}

if (route_handler_.isRouteLooped(route_sections)) {
if (route_handler::RouteHandler::isRouteLooped(route_sections)) {
RCLCPP_WARN(logger, "Loop detected within route!");
return route_msg;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_traffic_rules/TrafficRulesFactory.h>

#include <memory>
#include <vector>

namespace autoware::mission_planner::lanelet2
Expand All @@ -44,15 +43,17 @@ struct DefaultPlannerParameters
class DefaultPlanner : public mission_planner::PlannerPlugin
{
public:
DefaultPlanner() : vehicle_info_(), is_graph_ready_(false), param_(), node_(nullptr) {}

void initialize(rclcpp::Node * node) override;
void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override;
bool ready() const override;
[[nodiscard]] bool ready() const override;
LaneletRoute plan(const RoutePoints & points) override;
void updateRoute(const PlannerPlugin::LaneletRoute & route) override;
void clearRoute() override;
MarkerArray visualize(const LaneletRoute & route) const override;
MarkerArray visualize_debug_footprint(
autoware::universe_utils::LinearRing2d goal_footprint_) const;
[[nodiscard]] MarkerArray visualize(const LaneletRoute & route) const override;
[[nodiscard]] static MarkerArray visualize_debug_footprint(
autoware::universe_utils::LinearRing2d goal_footprint);
autoware::vehicle_info_utils::VehicleInfo vehicle_info_;

private:
Expand Down Expand Up @@ -83,17 +84,16 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
* @param next_lane_length the accumulated total length from the start lanelet of the search to
* the lanelet of current goal query
*/
bool check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet,
const lanelet::ConstLanelet & combined_prev_lanelet,
const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length,
const double search_margin = 2.0);
[[nodiscard]] bool check_goal_footprint_inside_lanes(
const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets,
const universe_utils::Polygon2d & goal_footprint) const;

/**
* @brief return true if (1)the goal is in parking area or (2)the goal is on the lanes and the
* footprint around the goal does not overlap the lanes
*/
bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets);
bool is_goal_valid(
const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets);

/**
* @brief project the specified goal pose onto the goal lanelet(the last preferred lanelet of
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,6 @@

#include <lanelet2_core/geometry/Lanelet.h>

#include <unordered_set>
#include <utility>

autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon(
autoware::universe_utils::LinearRing2d footprint)
{
Expand All @@ -41,63 +38,6 @@ void insert_marker_array(
a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
}

lanelet::ConstLanelet combine_lanelets_with_shoulder(
const lanelet::ConstLanelets & lanelets,
const autoware::route_handler::RouteHandler & route_handler)
{
lanelet::Points3d lefts;
lanelet::Points3d rights;
lanelet::Points3d centers;
std::vector<uint64_t> left_bound_ids;
std::vector<uint64_t> right_bound_ids;

for (const auto & llt : lanelets) {
if (llt.id() != lanelet::InvalId) {
left_bound_ids.push_back(llt.leftBound().id());
right_bound_ids.push_back(llt.rightBound().id());
}
}

// lambda to add bound to target_bound
const auto add_bound = [](const auto & bound, auto & target_bound) {
std::transform(
bound.begin(), bound.end(), std::back_inserter(target_bound),
[](const auto & pt) { return lanelet::Point3d(pt); });
};
for (const auto & llt : lanelets) {
// check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist
const auto left_shared_shoulder = route_handler.getLeftShoulderLanelet(llt);
if (left_shared_shoulder) {
// if exist, add left bound of SHOULDER lanelet to lefts
add_bound(left_shared_shoulder->leftBound(), lefts);
} else if (
// if not exist, add left bound of lanelet to lefts
// if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`,
// then its left bound constitutes the left boundary of the entire merged lanelet
std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
add_bound(llt.leftBound(), lefts);
}

// check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist
const auto right_shared_shoulder = route_handler.getRightShoulderLanelet(llt);
if (right_shared_shoulder) {
// if exist, add right bound of SHOULDER lanelet to rights
add_bound(right_shared_shoulder->rightBound(), rights);
} else if (
// if not exist, add right bound of lanelet to rights
// if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`,
// then its right bound constitutes the right boundary of the entire merged lanelet
std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
add_bound(llt.rightBound(), rights);
}
}

const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts);
const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights);
auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
return std::move(combined_lanelet);
}

std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet)
{
std::vector<geometry_msgs::msg::Point> centerline_points;
Expand Down
Loading

0 comments on commit 44c5577

Please sign in to comment.