diff --git a/launch/common.launch b/launch/common.launch
index ef3725e2..271548af 100644
--- a/launch/common.launch
+++ b/launch/common.launch
@@ -41,6 +41,12 @@
xyzir
}"/>
+
+
@@ -67,6 +74,7 @@
+
diff --git a/launch/driver.launch b/launch/driver.launch
index 0cc23f05..51699ca0 100644
--- a/launch/driver.launch
+++ b/launch/driver.launch
@@ -64,6 +64,13 @@
xyzir
}"/>
+
+
@@ -91,6 +98,7 @@
+
diff --git a/launch/record.launch b/launch/record.launch
index ab32e6b5..b509d01e 100644
--- a/launch/record.launch
+++ b/launch/record.launch
@@ -67,6 +67,13 @@
xyzir
}"/>
+
+
@@ -106,6 +113,7 @@
+
diff --git a/launch/replay.launch b/launch/replay.launch
index 85a85896..f51b22b0 100644
--- a/launch/replay.launch
+++ b/launch/replay.launch
@@ -47,6 +47,13 @@
xyzir
}"/>
+
+
@@ -81,6 +88,7 @@
+
diff --git a/launch/sensor.launch b/launch/sensor.launch
index 003fed6a..de282c43 100644
--- a/launch/sensor.launch
+++ b/launch/sensor.launch
@@ -72,6 +72,13 @@
xyzir
}"/>
+
+
@@ -111,6 +118,7 @@
+
diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch
index db05f5eb..ee9cbb49 100644
--- a/launch/sensor_mtp.launch
+++ b/launch/sensor_mtp.launch
@@ -76,6 +76,13 @@
xyzir
}"/>
+
+
+
+
diff --git a/src/image_processor.h b/src/image_processor.h
index 68a2d263..4c003030 100644
--- a/src/image_processor.h
+++ b/src/image_processor.h
@@ -25,8 +25,12 @@ namespace viz = ouster::viz;
class ImageProcessor {
public:
- using OutputType =
+ using ProcessedData =
std::map>;
+ struct OutputType {
+ int num_valid_columns;
+ ProcessedData image_msgs;
+ };
using PostProcessingFn = std::function;
public:
@@ -76,12 +80,17 @@ class ImageProcessor {
private:
void process(const ouster::LidarScan& lidar_scan, uint64_t,
const ros::Time& msg_ts) {
+ OutputType out;
process_return(lidar_scan, 0);
if (get_n_returns(info_) == 2) process_return(lidar_scan, 1);
for (auto it = image_msgs.begin(); it != image_msgs.end(); ++it) {
it->second->header.stamp = msg_ts;
}
- if (post_processing_fn) post_processing_fn(image_msgs);
+ out.image_msgs = image_msgs;
+ out.num_valid_columns =
+ (lidar_scan.measurement_id().array() != 0).count() + 1;
+
+ if (post_processing_fn) post_processing_fn(out);
}
void process_return(const ouster::LidarScan& lidar_scan, int return_index) {
@@ -188,7 +197,7 @@ class ImageProcessor {
private:
std::string frame;
- OutputType image_msgs;
+ ProcessedData image_msgs;
PostProcessingFn post_processing_fn;
sensor::sensor_info info_;
@@ -196,4 +205,4 @@ class ImageProcessor {
viz::BeamUniformityCorrector nearir_buc;
};
-} // namespace ouster_ros
\ No newline at end of file
+} // namespace ouster_ros
diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h
index ccd184e7..7ec0ce46 100644
--- a/src/laser_scan_processor.h
+++ b/src/laser_scan_processor.h
@@ -18,7 +18,11 @@ namespace ouster_ros {
class LaserScanProcessor {
public:
- using OutputType = std::vector>;
+ using ProcessedData = std::vector>;
+ struct OutputType {
+ int num_valid_columns;
+ ProcessedData scan_msgs;
+ };
using PostProcessingFn = std::function;
public:
@@ -35,12 +39,16 @@ class LaserScanProcessor {
private:
void process(const ouster::LidarScan& lidar_scan, uint64_t,
const ros::Time& msg_ts) {
+ OutputType out;
for (int i = 0; i < static_cast(scan_msgs.size()); ++i) {
*scan_msgs[i] = lidar_scan_to_laser_scan_msg(
lidar_scan, msg_ts, frame, ld_mode, ring_, i);
}
+ out.scan_msgs = scan_msgs;
+ out.num_valid_columns =
+ (lidar_scan.measurement_id().array() != 0).count() + 1;
- if (post_processing_fn) post_processing_fn(scan_msgs);
+ if (post_processing_fn) post_processing_fn(out);
}
public:
@@ -60,8 +68,8 @@ class LaserScanProcessor {
std::string frame;
sensor::lidar_mode ld_mode;
uint16_t ring_;
- OutputType scan_msgs;
+ ProcessedData scan_msgs;
PostProcessingFn post_processing_fn;
};
-} // namespace ouster_ros
\ No newline at end of file
+} // namespace ouster_ros
diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp
index c1f7f3f8..c673ca18 100644
--- a/src/os_cloud_nodelet.cpp
+++ b/src/os_cloud_nodelet.cpp
@@ -92,6 +92,7 @@ class OusterCloud : public nodelet::Nodelet {
auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
+ num_columns_required_ = pnh.param("num_columns_required", 0);
auto& nh = getNodeHandle();
@@ -124,11 +125,18 @@ class OusterCloud : public nodelet::Nodelet {
PointCloudProcessorFactory::create_point_cloud_processor(point_type,
info, tf_bcast.point_cloud_frame_id(),
tf_bcast.apply_lidar_to_sensor_transform(),
- [this](PointCloudProcessor_OutputType msgs) {
- for (size_t i = 0; i < msgs.size(); ++i) {
- if (msgs[i]->header.stamp > last_msg_ts)
- last_msg_ts = msgs[i]->header.stamp;
- lidar_pubs[i].publish(*msgs[i]);
+ [this](PointCloudProcessor_OutputType data) {
+ if (data.num_valid_columns < num_columns_required_) {
+ ROS_WARN_STREAM(
+ "Incomplete cloud, not publishing. Got "
+ << data.num_valid_columns << " columns, expected "
+ << num_columns_required_ << ".");
+ return;
+ }
+ for (size_t i = 0; i < data.pc_msgs.size(); ++i) {
+ if (data.pc_msgs[i]->header.stamp > last_msg_ts)
+ last_msg_ts = data.pc_msgs[i]->header.stamp;
+ lidar_pubs[i].publish(*data.pc_msgs[i]);
}
}
)
@@ -162,11 +170,11 @@ class OusterCloud : public nodelet::Nodelet {
processors.push_back(LaserScanProcessor::create(
info, tf_bcast.lidar_frame_id(), scan_ring,
- [this](LaserScanProcessor::OutputType msgs) {
- for (size_t i = 0; i < msgs.size(); ++i) {
- if (msgs[i]->header.stamp > last_msg_ts)
- last_msg_ts = msgs[i]->header.stamp;
- scan_pubs[i].publish(*msgs[i]);
+ [this](LaserScanProcessor::OutputType data) {
+ for (size_t i = 0; i < data.scan_msgs.size(); ++i) {
+ if (data.scan_msgs[i]->header.stamp > last_msg_ts)
+ last_msg_ts = data.scan_msgs[i]->header.stamp;
+ scan_pubs[i].publish(*data.scan_msgs[i]);
}
}));
}
@@ -197,6 +205,7 @@ class OusterCloud : public nodelet::Nodelet {
ros::Timer timer_;
ros::Time last_msg_ts;
+ int num_columns_required_;
};
} // namespace ouster_ros
diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp
index bfe84927..8bc1e0ae 100644
--- a/src/os_driver_nodelet.cpp
+++ b/src/os_driver_nodelet.cpp
@@ -57,6 +57,7 @@ class OusterDriver : public OusterSensor {
auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
+ num_columns_required_ = pnh.param("num_columns_required", 0);
auto& nh = getNodeHandle();
@@ -81,8 +82,16 @@ class OusterDriver : public OusterSensor {
processors.push_back(
PointCloudProcessorFactory::create_point_cloud_processor(point_type, info,
tf_bcast.point_cloud_frame_id(), tf_bcast.apply_lidar_to_sensor_transform(),
- [this](PointCloudProcessor_OutputType msgs) {
- for (size_t i = 0; i < msgs.size(); ++i) lidar_pubs[i].publish(*msgs[i]);
+ [this](PointCloudProcessor_OutputType data) {
+ if (data.num_valid_columns < num_columns_required_) {
+ ROS_WARN_STREAM(
+ "Incomplete cloud, not publishing. Got "
+ << data.num_valid_columns << " columns, expected "
+ << num_columns_required_ << ".");
+ return;
+ }
+ for (size_t i = 0; i < data.pc_msgs.size(); ++i)
+ lidar_pubs[i].publish(*data.pc_msgs[i]);
}
)
);
@@ -115,9 +124,11 @@ class OusterDriver : public OusterSensor {
processors.push_back(LaserScanProcessor::create(
info, tf_bcast.lidar_frame_id(), scan_ring,
- [this](LaserScanProcessor::OutputType msgs) {
- for (size_t i = 0; i < msgs.size(); ++i) {
- scan_pubs[i].publish(*msgs[i]);
+ [this](LaserScanProcessor::OutputType data) {
+ if (data.num_valid_columns < num_columns_required_)
+ return;
+ for (size_t i = 0; i < data.scan_msgs.size(); ++i) {
+ scan_pubs[i].publish(*data.scan_msgs[i]);
}
}));
}
@@ -149,8 +160,11 @@ class OusterDriver : public OusterSensor {
processors.push_back(ImageProcessor::create(
info, tf_bcast.point_cloud_frame_id(),
- [this](ImageProcessor::OutputType msgs) {
- for (auto it = msgs.begin(); it != msgs.end(); ++it) {
+ [this](ImageProcessor::OutputType data) {
+ if (data.num_valid_columns < num_columns_required_)
+ return;
+ for (auto it = data.image_msgs.begin();
+ it != data.image_msgs.end(); ++it) {
image_pubs[it->first].publish(*it->second);
}
}));
@@ -177,6 +191,7 @@ class OusterDriver : public OusterSensor {
std::vector lidar_pubs;
std::vector scan_pubs;
std::map image_pubs;
+ int num_columns_required_;
OusterTransformsBroadcaster tf_bcast;
diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp
index 736fae69..dde76bb4 100644
--- a/src/os_image_nodelet.cpp
+++ b/src/os_image_nodelet.cpp
@@ -50,12 +50,14 @@ class OusterImage : public nodelet::Nodelet {
}
void create_publishers_subscribers(int n_returns) {
- // TODO: avoid having to replicate the parameters:
+ // TODO: avoid having to replicate the parameters:
// timestamp_mode, ptp_utc_tai_offset, use_system_default_qos in yet
// another node.
auto& pnh = getPrivateNodeHandle();
auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
+ num_columns_required_ = pnh.param("num_columns_required", 0);
+
const std::map
channel_field_topic_map_1 {
@@ -87,8 +89,11 @@ class OusterImage : public nodelet::Nodelet {
std::vector processors {
ImageProcessor::create(
info, "os_lidar", /*TODO: tf_bcast.point_cloud_frame_id()*/
- [this](ImageProcessor::OutputType msgs) {
- for (auto it = msgs.begin(); it != msgs.end(); ++it) {
+ [this](ImageProcessor::OutputType data) {
+ if (data.num_valid_columns < num_columns_required_)
+ return;
+ for (auto it = data.image_msgs.begin();
+ it != data.image_msgs.end(); ++it) {
image_pubs[it->first].publish(*it->second);
}
})
@@ -107,6 +112,7 @@ class OusterImage : public nodelet::Nodelet {
private:
ros::Subscriber metadata_sub;
sensor::sensor_info info;
+ int num_columns_required_;
ros::Subscriber lidar_packet_sub;
std::map image_pubs;
diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h
index 593e8d7d..57688626 100644
--- a/src/point_cloud_processor.h
+++ b/src/point_cloud_processor.h
@@ -20,8 +20,12 @@
namespace ouster_ros {
// Moved out of PointCloudProcessor to avoid type templatization
-using PointCloudProcessor_OutputType =
+using ProcessedData =
std::vector>;
+struct PointCloudProcessor_OutputType {
+ int num_valid_columns;
+ ProcessedData pc_msgs;
+};
using PointCloudProcessor_PostProcessingFn = std::function;
@@ -75,6 +79,7 @@ class PointCloudProcessor {
void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts,
const ros::Time& msg_ts) {
+ PointCloudProcessor_OutputType out;
for (int i = 0; i < static_cast(pc_msgs.size()); ++i) {
auto range_channel = static_cast(sensor::ChanField::RANGE + i);
auto range = lidar_scan.field(range_channel);
@@ -87,8 +92,11 @@ class PointCloudProcessor {
pc_msgs[i]->header.stamp = msg_ts;
pc_msgs[i]->header.frame_id = frame;
}
+ out.pc_msgs = pc_msgs;
+ out.num_valid_columns =
+ (lidar_scan.measurement_id().array() != 0).count() + 1;
- if (post_processing_fn) post_processing_fn(pc_msgs);
+ if (post_processing_fn) post_processing_fn(out);
}
public:
@@ -118,9 +126,9 @@ class PointCloudProcessor {
ouster::PointsF points;
std::vector pixel_shift_by_row;
ouster_ros::Cloud cloud;
- PointCloudProcessor_OutputType pc_msgs;
+ ProcessedData pc_msgs;
ScanToCloudFn scan_to_cloud_fn;
PointCloudProcessor_PostProcessingFn post_processing_fn;
};
-} // namespace ouster_ros
\ No newline at end of file
+} // namespace ouster_ros