From e9594c6043b64e4f8b1b86bf4fb5018a129246c6 Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Tue, 30 Apr 2024 13:51:59 +0900
Subject: [PATCH 01/10] make remove objects from history templated
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../map_based_prediction_node.hpp | 3 +-
.../src/map_based_prediction_node.cpp | 93 ++++++++++---------
2 files changed, 51 insertions(+), 45 deletions(-)
diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index 829c4d6e4a114..f240106caa8b6 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -222,8 +222,7 @@ class MapBasedPredictionNode : public rclcpp::Node
PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object);
- void removeOldObjectsHistory(
- const double current_time, const TrackedObjects::ConstSharedPtr in_objects);
+ void cleanupOldStoppedOnGreenTimes(const TrackedObjects::ConstSharedPtr in_objects);
LaneletsData getCurrentLanelets(const TrackedObject & object);
bool checkCloseLaneletCondition(
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 56dc1c2293583..78c48cb49735b 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -585,6 +585,53 @@ bool hasPotentialToReach(
return false;
}
+template <typename T>
+std::unordered_set<std::string> removeOldObjectsHistory(
+ const double current_time, const double buffer_time, std::unordered_map<std::string, std::deque<T>> & target_objects)
+{
+ std::unordered_set<std::string> invalid_object_id;
+ for (auto iter = target_objects.begin(); iter != target_objects.end(); ++iter) {
+ const std::string object_id = iter->first;
+ std::deque<T> & object_data = iter->second;
+
+ // If object data is empty, we are going to delete the buffer for the obstacle
+ if (object_data.empty()) {
+ invalid_object_id.insert(object_id);
+ continue;
+ }
+
+ const double latest_object_time = rclcpp::Time(object_data.back().header.stamp).seconds();
+
+ // Delete Old Objects
+ if (current_time - latest_object_time > buffer_time) {
+ invalid_object_id.insert(object_id);
+ continue;
+ }
+
+ // Delete old information
+ while (!object_data.empty()) {
+ const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds();
+ if (current_time - post_object_time > buffer_time) {
+ // Delete Old Position
+ object_data.pop_front();
+ } else {
+ break;
+ }
+ }
+
+ if (object_data.empty()) {
+ invalid_object_id.insert(object_id);
+ continue;
+ }
+ }
+
+ for (const auto & key : invalid_object_id) {
+ target_objects.erase(key);
+ }
+
+ return invalid_object_id;
+}
+
/**
* @brief change label for prediction
*
@@ -916,7 +963,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
// Remove old objects information in object history
const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds();
- removeOldObjectsHistory(objects_detected_time, in_objects);
+ removeOldObjectsHistory(objects_detected_time, object_buffer_time_length_, objects_history_);
+ cleanupOldStoppedOnGreenTimes(in_objects);
// result output
PredictedObjects output;
@@ -1343,49 +1391,8 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object)
return;
}
-void MapBasedPredictionNode::removeOldObjectsHistory(
- const double current_time, const TrackedObjects::ConstSharedPtr in_objects)
+void MapBasedPredictionNode::cleanupOldStoppedOnGreenTimes(const TrackedObjects::ConstSharedPtr in_objects)
{
- std::vector<std::string> invalid_object_id;
- for (auto iter = objects_history_.begin(); iter != objects_history_.end(); ++iter) {
- const std::string object_id = iter->first;
- std::deque<ObjectData> & object_data = iter->second;
-
- // If object data is empty, we are going to delete the buffer for the obstacle
- if (object_data.empty()) {
- invalid_object_id.push_back(object_id);
- continue;
- }
-
- const double latest_object_time = rclcpp::Time(object_data.back().header.stamp).seconds();
-
- // Delete Old Objects
- if (current_time - latest_object_time > object_buffer_time_length_) {
- invalid_object_id.push_back(object_id);
- continue;
- }
-
- // Delete old information
- while (!object_data.empty()) {
- const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds();
- if (current_time - post_object_time > object_buffer_time_length_) {
- // Delete Old Position
- object_data.pop_front();
- } else {
- break;
- }
- }
-
- if (object_data.empty()) {
- invalid_object_id.push_back(object_id);
- continue;
- }
- }
-
- for (const auto & key : invalid_object_id) {
- objects_history_.erase(key);
- }
-
for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) {
const bool isDisappeared = std::none_of(
in_objects->objects.begin(), in_objects->objects.end(),
From 39914e97ed24d928e831e91bb4435ab7454eac87 Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Tue, 30 Apr 2024 13:52:49 +0900
Subject: [PATCH 02/10] pre-commit
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../map_based_prediction/src/map_based_prediction_node.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 78c48cb49735b..04c64eb3edc1f 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -587,7 +587,8 @@ bool hasPotentialToReach(
template <typename T>
std::unordered_set<std::string> removeOldObjectsHistory(
- const double current_time, const double buffer_time, std::unordered_map<std::string, std::deque<T>> & target_objects)
+ const double current_time, const double buffer_time,
+ std::unordered_map<std::string, std::deque<T>> & target_objects)
{
std::unordered_set<std::string> invalid_object_id;
for (auto iter = target_objects.begin(); iter != target_objects.end(); ++iter) {
@@ -1391,7 +1392,8 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object)
return;
}
-void MapBasedPredictionNode::cleanupOldStoppedOnGreenTimes(const TrackedObjects::ConstSharedPtr in_objects)
+void MapBasedPredictionNode::cleanupOldStoppedOnGreenTimes(
+ const TrackedObjects::ConstSharedPtr in_objects)
{
for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) {
const bool isDisappeared = std::none_of(
From 05e5c804d4a8260cc815973d379a26f1e57e8b71 Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Tue, 30 Apr 2024 14:00:13 +0900
Subject: [PATCH 03/10] create crosswalk user history
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../map_based_prediction_node.hpp | 10 ++++++++++
.../src/map_based_prediction_node.cpp | 16 ++++++++++++++++
2 files changed, 26 insertions(+)
diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index f240106caa8b6..e0a9ee51aa30d 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -85,6 +85,12 @@ struct ObjectData
Maneuver::UNINITIALIZED}; // output maneuver considering previous one shot maneuvers
};
+struct CrosswalkUserData
+{
+ std_msgs::msg::Header header;
+ autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
+};
+
struct LaneletData
{
lanelet::Lanelet lanelet;
@@ -137,6 +143,7 @@ class MapBasedPredictionNode : public rclcpp::Node
// Object History
std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
std::map<std::pair<std::string, lanelet::Id>, rclcpp::Time> stopped_times_against_green_;
+ std::unordered_map<std::string, std::deque<CrosswalkUserData>> crosswalk_users_history_;
// Lanelet Map Pointers
std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
@@ -234,6 +241,9 @@ class MapBasedPredictionNode : public rclcpp::Node
void updateObjectsHistory(
const std_msgs::msg::Header & header, const TrackedObject & object,
const LaneletsData & current_lanelets_data);
+ void updateCrosswalkUserHistory(
+ const std_msgs::msg::Header & header, const TrackedObject & object,
+ const std::string & object_id);
std::vector<PredictedRefPath> getPredictedReferencePath(
const TrackedObject & object, const LaneletsData & current_lanelets_data,
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 04c64eb3edc1f..fdea487eb1e3f 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -967,6 +967,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
removeOldObjectsHistory(objects_detected_time, object_buffer_time_length_, objects_history_);
cleanupOldStoppedOnGreenTimes(in_objects);
+ auto invalidated_crosswalk_users = removeOldObjectsHistory(
+ objects_detected_time, object_buffer_time_length_, crosswalk_users_history_);
+
// result output
PredictedObjects output;
output.header = in_objects->header;
@@ -1172,6 +1175,19 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
"debug/processing_time_ms", processing_time_ms);
}
+void MapBasedPredictionNode::updateCrosswalkUserHistory(
+ const std_msgs::msg::Header & header, const TrackedObject & object, const std::string & object_id)
+{
+ CrosswalkUserData crosswalk_user_data;
+ crosswalk_user_data.header = header;
+ crosswalk_user_data.tracked_object = object;
+ if (crosswalk_users_history_.count(object_id) == 0) {
+ crosswalk_users_history_.emplace(object_id, std::deque<CrosswalkUserData>{crosswalk_user_data});
+ } else {
+ crosswalk_users_history_.at(object_id).push_back(crosswalk_user_data);
+ }
+}
+
bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)
{
const lanelet::ConstLineStrings3d & all_fences =
From 1823b7f2f7a750e851c7ad07ea2dacd54f166f08 Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Tue, 30 Apr 2024 14:15:33 +0900
Subject: [PATCH 04/10] use crosswalk user history
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../config/map_based_prediction.param.yaml | 3 +
.../map_based_prediction_node.hpp | 8 +-
.../src/map_based_prediction_node.cpp | 90 ++++++++++++++++++-
3 files changed, 99 insertions(+), 2 deletions(-)
diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml
index f8ad371ab92a6..c825e2c3776da 100644
--- a/perception/map_based_prediction/config/map_based_prediction.param.yaml
+++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml
@@ -27,6 +27,9 @@
timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map
# parameter for shoulder lane prediction
prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
+ use_crosswalk_user_history:
+ match_lost_and_appeared_users: true
+ remember_lost_users: true
# parameters for lc prediction
lane_change_detection:
diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index e0a9ee51aa30d..b98b257808be2 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -48,6 +48,7 @@
#include <optional>
#include <string>
#include <unordered_map>
+#include <unordered_set>
#include <utility>
#include <vector>
@@ -144,6 +145,7 @@ class MapBasedPredictionNode : public rclcpp::Node
std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
std::map<std::pair<std::string, lanelet::Id>, rclcpp::Time> stopped_times_against_green_;
std::unordered_map<std::string, std::deque<CrosswalkUserData>> crosswalk_users_history_;
+ std::unordered_map<std::string, std::string> known_matches_;
// Lanelet Map Pointers
std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
@@ -207,6 +209,9 @@ class MapBasedPredictionNode : public rclcpp::Node
std::vector<double> distance_set_for_no_intention_to_walk_;
std::vector<double> timeout_set_for_no_intention_to_walk_;
+ bool match_lost_and_appeared_crosswalk_users_;
+ bool remember_lost_crosswalk_users_;
+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
// Member Functions
@@ -244,7 +249,8 @@ class MapBasedPredictionNode : public rclcpp::Node
void updateCrosswalkUserHistory(
const std_msgs::msg::Header & header, const TrackedObject & object,
const std::string & object_id);
-
+ std::string tryMatchNewObjectToDisappeared(
+ const std::string & object_id, std::unordered_map<std::string, TrackedObject> & current_users);
std::vector<PredictedRefPath> getPredictedReferencePath(
const TrackedObject & object, const LaneletsData & current_lanelets_data,
const double object_detected_time);
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index fdea487eb1e3f..e8b578537f465 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -822,7 +822,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
* parameter control the estimated path length = vx * th * (rate) */
prediction_time_horizon_rate_for_validate_lane_length_ =
declare_parameter<double>("prediction_time_horizon_rate_for_validate_shoulder_lane_length");
-
+ match_lost_and_appeared_crosswalk_users_ =
+ declare_parameter<bool>("use_crosswalk_user_history.match_lost_and_appeared_users");
+ remember_lost_crosswalk_users_ =
+ declare_parameter<bool>("use_crosswalk_user_history.remember_lost_users");
use_vehicle_acceleration_ = declare_parameter<bool>("use_vehicle_acceleration");
speed_limit_multiplier_ = declare_parameter<double>("speed_limit_multiplier");
acceleration_exponential_half_life_ =
@@ -969,6 +972,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
auto invalidated_crosswalk_users = removeOldObjectsHistory(
objects_detected_time, object_buffer_time_length_, crosswalk_users_history_);
+ // delete matches that point to invalid object
+ for (auto it = known_matches_.begin(); it != known_matches_.end();) {
+ if (invalidated_crosswalk_users.count(it->second)) {
+ it = known_matches_.erase(it);
+ } else {
+ ++it;
+ }
+ }
// result output
PredictedObjects output;
@@ -978,6 +989,19 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
// result debug
visualization_msgs::msg::MarkerArray debug_markers;
+ // get current crosswalk users for later prediction
+ std::unordered_map<std::string, TrackedObject> current_crosswalk_users;
+ for (const auto & object : in_objects->objects) {
+ const auto label_for_prediction =
+ changeLabelForPrediction(object.classification.front().label, object, lanelet_map_ptr_);
+ if (
+ label_for_prediction == ObjectClassification::PEDESTRIAN ||
+ label_for_prediction == ObjectClassification::BICYCLE) {
+ current_crosswalk_users[tier4_autoware_utils::toHexString(object.object_id)] = object;
+ }
+ }
+ std::unordered_set<std::string> predicted_crosswalk_users_ids;
+
for (const auto & object : in_objects->objects) {
std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
TrackedObject transformed_object = object;
@@ -998,6 +1022,12 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
switch (label) {
case ObjectClassification::PEDESTRIAN:
case ObjectClassification::BICYCLE: {
+ std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
+ if (match_lost_and_appeared_crosswalk_users_) {
+ object_id = tryMatchNewObjectToDisappeared(object_id, current_crosswalk_users);
+ }
+ predicted_crosswalk_users_ids.insert(object_id);
+ updateCrosswalkUserHistory(output.header, transformed_object, object_id);
const auto predicted_object_crosswalk =
getPredictedObjectAsCrosswalkUser(transformed_object);
output.objects.push_back(predicted_object_crosswalk);
@@ -1163,6 +1193,19 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
}
}
+ // process lost crosswalk users to tackle unstable detection
+ if (remember_lost_crosswalk_users_) {
+ for (const auto & [id, crosswalk_user] : crosswalk_users_history_) {
+ // get a predicted path for crosswalk users in history who didn't get path yet using latest
+ // message
+ if (predicted_crosswalk_users_ids.count(id) == 0) {
+ const auto predicted_object =
+ getPredictedObjectAsCrosswalkUser(crosswalk_user.back().tracked_object);
+ output.objects.push_back(predicted_object);
+ }
+ }
+ }
+
// Publish Results
pub_objects_->publish(output);
published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp);
@@ -1188,6 +1231,51 @@ void MapBasedPredictionNode::updateCrosswalkUserHistory(
}
}
+std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
+ const std::string & object_id, std::unordered_map<std::string, TrackedObject> & current_users)
+{
+ if (known_matches_.count(object_id)) {
+ std::string match_id = known_matches_[object_id];
+ // object in the history is already matched to something (possibly itself)
+ if (crosswalk_users_history_.count(match_id)) {
+ // avoid matching two appeared users to one user in history
+ current_users[match_id] = crosswalk_users_history_[match_id].back().tracked_object;
+ return match_id;
+ } else {
+ RCLCPP_WARN_STREAM(
+ get_logger(), "Crosswalk user was " << object_id << "was matched to " << match_id
+ << " but the history has deleted. Rematching");
+ }
+ }
+ std::string match_id = object_id;
+ double best_score = std::numeric_limits<double>::max();
+ auto pos0 = current_users[object_id].kinematics.pose_with_covariance.pose.position;
+ for (const auto & [user_id, user_history] : crosswalk_users_history_) {
+ // user present in current_users and will be matched to itself
+ if (current_users.count(user_id)) {
+ continue;
+ }
+ // TODO(dkoldaev): implement more sophisticated scoring, for now simply dst to last position in
+ // history
+ auto pos = user_history.back().tracked_object.kinematics.pose_with_covariance.pose.position;
+ double score = std::hypot(pos.x - pos0.x, pos.y - pos0.y);
+ if (score < best_score) {
+ best_score = score;
+ match_id = user_id;
+ }
+ }
+
+ if (object_id != match_id) {
+ RCLCPP_INFO_STREAM(
+ get_logger(), "[Map Based Prediction]: Matched " << object_id << " to " << match_id);
+ // avoid matching two appeared users to one user in history
+ current_users[match_id] = crosswalk_users_history_[match_id].back().tracked_object;
+ }
+
+ known_matches_[object_id] = match_id;
+ return match_id;
+}
+
bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)
{
const lanelet::ConstLineStrings3d & all_fences =
From ff796f934e6cf8f5e2e9e3f302f471c78456451a Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Wed, 8 May 2024 10:21:23 +0900
Subject: [PATCH 05/10] renaming and log fixes
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../map_based_prediction_node.hpp | 6 +--
.../src/map_based_prediction_node.cpp | 44 ++++++++++---------
2 files changed, 26 insertions(+), 24 deletions(-)
diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index b98b257808be2..213f18d63ef3a 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -142,7 +142,7 @@ class MapBasedPredictionNode : public rclcpp::Node
std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
// Object History
- std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
+ std::unordered_map<std::string, std::deque<ObjectData>> road_users_history;
std::map<std::pair<std::string, lanelet::Id>, rclcpp::Time> stopped_times_against_green_;
std::unordered_map<std::string, std::deque<CrosswalkUserData>> crosswalk_users_history_;
std::unordered_map<std::string, std::string> known_matches_;
@@ -234,7 +234,7 @@ class MapBasedPredictionNode : public rclcpp::Node
PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object);
- void cleanupOldStoppedOnGreenTimes(const TrackedObjects::ConstSharedPtr in_objects);
+ void removeStaleTrafficLightInfo(const TrackedObjects::ConstSharedPtr in_objects);
LaneletsData getCurrentLanelets(const TrackedObject & object);
bool checkCloseLaneletCondition(
@@ -243,7 +243,7 @@ class MapBasedPredictionNode : public rclcpp::Node
const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const;
void updateObjectData(TrackedObject & object);
- void updateObjectsHistory(
+ void updateRoadUsersHistory(
const std_msgs::msg::Header & header, const TrackedObject & object,
const LaneletsData & current_lanelets_data);
void updateCrosswalkUserHistory(
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index e8b578537f465..ebc8cc65a9850 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -967,8 +967,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
// Remove old objects information in object history
const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds();
- removeOldObjectsHistory(objects_detected_time, object_buffer_time_length_, objects_history_);
- cleanupOldStoppedOnGreenTimes(in_objects);
+ removeOldObjectsHistory(objects_detected_time, object_buffer_time_length_, road_users_history);
+ removeStaleTrafficLightInfo(in_objects);
auto invalidated_crosswalk_users = removeOldObjectsHistory(
objects_detected_time, object_buffer_time_length_, crosswalk_users_history_);
@@ -997,7 +997,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
if (
label_for_prediction == ObjectClassification::PEDESTRIAN ||
label_for_prediction == ObjectClassification::BICYCLE) {
- current_crosswalk_users[tier4_autoware_utils::toHexString(object.object_id)] = object;
+ const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
+ current_crosswalk_users.emplace(object_id, object);
}
}
std::unordered_set<std::string> predicted_crosswalk_users_ids;
@@ -1045,7 +1046,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
const auto current_lanelets = getCurrentLanelets(transformed_object);
// Update Objects History
- updateObjectsHistory(output.header, transformed_object, current_lanelets);
+ updateRoadUsersHistory(output.header, transformed_object, current_lanelets);
// For off lane obstacles
if (current_lanelets.empty()) {
@@ -1243,8 +1244,9 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
return match_id;
} else {
RCLCPP_WARN_STREAM(
- get_logger(), "Crosswalk user was " << object_id << "was matched to " << match_id
- << " but the history has deleted. Rematching");
+ get_logger(), "Crosswalk user was "
+ << object_id << "was matched to " << match_id
+ << " but history for the crosswalk user was deleted. Rematching");
}
}
std::string match_id = object_id;
@@ -1496,7 +1498,7 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object)
return;
}
-void MapBasedPredictionNode::cleanupOldStoppedOnGreenTimes(
+void MapBasedPredictionNode::removeStaleTrafficLightInfo(
const TrackedObjects::ConstSharedPtr in_objects)
{
for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) {
@@ -1613,9 +1615,9 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition(
// If the object is in the objects history, we check if the target lanelet is
// inside the current lanelets id or following lanelets
const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
- if (objects_history_.count(object_id) != 0) {
+ if (road_users_history.count(object_id) != 0) {
const std::vector<lanelet::ConstLanelet> & possible_lanelet =
- objects_history_.at(object_id).back().future_possible_lanelets;
+ road_users_history.at(object_id).back().future_possible_lanelets;
bool not_in_possible_lanelet =
std::find(possible_lanelet.begin(), possible_lanelet.end(), lanelet.second) ==
@@ -1682,7 +1684,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood(
return static_cast<float>(1.0 / dist);
}
-void MapBasedPredictionNode::updateObjectsHistory(
+void MapBasedPredictionNode::updateRoadUsersHistory(
const std_msgs::msg::Header & header, const TrackedObject & object,
const LaneletsData & current_lanelets_data)
{
@@ -1706,13 +1708,13 @@ void MapBasedPredictionNode::updateObjectsHistory(
single_object_data.lateral_kinematics_set[current_lane] = lateral_kinematics;
}
- if (objects_history_.count(object_id) == 0) {
+ if (road_users_history.count(object_id) == 0) {
// New Object(Create a new object in object histories)
std::deque<ObjectData> object_data = {single_object_data};
- objects_history_.emplace(object_id, object_data);
+ road_users_history.emplace(object_id, object_data);
} else {
// Object that is already in the object buffer
- std::deque<ObjectData> & object_data = objects_history_.at(object_id);
+ std::deque<ObjectData> & object_data = road_users_history.at(object_id);
// get previous object data and update
const auto prev_object_data = object_data.back();
updateLateralKinematicsVector(
@@ -1900,10 +1902,10 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver(
}();
const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
- if (objects_history_.count(object_id) == 0) {
+ if (road_users_history.count(object_id) == 0) {
return current_maneuver;
}
- auto & object_info = objects_history_.at(object_id);
+ auto & object_info = road_users_history.at(object_id);
// update maneuver in object history
if (!object_info.empty()) {
@@ -1939,11 +1941,11 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange(
{
// Step1. Check if we have the object in the buffer
const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
- if (objects_history_.count(object_id) == 0) {
+ if (road_users_history.count(object_id) == 0) {
return Maneuver::LANE_FOLLOW;
}
- const std::deque<ObjectData> & object_info = objects_history_.at(object_id);
+ const std::deque<ObjectData> & object_info = road_users_history.at(object_id);
// Step2. Check if object history length longer than history_time_length
const int latest_id = static_cast<int>(object_info.size()) - 1;
@@ -2010,11 +2012,11 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
{
// Step1. Check if we have the object in the buffer
const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
- if (objects_history_.count(object_id) == 0) {
+ if (road_users_history.count(object_id) == 0) {
return Maneuver::LANE_FOLLOW;
}
- const std::deque<ObjectData> & object_info = objects_history_.at(object_id);
+ const std::deque<ObjectData> & object_info = road_users_history.at(object_id);
const double current_time = (this->get_clock()->now()).seconds();
// Step2. Get the previous id
@@ -2171,12 +2173,12 @@ void MapBasedPredictionNode::updateFuturePossibleLanelets(
const TrackedObject & object, const lanelet::routing::LaneletPaths & paths)
{
std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
- if (objects_history_.count(object_id) == 0) {
+ if (road_users_history.count(object_id) == 0) {
return;
}
std::vector<lanelet::ConstLanelet> & possible_lanelets =
- objects_history_.at(object_id).back().future_possible_lanelets;
+ road_users_history.at(object_id).back().future_possible_lanelets;
for (const auto & path : paths) {
for (const auto & lanelet : path) {
bool not_in_buffer = std::find(possible_lanelets.begin(), possible_lanelets.end(), lanelet) ==
From 9cc4902deacb0ed7184409a9328a231d21e9b097 Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Wed, 8 May 2024 10:29:13 +0900
Subject: [PATCH 06/10] const for non changing objects
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../src/map_based_prediction_node.cpp | 8 +++++---
1 file changed, 5 insertions(+), 3 deletions(-)
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index ebc8cc65a9850..23991afe44445 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1251,7 +1251,7 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
}
std::string match_id = object_id;
double best_score = std::numeric_limits<double>::max();
- auto pos0 = current_users[object_id].kinematics.pose_with_covariance.pose.position;
+ const auto object_pos = current_users[object_id].kinematics.pose_with_covariance.pose.position;
for (const auto & [user_id, user_history] : crosswalk_users_history_) {
// user present in current_users and will be matched to itself
if (current_users.count(user_id)) {
@@ -1259,8 +1259,10 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
}
// TODO(dkoldaev): implement more sophisticated scoring, for now simply dst to last position in
// history
- auto pos = user_history.back().tracked_object.kinematics.pose_with_covariance.pose.position;
- double score = std::hypot(pos.x - pos0.x, pos.y - pos0.y);
+ const auto match_candidate_pos =
+ user_history.back().tracked_object.kinematics.pose_with_covariance.pose.position;
+ const double score =
+ std::hypot(match_candidate_pos.x - object_pos.x, match_candidate_pos.y - object_pos.y);
if (score < best_score) {
best_score = score;
match_id = user_id;
From 2debdd6ce462dfdc8b7d4a210a5a7c9bf83da198 Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Wed, 8 May 2024 10:32:26 +0900
Subject: [PATCH 07/10] disable history by default
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../config/map_based_prediction.param.yaml | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml
index c825e2c3776da..a1b776bdb6393 100644
--- a/perception/map_based_prediction/config/map_based_prediction.param.yaml
+++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml
@@ -28,8 +28,8 @@
# parameter for shoulder lane prediction
prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
use_crosswalk_user_history:
- match_lost_and_appeared_users: true
- remember_lost_users: true
+ match_lost_and_appeared_users: false
+ remember_lost_users: false
# parameters for lc prediction
lane_change_detection:
From cc9f2fe1600d221dd50c451b64b4be9d8c995bb4 Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Wed, 8 May 2024 10:46:51 +0900
Subject: [PATCH 08/10] use RAII to improve early return readability in
tryMatch... function
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../src/map_based_prediction_node.cpp | 12 +++++++++++-
1 file changed, 11 insertions(+), 1 deletion(-)
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 23991afe44445..88530a1c989a2 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1235,7 +1235,11 @@ void MapBasedPredictionNode::updateCrosswalkUserHistory(
std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
const std::string & object_id, std::unordered_map<std::string, TrackedObject> & current_users)
{
- if (known_matches_.count(object_id)) {
+ const auto known_match_opt = [&]() -> std::optional<std::string> {
+ if (!known_matches_.count(object_id)) {
+ return std::nullopt;
+ }
+
std::string match_id = known_matches_[object_id];
// object in the history is already matched to something (possibly itself)
if (crosswalk_users_history_.count(match_id)) {
@@ -1248,7 +1252,13 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
<< object_id << "was matched to " << match_id
<< " but history for the crosswalk user was deleted. Rematching");
}
+ return std::nullopt;
+ }();
+ // early return if the match is already known
+ if (known_match_opt.has_value()) {
+ return known_match_opt.value();
}
+
std::string match_id = object_id;
double best_score = std::numeric_limits<double>::max();
const auto object_pos = current_users[object_id].kinematics.pose_with_covariance.pose.position;
From 52b49e3bb48f5cf63713e3add76d326acd2f20ce Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Wed, 8 May 2024 10:49:55 +0900
Subject: [PATCH 09/10] less conditionals in update crosswalk history
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../map_based_prediction/src/map_based_prediction_node.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 88530a1c989a2..dbc117fbdbe3c 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1225,11 +1225,13 @@ void MapBasedPredictionNode::updateCrosswalkUserHistory(
CrosswalkUserData crosswalk_user_data;
crosswalk_user_data.header = header;
crosswalk_user_data.tracked_object = object;
+
if (crosswalk_users_history_.count(object_id) == 0) {
crosswalk_users_history_.emplace(object_id, std::deque<CrosswalkUserData>{crosswalk_user_data});
- } else {
- crosswalk_users_history_.at(object_id).push_back(crosswalk_user_data);
+ return;
}
+
+ crosswalk_users_history_.at(object_id).push_back(crosswalk_user_data);
}
std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
From 6826892fc8ac44116ea24ac42a0fa14f8065133f Mon Sep 17 00:00:00 2001
From: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
Date: Wed, 8 May 2024 14:44:35 +0900
Subject: [PATCH 10/10] improve readability in removing stale object data
Signed-off-by: Dmitrii Koldaev <dmitrii.koldaev@tier4.jp>
---
.../map_based_prediction/src/map_based_prediction_node.cpp | 6 ++----
1 file changed, 2 insertions(+), 4 deletions(-)
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index dbc117fbdbe3c..3942af905ea22 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -612,12 +612,10 @@ std::unordered_set<std::string> removeOldObjectsHistory(
// Delete old information
while (!object_data.empty()) {
const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds();
- if (current_time - post_object_time > buffer_time) {
- // Delete Old Position
- object_data.pop_front();
- } else {
+ if (current_time - post_object_time <= buffer_time) {
break;
}
+ object_data.pop_front();
}
if (object_data.empty()) {