127 : Node(
"scan_to_cloud")
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",
"");
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);
138 tf_timeout_ = declare_parameter<double>(
"tf_timeout", 0.1);
139 tf_cache_sec_ = declare_parameter<double>(
"tf_cache_sec", 5.0);
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);
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_);
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));
157 rclcpp::QoS pub_qos(5);
158 if (!reliable_output_) {
159 pub_qos.best_effort();
161 cloud_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>(cloud_topic_, pub_qos);
164 timer_ = create_wall_timer(
165 std::chrono::milliseconds(10), std::bind(&ScanToCloud::process_pending,
this));
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());
174 void on_servo_state(
const sensor_msgs::msg::JointState::ConstSharedPtr state)
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]))
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) {
187 servo_samples_.push_back(sample);
188 while (servo_samples_.size() > 2000) {
189 servo_samples_.pop_front();
193 void on_scan(
const sensor_msgs::msg::LaserScan::ConstSharedPtr 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(
202 static_cast<double>(scan->time_increment) *
static_cast<double>(scan->ranges.size()));
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");
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());
219 pending_.push_back(item);
224 void process_pending()
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;
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))
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();
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();
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();
282 convert_scan(item, src);
283 pending_.pop_front();
289 bool convert_scan(
const PendingScan & item,
const std::string & src)
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());
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;
306 geometry_msgs::msg::TransformStamped reference_transform;
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());
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) {
328 const double beam_time = item.t0 +
static_cast<double>(i) * time_increment;
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");
337 geometry_msgs::msg::TransformStamped fixed_from_servo;
339 fixed_from_servo = tf_buffer_->lookupTransform(
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());
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 =
356 const nodding_lidar::Vec3 point_servo =
358 const tf2::Vector3 point_fixed = apply_transform(fixed_from_servo, point_servo);
360 {point_fixed.x(), point_fixed.y(), point_fixed.z()},
361 has_intensity ? scan.intensities[i] : 0.0F});
364 if (points.empty()) {
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);
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);
379 modifier.setPointCloud2FieldsByString(1,
"xyz");
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;
387 intensity_iter = std::make_unique<sensor_msgs::PointCloud2Iterator<float>>(
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);
397 if (intensity_iter) {
398 **intensity_iter = point.intensity;
403 sensor_msgs::msg::PointCloud2 cloud_in_reference;
404 tf2::doTransform(cloud, cloud_in_reference, reference_transform);
405 cloud_pub_->publish(cloud_in_reference);
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};
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_;
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_;