Skip to content

Commit

Permalink
perf(map_based_prediction): improve world to map transform calculation (
Browse files Browse the repository at this point in the history
autowarefoundation#8413)

* perf(map_based_prediction): improve world to map transform calculation

1. remove unused transforms
2. make transform loading late as possible

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* perf(map_based_prediction): get transform only when it is necessary

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
  • Loading branch information
technolojin committed Aug 15, 2024
1 parent 0ec9e5a commit ae665b5
Showing 1 changed file with 13 additions and 18 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -971,23 +971,6 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
return;
}

auto world2map_transform = transform_listener_.getTransform(
"map", // target
in_objects->header.frame_id, // src
in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1));
auto map2world_transform = transform_listener_.getTransform(
in_objects->header.frame_id, // target
"map", // src
in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1));
auto debug_map2lidar_transform = transform_listener_.getTransform(
"base_link", // target
"map", // src
rclcpp::Time(), rclcpp::Duration::from_seconds(0.1));

if (!world2map_transform || !map2world_transform || !debug_map2lidar_transform) {
return;
}

// 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_, road_users_history);
Expand Down Expand Up @@ -1026,12 +1009,24 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
}
std::unordered_set<std::string> predicted_crosswalk_users_ids;

// get world to map transform
geometry_msgs::msg::TransformStamped::ConstSharedPtr world2map_transform;

bool is_object_not_in_map_frame = in_objects->header.frame_id != "map";
if (is_object_not_in_map_frame) {
world2map_transform = transform_listener_.getTransform(
"map", // target
in_objects->header.frame_id, // src
in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1));
if (!world2map_transform) return;
}

for (const auto & object : in_objects->objects) {
std::string object_id = autoware::universe_utils::toHexString(object.object_id);
TrackedObject transformed_object = object;

// transform object frame if it's based on map frame
if (in_objects->header.frame_id != "map") {
if (is_object_not_in_map_frame) {
geometry_msgs::msg::PoseStamped pose_in_map;
geometry_msgs::msg::PoseStamped pose_orig;
pose_orig.pose = object.kinematics.pose_with_covariance.pose;
Expand Down

0 comments on commit ae665b5

Please sign in to comment.