Skip to content

Commit

Permalink
feat(dynamic_avoidance): apply ego's lateral acc/jerk constraints to …
Browse files Browse the repository at this point in the history 
…object polygon (#5953)

* feat(dynamic_avoidance): apply ego's lateral acc/jerk constraints to object polygon

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 committed Jan 5, 2024
1 parent ed21acf commit 5de111f
Show file tree
Hide file tree
Showing 6 changed files with 249 additions and 64 deletions.
4 changes: 4 additions & 0 deletions planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,10 @@
max_time_for_object_lat_shift: 0.0 # [s]
lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]

max_ego_lat_acc: 0.3 # [m/ss]
max_ego_lat_jerk: 0.3 # [m/sss]
delay_time_ego_shift: 1.0 # [s]

# for same directional object
overtaking_object:
max_time_to_collision: 40.0 # [s]
Expand Down
103 changes: 69 additions & 34 deletions ...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,25 @@
#include <utility>
#include <vector>

namespace
{
template <typename T>
bool isInVector(const T & val, const std::vector<T> & vec)
{
return std::find(vec.begin(), vec.end(), val) != vec.end();
}

template <typename T, typename S>
std::vector<T> getAllKeys(const std::unordered_map<T, S> & map)
{
std::vector<T> keys;
for (const auto & pair : map) {
keys.push_back(pair.first);
}
return keys;
}
} // namespace

namespace behavior_path_planner
{
using autoware_auto_perception_msgs::msg::PredictedPath;
Expand Down Expand Up @@ -97,6 +116,10 @@ struct DynamicAvoidanceParameters
double max_time_for_lat_shift{0.0};
double lpf_gain_for_lat_avoid_to_offset{0.0};

double max_ego_lat_acc{0.0};
double max_ego_lat_jerk{0.0};
double delay_time_ego_shift{0.0};

double max_time_to_collision_overtaking_object{0.0};
double start_duration_to_avoid_overtaking_object{0.0};
double end_duration_to_avoid_overtaking_object{0.0};
Expand All @@ -113,6 +136,11 @@ struct TimeWhileCollision
double time_to_end_collision;
};

struct LatFeasiblePaths
{
std::vector<geometry_msgs::msg::Point> left_path;
std::vector<geometry_msgs::msg::Point> right_path;
};
class DynamicAvoidanceModule : public SceneModuleInterface
{
public:
Expand Down Expand Up @@ -151,6 +179,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
bool is_collision_left{false};
bool should_be_avoided{false};
std::vector<PathPointWithLaneId> ref_path_points_for_obj_poly;
LatFeasiblePaths ego_lat_feasible_paths;

void update(
const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid,
Expand All @@ -172,14 +201,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface
{
}
int max_count_{0};
int min_count_{0};
int min_count_{0}; // TODO(murooka): The sign needs to be opposite?

void initialize() { current_uuids_.clear(); }
void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object)
{
// add/update object
if (object_map_.count(uuid) != 0) {
const auto prev_object = object_map_.at(uuid);
object_map_.at(uuid) = object;
// TODO(murooka) refactor this. Counter can be moved to DynamicObject,
// and TargetObjectsManager can be removed.
object_map_.at(uuid).ego_lat_feasible_paths = prev_object.ego_lat_feasible_paths;
} else {
object_map_.emplace(uuid, object);
}
Expand All @@ -195,14 +228,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface
current_uuids_.push_back(uuid);
}

void finalize()
void finalize(const LatFeasiblePaths & ego_lat_feasible_paths)
{
// decrease counter for not updated uuids
std::vector<std::string> not_updated_uuids;
for (const auto & object : object_map_) {
if (
std::find(current_uuids_.begin(), current_uuids_.end(), object.first) ==
current_uuids_.end()) {
if (!isInVector(object.first, current_uuids_)) {
not_updated_uuids.push_back(object.first);
}
}
Expand All @@ -214,45 +245,46 @@ class DynamicAvoidanceModule : public SceneModuleInterface
}
}

// remove objects whose counter is lower than threshold
std::vector<std::string> obsolete_uuids;
// update valid object uuids and its variable
for (const auto & counter : counter_map_) {
if (counter.second < min_count_) {
obsolete_uuids.push_back(counter.first);
if (!isInVector(counter.first, valid_object_uuids_) && max_count_ <= counter.second) {
valid_object_uuids_.push_back(counter.first);
object_map_.at(counter.first).ego_lat_feasible_paths = ego_lat_feasible_paths;
}
}
for (const auto & obsolete_uuid : obsolete_uuids) {
counter_map_.erase(obsolete_uuid);
object_map_.erase(obsolete_uuid);
valid_object_uuids_.erase(
std::remove_if(
valid_object_uuids_.begin(), valid_object_uuids_.end(),
[&](const auto & uuid) {
return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < max_count_;
}),
valid_object_uuids_.end());

// remove objects whose counter is lower than threshold
const auto counter_map_keys = getAllKeys(counter_map_);
for (const auto & key : counter_map_keys) {
if (counter_map_.at(key) < min_count_) {
counter_map_.erase(key);
object_map_.erase(key);
}
}
}
std::vector<DynamicAvoidanceObject> getValidObjects() const
{
std::vector<DynamicAvoidanceObject> objects;
for (const auto & object : object_map_) {
if (counter_map_.count(object.first) != 0) {
if (max_count_ <= counter_map_.at(object.first)) {
objects.push_back(object.second);
}
std::vector<DynamicAvoidanceObject> valid_objects;
for (const auto & valid_object_uuid : valid_object_uuids_) {
if (object_map_.count(valid_object_uuid) == 0) {
std::cerr
<< "[DynamicAvoidance] Internal calculation has an error when getting valid objects."
<< std::endl;
continue;
}
valid_objects.push_back(object_map_.at(valid_object_uuid));
}
return objects;
}
std::optional<DynamicAvoidanceObject> getValidObject(const std::string & uuid) const
{
// add/update object
if (counter_map_.count(uuid) == 0) {
return std::nullopt;
}
if (counter_map_.at(uuid) < max_count_) {
return std::nullopt;
}
if (object_map_.count(uuid) == 0) {
return std::nullopt;
}
return object_map_.at(uuid);

return valid_objects;
}
void updateObject(
void updateObjectVariables(
const std::string & uuid, const MinMaxValue & lon_offset_to_avoid,
const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left,
const bool should_be_avoided,
Expand All @@ -269,6 +301,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
// NOTE: positive is for meeting entry condition, and negative is for exiting.
std::unordered_map<std::string, int> counter_map_;
std::unordered_map<std::string, DynamicAvoidanceObject> object_map_;
std::vector<std::string> valid_object_uuids_{};
};

struct DecisionWithReason
Expand Down Expand Up @@ -319,6 +352,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface

bool isLabelTargetObstacle(const uint8_t label) const;
void updateTargetObjects();
LatFeasiblePaths generateLateralFeasiblePaths(
const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const;
void updateRefPathBeforeLaneChange(const std::vector<PathPointWithLaneId> & ego_ref_path_points);
bool willObjectCutIn(
const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & predicted_path,
Expand Down
Loading

0 comments on commit 5de111f

Please sign in to comment.