Skip to content

Commit

Permalink
updated point type in point_cloud_fusion and point_cloud_fusion_nodes…
Browse files Browse the repository at this point in the history
… include
  • Loading branch information
JonSmet committed Mar 28, 2024
1 parent 71c63d8 commit 553f517
Show file tree
Hide file tree
Showing 3 changed files with 9 additions and 9 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ namespace filters
{
namespace point_cloud_fusion
{
using autoware::common::types::PointXYZI;
using autoware::common::types::PointXYZIRing;

class POINT_CLOUD_FUSION_PUBLIC PointCloudFusion
{
Expand Down Expand Up @@ -67,7 +67,7 @@ class POINT_CLOUD_FUSION_PUBLIC PointCloudFusion
void concatenate_pointcloud(
const PointCloudMsgT & pc_in,
uint32_t & concat_idx,
point_cloud_msg_wrapper::PointCloud2Modifier<PointXYZI> & modifier) const;
point_cloud_msg_wrapper::PointCloud2Modifier<PointXYZIRing> & modifier) const;

uint32_t m_cloud_capacity;
size_t m_input_topics_size;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@ uint32_t PointCloudFusion::fuse_pc_msgs(
{
uint32_t pc_concat_idx = 0;

using autoware::common::types::PointXYZI;
point_cloud_msg_wrapper::PointCloud2Modifier<PointXYZI> modifier{cloud_concatenated};
using autoware::common::types::PointXYZIRing;
point_cloud_msg_wrapper::PointCloud2Modifier<PointXYZIRing> modifier{cloud_concatenated};

for (size_t i = 0; i < m_input_topics_size; ++i) {
concatenate_pointcloud(*msgs[i], pc_concat_idx, modifier);
Expand All @@ -54,18 +54,18 @@ uint32_t PointCloudFusion::fuse_pc_msgs(
void PointCloudFusion::concatenate_pointcloud(
const sensor_msgs::msg::PointCloud2 & pc_in,
uint32_t & concat_idx,
point_cloud_msg_wrapper::PointCloud2Modifier<PointXYZI> & modifier) const
point_cloud_msg_wrapper::PointCloud2Modifier<PointXYZIRing> & modifier) const
{
if ((pc_in.width + concat_idx) > m_cloud_capacity) {
throw Error::TOO_LARGE;
}

using autoware::common::types::PointXYZI;
point_cloud_msg_wrapper::PointCloud2View<PointXYZI> view{pc_in};
using autoware::common::types::PointXYZIRing;
point_cloud_msg_wrapper::PointCloud2View<PointXYZIRing> view{pc_in};

auto view_it = view.cbegin();
while (view_it != view.cend()) {
common::types::PointXYZI pt;
common::types::PointXYZIRing pt;
pt.x = (*view_it).x;
pt.y = (*view_it).y;
pt.z = (*view_it).z;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ class POINT_CLOUD_FUSION_NODES_PUBLIC PointCloudFusionNode : public rclcpp::Node
const rclcpp::NodeOptions & node_options);

private:
using PointT = common::types::PointXYZIF;
using PointT = common::types::PointXYZIRing;
using PointCloudMsgT = sensor_msgs::msg::PointCloud2;
using PointCloudT = sensor_msgs::msg::PointCloud2;
using SyncPolicyT = message_filters::sync_policies::ApproximateTime<PointCloudMsgT,
Expand Down

0 comments on commit 553f517

Please sign in to comment.