@@ -23,7 +23,7 @@ namespace stereolabs
23
23
24
24
PointCloudComponent::PointCloudComponent (const rclcpp::NodeOptions & options)
25
25
: Node(" pointcloud_node" , options)
26
- , _qos(1 )
26
+ , _qos(1 )
27
27
{
28
28
RCLCPP_INFO (get_logger (), " ********************************" );
29
29
RCLCPP_INFO (get_logger (), " Point Cloud Sub Component " );
@@ -44,12 +44,12 @@ PointCloudComponent::PointCloudComponent(const rclcpp::NodeOptions & options)
44
44
readParameters ();
45
45
46
46
_stats.resize (_camCount);
47
- for (int i = 0 ; i < _camCount; i++) {
47
+ for (int i = 0 ; i < _camCount; i++) {
48
48
_stats[i] = std::make_unique<WinAvg>(500 );
49
49
}
50
50
_times.resize (_camCount);
51
- _firsts.resize (_camCount,false );
52
- _counters.resize (_camCount,0 );
51
+ _firsts.resize (_camCount, false );
52
+ _counters.resize (_camCount, 0 );
53
53
54
54
createSubscribers ();
55
55
}
@@ -90,7 +90,7 @@ void PointCloudComponent::createSubscribers()
90
90
RCLCPP_INFO_STREAM (get_logger (), " * Subscribing to topic: " << topic_name);
91
91
92
92
std::function<void (const sensor_msgs::msg::PointCloud2::SharedPtr msg)> bound_callback_func =
93
- std::bind (&PointCloudComponent::callback_pointcloud, this , _1, topic_name);
93
+ std::bind (&PointCloudComponent::callback_pointcloud, this , _1, topic_name);
94
94
95
95
_qos.reliability (RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
96
96
@@ -122,7 +122,7 @@ void PointCloudComponent::callback_pointcloud(
122
122
if (_firsts[idx]) {
123
123
_times[idx] =
124
124
std::chrono::high_resolution_clock::now (); // Set the start time point
125
- _firsts[idx] = false ;
125
+ _firsts[idx] = false ;
126
126
return ;
127
127
}
128
128
@@ -135,8 +135,9 @@ void PointCloudComponent::callback_pointcloud(
135
135
double freq = 1e6 / elapsed_usec;
136
136
double avg_freq = _stats[idx]->addValue (freq);
137
137
138
- if (freq> 60.0 )
138
+ if (freq > 60.0 ) {
139
139
return ;
140
+ }
140
141
141
142
static double bw_scale = 8 . / (1024 . * 1024 .);
142
143
@@ -146,9 +147,9 @@ void PointCloudComponent::callback_pointcloud(
146
147
147
148
std::stringstream ss;
148
149
ss << std::fixed << std::setprecision (2 ) << " #"
149
- << ++_counters[idx] << " - Freq: " << freq << " Hz (Avg: " << avg_freq
150
- << " Hz) - BW: " << bw << " Mbps (Avg: " << bw_avg
151
- << " Mbps) - Msg size: " << data_size / (1024 . * 1024 .) << " MB" ;
150
+ << ++_counters[idx] << " - Freq: " << freq << " Hz (Avg: " << avg_freq
151
+ << " Hz) - BW: " << bw << " Mbps (Avg: " << bw_avg
152
+ << " Mbps) - Msg size: " << data_size / (1024 . * 1024 .) << " MB" ;
152
153
153
154
RCLCPP_INFO_STREAM (get_logger (), ss.str ());
154
155
// <---- Calculate statistics
0 commit comments