Skip to content

Commit

Permalink
Merge pull request #9 from tier4/fix/cherry-pick-336
Browse files Browse the repository at this point in the history 
fix: changed pointcloud conversion method (autowarefoundation#336)
  • Loading branch information
@0x126
0x126 authored Feb 3, 2022
2 parents 08b5207 + a161d3d commit fe7b6f7
Show file tree
Hide file tree
Showing 2 changed files with 48 additions and 29 deletions.
25 changes: 9 additions & 16 deletions ...reprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,12 +61,16 @@
#include <vector>

// ROS includes
#include "autoware_point_types/types.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>

#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <tier4_debug_msgs/msg/int32_stamped.hpp>
#include <tier4_debug_msgs/msg/string_stamped.hpp>

Expand All @@ -75,12 +79,13 @@
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/synchronizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

namespace pointcloud_preprocessor
{
using autoware_point_types::PointXYZI;
using point_cloud_msg_wrapper::PointCloud2Modifier;
/** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data
* synchronizer: it listens for a set of input PointCloud messages on the same topic,
* checks their timestamps, and concatenates their fields together into a single
Expand Down Expand Up @@ -150,29 +155,17 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
void publish();

void convertToXYZICloud(
const sensor_msgs::msg::PointCloud2 & input_cloud,
sensor_msgs::msg::PointCloud2 & output_cloud);
const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr,
sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr);
void setPeriod(const int64_t new_period);
void cloud_callback(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr,
const std::string & topic_name);
const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name);
void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input);
void timer_callback();

void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
};

struct PointXYZI
{
PCL_ADD_POINT4D;
float intensity;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

} // namespace pointcloud_preprocessor

POINT_CLOUD_REGISTER_POINT_STRUCT(
pointcloud_preprocessor::PointXYZI,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity))

#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_
52 changes: 39 additions & 13 deletions sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -297,14 +297,42 @@ void PointCloudConcatenateDataSynchronizerComponent::publish()
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud(
const sensor_msgs::msg::PointCloud2 & input_cloud, sensor_msgs::msg::PointCloud2 & output_cloud)
const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr,
sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr)
{
pcl::PointCloud<PointXYZI> tmp_cloud;
pcl::fromROSMsg(input_cloud, tmp_cloud);
pcl::toROSMsg(tmp_cloud, output_cloud);
output_cloud.header = input_cloud.header;
output_ptr->header = input_ptr->header;
PointCloud2Modifier<PointXYZI> output_modifier{*output_ptr, input_ptr->header.frame_id};
output_modifier.reserve(input_ptr->width);

bool has_intensity = std::any_of(
input_ptr->fields.begin(), input_ptr->fields.end(),
[](auto & field) { return field.name == "intensity"; });

sensor_msgs::PointCloud2Iterator<float> it_x(*input_ptr, "x");
sensor_msgs::PointCloud2Iterator<float> it_y(*input_ptr, "y");
sensor_msgs::PointCloud2Iterator<float> it_z(*input_ptr, "z");

if (has_intensity) {
sensor_msgs::PointCloud2Iterator<float> it_i(*input_ptr, "intensity");
for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) {
PointXYZI point;
point.x = *it_x;
point.y = *it_y;
point.z = *it_z;
point.intensity = *it_i;
output_modifier.push_back(std::move(point));
}
} else {
for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) {
PointXYZI point;
point.x = *it_x;
point.y = *it_y;
point.z = *it_z;
point.intensity = 0.0f;
output_modifier.push_back(std::move(point));
}
}
}

void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new_period)
Expand All @@ -324,22 +352,20 @@ void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new
}

void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name)
const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name)
{
std::lock_guard<std::mutex> lock(mutex_);

sensor_msgs::msg::PointCloud2 xyz_cloud;
convertToXYZICloud(*input_ptr, xyz_cloud);
sensor_msgs::msg::PointCloud2::ConstSharedPtr xyz_input_ptr(
new sensor_msgs::msg::PointCloud2(xyz_cloud));
sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2());
convertToXYZICloud(input_ptr, 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_),
[](const auto & e) { return e.second != nullptr; });

if (is_already_subscribed_this) {
cloud_stdmap_tmp_[topic_name] = xyz_input_ptr;
cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr;

if (!is_already_subscribed_tmp) {
auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
Expand All @@ -352,7 +378,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
timer_->reset();
}
} else {
cloud_stdmap_[topic_name] = xyz_input_ptr;
cloud_stdmap_[topic_name] = xyzi_input_ptr;

const bool is_subscribed_all = std::all_of(
std::begin(cloud_stdmap_), std::end(cloud_stdmap_),
Expand Down

0 comments on commit fe7b6f7

Please sign in to comment.