Skip to content

Commit

Permalink
perf(motion_velocity_planner): fix heavy resampling and transform loo…
Browse files Browse the repository at this point in the history 
…kup (#8839)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
  • Loading branch information
@maxime-clem
maxime-clem authored Sep 13, 2024
1 parent dec6e71 commit f625fbb
Show file tree
Hide file tree
Showing 2 changed files with 48 additions and 20 deletions.
62 changes: 44 additions & 18 deletions planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include <autoware/motion_utils/resample/resample.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/ros/update_param.hpp>
#include <autoware/universe_utils/ros/wait_for_param.hpp>
#include <autoware/universe_utils/system/stop_watch.hpp>
Expand All @@ -30,7 +31,9 @@

#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2/time.h>

#include <chrono>
#include <functional>
#include <map>
#include <memory>
Expand Down Expand Up @@ -129,7 +132,8 @@ void MotionVelocityPlannerNode::on_unload_plugin(
}

// NOTE: argument planner_data must not be referenced for multithreading
bool MotionVelocityPlannerNode::update_planner_data()
bool MotionVelocityPlannerNode::update_planner_data(
std::map<std::string, double> & processing_times)
{
auto clock = *get_clock();
auto is_ready = true;
Expand All @@ -143,39 +147,48 @@ bool MotionVelocityPlannerNode::update_planner_data()
return true;
};

universe_utils::StopWatch<std::chrono::milliseconds> sw;
const auto ego_state_ptr = sub_vehicle_odometry_.takeData();
if (check_with_log(ego_state_ptr, "Waiting for current odometry"))
planner_data_.current_odometry = *ego_state_ptr;
processing_times["update_planner_data.odom"] = sw.toc(true);

const auto ego_accel_ptr = sub_acceleration_.takeData();
if (check_with_log(ego_accel_ptr, "Waiting for current acceleration"))
planner_data_.current_acceleration = *ego_accel_ptr;
processing_times["update_planner_data.accel"] = sw.toc(true);

const auto predicted_objects_ptr = sub_predicted_objects_.takeData();
if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects"))
planner_data_.predicted_objects = *predicted_objects_ptr;
processing_times["update_planner_data.pred_obj"] = sw.toc(true);

const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.takeData();
if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) {
const auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr);
if (no_ground_pointcloud) planner_data_.no_ground_pointcloud = *no_ground_pointcloud;
}
processing_times["update_planner_data.pcd"] = sw.toc(true);

const auto occupancy_grid_ptr = sub_occupancy_grid_.takeData();
if (check_with_log(occupancy_grid_ptr, "Waiting for the occupancy grid"))
planner_data_.occupancy_grid = *occupancy_grid_ptr;
processing_times["update_planner_data.occ_grid"] = sw.toc(true);

// here we use bitwise operator to not short-circuit the logging messages
is_ready &= check_with_log(map_ptr_, "Waiting for the map");
processing_times["update_planner_data.map"] = sw.toc(true);
is_ready &= check_with_log(
planner_data_.velocity_smoother_, "Waiting for the initialization of the velocity smoother");
processing_times["update_planner_data.smoother"] = sw.toc(true);

// optional data
const auto traffic_signals_ptr = sub_traffic_signals_.takeData();
if (traffic_signals_ptr) process_traffic_signals(traffic_signals_ptr);
const auto virtual_traffic_light_states_ptr = sub_virtual_traffic_light_states_.takeData();
if (virtual_traffic_light_states_ptr)
planner_data_.virtual_traffic_light_states = *virtual_traffic_light_states_ptr;
processing_times["update_planner_data.traffic_lights"] = sw.toc(true);

return is_ready;
}
Expand All @@ -186,8 +199,7 @@ MotionVelocityPlannerNode::process_no_ground_pointcloud(
{
geometry_msgs::msg::TransformStamped transform;
try {
transform = tf_buffer_.lookupTransform(
"map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1));
transform = tf_buffer_.lookupTransform("map", msg->header.frame_id, tf2::TimePointZero);
} catch (tf2::TransformException & e) {
RCLCPP_WARN(get_logger(), "no transform found for no_ground_pointcloud: %s", e.what());
return {};
Expand Down Expand Up @@ -261,7 +273,7 @@ void MotionVelocityPlannerNode::on_trajectory(
std::map<std::string, double> processing_times;
stop_watch.tic("Total");

if (!update_planner_data()) {
if (!update_planner_data(processing_times)) {
return;
}
processing_times["update_planner_data"] = stop_watch.toc(true);
Expand All @@ -279,7 +291,7 @@ void MotionVelocityPlannerNode::on_trajectory(

autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{
input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()};
auto output_trajectory_msg = generate_trajectory(input_trajectory_points);
auto output_trajectory_msg = generate_trajectory(input_trajectory_points, processing_times);
output_trajectory_msg.header = input_trajectory_msg->header;
processing_times["generate_trajectory"] = stop_watch.toc(true);

Expand Down Expand Up @@ -360,9 +372,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s
smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false);

// Resample trajectory with ego-velocity based interval distances
auto traj_resampled = smoother->resampleTrajectory(
traj_steering_rate_limited, v0, current_pose, planner_data.ego_nearest_dist_threshold,
planner_data.ego_nearest_yaw_threshold);
auto traj_resampled = traj_steering_rate_limited;
const size_t traj_resampled_closest =
autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
traj_resampled, current_pose, planner_data.ego_nearest_dist_threshold,
Expand All @@ -385,25 +395,41 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s
}

autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory(
autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points)
autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points,
std::map<std::string, double> & processing_times)
{
universe_utils::StopWatch<std::chrono::microseconds> stop_watch;
universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
autoware_planning_msgs::msg::Trajectory output_trajectory_msg;
output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()};
if (smooth_velocity_before_planning_) {
stop_watch.tic("smooth");
input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_);
RCLCPP_DEBUG(get_logger(), "smooth: %2.2f us", stop_watch.toc("smooth"));
processing_times["velocity_smoothing"] = stop_watch.toc("smooth");
}
autoware_planning_msgs::msg::Trajectory smooth_velocity_trajectory;
smooth_velocity_trajectory.points = {
input_trajectory_points.begin(), input_trajectory_points.end()};
auto resampled_trajectory =
autoware::motion_utils::resampleTrajectory(smooth_velocity_trajectory, 0.5);
stop_watch.tic("resample");
TrajectoryPoints resampled_trajectory;
// skip points that are too close together to make computation easier
if (!input_trajectory_points.empty()) {
resampled_trajectory.push_back(input_trajectory_points.front());
constexpr auto min_interval_squared = 0.5 * 0.5; // TODO(Maxime): change to a parameter
for (auto i = 1UL; i < input_trajectory_points.size(); ++i) {
const auto & p = input_trajectory_points[i];
const auto dist_to_prev_point =
universe_utils::calcSquaredDistance2d(resampled_trajectory.back(), p);
if (dist_to_prev_point > min_interval_squared) {
resampled_trajectory.push_back(p);
}
}
}
processing_times["resample"] = stop_watch.toc("resample");
stop_watch.tic("calculate_time_from_start");
motion_utils::calculate_time_from_start(
resampled_trajectory.points, planner_data_.current_odometry.pose.pose.position);
resampled_trajectory, planner_data_.current_odometry.pose.pose.position);
processing_times["calculate_time_from_start"] = stop_watch.toc("calculate_time_from_start");
stop_watch.tic("plan_velocities");
const auto planning_results = planner_manager_.plan_velocities(
resampled_trajectory.points, std::make_shared<const PlannerData>(planner_data_));
resampled_trajectory, std::make_shared<const PlannerData>(planner_data_));
processing_times["plan_velocities"] = stop_watch.toc("plan_velocities");

autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors;
velocity_factors.header.frame_id = "map";
Expand Down
6 changes: 4 additions & 2 deletions planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <map>
#include <memory>
#include <mutex>
#include <string>
Expand Down Expand Up @@ -133,7 +134,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node
// function
/// @brief update the PlannerData instance with the latest messages received
/// @return false if some data is not available
bool update_planner_data();
bool update_planner_data(std::map<std::string, double> & processing_times);
void insert_stop(
autoware_planning_msgs::msg::Trajectory & trajectory,
const geometry_msgs::msg::Point & stop_point) const;
Expand All @@ -144,7 +145,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node
const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points,
const autoware::motion_velocity_planner::PlannerData & planner_data) const;
autoware_planning_msgs::msg::Trajectory generate_trajectory(
autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points);
autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points,
std::map<std::string, double> & processing_times);

std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
};
Expand Down

0 comments on commit f625fbb

Please sign in to comment.