Skip to content

Commit

Permalink
feat(map_based_prediction): add time_keeper (#8176)
Browse files Browse the repository at this point in the history
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
soblin authored Jul 24, 2024
1 parent 799f5b1 commit e46c927
Show file tree
Hide file tree
Showing 2 changed files with 65 additions and 0 deletions.
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
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 @@ -1236,6 +1247,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 @@ -1251,6 +1264,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 @@ -1308,6 +1323,8 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(

bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);

lanelet::BasicLineString2d predicted_path_ls;
for (const auto & p : predicted_path.path)
predicted_path_ls.emplace_back(p.position.x, p.position.y);
Expand All @@ -1325,6 +1342,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 @@ -1353,6 +1372,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 @@ -1485,6 +1506,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 @@ -1535,6 +1558,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 @@ -1551,6 +1576,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 @@ -1641,6 +1668,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 @@ -1687,6 +1716,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 @@ -1722,6 +1753,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 @@ -1763,6 +1796,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 @@ -1925,6 +1960,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 @@ -1975,6 +2012,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 @@ -2046,6 +2085,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 @@ -2189,6 +2230,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 @@ -2208,6 +2251,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 @@ -2232,6 +2277,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 @@ -2251,6 +2298,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 @@ -2313,6 +2362,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 @@ -2385,6 +2436,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 @@ -2402,6 +2455,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 @@ -2418,6 +2473,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 @@ -2434,6 +2491,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 @@ -2450,6 +2509,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

0 comments on commit e46c927

Please sign in to comment.