Skip to content

Commit

Permalink
feat(lane_change): use external velocity limit in safety check (autow…
Browse files Browse the repository at this point in the history
…arefoundation#6760)

* feat(lane_change): use external velocity limit in safety check

Signed-off-by: Muhammad Zulfaqar <[email protected]>

* style(pre-commit): autofix

* Minor refactoring

Signed-off-by: Muhammad Zulfaqar <[email protected]>

* Fix spell check and remove headers

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* Add warning

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

---------

Signed-off-by: Muhammad Zulfaqar <[email protected]>
Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
zulfaqar-azmi-t4 and pre-commit-ci[bot] committed Apr 10, 2024
1 parent 712ab11 commit 071333e
Show file tree
Hide file tree
Showing 7 changed files with 48 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -170,6 +170,8 @@ class NormalLaneChange : public LaneChangeBase
bool isVehicleStuck(
const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const;

double get_max_velocity_for_safety_check() const;

bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const;

bool check_prepare_phase() const;
Expand Down
13 changes: 12 additions & 1 deletion planning/behavior_path_lane_change_module/src/scene.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1976,7 +1976,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
for (const auto & obj_path : obj_predicted_paths) {
const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons(
path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
current_debug_data.second);
get_max_velocity_for_safety_check(), current_debug_data.second);

if (collided_polygons.empty()) {
utils::path_safety_checker::updateCollisionCheckDebugMap(
Expand Down Expand Up @@ -2071,6 +2071,17 @@ bool NormalLaneChange::isVehicleStuck(
return false;
}

double NormalLaneChange::get_max_velocity_for_safety_check() const
{
const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity;
if (external_velocity_limit_ptr) {
return std::min(
static_cast<double>(external_velocity_limit_ptr->max_velocity), getCommonParam().max_vel);
}

return getCommonParam().max_vel;
}

bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const
{
if (current_lanes.empty()) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include <tier4_planning_msgs/msg/reroute_availability.hpp>
#include <tier4_planning_msgs/msg/scenario.hpp>
#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
#include <visualization_msgs/msg/marker.hpp>

#include <map>
Expand Down Expand Up @@ -101,6 +102,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_publisher_;
rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reason_publisher_;
rclcpp::Publisher<RerouteAvailability>::SharedPtr reroute_availability_publisher_;
rclcpp::Subscription<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr
external_limit_max_velocity_subscriber_;
rclcpp::TimerBase::SharedPtr timer_;

std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;
Expand Down Expand Up @@ -140,6 +143,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
void onRoute(const LaneletRoute::ConstSharedPtr route_msg);
void onOperationMode(const OperationModeState::ConstSharedPtr msg);
void onLateralOffset(const LateralOffset::ConstSharedPtr msg);
void on_external_velocity_limiter(
const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);

SetParametersResult onSetParam(const std::vector<rclcpp::Parameter> & parameters);

OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
Expand Down
18 changes: 18 additions & 0 deletions planning/behavior_path_planner/src/behavior_path_planner_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
current_scenario_ = std::make_shared<Scenario>(*msg);
},
createSubscriptionOptions(this));
external_limit_max_velocity_subscriber_ =
create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
"/planning/scenario_planning/max_velocity", 1,
std::bind(&BehaviorPathPlannerNode::on_external_velocity_limiter, this, _1),
createSubscriptionOptions(this));

// route_handler
vector_map_subscriber_ = create_subscription<HADMapBin>(
Expand Down Expand Up @@ -815,6 +820,19 @@ void BehaviorPathPlannerNode::onOperationMode(const OperationModeState::ConstSha
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->operation_mode = msg;
}

void BehaviorPathPlannerNode::on_external_velocity_limiter(
const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg)
{
// Note: Using this parameter during path planning might cause unexpected deceleration or jerk.
// Therefore, do not use it for anything other than safety checks.
if (!msg) {
return;
}

const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->external_limit_max_velocity = msg;
}
void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tier4_planning_msgs/msg/detail/velocity_limit__struct.hpp>
#include <tier4_planning_msgs/msg/lateral_offset.hpp>

#include <limits>
Expand Down Expand Up @@ -64,6 +65,7 @@ using route_handler::RouteHandler;
using tier4_planning_msgs::msg::LateralOffset;
using PlanResult = PathWithLaneId::SharedPtr;
using lanelet::TrafficLight;
using tier4_planning_msgs::msg::VelocityLimit;
using unique_identifier_msgs::msg::UUID;

struct TrafficSignalStamped
Expand Down Expand Up @@ -160,6 +162,7 @@ struct PlannerData
std::map<int64_t, TrafficSignalStamped> traffic_light_id_map;
BehaviorPathPlannerParameters parameters{};
drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{};
VelocityLimit::ConstSharedPtr external_limit_max_velocity{};

mutable std::vector<geometry_msgs::msg::Pose> drivable_area_expansion_prev_path_poses{};
mutable std::vector<double> drivable_area_expansion_prev_curvatures{};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,7 @@ std::vector<Polygon2d> getCollidedPolygons(
const ExtendedPredictedObject & target_object,
const PredictedPathWithPolygon & target_object_path,
const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
const double hysteresis_factor, CollisionCheckDebug & debug);
const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug);

bool checkPolygonsIntersects(
const std::vector<Polygon2d> & polys_1, const std::vector<Polygon2d> & polys_2);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@
#include <boost/geometry/algorithms/union.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#include <limits>

namespace behavior_path_planner::utils::path_safety_checker
{

Expand Down Expand Up @@ -478,7 +480,7 @@ bool checkCollision(
{
const auto collided_polygons = getCollidedPolygons(
planned_path, predicted_ego_path, target_object, target_object_path, common_parameters,
rss_parameters, hysteresis_factor, debug);
rss_parameters, hysteresis_factor, std::numeric_limits<double>::max(), debug);
return collided_polygons.empty();
}

Expand All @@ -488,7 +490,7 @@ std::vector<Polygon2d> getCollidedPolygons(
const ExtendedPredictedObject & target_object,
const PredictedPathWithPolygon & target_object_path,
const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
double hysteresis_factor, CollisionCheckDebug & debug)
double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug)
{
{
debug.ego_predicted_path = predicted_ego_path;
Expand All @@ -504,7 +506,7 @@ std::vector<Polygon2d> getCollidedPolygons(
// get object information at current time
const auto & obj_pose = obj_pose_with_poly.pose;
const auto & obj_polygon = obj_pose_with_poly.poly;
const auto & object_velocity = obj_pose_with_poly.velocity;
const auto object_velocity = obj_pose_with_poly.velocity;

// get ego information at current time
// Note: we can create these polygons in advance. However, it can decrease the readability and
Expand All @@ -517,7 +519,7 @@ std::vector<Polygon2d> getCollidedPolygons(
}
const auto & ego_pose = interpolated_data->pose;
const auto & ego_polygon = interpolated_data->poly;
const auto & ego_velocity = interpolated_data->velocity;
const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit);

// check overlap
if (boost::geometry::overlaps(ego_polygon, obj_polygon)) {
Expand Down

0 comments on commit 071333e

Please sign in to comment.