Skip to content

Commit

Permalink
feat(motion_velocity_planner, lane_departure_checker): add processing…
Browse files Browse the repository at this point in the history
… time Float64 publishers (autowarefoundation#7683)

Signed-off-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
maxime-clem committed Jul 1, 2024
1 parent 6b13edc commit 9e55740
Show file tree
Hide file tree
Showing 10 changed files with 54 additions and 19 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,9 @@ class LaneDepartureCheckerNode : public rclcpp::Node

// Publisher
autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"};
autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this};
autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{
this, "~/debug/processing_time_ms_diag"};
rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;

// Timer
rclcpp::TimerBase::SharedPtr timer_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -170,6 +170,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
lane_departure_checker_->setParam(param_, vehicle_info);

// Publisher
processing_time_publisher_ =
this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
// Nothing

// Diagnostic Updater
Expand Down Expand Up @@ -347,7 +349,11 @@ void LaneDepartureCheckerNode::onTimer()
}

processing_time_map["Total"] = stop_watch.toc("Total");
processing_time_publisher_.publish(processing_time_map);
processing_diag_publisher_.publish(processing_time_map);
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = get_clock()->now();
processing_time_msg.data = processing_time_map["Total"];
processing_time_publisher_->publish(processing_time_msg);
}

rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,10 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
virtual_wall_publisher_ =
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
&node, "~/debug/" + ns_ + "/processing_time_ms");
processing_diag_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
&node, "~/debug/" + ns_ + "/processing_time_ms_diag");
processing_time_publisher_ = node.create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
"~/debug/" + ns_ + "/processing_time_ms", 1);

using autoware::universe_utils::getOrDeclareParameter;
auto & p = params_;
Expand Down Expand Up @@ -187,7 +189,11 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
processing_times["footprints"] = footprints_duration_us / 1000;
processing_times["collisions"] = collisions_duration_us / 1000;
processing_times["Total"] = total_time_us / 1000;
processing_time_publisher_->publish(processing_times);
processing_diag_publisher_->publish(processing_times);
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = clock_->now();
processing_time_msg.data = processing_times["Total"];
processing_time_publisher_->publish(processing_time_msg);
return result;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,10 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
virtual_wall_publisher_ =
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
&node, "~/debug/" + ns_ + "/processing_time_ms");
processing_diag_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
&node, "~/debug/" + ns_ + "/processing_time_ms_diag");
processing_time_publisher_ = node.create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
"~/debug/" + ns_ + "/processing_time_ms", 1);

const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
vehicle_lateral_offset_ = static_cast<double>(vehicle_info.max_lateral_offset_m);
Expand Down Expand Up @@ -226,7 +228,11 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
processing_times["obstacles"] = obstacles_us / 1000;
processing_times["slowdowns"] = slowdowns_us / 1000;
processing_times["Total"] = total_us / 1000;
processing_time_publisher_->publish(processing_times);
processing_diag_publisher_->publish(processing_times);
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = clock_->now();
processing_time_msg.data = processing_times["Total"];
processing_time_publisher_->publish(processing_time_msg);
return result;
}
} // namespace autoware::motion_velocity_planner
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,10 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name)
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
virtual_wall_publisher_ =
node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
&node, "~/debug/" + ns_ + "/processing_time_ms");
processing_diag_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
&node, "~/debug/" + ns_ + "/processing_time_ms_diag");
processing_time_publisher_ = node.create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
"~/debug/" + ns_ + "/processing_time_ms", 1);
}
void OutOfLaneModule::init_parameters(rclcpp::Node & node)
{
Expand Down Expand Up @@ -306,7 +308,11 @@ VelocityPlanningResult OutOfLaneModule::plan(
processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000;
processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000;
processing_times["Total"] = total_time_us / 1000;
processing_time_publisher_->publish(processing_times);
processing_diag_publisher_->publish(processing_times);
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = clock_->now();
processing_time_msg.data = processing_times["Total"];
processing_time_publisher_->publish(processing_time_msg);
return result;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <rclcpp/rclcpp.hpp>

#include <autoware_planning_msgs/msg/trajectory_point.hpp>
#include <tier4_debug_msgs/msg/float64_stamped.hpp>

#include <memory>
#include <string>
Expand All @@ -45,7 +46,8 @@ class PluginModuleInterface
rclcpp::Logger logger_ = rclcpp::get_logger("");
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_publisher_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_publisher_;
std::shared_ptr<autoware::universe_utils::ProcessingTimePublisher> processing_time_publisher_;
std::shared_ptr<autoware::universe_utils::ProcessingTimePublisher> processing_diag_publisher_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{};
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
<depend>lanelet2_extension</depend>
<depend>libboost-dev</depend>
<depend>rclcpp</depend>
<depend>tier4_debug_msgs</depend>
<depend>tier4_planning_msgs</depend>
<depend>visualization_msgs</depend>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
<depend>tf2_eigen</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tier4_debug_msgs</depend>
<depend>tier4_planning_msgs</depend>
<depend>visualization_msgs</depend>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
velocity_factor_publisher_ =
this->create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
"~/output/velocity_factors", 1);
processing_time_publisher_ = this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>(
"~/debug/total_time/processing_time_ms", 1);

// Parameters
smooth_velocity_before_planning_ = declare_parameter<bool>("smooth_velocity_before_planning");
Expand All @@ -105,8 +107,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1));

logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
published_time_publisher_ =
std::make_unique<autoware::universe_utils::PublishedTimePublisher>(this);
}

void MotionVelocityPlannerNode::on_load_plugin(
Expand Down Expand Up @@ -284,10 +284,14 @@ void MotionVelocityPlannerNode::on_trajectory(
lk.unlock();

trajectory_pub_->publish(output_trajectory_msg);
published_time_publisher_->publish_if_subscribed(
published_time_publisher_.publish_if_subscribed(
trajectory_pub_, output_trajectory_msg.header.stamp);
processing_times["Total"] = stop_watch.toc("Total");
processing_time_publisher_.publish(processing_times);
processing_diag_publisher_.publish(processing_times);
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = get_clock()->now();
processing_time_msg.data = processing_times["Total"];
processing_time_publisher_->publish(processing_time_msg);
}

void MotionVelocityPlannerNode::insert_stop(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tier4_debug_msgs/msg/float64_stamped.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <tf2_ros/buffer.h>
Expand Down Expand Up @@ -96,7 +97,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
velocity_factor_publisher_;
autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this};
autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this};
rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this};

// parameters
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_;
Expand Down Expand Up @@ -140,8 +143,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node
autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points);

std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;

std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
};
} // namespace autoware::motion_velocity_planner

Expand Down

0 comments on commit 9e55740

Please sign in to comment.