Skip to content

Commit

Permalink
chore(behavior_velocity): unite create detection area polygon functio…
Browse files Browse the repository at this point in the history 
…n as util

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
  • Loading branch information
@taikitanaka3
taikitanaka3 committed Mar 15, 2022
1 parent d23fecb commit 8b841ff
Show file tree
Hide file tree
Showing 5 changed files with 92 additions and 99 deletions.
12 changes: 1 addition & 11 deletions planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,23 +17,13 @@

#include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>

#include <boost/geometry.hpp>

#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/geometry/Point.h>

#include <vector>

namespace behavior_velocity_planner
{
namespace occlusion_spot_utils
{
//!< @brief build slices all along the trajectory
// using the given range and desired slice length and width
void createDetectionAreaPolygons(
Polygons2d & slices, const PathWithLaneId & path, const DetectionRange da_range,
const double obstacle_vel_mps);
//!< @brief build detection_area slice from path
//!< @brief wrapper for detection area polygon generation
void buildDetectionAreaPolygon(
Polygons2d & slices, const PathWithLaneId & path, const double offset,
const PlannerParam & param);
Expand Down
7 changes: 6 additions & 1 deletion planning/behavior_velocity_planner/include/utilization/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,9 +123,14 @@ inline geometry_msgs::msg::Pose getPose(
return traj.points.at(idx).pose;
}

// create detection area from given range
void createDetectionAreaPolygons(
Polygons2d & slices, const PathWithLaneId & path, const DetectionRange da_range,
const double obstacle_vel_mps);

Point2d calculateLateralOffsetPoint2d(const Pose & p, const double offset);

void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, Polygons2d & polys);
void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys);
void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input);
void insertVelocity(
PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v,
Expand Down
93 changes: 9 additions & 84 deletions planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <lanelet2_extension/utility/query.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <rclcpp/rclcpp.hpp>
#include <scene_module/occlusion_spot/geometry.hpp>
#include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
#include <scene_module/occlusion_spot/risk_predictive_braking.hpp>
#include <utilization/boost_geometry_helper.hpp>

#include <algorithm>
#include <vector>
Expand All @@ -27,79 +23,6 @@ namespace behavior_velocity_planner
{
namespace occlusion_spot_utils
{
void createDetectionAreaPolygons(
Polygons2d & da_polys, const PathWithLaneId & path, const DetectionRange da_range,
const double obstacle_vel_mps)
{
using planning_utils::calculateLateralOffsetPoint2d;
/**
* @brief relationships for interpolated polygon
*
* +(min_length,max_distance)-+ - +---+(max_length,max_distance) = outer_polygons
* | |
* +--------------------------+ - +---+(max_length,min_distance) = inner_polygons
*/
const double min_len = da_range.min_longitudinal_distance;
const double max_len = da_range.max_longitudinal_distance;
const double min_dst = da_range.min_lateral_distance;
const double max_dst = da_range.max_lateral_distance;
const double interval = da_range.interval;
//! max index is the last index of path point
const int max_index = static_cast<int>(path.points.size() - 1);
double dist_sum = -min_len;
double ttc = 0.0;
double length = dist_sum;
auto p0 = path.points.at(0).point;
// initial condition
LineString2d left_inner_bound;
LineString2d left_outer_bound;
LineString2d right_inner_bound;
LineString2d right_outer_bound;
for (int s = 0; s <= max_index; s++) {
const auto p1 = path.points.at(s).point;
const double ds = tier4_autoware_utils::calcDistance2d(p0, p1);
dist_sum += ds;
length += ds;
if (dist_sum <= 0) {
p0 = p1;
continue;
}
// calculate the distance that obstacles can move until ego reach the trajectory point
const double v_average = 0.5 * (p0.longitudinal_velocity_mps + p1.longitudinal_velocity_mps);
const double v = std::max(v_average, 0.3);
const double dt = ds / v;
ttc += dt;
//! avoid bug with same point polygon
const double eps = 1e-3;
// for offset calculation
const double max_lateral_distance = std::min(max_dst, min_dst + ttc * obstacle_vel_mps + eps);
// left bound
if (da_range.use_left) {
left_inner_bound.emplace_back(calculateLateralOffsetPoint2d(p0.pose, min_dst));
left_outer_bound.emplace_back(calculateLateralOffsetPoint2d(p0.pose, max_lateral_distance));
}
// right bound
if (da_range.use_right) {
right_inner_bound.emplace_back(calculateLateralOffsetPoint2d(p0.pose, -min_dst));
right_outer_bound.emplace_back(calculateLateralOffsetPoint2d(p0.pose, -max_lateral_distance));
}
// replace previous point with next point
p0 = p1;
// separate detection area polygon with fixed interval or at the end of detection max length
if (length > interval || max_len < dist_sum || s == max_index) {
if (left_inner_bound.size() > 1)
da_polys.emplace_back(lines2polygon(left_inner_bound, left_outer_bound));
if (right_inner_bound.size() > 1)
da_polys.emplace_back(lines2polygon(right_outer_bound, right_inner_bound));
left_inner_bound = {left_inner_bound.back()};
left_outer_bound = {left_outer_bound.back()};
right_inner_bound = {right_inner_bound.back()};
right_outer_bound = {right_outer_bound.back()};
length = 0;
continue;
}
}
}

PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0)
{
Expand All @@ -116,15 +39,17 @@ void buildDetectionAreaPolygon(
Polygons2d & slices, const PathWithLaneId & path, const double offset, const PlannerParam & param)
{
const auto & p = param;
[[maybe_unused]] const double min_len = offset + p.baselink_to_front;
[[maybe_unused]] const double max_len = 50;
// std::min(p.detection_area_max_length, p.detection_area_length);
[[maybe_unused]] const double min_dst = p.half_vehicle_width;
[[maybe_unused]] const double max_dst = p.detection_area.max_lateral_distance;
DetectionRange da_range = {p.detection_area.slice_length, min_len, max_len, min_dst, max_dst};
DetectionRange da_range;
da_range.interval = p.detection_area.slice_length;
da_range.min_longitudinal_distance = offset + p.baselink_to_front;
//! this value 50 is for temporary visualization
da_range.max_longitudinal_distance =
50.0; // std::min(p.detection_area_max_length, p.detection_area_length);
da_range.min_lateral_distance = p.half_vehicle_width;
da_range.max_lateral_distance = p.detection_area.max_lateral_distance;
PathWithLaneId applied_path = applyVelocityToPath(path, param.v.v_ego);
// in most case lateral distance is much more effective for velocity planning
createDetectionAreaPolygons(slices, path, da_range, p.pedestrian_vel);
planning_utils::createDetectionAreaPolygons(slices, path, da_range, p.pedestrian_vel);
return;
}

Expand Down
6 changes: 3 additions & 3 deletions ..._velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,9 +89,9 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
detection_area_polygons, interp_path, offset_from_start_to_ego, param_);
const auto filtered_obj = utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons);
// Note: Don't consider offset from path start to ego here
std::vector<PossibleCollisionInfo> possible_collisions; // =
// utils::generatePossibleCollisionBehindParkedVehicle(
// interp_path, param_, offset_from_start_to_ego, filtered_obj);
std::vector<PossibleCollisionInfo> possible_collisions =
utils::generatePossibleCollisionBehindParkedVehicle(
interp_path, param_, offset_from_start_to_ego, filtered_obj);
utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions);
// Note: Consider offset from path start to ego here
utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego);
Expand Down
73 changes: 73 additions & 0 deletions planning/behavior_velocity_planner/src/utilization/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,79 @@ Point2d calculateLateralOffsetPoint2d(const Pose & pose, const double offset)
return to_bg2d(calcOffsetPose(pose, 0.0, offset, 0.0));
}

void createDetectionAreaPolygons(
Polygons2d & da_polys, const PathWithLaneId & path, const DetectionRange da_range,
const double obstacle_vel_mps)
{
/**
* @brief relationships for interpolated polygon
*
* +(min_length,max_distance)-+ - +---+(max_length,max_distance) = outer_polygons
* | |
* +--------------------------+ - +---+(max_length,min_distance) = inner_polygons
*/
const double min_len = da_range.min_longitudinal_distance;
const double max_len = da_range.max_longitudinal_distance;
const double min_dst = da_range.min_lateral_distance;
const double max_dst = da_range.max_lateral_distance;
const double interval = da_range.interval;
//! max index is the last index of path point
const int max_index = static_cast<int>(path.points.size() - 1);
double dist_sum = -min_len;
double ttc = 0.0;
double length = dist_sum;
auto p0 = path.points.at(0).point;
// initial condition
LineString2d left_inner_bound;
LineString2d left_outer_bound;
LineString2d right_inner_bound;
LineString2d right_outer_bound;
for (int s = 0; s <= max_index; s++) {
const auto p1 = path.points.at(s).point;
const double ds = tier4_autoware_utils::calcDistance2d(p0, p1);
dist_sum += ds;
length += ds;
if (dist_sum <= 0) {
p0 = p1;
continue;
}
// calculate the distance that obstacles can move until ego reach the trajectory point
const double v_average = 0.5 * (p0.longitudinal_velocity_mps + p1.longitudinal_velocity_mps);
const double v = std::max(v_average, 0.3);
const double dt = ds / v;
ttc += dt;
//! avoid bug with same point polygon
const double eps = 1e-3;
// for offset calculation
const double max_lateral_distance = std::min(max_dst, min_dst + ttc * obstacle_vel_mps + eps);
// left bound
if (da_range.use_left) {
left_inner_bound.emplace_back(calculateLateralOffsetPoint2d(p0.pose, min_dst));
left_outer_bound.emplace_back(calculateLateralOffsetPoint2d(p0.pose, max_lateral_distance));
}
// right bound
if (da_range.use_right) {
right_inner_bound.emplace_back(calculateLateralOffsetPoint2d(p0.pose, -min_dst));
right_outer_bound.emplace_back(calculateLateralOffsetPoint2d(p0.pose, -max_lateral_distance));
}
// replace previous point with next point
p0 = p1;
// separate detection area polygon with fixed interval or at the end of detection max length
if (length > interval || max_len < dist_sum || s == max_index) {
if (left_inner_bound.size() > 1)
da_polys.emplace_back(lines2polygon(left_inner_bound, left_outer_bound));
if (right_inner_bound.size() > 1)
da_polys.emplace_back(lines2polygon(right_outer_bound, right_inner_bound));
left_inner_bound = {left_inner_bound.back()};
left_outer_bound = {left_outer_bound.back()};
right_inner_bound = {right_inner_bound.back()};
right_outer_bound = {right_outer_bound.back()};
length = 0;
if (max_len < dist_sum || s == max_index) return;
}
}
}

void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys)
{
const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll);
Expand Down

0 comments on commit 8b841ff

Please sign in to comment.