Skip to content

Commit

Permalink
fixup! tmp
Browse files Browse the repository at this point in the history
  • Loading branch information
@satoshi-ota
satoshi-ota committed Jul 7, 2024
1 parent 192195a commit 2290c05
Show file tree
Hide file tree
Showing 4 changed files with 4 additions and 140 deletions.
7 changes: 2 additions & 5 deletions ...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,11 +113,8 @@ void AvoidanceByLaneChange::updateSpecialData()
: Direction::RIGHT;
}

utils::static_obstacle_avoidance::updateRegisteredObject(
registered_objects_, avoidance_data_.target_objects, p);
utils::static_obstacle_avoidance::compensateDetectionLost(
registered_objects_, avoidance_data_, planner_data_);
// registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects);
utils::static_obstacle_avoidance::compensateLostTargetObjects(
registered_objects_, avoidance_data_, clock_.now(), planner_data_, p);

std::sort(
avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(),
Expand Down
12 changes: 0 additions & 12 deletions ...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,20 +118,8 @@ void fillObjectStoppableJudge(
ObjectData & object_data, const ObjectDataArray & registered_objects,
const double feasible_stop_distance, const std::shared_ptr<AvoidanceParameters> & parameters);

void updateRegisteredObject(
ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
const std::shared_ptr<AvoidanceParameters> & parameters);

void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data);

// void compensateDetectionLost(
// const ObjectDataArray & registered_objects, ObjectDataArray & now_objects,
// ObjectDataArray & other_objects);

void compensateDetectionLost(
ObjectDataArray & stored_objects, AvoidancePlanningData & data,
const std::shared_ptr<const PlannerData> & planner_data);

void compensateLostTargetObjects(
ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now,
const std::shared_ptr<const PlannerData> & planner_data,
Expand Down
8 changes: 2 additions & 6 deletions ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -293,12 +293,8 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
utils::static_obstacle_avoidance::compensateLostTargetObjects(
registered_objects_, data, clock_->now(), planner_data_, parameters_);

// // lost object compensation
// utils::static_obstacle_avoidance::updateRegisteredObject(
// registered_objects_, data.target_objects, parameters_);
// utils::static_obstacle_avoidance::compensateDetectionLost(
// registered_objects_, data, planner_data_);
// registered_objects_, data.target_objects, data.other_objects);
// once an object filtered for boundary clipping, this module keeps the information until the end
// of execution.
utils::static_obstacle_avoidance::updateClipObject(clip_objects_, data);

fillAvoidanceTargetNecessaryData(data.target_objects);
Expand Down
117 changes: 0 additions & 117 deletions ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1645,123 +1645,6 @@ void fillObjectStoppableJudge(
object_data.is_stoppable = same_id_obj->is_stoppable;
}

void updateRegisteredObject(
ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
const auto updateIfDetectedNow = [&now_objects](auto & registered_object) {
const auto & n = now_objects;
const auto r_id = registered_object.object.object_id;
const auto same_id_obj = std::find_if(
n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });

// same id object is detected. update registered.
if (same_id_obj != n.end()) {
registered_object = *same_id_obj;
return true;
}

constexpr auto POS_THR = 1.5;
const auto r_pos = registered_object.getPose();
const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) {
return calcDistance2d(r_pos, o.getPose()) < POS_THR;
});

// same id object is not detected, but object is found around registered. update registered.
if (similar_pos_obj != n.end()) {
registered_object = *similar_pos_obj;
return true;
}

// Same ID nor similar position object does not found.
return false;
};

const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now();

// -- check registered_objects, remove if lost_count exceeds limit. --
for (int i = static_cast<int>(registered_objects.size()) - 1; i >= 0; --i) {
auto & r = registered_objects.at(i);

// registered object is not detected this time. lost count up.
if (!updateIfDetectedNow(r)) {
r.lost_time = (now - r.last_seen).seconds();
} else {
r.last_seen = now;
r.lost_time = 0.0;
}

// lost count exceeds threshold. remove object from register.
if (r.lost_time > parameters->object_last_seen_threshold) {
registered_objects.erase(registered_objects.begin() + i);
}
}

const auto isAlreadyRegistered = [&](const auto & n_id) {
const auto & r = registered_objects;
return std::any_of(
r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; });
};

// -- check now_objects, add it if it has new object id --
for (const auto & now_obj : now_objects) {
if (!isAlreadyRegistered(now_obj.object.object_id)) {
registered_objects.push_back(now_obj);
}
}
}

// void compensateDetectionLost(
// const ObjectDataArray & registered_objects, ObjectDataArray & now_objects,
// ObjectDataArray & other_objects)
// {
// const auto isDetectedNow = [&](const auto & r_id) {
// const auto & n = now_objects;
// return std::any_of(
// n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
// };

// const auto isIgnoreObject = [&](const auto & r_id) {
// const auto & n = other_objects;
// return std::any_of(
// n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
// };

// for (const auto & registered : registered_objects) {
// if (
// !isDetectedNow(registered.object.object_id) &&
// !isIgnoreObject(registered.object.object_id)) { now_objects.push_back(registered);
// }
// }
// }

void compensateDetectionLost(
ObjectDataArray & registered_objects, AvoidancePlanningData & data,
const std::shared_ptr<const PlannerData> & planner_data)
{
const auto isDetectedNow = [&](const auto & r_id) {
const auto & n = data.target_objects;
return std::any_of(
n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
};

const auto isIgnoreObject = [&](const auto & r_id) {
const auto & n = data.other_objects;
return std::any_of(
n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
};

for (auto & registered : registered_objects) {
if (
!isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) {
const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
fillLongitudinalAndLengthByClosestEnvelopeFootprint(
data.reference_path_rough, ego_pos, registered);
data.target_objects.push_back(registered);
}
}
}

void compensateLostTargetObjects(
ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now,
const std::shared_ptr<const PlannerData> & planner_data,
Expand Down

0 comments on commit 2290c05

Please sign in to comment.