Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(pointcloud_preprocessor): enable to count empty pointclouds topic in concatenate pointclouds nodelet #6277

Merged
merged 2 commits into from
Feb 6, 2024
Merged

feat(pointcloud_preprocessor): enable to count empty pointclouds topic in concatenate pointclouds nodelet #6277

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6ce2a74
feat: enable to count empty pointclouds topic
YoshiRi Feb 1, 2024
dbd8bcb
fix: time sync function to adapt empty pc topic
YoshiRi Feb 1, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 10 additions & 4 deletions sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
View file
Original file line number Diff line number Diff line change
@@ -318,6 +318,9 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
for (const auto & e : cloud_stdmap_) {
transformed_clouds[e.first] = nullptr;
if (e.second != nullptr) {
if (e.second->data.size() == 0) {
continue;
}
pc_stamps.push_back(rclcpp::Time(e.second->header.stamp));
}
}
@@ -332,6 +335,9 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
// Step2. Calculate compensation transform and concatenate with the oldest stamp
for (const auto & e : cloud_stdmap_) {
if (e.second != nullptr) {
if (e.second->data.size() == 0) {
continue;
}
sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr(
new sensor_msgs::msg::PointCloud2());
transformPointCloud(e.second, transformed_cloud_ptr);
@@ -479,16 +485,16 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name)
{
std::lock_guard<std::mutex> lock(mutex_);
sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2());
auto input = std::make_shared<sensor_msgs::msg::PointCloud2>(*input_ptr);
if (input->data.empty()) {
RCLCPP_WARN_STREAM_THROTTLE(
this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
return;
} else {
// convert to XYZI pointcloud if pointcloud is not empty
convertToXYZICloud(input, xyzi_input_ptr);
}

sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2());
convertToXYZICloud(input, xyzi_input_ptr);

const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr);
const bool is_already_subscribed_tmp = std::any_of(
std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_),
24 changes: 20 additions & 4 deletions sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp
View file
Original file line number Diff line number Diff line change
@@ -274,6 +274,9 @@ PointCloudDataSynchronizerComponent::synchronizeClouds()
for (const auto & e : cloud_stdmap_) {
transformed_clouds[e.first] = nullptr;
if (e.second != nullptr) {
if (e.second->data.size() == 0) {
continue;
}
pc_stamps.push_back(rclcpp::Time(e.second->header.stamp));
}
}
@@ -288,11 +291,22 @@ PointCloudDataSynchronizerComponent::synchronizeClouds()
// Step2. Calculate compensation transform and concatenate with the oldest stamp
for (const auto & e : cloud_stdmap_) {
if (e.second != nullptr) {
// transform pointcloud
sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr(
new sensor_msgs::msg::PointCloud2());
sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr(
new sensor_msgs::msg::PointCloud2());
if (e.second->data.size() == 0) {
// gather transformed clouds
transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp;
transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;
transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr;
continue;
}
// transform pointcloud to output frame
transformPointCloud(e.second, transformed_cloud_ptr);

// calculate transforms to oldest stamp
// calculate transforms to oldest stamp and transform pointcloud to oldest stamp
Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity();
rclcpp::Time transformed_stamp = rclcpp::Time(e.second->header.stamp);
for (const auto & stamp : pc_stamps) {
@@ -301,15 +315,15 @@ PointCloudDataSynchronizerComponent::synchronizeClouds()
adjust_to_old_data_transform = new_to_old_transform * adjust_to_old_data_transform;
transformed_stamp = std::min(transformed_stamp, stamp);
}
sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr(
new sensor_msgs::msg::PointCloud2());
pcl_ros::transformPointCloud(
adjust_to_old_data_transform, *transformed_cloud_ptr,
*transformed_delay_compensated_cloud_ptr);

// gather transformed clouds
transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp;
transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;
transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr;

} else {
not_subscribed_topic_names_.insert(e.first);
}
@@ -414,7 +428,9 @@ void PointCloudDataSynchronizerComponent::cloud_callback(
std::lock_guard<std::mutex> lock(mutex_);
auto input = std::make_shared<sensor_msgs::msg::PointCloud2>(*input_ptr);
sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2());
convertToXYZICloud(input, xyzi_input_ptr);
if (input->data.size() > 0) {
convertToXYZICloud(input, xyzi_input_ptr);
}

const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr);
const bool is_already_subscribed_tmp = std::any_of(