Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(map_based_prediction): add time_keeper (#8176) #1426

Merged
merged 1 commit into from
Jul 25, 2024
Merged

feat(map_based_prediction): add time_keeper (#8176) #1426

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions .../autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include <autoware/universe_utils/ros/transform_listener.hpp>
#include <autoware/universe_utils/ros/uuid_helper.hpp>
#include <autoware/universe_utils/system/stop_watch.hpp>
#include <autoware/universe_utils/system/time_keeper.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
Expand Down Expand Up @@ -222,6 +223,9 @@ class MapBasedPredictionNode : public rclcpp::Node
bool remember_lost_crosswalk_users_;

std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
detailed_processing_time_publisher_;
mutable autoware::universe_utils::TimeKeeper time_keeper_;

// Member Functions
void mapCallback(const LaneletMapBin::ConstSharedPtr msg);
Expand Down
60 changes: 60 additions & 0 deletions perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -863,6 +863,11 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_

published_time_publisher_ =
std::make_unique<autoware::universe_utils::PublishedTimePublisher>(this);
detailed_processing_time_publisher_ =
this->create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
"~/debug/processing_time_detail_ms", 1);
time_keeper_ = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_);

set_param_res_ = this->add_on_set_parameters_callback(
std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1));

Expand Down Expand Up @@ -922,6 +927,8 @@ PredictedObject MapBasedPredictionNode::convertToPredictedObject(

void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet");
lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(
Expand All @@ -938,6 +945,8 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg
void MapBasedPredictionNode::trafficSignalsCallback(
const TrafficLightGroupArray::ConstSharedPtr msg)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

traffic_signal_id_map_.clear();
for (const auto & signal : msg->traffic_light_groups) {
traffic_signal_id_map_[signal.traffic_light_group_id] = signal;
Expand All @@ -946,6 +955,8 @@ void MapBasedPredictionNode::trafficSignalsCallback(

void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

stop_watch_ptr_->toc("processing_time", true);

// take traffic_signal
Expand Down Expand Up @@ -1237,6 +1248,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
void MapBasedPredictionNode::updateCrosswalkUserHistory(
const std_msgs::msg::Header & header, const TrackedObject & object, const std::string & object_id)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

CrosswalkUserData crosswalk_user_data;
crosswalk_user_data.header = header;
crosswalk_user_data.tracked_object = object;
Expand All @@ -1252,6 +1265,8 @@ void MapBasedPredictionNode::updateCrosswalkUserHistory(
std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
const std::string & object_id, std::unordered_map<std::string, TrackedObject> & current_users)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

const auto known_match_opt = [&]() -> std::optional<std::string> {
if (!known_matches_.count(object_id)) {
return std::nullopt;
Expand Down Expand Up @@ -1309,6 +1324,7 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(

bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
const lanelet::ConstLineStrings3d & all_fences =
lanelet::utils::query::getAllFences(lanelet_map_ptr_);
for (const auto & fence_line : all_fences) {
Expand All @@ -1322,6 +1338,8 @@ bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predict
bool MapBasedPredictionNode::doesPathCrossFence(
const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

// check whether the predicted path cross with fence
for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) {
for (size_t j = 0; j < fence_line.size() - 1; ++j) {
Expand Down Expand Up @@ -1350,6 +1368,8 @@ bool MapBasedPredictionNode::isIntersecting(
PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
const TrackedObject & object)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

auto predicted_object = convertToPredictedObject(object);
{
PredictedPath predicted_path =
Expand Down Expand Up @@ -1482,6 +1502,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(

void MapBasedPredictionNode::updateObjectData(TrackedObject & object)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

if (
object.kinematics.orientation_availability ==
autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) {
Expand Down Expand Up @@ -1532,6 +1554,8 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object)
void MapBasedPredictionNode::removeStaleTrafficLightInfo(
const TrackedObjects::ConstSharedPtr in_objects)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

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(),
Expand All @@ -1548,6 +1572,8 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo(

LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

// obstacle point
lanelet::BasicPoint2d search_point(
object.kinematics.pose_with_covariance.pose.position.x,
Expand Down Expand Up @@ -1638,6 +1664,8 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob
bool MapBasedPredictionNode::checkCloseLaneletCondition(
const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

// Step1. If we only have one point in the centerline, we will ignore the lanelet
if (lanelet.second.centerline().size() <= 1) {
return false;
Expand Down Expand Up @@ -1684,6 +1712,8 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition(
float MapBasedPredictionNode::calculateLocalLikelihood(
const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

const auto & obj_point = object.kinematics.pose_with_covariance.pose.position;

// compute yaw difference between the object and lane
Expand Down Expand Up @@ -1719,6 +1749,8 @@ void MapBasedPredictionNode::updateRoadUsersHistory(
const std_msgs::msg::Header & header, const TrackedObject & object,
const LaneletsData & current_lanelets_data)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

std::string object_id = autoware::universe_utils::toHexString(object.object_id);
const auto current_lanelets = getLanelets(current_lanelets_data);

Expand Down Expand Up @@ -1760,6 +1792,8 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
const TrackedObject & object, const LaneletsData & current_lanelets_data,
const double object_detected_time, const double time_horizon)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

const double obj_vel = std::hypot(
object.kinematics.twist_with_covariance.twist.linear.x,
object.kinematics.twist_with_covariance.twist.linear.y);
Expand Down Expand Up @@ -1921,6 +1955,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver(
const TrackedObject & object, const LaneletData & current_lanelet_data,
const double object_detected_time)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

// calculate maneuver
const auto current_maneuver = [&]() {
if (lane_change_detection_method_ == "time_to_change_lane") {
Expand Down Expand Up @@ -1971,6 +2007,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange(
const TrackedObject & object, const LaneletData & current_lanelet_data,
const double /*object_detected_time*/)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

// Step1. Check if we have the object in the buffer
const std::string object_id = autoware::universe_utils::toHexString(object.object_id);
if (road_users_history.count(object_id) == 0) {
Expand Down Expand Up @@ -2042,6 +2080,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
const TrackedObject & object, const LaneletData & current_lanelet_data,
const double /*object_detected_time*/)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

// Step1. Check if we have the object in the buffer
const std::string object_id = autoware::universe_utils::toHexString(object.object_id);
if (road_users_history.count(object_id) == 0) {
Expand Down Expand Up @@ -2185,6 +2225,8 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay(
double MapBasedPredictionNode::calcRightLateralOffset(
const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

std::vector<geometry_msgs::msg::Point> boundary_path(boundary_line.size());
for (size_t i = 0; i < boundary_path.size(); ++i) {
const double x = boundary_line[i].x();
Expand All @@ -2204,6 +2246,8 @@ double MapBasedPredictionNode::calcLeftLateralOffset(
void MapBasedPredictionNode::updateFuturePossibleLanelets(
const TrackedObject & object, const lanelet::routing::LaneletPaths & paths)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

std::string object_id = autoware::universe_utils::toHexString(object.object_id);
if (road_users_history.count(object_id) == 0) {
return;
Expand All @@ -2228,6 +2272,8 @@ void MapBasedPredictionNode::addReferencePaths(
const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths,
const double speed_limit)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

if (!candidate_paths.empty()) {
updateFuturePossibleLanelets(object, candidate_paths);
const auto converted_paths = convertPathType(candidate_paths);
Expand All @@ -2247,6 +2293,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability(
const lanelet::routing::LaneletPaths & right_paths,
const lanelet::routing::LaneletPaths & center_paths)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

float left_lane_change_probability = 0.0;
float right_lane_change_probability = 0.0;
float lane_follow_probability = 0.0;
Expand Down Expand Up @@ -2309,6 +2357,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability(
std::vector<PosePath> MapBasedPredictionNode::convertPathType(
const lanelet::routing::LaneletPaths & paths)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

std::vector<PosePath> converted_paths;
for (const auto & path : paths) {
PosePath converted_path;
Expand Down Expand Up @@ -2381,6 +2431,8 @@ bool MapBasedPredictionNode::isDuplicated(
const std::pair<double, lanelet::ConstLanelet> & target_lanelet,
const LaneletsData & lanelets_data)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

const double CLOSE_LANELET_THRESHOLD = 0.1;
for (const auto & lanelet_data : lanelets_data) {
const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back();
Expand All @@ -2398,6 +2450,8 @@ bool MapBasedPredictionNode::isDuplicated(
bool MapBasedPredictionNode::isDuplicated(
const PredictedPath & predicted_path, const std::vector<PredictedPath> & predicted_paths)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

const double CLOSE_PATH_THRESHOLD = 0.1;
for (const auto & prev_predicted_path : predicted_paths) {
const auto prev_path_end = prev_predicted_path.path.back().position;
Expand All @@ -2414,6 +2468,8 @@ bool MapBasedPredictionNode::isDuplicated(
std::optional<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
const lanelet::ConstLanelet & way_lanelet)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

const auto traffic_light_reg_elems =
way_lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
if (traffic_light_reg_elems.empty()) {
Expand All @@ -2430,6 +2486,8 @@ std::optional<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
std::optional<TrafficLightElement> MapBasedPredictionNode::getTrafficSignalElement(
const lanelet::Id & id)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

if (traffic_signal_id_map_.count(id) != 0) {
const auto & signal_elements = traffic_signal_id_map_.at(id).elements;
if (signal_elements.size() > 1) {
Expand All @@ -2446,6 +2504,8 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal(
const TrackedObject & object, const lanelet::ConstLanelet & crosswalk,
const lanelet::Id & signal_id)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

const auto signal_color = [&] {
const auto elem_opt = getTrafficSignalElement(signal_id);
return elem_opt ? elem_opt.value().color : TrafficLightElement::UNKNOWN;
Expand Down
Loading