@@ -1645,93 +1645,94 @@ void fillObjectStoppableJudge(
16451645 object_data.is_stoppable = same_id_obj->is_stoppable ;
16461646}
16471647
1648- void updateRegisteredObject (
1649- ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
1648+ void compensateLostTargetObjects (
1649+ ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now,
1650+ const std::shared_ptr<const PlannerData> & planner_data,
16501651 const std::shared_ptr<AvoidanceParameters> & parameters)
16511652{
1652- const auto updateIfDetectedNow = [&now_objects](auto & registered_object) {
1653- const auto & n = now_objects;
1654- const auto r_id = registered_object.object .object_id ;
1655- const auto same_id_obj = std::find_if (
1656- n.begin (), n.end (), [&r_id](const auto & o) { return o.object .object_id == r_id; });
1653+ const auto include = [](const auto & objects, const auto & search_id) {
1654+ return std::all_of (objects.begin (), objects.end (), [&search_id](const auto & o) {
1655+ return o.object .object_id != search_id;
1656+ });
1657+ };
1658+
1659+ // STEP.1 UPDATE STORED OBJECTS.
1660+ const auto match = [&data](auto & object) {
1661+ const auto & search_id = object.object .object_id ;
1662+ const auto same_id_object = std::find_if (
1663+ data.target_objects .begin (), data.target_objects .end (),
1664+ [&search_id](const auto & o) { return o.object .object_id == search_id; });
16571665
16581666 // same id object is detected. update registered.
1659- if (same_id_obj != n .end ()) {
1660- registered_object = *same_id_obj ;
1667+ if (same_id_object != data. target_objects .end ()) {
1668+ object = *same_id_object ;
16611669 return true ;
16621670 }
16631671
1664- constexpr auto POS_THR = 1.5 ;
1665- const auto r_pos = registered_object. getPose ();
1666- const auto similar_pos_obj = std::find_if (n. begin (), n. end (), [&]( const auto & o) {
1667- return calcDistance2d (r_pos , o.getPose ()) < POS_THR;
1668- });
1672+ const auto similar_pos_obj = std::find_if (
1673+ data. target_objects . begin (), data. target_objects . end (), [&object]( const auto & o) {
1674+ constexpr auto POS_THR = 1.5 ;
1675+ return calcDistance2d (object. getPose () , o.getPose ()) < POS_THR;
1676+ });
16691677
16701678 // same id object is not detected, but object is found around registered. update registered.
1671- if (similar_pos_obj != n .end ()) {
1672- registered_object = *similar_pos_obj;
1679+ if (similar_pos_obj != data. target_objects .end ()) {
1680+ object = *similar_pos_obj;
16731681 return true ;
16741682 }
16751683
16761684 // Same ID nor similar position object does not found.
16771685 return false ;
16781686 };
16791687
1680- const rclcpp::Time now = rclcpp::Clock (RCL_ROS_TIME).now ();
1681-
1682- // -- check registered_objects, remove if lost_count exceeds limit. --
1683- for (int i = static_cast <int >(registered_objects.size ()) - 1 ; i >= 0 ; --i) {
1684- auto & r = registered_objects.at (i);
1685-
1686- // registered object is not detected this time. lost count up.
1687- if (!updateIfDetectedNow (r)) {
1688- r.lost_time = (now - r.last_seen ).seconds ();
1689- } else {
1690- r.last_seen = now;
1691- r.lost_time = 0.0 ;
1692- }
1688+ // STEP1-1: REMOVE EXPIRED OBJECTS.
1689+ const auto itr = std::remove_if (
1690+ stored_objects.begin (), stored_objects.end (), [&now, &match, ¶meters](auto & o) {
1691+ if (!match (o)) {
1692+ o.lost_time = (now - o.last_seen ).seconds ();
1693+ } else {
1694+ o.last_seen = now;
1695+ o.lost_time = 0.0 ;
1696+ }
16931697
1694- // lost count exceeds threshold. remove object from register.
1695- if (r.lost_time > parameters->object_last_seen_threshold ) {
1696- registered_objects.erase (registered_objects.begin () + i);
1697- }
1698- }
1698+ return o.lost_time > parameters->object_last_seen_threshold ;
1699+ });
16991700
1700- const auto isAlreadyRegistered = [&](const auto & n_id) {
1701- const auto & r = registered_objects;
1702- return std::any_of (
1703- r.begin (), r.end (), [&n_id](const auto & o) { return o.object .object_id == n_id; });
1704- };
1701+ stored_objects.erase (itr, stored_objects.end ());
17051702
1706- // -- check now_objects, add it if it has new object id --
1707- for (const auto & now_obj : now_objects ) {
1708- if (!isAlreadyRegistered (now_obj .object .object_id )) {
1709- registered_objects .push_back (now_obj );
1703+ // STEP1-2: UPDATE STORED OBJECTS IF THERE ARE NEW OBJECTS.
1704+ for (const auto & current_object : data. target_objects ) {
1705+ if (!include (stored_objects, current_object .object .object_id )) {
1706+ stored_objects .push_back (current_object );
17101707 }
17111708 }
1712- }
17131709
1714- void compensateDetectionLost (
1715- const ObjectDataArray & registered_objects, ObjectDataArray & now_objects,
1716- ObjectDataArray & other_objects)
1717- {
1718- const auto isDetectedNow = [&](const auto & r_id) {
1719- const auto & n = now_objects;
1710+ // STEP2: COMPENSATE CURRENT TARGET OBJECTS
1711+ const auto is_detected = [&](const auto & object_id) {
17201712 return std::any_of (
1721- n.begin (), n.end (), [&r_id](const auto & o) { return o.object .object_id == r_id; });
1713+ data.target_objects .begin (), data.target_objects .end (),
1714+ [&object_id](const auto & o) { return o.object .object_id == object_id; });
17221715 };
17231716
1724- const auto isIgnoreObject = [&](const auto & r_id) {
1725- const auto & n = other_objects;
1717+ const auto is_ignored = [&](const auto & object_id) {
17261718 return std::any_of (
1727- n.begin (), n.end (), [&r_id](const auto & o) { return o.object .object_id == r_id; });
1719+ data.other_objects .begin (), data.other_objects .end (),
1720+ [&object_id](const auto & o) { return o.object .object_id == object_id; });
17281721 };
17291722
1730- for (const auto & registered : registered_objects) {
1731- if (
1732- !isDetectedNow (registered.object .object_id ) && !isIgnoreObject (registered.object .object_id )) {
1733- now_objects.push_back (registered);
1723+ for (auto & stored_object : stored_objects) {
1724+ if (is_detected (stored_object.object .object_id )) {
1725+ continue ;
1726+ }
1727+ if (is_ignored (stored_object.object .object_id )) {
1728+ continue ;
17341729 }
1730+
1731+ const auto & ego_pos = planner_data->self_odometry ->pose .pose .position ;
1732+ fillLongitudinalAndLengthByClosestEnvelopeFootprint (
1733+ data.reference_path_rough , ego_pos, stored_object);
1734+
1735+ data.target_objects .push_back (stored_object);
17351736 }
17361737}
17371738
0 commit comments