Skip to content

Commit 1762aa1

Browse files
committed
refactor: remove timer
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent e70dd88 commit 1762aa1

File tree

2 files changed

+9
-23
lines changed

2 files changed

+9
-23
lines changed

evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp

+2-6
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node
5050
{
5151
public:
5252
explicit PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options);
53-
~PerceptionOnlineEvaluatorNode();
53+
~PerceptionOnlineEvaluatorNode() {};
5454

5555
/**
5656
* @brief callback on receiving a dynamic objects array
@@ -62,11 +62,6 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node
6262
const std::string metric, const Stat<double> & metric_stat) const;
6363

6464
private:
65-
// Timer
66-
rclcpp::TimerBase::SharedPtr timer_;
67-
void initTimer(double period_s);
68-
void onTimer();
69-
7065
// Subscribers and publishers
7166
rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
7267
rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
@@ -86,6 +81,7 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node
8681
// Metrics Calculator
8782
MetricsCalculator metrics_calculator_;
8883
std::deque<rclcpp::Time> stamps_;
84+
void publishMetrics();
8985

9086
// Debug
9187
void publishDebugMarker();

evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp

+7-17
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,8 @@
3535

3636
namespace perception_diagnostics
3737
{
38-
PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options)
38+
PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(
39+
const rclcpp::NodeOptions & node_options)
3940
: Node("perception_online_evaluator", node_options),
4041
parameters_(std::make_shared<Parameters>()),
4142
metrics_calculator_(parameters_)
@@ -58,26 +59,13 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(const rclcpp::NodeO
5859

5960
set_param_res_ = this->add_on_set_parameters_callback(
6061
std::bind(&PerceptionOnlineEvaluatorNode::onParameter, this, std::placeholders::_1));
61-
62-
// Timer
63-
initTimer(/*period_s=*/0.1);
64-
}
65-
66-
PerceptionOnlineEvaluatorNode::~PerceptionOnlineEvaluatorNode()
67-
{
6862
}
6963

70-
void PerceptionOnlineEvaluatorNode::initTimer(double period_s)
71-
{
72-
const auto period_ns =
73-
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
74-
timer_ = rclcpp::create_timer(
75-
this, get_clock(), period_ns, std::bind(&PerceptionOnlineEvaluatorNode::onTimer, this));
76-
}
77-
78-
void PerceptionOnlineEvaluatorNode::onTimer()
64+
void PerceptionOnlineEvaluatorNode::publishMetrics()
7965
{
8066
DiagnosticArray metrics_msg;
67+
68+
// calculate metrics
8169
for (const Metric & metric : parameters_->metrics) {
8270
const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric));
8371
if (!metric_stat_map.has_value()) {
@@ -91,6 +79,7 @@ void PerceptionOnlineEvaluatorNode::onTimer()
9179
}
9280
}
9381

82+
// publish metrics
9483
if (!metrics_msg.status.empty()) {
9584
metrics_msg.header.stamp = now();
9685
metrics_pub_->publish(metrics_msg);
@@ -123,6 +112,7 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus(
123112
void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg)
124113
{
125114
metrics_calculator_.setPredictedObjects(*objects_msg);
115+
publishMetrics();
126116
}
127117

128118
void PerceptionOnlineEvaluatorNode::publishDebugMarker()

0 commit comments

Comments
 (0)