nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
scan_to_cloud_node.cpp
[詳解]
1#include <algorithm>
2#include <chrono>
3#include <cmath>
4#include <deque>
5#include <functional>
6#include <memory>
7#include <string>
8#include <iterator>
9#include <utility>
10#include <vector>
11
12#include "builtin_interfaces/msg/time.hpp"
13#include "geometry_msgs/msg/transform_stamped.hpp"
14#include "rclcpp/rclcpp.hpp"
15#include "sensor_msgs/msg/laser_scan.hpp"
16#include "sensor_msgs/msg/point_cloud2.hpp"
17#include "sensor_msgs/msg/joint_state.hpp"
18#include "sensor_msgs/point_cloud2_iterator.hpp"
19#include "tf2/LinearMath/Transform.h"
20#include "tf2/time.h"
21#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
22#include "tf2_ros/buffer.h"
23#include "tf2_ros/transform_listener.h"
24
26
27namespace
28{
29
30struct PendingScan
31{
32 sensor_msgs::msg::LaserScan::ConstSharedPtr scan;
33 double t0; // スキャン開始時刻 [s]
34 double duration; // 推定したスキャン時間 [s]
35 std::chrono::steady_clock::time_point received;
36};
37
38struct ServoSample
39{
40 double stamp{0.0};
41 double position{0.0};
42 double velocity{0.0};
43};
44
45struct ProjectedPoint
46{
47 nodding_lidar::Vec3 position;
48 float intensity{0.0F};
49};
50
51bool interpolate_servo_angle(
52 const std::deque<ServoSample> & samples, double stamp, double & position)
53{
54 if (samples.empty() || stamp < samples.front().stamp || stamp > samples.back().stamp) {
55 return false;
56 }
57 if (stamp == samples.back().stamp) {
58 position = samples.back().position;
59 return true;
60 }
61
62 const auto right = std::upper_bound(
63 samples.begin(), samples.end(), stamp,
64 [](double t, const ServoSample & sample) {return t < sample.stamp;});
65 if (right == samples.begin() || right == samples.end()) {
66 return false;
67 }
68 const ServoSample & b = *right;
69 const ServoSample & a = *std::prev(right);
70 const double duration = b.stamp - a.stamp;
71 if (duration <= 0.0) {
72 return false;
73 }
74 const double u = (stamp - a.stamp) / duration;
75 const double u2 = u * u;
76 const double u3 = u2 * u;
77 const double h00 = 2.0 * u3 - 3.0 * u2 + 1.0;
78 const double h10 = u3 - 2.0 * u2 + u;
79 const double h01 = -2.0 * u3 + 3.0 * u2;
80 const double h11 = u3 - u2;
81 position = h00 * a.position + h10 * duration * a.velocity +
82 h01 * b.position + h11 * duration * b.velocity;
83 return std::isfinite(position);
84}
85
86tf2::Vector3 apply_transform(
87 const geometry_msgs::msg::TransformStamped & transform, const nodding_lidar::Vec3 & point)
88{
89 const auto & t = transform.transform.translation;
90 const auto & r = transform.transform.rotation;
91 const tf2::Transform tf(
92 tf2::Quaternion(r.x, r.y, r.z, r.w), tf2::Vector3(t.x, t.y, t.z));
93 return tf * tf2::Vector3(point.x, point.y, point.z);
94}
95
96double to_sec(const builtin_interfaces::msg::Time & t)
97{
98 return static_cast<double>(t.sec) + static_cast<double>(t.nanosec) * 1e-9;
99}
100
101builtin_interfaces::msg::Time to_msg_time(double sec)
102{
103 const int64_t nanoseconds = static_cast<int64_t>(sec * 1e9);
104 int64_t seconds = nanoseconds / 1000000000LL;
105 int64_t remainder = nanoseconds % 1000000000LL;
106 if (remainder < 0) {
107 --seconds;
108 remainder += 1000000000LL;
109 }
110 builtin_interfaces::msg::Time stamp;
111 stamp.sec = static_cast<int32_t>(seconds);
112 stamp.nanosec = static_cast<uint32_t>(remainder);
113 return stamp;
114}
115
116tf2::TimePoint to_tf_time(double sec)
117{
118 return tf2::TimePoint(std::chrono::nanoseconds(static_cast<int64_t>(sec * 1e9)));
119}
120
121} // namespace
122
123class ScanToCloud : public rclcpp::Node
124{
125public:
127 : Node("scan_to_cloud")
128 {
129 scan_topic_ = declare_parameter<std::string>("scan_topic", "/scan");
130 cloud_topic_ = declare_parameter<std::string>("cloud_topic", "/points");
131 output_frame_ = declare_parameter<std::string>("output_frame", "base_link");
132 fixed_frame_ = declare_parameter<std::string>("fixed_frame", "odom");
133 lidar_frame_ = declare_parameter<std::string>("lidar_frame", ""); // 空なら /scan の frame_id
134 servo_state_topic_ = declare_parameter<std::string>("servo_state_topic", "/servo_state");
135 servo_parent_frame_ = declare_parameter<std::string>("servo_parent_frame", "servo_link");
136 lidar_height_ = declare_parameter<double>("lidar_height", 0.0325);
137
138 tf_timeout_ = declare_parameter<double>("tf_timeout", 0.1);
139 tf_cache_sec_ = declare_parameter<double>("tf_cache_sec", 5.0);
140
141 default_scan_time_ = declare_parameter<double>("default_scan_time", 0.1);
142 stamp_is_scan_end_ = declare_parameter<bool>("stamp_is_scan_end", false);
143 reverse_time_order_ = declare_parameter<bool>("reverse_time_order", false);
144 reliable_output_ = declare_parameter<bool>("reliable_output", true);
145
146 tf_buffer_ = std::make_unique<tf2_ros::Buffer>(
147 get_clock(), tf2::durationFromSec(tf_cache_sec_));
148 tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
149
150 scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
151 scan_topic_, rclcpp::SensorDataQoS(),
152 std::bind(&ScanToCloud::on_scan, this, std::placeholders::_1));
153 servo_sub_ = create_subscription<sensor_msgs::msg::JointState>(
154 servo_state_topic_, rclcpp::QoS(100),
155 std::bind(&ScanToCloud::on_servo_state, this, std::placeholders::_1));
156
157 rclcpp::QoS pub_qos(5);
158 if (!reliable_output_) {
159 pub_qos.best_effort();
160 }
161 cloud_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>(cloud_topic_, pub_qos);
162
163 // TFの到着待ちのスキャンを、定期的に処理する
164 timer_ = create_wall_timer(
165 std::chrono::milliseconds(10), std::bind(&ScanToCloud::process_pending, this));
166
167 RCLCPP_INFO(
168 get_logger(),
169 "scan_to_cloud started: %s -> %s (frame=%s, one cloud/scan, Hermite servo interpolation)",
170 scan_topic_.c_str(), cloud_topic_.c_str(), output_frame_.c_str());
171 }
172
173private:
174 void on_servo_state(const sensor_msgs::msg::JointState::ConstSharedPtr state)
175 {
176 if ((state->header.stamp.sec == 0 && state->header.stamp.nanosec == 0) ||
177 state->position.empty() || state->velocity.empty() ||
178 !std::isfinite(state->position[0]) || !std::isfinite(state->velocity[0]))
179 {
180 return;
181 }
182 const ServoSample sample{
183 to_sec(state->header.stamp), state->position[0], state->velocity[0]};
184 if (!servo_samples_.empty() && sample.stamp <= servo_samples_.back().stamp) {
185 return;
186 }
187 servo_samples_.push_back(sample);
188 while (servo_samples_.size() > 2000) {
189 servo_samples_.pop_front();
190 }
191 }
192
193 void on_scan(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
194 {
195 PendingScan item;
196 item.scan = scan;
197 item.duration = std::isfinite(scan->scan_time) && scan->scan_time > 0.0 ?
198 static_cast<double>(scan->scan_time) : default_scan_time_;
199 if (std::isfinite(scan->time_increment) && scan->time_increment > 0.0F) {
200 item.duration = std::max(
201 item.duration,
202 static_cast<double>(scan->time_increment) * static_cast<double>(scan->ranges.size()));
203 }
204 const double stamp = to_sec(scan->header.stamp);
205 item.t0 = stamp_is_scan_end_ ? stamp - item.duration : stamp;
206 item.received = std::chrono::steady_clock::now();
207 if (scan->ranges.empty() || scan->header.frame_id.empty()) {
208 RCLCPP_WARN_THROTTLE(
209 get_logger(), *get_clock(), 2000, "Ignoring LaserScan with no ranges or frame_id");
210 return;
211 }
212 if (!lidar_frame_.empty() && scan->header.frame_id != lidar_frame_) {
213 RCLCPP_WARN_THROTTLE(
214 get_logger(), *get_clock(), 2000,
215 "Ignoring LaserScan in frame '%s'; expected lidar frame '%s'",
216 scan->header.frame_id.c_str(), lidar_frame_.c_str());
217 return;
218 }
219 pending_.push_back(item);
220 process_pending();
221 }
222
223 // TFが届いたスキャンから順に変換する
224 void process_pending()
225 {
226 while (!pending_.empty()) {
227 const PendingScan & item = pending_.front();
228 const std::string src = servo_parent_frame_;
229 const double t_end = item.t0 + item.duration;
230
231 const double t_last_beam = item.t0 +
232 (item.scan->ranges.size() > 1 ?
233 static_cast<double>(item.scan->ranges.size() - 1) *
234 (item.scan->time_increment > 0.0F ? item.scan->time_increment :
235 item.duration / static_cast<double>(item.scan->ranges.size())) : 0.0);
236 double ignored_angle = 0.0;
237 if (!interpolate_servo_angle(servo_samples_, item.t0, ignored_angle) ||
238 !interpolate_servo_angle(servo_samples_, t_last_beam, ignored_angle))
239 {
240 const double waited = std::chrono::duration<double>(
241 std::chrono::steady_clock::now() - item.received).count();
242 if (waited > tf_timeout_) {
243 RCLCPP_WARN_THROTTLE(
244 get_logger(), *get_clock(), 2000,
245 "Dropping scan: servo angle/velocity samples do not cover scan interval");
246 pending_.pop_front();
247 continue;
248 }
249 break;
250 }
251
252 std::string err;
253 if (!tf_buffer_->canTransform(fixed_frame_, src, to_tf_time(t_end), &err)) {
254 const double waited = std::chrono::duration<double>(
255 std::chrono::steady_clock::now() - item.received).count();
256 if (waited > tf_timeout_) {
257 RCLCPP_WARN_THROTTLE(
258 get_logger(), *get_clock(), 2000,
259 "Dropping scan: TF unavailable (%s <- %s) after %.2f s: %s",
260 fixed_frame_.c_str(), src.c_str(), waited, err.c_str());
261 pending_.pop_front();
262 continue;
263 }
264 break;
265 }
266
267 std::string err0;
268 if (!tf_buffer_->canTransform(fixed_frame_, src, to_tf_time(item.t0), &err0)) {
269 const double waited = std::chrono::duration<double>(
270 std::chrono::steady_clock::now() - item.received).count();
271 if (waited > tf_timeout_) {
272 RCLCPP_WARN_THROTTLE(
273 get_logger(), *get_clock(), 2000,
274 "Dropping scan: start-time TF unavailable (%s <- %s): %s",
275 fixed_frame_.c_str(), src.c_str(), err0.c_str());
276 pending_.pop_front();
277 continue;
278 }
279 break;
280 }
281
282 convert_scan(item, src);
283 pending_.pop_front();
284 }
285 }
286
287 // Reconstruct each beam pose from angle/velocity samples, then apply the
288 // robot pose at that beam's acquisition time.
289 bool convert_scan(const PendingScan & item, const std::string & src)
290 {
291 sensor_msgs::msg::LaserScan scan = *item.scan;
292 double time_increment = scan.time_increment;
293 if (!std::isfinite(time_increment) || time_increment <= 0.0) {
294 time_increment = item.duration / static_cast<double>(scan.ranges.size());
295 }
296 if (reverse_time_order_) {
297 std::reverse(scan.ranges.begin(), scan.ranges.end());
298 std::reverse(scan.intensities.begin(), scan.intensities.end());
299 scan.angle_min += static_cast<float>(scan.ranges.size() - 1) * scan.angle_increment;
300 scan.angle_max = scan.angle_min + static_cast<float>(scan.ranges.size() - 1) *
301 (-scan.angle_increment);
302 scan.angle_increment = -scan.angle_increment;
303 }
304
305 // Express all beams in the robot frame at this scan's start time.
306 geometry_msgs::msg::TransformStamped reference_transform;
307 try {
308 reference_transform = tf_buffer_->lookupTransform(
309 output_frame_, fixed_frame_,
310 rclcpp::Time(to_msg_time(item.t0), get_clock()->get_clock_type()));
311 } catch (const tf2::TransformException & e) {
312 RCLCPP_WARN_THROTTLE(
313 get_logger(), *get_clock(), 2000,
314 "Dropping scan: cannot get %s <- %s at reference time: %s",
315 output_frame_.c_str(), fixed_frame_.c_str(), e.what());
316 return false;
317 }
318
319 const bool has_intensity = scan.intensities.size() == scan.ranges.size();
320 std::vector<ProjectedPoint> points;
321 points.reserve(scan.ranges.size());
322 const rclcpp::Clock::SharedPtr clock = get_clock();
323 for (size_t i = 0; i < scan.ranges.size(); ++i) {
324 const double range = scan.ranges[i];
325 if (!std::isfinite(range) || range < scan.range_min || range > scan.range_max) {
326 continue;
327 }
328 const double beam_time = item.t0 + static_cast<double>(i) * time_increment;
329 double angle = 0.0;
330 if (!interpolate_servo_angle(servo_samples_, beam_time, angle)) {
331 RCLCPP_WARN_THROTTLE(
332 get_logger(), *get_clock(), 2000,
333 "Dropping scan: no servo angle interpolation at beam time");
334 return false;
335 }
336
337 geometry_msgs::msg::TransformStamped fixed_from_servo;
338 try {
339 fixed_from_servo = tf_buffer_->lookupTransform(
340 fixed_frame_, src,
341 rclcpp::Time(to_msg_time(beam_time), clock->get_clock_type()));
342 } catch (const tf2::TransformException & e) {
343 RCLCPP_WARN_THROTTLE(
344 get_logger(), *get_clock(), 2000,
345 "Dropping scan: cannot get %s <- %s at beam time: %s",
346 fixed_frame_.c_str(), src.c_str(), e.what());
347 return false;
348 }
349
350 const double scan_angle = scan.angle_min +
351 static_cast<double>(i) * scan.angle_increment;
352 const nodding_lidar::Vec3 point_lidar{
353 range * std::cos(scan_angle), range * std::sin(scan_angle), 0.0};
354 const nodding_lidar::Pose lidar_pose =
355 nodding_lidar::servo_to_lidar_pose(angle, lidar_height_);
356 const nodding_lidar::Vec3 point_servo =
357 nodding_lidar::apply_pose(lidar_pose, point_lidar);
358 const tf2::Vector3 point_fixed = apply_transform(fixed_from_servo, point_servo);
359 points.push_back({
360 {point_fixed.x(), point_fixed.y(), point_fixed.z()},
361 has_intensity ? scan.intensities[i] : 0.0F});
362 }
363
364 if (points.empty()) {
365 return true;
366 }
367
368 sensor_msgs::msg::PointCloud2 cloud;
369 cloud.header.frame_id = fixed_frame_;
370 cloud.header.stamp = to_msg_time(item.t0);
371 sensor_msgs::PointCloud2Modifier modifier(cloud);
372 if (has_intensity) {
373 modifier.setPointCloud2Fields(
374 4, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
375 "y", 1, sensor_msgs::msg::PointField::FLOAT32,
376 "z", 1, sensor_msgs::msg::PointField::FLOAT32,
377 "intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
378 } else {
379 modifier.setPointCloud2FieldsByString(1, "xyz");
380 }
381 modifier.resize(points.size());
382 sensor_msgs::PointCloud2Iterator<float> x_iter(cloud, "x");
383 sensor_msgs::PointCloud2Iterator<float> y_iter(cloud, "y");
384 sensor_msgs::PointCloud2Iterator<float> z_iter(cloud, "z");
385 std::unique_ptr<sensor_msgs::PointCloud2Iterator<float>> intensity_iter;
386 if (has_intensity) {
387 intensity_iter = std::make_unique<sensor_msgs::PointCloud2Iterator<float>>(
388 cloud, "intensity");
389 }
390 for (const auto & point : points) {
391 *x_iter = static_cast<float>(point.position.x);
392 *y_iter = static_cast<float>(point.position.y);
393 *z_iter = static_cast<float>(point.position.z);
394 ++x_iter;
395 ++y_iter;
396 ++z_iter;
397 if (intensity_iter) {
398 **intensity_iter = point.intensity;
399 ++(*intensity_iter);
400 }
401 }
402
403 sensor_msgs::msg::PointCloud2 cloud_in_reference;
404 tf2::doTransform(cloud, cloud_in_reference, reference_transform);
405 cloud_pub_->publish(cloud_in_reference);
406 return true;
407 }
408
409 // パラメータ
410 std::string scan_topic_, cloud_topic_, output_frame_, fixed_frame_, lidar_frame_;
411 std::string servo_state_topic_, servo_parent_frame_;
412 double tf_timeout_{0.1};
413 double tf_cache_sec_{5.0};
414 double default_scan_time_{0.1};
415 double lidar_height_{0.0325};
416 bool stamp_is_scan_end_{false};
417 bool reverse_time_order_{false};
418 bool reliable_output_{true};
419
420 // 状態
421 std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
422 std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
423 std::deque<PendingScan> pending_;
424 std::deque<ServoSample> servo_samples_;
425
426 rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
427 rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr servo_sub_;
428 rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
429 rclcpp::TimerBase::SharedPtr timer_;
430};
431
432int main(int argc, char ** argv)
433{
434 rclcpp::init(argc, argv);
435 rclcpp::spin(std::make_shared<ScanToCloud>());
436 rclcpp::shutdown();
437 return 0;
438}
首振り角度と取付寸法を使い、LiDAR点を別の座標系へ変換する。
Vec3 apply_pose(const Pose &p, const Vec3 &v)
Pose servo_to_lidar_pose(double phi, double lidar_height)
int main(int argc, char **argv)