From df0b9ef85a58006e805f81d8e08e342f088097f0 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 28 Feb 2024 12:14:36 +0900 Subject: [PATCH] refactor: remove timer Signed-off-by: kosuke55 --- .../perception_online_evaluator_node.hpp | 8 ++----- .../src/perception_online_evaluator_node.cpp | 24 ++++++------------- 2 files changed, 9 insertions(+), 23 deletions(-) diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index 318148f14b24e..e6317f92179fe 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -50,7 +50,7 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node { public: explicit PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options); - ~PerceptionOnlineEvaluatorNode(); + ~PerceptionOnlineEvaluatorNode() {}; /** * @brief callback on receiving a dynamic objects array @@ -62,11 +62,6 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node const std::string metric, const Stat & metric_stat) const; private: - // Timer - rclcpp::TimerBase::SharedPtr timer_; - void initTimer(double period_s); - void onTimer(); - // Subscribers and publishers rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Publisher::SharedPtr metrics_pub_; @@ -86,6 +81,7 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node // Metrics Calculator MetricsCalculator metrics_calculator_; std::deque stamps_; + void publishMetrics(); // Debug void publishDebugMarker(); diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 48e80f487dd3a..e86d1b4217f78 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -35,7 +35,8 @@ namespace perception_diagnostics { -PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options) +PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( + const rclcpp::NodeOptions & node_options) : Node("perception_online_evaluator", node_options), parameters_(std::make_shared()), metrics_calculator_(parameters_) @@ -58,26 +59,13 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(const rclcpp::NodeO set_param_res_ = this->add_on_set_parameters_callback( std::bind(&PerceptionOnlineEvaluatorNode::onParameter, this, std::placeholders::_1)); - - // Timer - initTimer(/*period_s=*/0.1); -} - -PerceptionOnlineEvaluatorNode::~PerceptionOnlineEvaluatorNode() -{ } -void PerceptionOnlineEvaluatorNode::initTimer(double period_s) -{ - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&PerceptionOnlineEvaluatorNode::onTimer, this)); -} - -void PerceptionOnlineEvaluatorNode::onTimer() +void PerceptionOnlineEvaluatorNode::publishMetrics() { DiagnosticArray metrics_msg; + + // calculate metrics for (const Metric & metric : parameters_->metrics) { const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); if (!metric_stat_map.has_value()) { @@ -91,6 +79,7 @@ void PerceptionOnlineEvaluatorNode::onTimer() } } + // publish metrics if (!metrics_msg.status.empty()) { metrics_msg.header.stamp = now(); metrics_pub_->publish(metrics_msg); @@ -123,6 +112,7 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg); + publishMetrics(); } void PerceptionOnlineEvaluatorNode::publishDebugMarker()