Skip to content

Commit

Permalink
feat(map based prediction, crosswalk)!: transplantation of pedestrian…
Browse files Browse the repository at this point in the history 
…s' behavior prediction against green signal (autowarefoundation#6338)

* pedestrians' intention estimation feature against the green signal
* delete the reimplimented feature
* sync params
* update docs
---------
Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
@yuki-takagi-66 @StepTurtle
yuki-takagi-66 authored and StepTurtle committed Feb 27, 2024
1 parent 0ae5ec3 commit e897ed5
Show file tree
Hide file tree
Showing 8 changed files with 163 additions and 81 deletions.
14 changes: 10 additions & 4 deletions perception/map_based_prediction/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,6 +191,11 @@ This module treats **Pedestrians** and **Bicycles** as objects using the crosswa

If there are a reachable crosswalk entry points within the `prediction_time_horizon` and the objects satisfies above condition, this module outputs additional predicted path to cross the opposite side via the crosswalk entry point.

This module takes into account the corresponding traffic light information.
When RED signal is indicated, we assume the target object will not walk across.
In additon, if the target object is stopping (not moving) against GREEN signal, we assume the target object will not walk across either.
This prediction comes from the assumption that the object should move if the traffic light is green and the object is intended to cross.

<div align="center">
<img src="images/outside_road.svg" width=90%>
</div>
Expand All @@ -205,10 +210,11 @@ If the target object is inside the road or crosswalk, this module outputs one or

### Input

| Name | Type | Description |
| -------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------- |
| `~/perception/object_recognition/tracking/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. |
| `~/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | binary data of Lanelet2 Map. |
| Name | Type | Description |
| -------------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------------------------- |
| `~/perception/object_recognition/tracking/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. |
| `~/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | binary data of Lanelet2 Map. |
| '~/perception/traffic_light_recognition/traffic_signals' | 'autoware_perception_msgs::msg::TrafficSignalArray;' | rearranged information on the corresponding traffic lights |

### Output

Expand Down
7 changes: 6 additions & 1 deletion perception/map_based_prediction/config/map_based_prediction.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,12 @@
use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not
speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value
acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds
use_crosswalk_signal: true
crosswalk_with_signal:
use_crosswalk_signal: true
threshold_velocity_assumed_as_stopping: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped
# If the pedestrian does not move for X seconds against green signal, the module judge that the pedestrian has no intention to walk.
distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order, keys of map
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

Expand Down
11 changes: 10 additions & 1 deletion perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@

#include <algorithm>
#include <deque>
#include <map>
#include <memory>
#include <optional>
#include <string>
Expand Down Expand Up @@ -131,6 +132,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_;

// Lanelet Map Pointers
std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
Expand Down Expand Up @@ -190,6 +192,9 @@ class MapBasedPredictionNode : public rclcpp::Node
double acceleration_exponential_half_life_;

bool use_crosswalk_signal_;
double threshold_velocity_assumed_as_stopping_;
std::vector<double> distance_set_for_no_intention_to_walk_;
std::vector<double> timeout_set_for_no_intention_to_walk_;

// Stop watch
StopWatch<std::chrono::milliseconds> stop_watch_;
Expand All @@ -214,7 +219,8 @@ class MapBasedPredictionNode : public rclcpp::Node

PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object);

void removeOldObjectsHistory(const double current_time);
void removeOldObjectsHistory(
const double current_time, const TrackedObjects::ConstSharedPtr in_objects);

LaneletsData getCurrentLanelets(const TrackedObject & object);
bool checkCloseLaneletCondition(
Expand Down Expand Up @@ -262,6 +268,9 @@ class MapBasedPredictionNode : public rclcpp::Node
const PredictedPath & predicted_path, const std::vector<PredictedPath> & predicted_paths);
std::optional<lanelet::Id> getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet);
std::optional<TrafficSignalElement> getTrafficSignalElement(const lanelet::Id & id);
bool calcIntentionToCrossWithTrafficSgnal(
const TrackedObject & object, const lanelet::ConstLanelet & crosswalk,
const lanelet::Id & signal_id);

visualization_msgs::msg::Marker getDebugMarker(
const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num);
Expand Down
98 changes: 89 additions & 9 deletions perception/map_based_prediction/src/map_based_prediction_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -788,7 +788,13 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
acceleration_exponential_half_life_ =
declare_parameter<double>("acceleration_exponential_half_life");

use_crosswalk_signal_ = declare_parameter<bool>("use_crosswalk_signal");
use_crosswalk_signal_ = declare_parameter<bool>("crosswalk_with_signal.use_crosswalk_signal");
threshold_velocity_assumed_as_stopping_ =
declare_parameter<double>("crosswalk_with_signal.threshold_velocity_assumed_as_stopping");
distance_set_for_no_intention_to_walk_ = declare_parameter<std::vector<double>>(
"crosswalk_with_signal.distance_set_for_no_intention_to_walk");
timeout_set_for_no_intention_to_walk_ = declare_parameter<std::vector<double>>(
"crosswalk_with_signal.timeout_set_for_no_intention_to_walk");

path_generator_ = std::make_shared<PathGenerator>(
prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
Expand Down Expand Up @@ -912,7 +918,7 @@ 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);
removeOldObjectsHistory(objects_detected_time, in_objects);

// result output
PredictedObjects output;
Expand Down Expand Up @@ -1229,16 +1235,13 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
}
}
}

// try to find the edge points for all crosswalks and generate path to the crosswalk edge
for (const auto & crosswalk : crosswalks_) {
const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk);
if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) {
const auto signal_color = [&] {
const auto elem_opt = getTrafficSignalElement(crosswalk_signal_id_opt.value());
return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN;
}();

if (signal_color == TrafficSignalElement::RED) {
if (!calcIntentionToCrossWithTrafficSgnal(
object, crosswalk, crosswalk_signal_id_opt.value())) {
continue;
}
}
Expand Down Expand Up @@ -1330,7 +1333,8 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object)
return;
}

void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time)
void MapBasedPredictionNode::removeOldObjectsHistory(
const double current_time, const TrackedObjects::ConstSharedPtr in_objects)
{
std::vector<std::string> invalid_object_id;
for (auto iter = objects_history_.begin(); iter != objects_history_.end(); ++iter) {
Expand Down Expand Up @@ -1371,6 +1375,19 @@ void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time)
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(),
[&it](autoware_auto_perception_msgs::msg::TrackedObject obj) {
return tier4_autoware_utils::toHexString(obj.object_id) == it->first.first;
});
if (isDisappeared) {
it = stopped_times_against_green_.erase(it);
} else {
++it;
}
}
}

LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object)
Expand Down Expand Up @@ -2268,6 +2285,69 @@ std::optional<TrafficSignalElement> MapBasedPredictionNode::getTrafficSignalElem
return std::nullopt;
}

bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSgnal(
const TrackedObject & object, const lanelet::ConstLanelet & crosswalk,
const lanelet::Id & signal_id)
{
const auto signal_color = [&] {
const auto elem_opt = getTrafficSignalElement(signal_id);
return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN;
}();

const auto key = std::make_pair(tier4_autoware_utils::toHexString(object.object_id), signal_id);
if (
signal_color == TrafficSignalElement::GREEN &&
tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) <
threshold_velocity_assumed_as_stopping_) {
stopped_times_against_green_.try_emplace(key, this->get_clock()->now());

const auto timeout_no_intention_to_walk = [&]() {
auto InterpolateMap = [](
const std::vector<double> & key_set,
const std::vector<double> & value_set, const double query) {
if (query <= key_set.front()) {
return value_set.front();
} else if (query >= key_set.back()) {
return value_set.back();
}
for (size_t i = 0; i < key_set.size() - 1; ++i) {
if (key_set.at(i) <= query && query <= key_set.at(i + 1)) {
auto ratio =
(query - key_set.at(i)) / std::max(key_set.at(i + 1) - key_set.at(i), 1.0e-5);
ratio = std::clamp(ratio, 0.0, 1.0);
return value_set.at(i) + ratio * (value_set.at(i + 1) - value_set.at(i));
}
}
return value_set.back();
};

const auto obj_position = object.kinematics.pose_with_covariance.pose.position;
const double distance_to_crosswalk = boost::geometry::distance(
crosswalk.polygon2d().basicPolygon(),
lanelet::BasicPoint2d(obj_position.x, obj_position.y));
return InterpolateMap(
distance_set_for_no_intention_to_walk_, timeout_set_for_no_intention_to_walk_,
distance_to_crosswalk);
}();

if (
(this->get_clock()->now() - stopped_times_against_green_.at(key)).seconds() >
timeout_no_intention_to_walk) {
return false;
}

} else {
stopped_times_against_green_.erase(key);
// If the pedestrian disappears, another function erases the old data.
}

if (signal_color == TrafficSignalElement::RED) {
return false;
}

return true;
}

} // namespace map_based_prediction

#include <rclcpp_components/register_node_macro.hpp>
Expand Down
Loading

0 comments on commit e897ed5

Please sign in to comment.