From bddeb1fee5e97eed02f0d6098b9fbf0b66e87e78 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Thu, 21 Mar 2024 09:31:59 -0400 Subject: [PATCH 01/19] added launch and config file for using point_cloud_fusion_node --- .../launch/point_cloud_fusion.launch.py | 41 +++++++++++++++++++ .../param/point_cloud_fusion.param.yaml | 7 ++++ 2 files changed, 48 insertions(+) create mode 100644 AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py create mode 100644 AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py new file mode 100644 index 00000000..9eef1c7f --- /dev/null +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py @@ -0,0 +1,41 @@ +# Copyright 2020-2021, The Autoware Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +import launch +from launch_ros.actions import Node +from ament_index_python import get_package_share_directory + +import os + +point_cloud_fusion_node_pkg_prefix = get_package_share_directory('point_cloud_fusion_nodes') +point_cloud_fusion_node_param_file = os.path.join(point_cloud_fusion_node_pkg_prefix, + 'param/point_cloud_fusion.param.yaml') + + +def generate_launch_description(): + point_cloud_fusion_nodes = Node( + package='point_cloud_fusion_nodes', + executable='pointcloud_fusion_node_exe', + namespace='lidar', + parameters=[point_cloud_fusion_node_param_file], + remappings=[ + ("output_topic", "points_raw"), + ("input_topic1", "/velodyne_1/lidar/points_raw"), + ("input_topic2", "/velodyne_2/lidar/points_raw") + ] + ) + + return launch.LaunchDescription([point_cloud_fusion_nodes]) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml new file mode 100644 index 00000000..5f83540c --- /dev/null +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml @@ -0,0 +1,7 @@ + +--- +/**: + ros__parameters: + number_of_sources: 2 + output_frame_id: "concat_velodyne" + cloud_size: 55000 From 2e68b177409283875137530a0dd138ff7c61b21c Mon Sep 17 00:00:00 2001 From: carma Date: Wed, 27 Mar 2024 15:31:07 -0400 Subject: [PATCH 02/19] corrected namespace configuration --- .../launch/point_cloud_fusion.launch.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py index 9eef1c7f..b0dd85be 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py @@ -29,12 +29,11 @@ def generate_launch_description(): point_cloud_fusion_nodes = Node( package='point_cloud_fusion_nodes', executable='pointcloud_fusion_node_exe', - namespace='lidar', parameters=[point_cloud_fusion_node_param_file], remappings=[ ("output_topic", "points_raw"), - ("input_topic1", "/velodyne_1/lidar/points_raw"), - ("input_topic2", "/velodyne_2/lidar/points_raw") + ("input_topic1", "velodyne_1/lidar/points_raw"), + ("input_topic2", "velodyne_2/lidar/points_raw") ] ) From 5514539c2c1160dd6c42f67bb45c994af3a0c79b Mon Sep 17 00:00:00 2001 From: carma Date: Wed, 27 Mar 2024 15:31:56 -0400 Subject: [PATCH 03/19] updated pcd field types to include ring index (XYZI -> XYZIF) --- .../src/point_cloud_fusion_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp index 65c6c6a5..4021f4ee 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp @@ -57,8 +57,8 @@ void PointCloudFusionNode::init() m_cloud_capacity, m_input_topics.size()); - using autoware::common::types::PointXYZI; - point_cloud_msg_wrapper::PointCloud2Modifier{ + using autoware::common::types::PointXYZIF; + point_cloud_msg_wrapper::PointCloud2Modifier{ m_cloud_concatenated, m_output_frame_id}.reserve(m_cloud_capacity); if (m_input_topics.size() > 8 || m_input_topics.size() < 2) { @@ -104,8 +104,8 @@ PointCloudFusionNode::pointcloud_callback( msg8}; // reset pointcloud before using - using autoware::common::types::PointXYZI; - point_cloud_msg_wrapper::PointCloud2Modifier modifier{m_cloud_concatenated}; + using autoware::common::types::PointXYZIF; + point_cloud_msg_wrapper::PointCloud2Modifier modifier{m_cloud_concatenated}; modifier.clear(); modifier.reserve(m_cloud_capacity); From 71c63d878892d147227a4f551e7d4ccca918025c Mon Sep 17 00:00:00 2001 From: carma Date: Wed, 27 Mar 2024 16:30:18 -0400 Subject: [PATCH 04/19] added new point type that contains ring index --- .../include/common/types.hpp | 22 +++++++++++++++++++ .../src/point_cloud_fusion_node.cpp | 8 +++---- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp index f3d49fce..d573772b 100644 --- a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp +++ b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp @@ -120,6 +120,28 @@ struct COMMON_PUBLIC PointXYZI } }; +struct COMMON_PUBLIC PointXYZIRing +{ + float32_t x{0}; + float32_t y{0}; + float32_t z{0}; + float32_t intensity{0}; + uint16_t ring{0}; + static constexpr uint16_t END_OF_SCAN_ID = 65535u; + friend bool operator==( + const PointXYZIRing & p1, + const PointXYZIRing & p2) noexcept + { + using autoware::common::helper_functions::comparisons::rel_eq; + const auto epsilon = std::numeric_limits::epsilon(); + return rel_eq(p1.x, p2.x, epsilon) && + rel_eq(p1.y, p2.y, epsilon) && + rel_eq(p1.z, p2.z, epsilon) && + rel_eq(p1.intensity, p2.intensity, epsilon) && + (p1.ring == p2.ring); + } +}; + using PointBlock = std::vector; using PointPtrBlock = std::vector; /// \brief Stores basic configuration information, does some simple validity checking diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp index 4021f4ee..0317328c 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp @@ -57,8 +57,8 @@ void PointCloudFusionNode::init() m_cloud_capacity, m_input_topics.size()); - using autoware::common::types::PointXYZIF; - point_cloud_msg_wrapper::PointCloud2Modifier{ + using autoware::common::types::PointXYZIRing; + point_cloud_msg_wrapper::PointCloud2Modifier{ m_cloud_concatenated, m_output_frame_id}.reserve(m_cloud_capacity); if (m_input_topics.size() > 8 || m_input_topics.size() < 2) { @@ -104,8 +104,8 @@ PointCloudFusionNode::pointcloud_callback( msg8}; // reset pointcloud before using - using autoware::common::types::PointXYZIF; - point_cloud_msg_wrapper::PointCloud2Modifier modifier{m_cloud_concatenated}; + using autoware::common::types::PointXYZIRing; + point_cloud_msg_wrapper::PointCloud2Modifier modifier{m_cloud_concatenated}; modifier.clear(); modifier.reserve(m_cloud_capacity); From 553f5178a2c308f81aa355e3f623c0cf1917878b Mon Sep 17 00:00:00 2001 From: carma Date: Thu, 28 Mar 2024 09:28:15 -0400 Subject: [PATCH 05/19] updated point type in point_cloud_fusion and point_cloud_fusion_nodes include --- .../point_cloud_fusion/point_cloud_fusion.hpp | 4 ++-- .../point_cloud_fusion/src/point_cloud_fusion.cpp | 12 ++++++------ .../point_cloud_fusion_node.hpp | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp index 371ab867..9e2a6e63 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp @@ -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 { @@ -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 & modifier) const; + point_cloud_msg_wrapper::PointCloud2Modifier & modifier) const; uint32_t m_cloud_capacity; size_t m_input_topics_size; diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp index ae1545ee..e019610e 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp @@ -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 modifier{cloud_concatenated}; + using autoware::common::types::PointXYZIRing; + point_cloud_msg_wrapper::PointCloud2Modifier modifier{cloud_concatenated}; for (size_t i = 0; i < m_input_topics_size; ++i) { concatenate_pointcloud(*msgs[i], pc_concat_idx, modifier); @@ -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 & modifier) const + point_cloud_msg_wrapper::PointCloud2Modifier & 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 view{pc_in}; + using autoware::common::types::PointXYZIRing; + point_cloud_msg_wrapper::PointCloud2View 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; diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp index 587f0388..bf8fe794 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp @@ -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 Date: Thu, 28 Mar 2024 09:58:01 -0400 Subject: [PATCH 06/19] removed end of scan id --- .../src/common/autoware_auto_common/include/common/types.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp index d573772b..cf5ee7f5 100644 --- a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp +++ b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp @@ -127,7 +127,6 @@ struct COMMON_PUBLIC PointXYZIRing float32_t z{0}; float32_t intensity{0}; uint16_t ring{0}; - static constexpr uint16_t END_OF_SCAN_ID = 65535u; friend bool operator==( const PointXYZIRing & p1, const PointXYZIRing & p2) noexcept From 48592646cb3bb04ee8ca95f9d27480136c04e545 Mon Sep 17 00:00:00 2001 From: carma Date: Thu, 28 Mar 2024 11:07:49 -0400 Subject: [PATCH 07/19] removed padding from point type struct to align struct size with the point step provided by the lidar driver --- .../src/common/autoware_auto_common/include/common/types.hpp | 2 ++ .../filters/point_cloud_fusion/src/point_cloud_fusion.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp index cf5ee7f5..63c45c9c 100644 --- a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp +++ b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp @@ -120,6 +120,7 @@ struct COMMON_PUBLIC PointXYZI } }; +#pragma pack(push,1) struct COMMON_PUBLIC PointXYZIRing { float32_t x{0}; @@ -140,6 +141,7 @@ struct COMMON_PUBLIC PointXYZIRing (p1.ring == p2.ring); } }; +#pragma pack(pop) using PointBlock = std::vector; using PointPtrBlock = std::vector; diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp index e019610e..9d38ddbd 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp @@ -70,6 +70,7 @@ void PointCloudFusion::concatenate_pointcloud( pt.y = (*view_it).y; pt.z = (*view_it).z; pt.intensity = (*view_it).intensity; + pt.ring = (*view_it).ring; modifier.push_back(pt); ++concat_idx; From 744674fe60d07ceb15cec5f09785cff56d13940b Mon Sep 17 00:00:00 2001 From: carma Date: Fri, 29 Mar 2024 12:07:34 -0400 Subject: [PATCH 08/19] increased cloud capacity and fixed topic namespace --- .../launch/point_cloud_fusion.launch.py | 2 +- .../param/point_cloud_fusion.param.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py index b0dd85be..86b17224 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): executable='pointcloud_fusion_node_exe', parameters=[point_cloud_fusion_node_param_file], remappings=[ - ("output_topic", "points_raw"), + ("output_topic", "lidar/points_raw"), ("input_topic1", "velodyne_1/lidar/points_raw"), ("input_topic2", "velodyne_2/lidar/points_raw") ] diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml index 5f83540c..c3fb28b9 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml @@ -4,4 +4,4 @@ ros__parameters: number_of_sources: 2 output_frame_id: "concat_velodyne" - cloud_size: 55000 + cloud_size: 80000 From 335f9aa54551cdaae9fad5e3812309c959a6cdbe Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 1 Apr 2024 13:16:04 -0400 Subject: [PATCH 09/19] changed point cloud type in unit test and restructure package.xml to allow unit tests to pass --- .../filters/point_cloud_fusion/package.xml | 2 +- .../point_cloud_fusion_nodes/package.xml | 3 ++- .../test/test_point_cloud_fusion_nodes.hpp | 19 +++++++++++++++---- 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/package.xml b/AutowareAuto/src/perception/filters/point_cloud_fusion/package.xml index 4260e663..1f38f6e1 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/package.xml +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/package.xml @@ -13,6 +13,7 @@ lidar_utils autoware_auto_common + carma_cmake_common ament_cmake_gtest ament_lint_auto @@ -21,5 +22,4 @@ ament_cmake - carma_cmake_common diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/package.xml b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/package.xml index e26280e2..421377b3 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/package.xml +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/package.xml @@ -22,6 +22,7 @@ eigen autoware_auto_common + carma_cmake_common eigen @@ -36,5 +37,5 @@ ament_cmake - carma_cmake_common + diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp index d9ff7759..c2f109dc 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp @@ -47,17 +47,18 @@ sensor_msgs::msg::PointCloud2 make_pc( std::vector seeds, builtin_interfaces::msg::Time stamp) { - using autoware::common::types::PointXYZI; + using autoware::common::types::PointXYZIRing; sensor_msgs::msg::PointCloud2 msg; - point_cloud_msg_wrapper::PointCloud2Modifier modifier{msg, "base_link"}; + point_cloud_msg_wrapper::PointCloud2Modifier modifier{msg, "base_link"}; // modifier.reserve(seeds.size()); for (auto seed : seeds) { - PointXYZI pt; + PointXYZIRing pt; pt.x = seed; pt.y = seed; pt.z = seed; pt.intensity = seed; + pt.ring = 0; modifier.push_back(pt); } @@ -77,36 +78,44 @@ void check_pcl_eq(sensor_msgs::msg::PointCloud2 & msg1, sensor_msgs::msg::PointC sensor_msgs::PointCloud2ConstIterator y_it_1(msg1, "y"); sensor_msgs::PointCloud2ConstIterator z_it_1(msg1, "z"); sensor_msgs::PointCloud2ConstIterator intensity_it_1(msg1, "intensity"); + sensor_msgs::PointCloud2ConstIterator ring_it_1(msg1, "ring"); sensor_msgs::PointCloud2ConstIterator x_it_2(msg2, "x"); sensor_msgs::PointCloud2ConstIterator y_it_2(msg2, "y"); sensor_msgs::PointCloud2ConstIterator z_it_2(msg2, "z"); sensor_msgs::PointCloud2ConstIterator intensity_it_2(msg2, "intensity"); + sensor_msgs::PointCloud2ConstIterator ring_it_2(msg2, "ring"); + while (x_it_1 != x_it_1.end() && y_it_1 != y_it_1.end() && z_it_1 != z_it_1.end() && intensity_it_1 != intensity_it_1.end() && + ring_it_1 != ring_it_1.end() && x_it_2 != x_it_2.end() && y_it_2 != y_it_2.end() && z_it_2 != z_it_2.end() && - intensity_it_2 != intensity_it_2.end() + intensity_it_2 != intensity_it_2.end() && + ring_it_2 != ring_it_2.end() ) { EXPECT_FLOAT_EQ(*x_it_1, *x_it_2); EXPECT_FLOAT_EQ(*y_it_1, *y_it_2); EXPECT_FLOAT_EQ(*z_it_1, *z_it_2); EXPECT_FLOAT_EQ(*intensity_it_1, *intensity_it_2); + EXPECT_EQ(*ring_it_1, *ring_it_2); ++x_it_1; ++y_it_1; ++z_it_1; ++intensity_it_1; + ++ring_it_1; ++x_it_2; ++y_it_2; ++z_it_2; ++intensity_it_2; + ++ring_it_2; } // Operator== is not defined for some reason @@ -114,11 +123,13 @@ void check_pcl_eq(sensor_msgs::msg::PointCloud2 & msg1, sensor_msgs::msg::PointC EXPECT_FALSE(y_it_1 != y_it_1.end()); EXPECT_FALSE(z_it_1 != z_it_1.end()); EXPECT_FALSE(intensity_it_1 != intensity_it_1.end()); + EXPECT_FALSE(ring_it_1 != ring_it_1.end()); EXPECT_FALSE(x_it_2 != x_it_2.end()); EXPECT_FALSE(y_it_2 != y_it_2.end()); EXPECT_FALSE(z_it_2 != z_it_2.end()); EXPECT_FALSE(intensity_it_2 != intensity_it_2.end()); + EXPECT_FALSE(ring_it_2 != ring_it_2.end()); } builtin_interfaces::msg::Time to_msg_time( From 4aa490ca2c55a17f77d6973aaae4f60e18317873 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 8 Apr 2024 11:51:23 -0400 Subject: [PATCH 10/19] added docs detailing and justifying the changes made to define the new PointXYZIRing field type --- .../include/common/types.hpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp index 63c45c9c..04e56348 100644 --- a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp +++ b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp @@ -16,6 +16,11 @@ /// \file /// \brief This file includes common type definition +/** + * Modification Copyright (C) Leidos 2024 + * - Added new point type PointXYZIRing that contains x, y, z, intensity, and ring index + */ + #ifndef COMMON__TYPES_HPP_ #define COMMON__TYPES_HPP_ @@ -120,7 +125,19 @@ struct COMMON_PUBLIC PointXYZI } }; +// Stores the current packing alignment value and instructs the compiler to not add padding to the PointXYZIRing struct +// This is needed as the package used to process point clouds (point_cloud_msg_wrapper) expects the size of the struct to be 18 bytes +// Without this command, the compiler will add 2 bytes of padding to the end of the struct #pragma pack(push,1) + +/** + * @brief Point cloud containing x, y, z, intensity, and ring index + * + * The package used to process point clouds (point_cloud_msg_wrapper) expects the variable names of the struct to match with + * the field names in the received ROS message. The velodyne driver used by CARMA platform publishes messages with the fields "x", + * "y", "z", "intensity", and "ring". Thus, although the definition of this point type is similar to PointXYZIF, it is required + * that the final variable is named "ring". +*/ struct COMMON_PUBLIC PointXYZIRing { float32_t x{0}; @@ -141,6 +158,7 @@ struct COMMON_PUBLIC PointXYZIRing (p1.ring == p2.ring); } }; +// Restores the previously saved packing alignment value #pragma pack(pop) using PointBlock = std::vector; From 1dde9e1556dbe17a09089e4b702156df23d7ab43 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 8 Apr 2024 12:22:06 -0400 Subject: [PATCH 11/19] added copyright docs for changes and new launch file --- .../point_cloud_fusion/point_cloud_fusion.hpp | 5 ++++ .../src/point_cloud_fusion.cpp | 5 ++++ .../point_cloud_fusion_node.hpp | 5 ++++ .../launch/point_cloud_fusion.launch.py | 23 ++++++++++--------- .../src/point_cloud_fusion_node.cpp | 5 ++++ .../test/test_point_cloud_fusion_nodes.hpp | 5 ++++ 6 files changed, 37 insertions(+), 11 deletions(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp index 9e2a6e63..7aa13e9f 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp @@ -14,6 +14,11 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/** + * Modification Copyright (C) Leidos 2024 + * - Changed point type from PointXYZI to PointXYZIRing, which contains x, y, z, intensity, and ring index + */ + #ifndef POINT_CLOUD_FUSION__POINT_CLOUD_FUSION_HPP_ #define POINT_CLOUD_FUSION__POINT_CLOUD_FUSION_HPP_ diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp index 9d38ddbd..840df4b1 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp @@ -14,6 +14,11 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/** + * Modification Copyright (C) Leidos 2024 + * - Changed point type from PointXYZI to PointXYZIRing, which contains x, y, z, intensity, and ring index + */ + #include #include #include diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp index bf8fe794..1b891a4a 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp @@ -14,6 +14,11 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/** + * Modification Copyright (C) Leidos 2024 + * - Changed point type from PointXYZI to PointXYZIRing, which contains x, y, z, intensity, and ring index + */ + #ifndef POINT_CLOUD_FUSION_NODES__POINT_CLOUD_FUSION_NODE_HPP_ #define POINT_CLOUD_FUSION_NODES__POINT_CLOUD_FUSION_NODE_HPP_ diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py index 86b17224..2a6101e4 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py @@ -1,18 +1,19 @@ -# Copyright 2020-2021, The Autoware Foundation +# Copyright (C) 2024 LEIDOS. # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at # -# http://www.apache.org/licenses/LICENSE-2.0 +# http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# Co-developed by Tier IV, Inc. and Apex.AI, Inc. +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +# This file is intended to launch point_cloud_fusion_nodes on CARMA freightliner trucks +# consisting of two Velodyne LiDARs import launch from launch_ros.actions import Node diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp index 0317328c..d844787f 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp @@ -14,6 +14,11 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/** + * Modification Copyright (C) Leidos 2024 + * - Changed point type from PointXYZI to PointXYZIRing, which contains x, y, z, intensity, and ring index + */ + #include #include #include diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp index c2f109dc..680bb052 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp @@ -14,6 +14,11 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/** + * Modification Copyright (C) Leidos 2024 + * - Changed point type from PointXYZI to PointXYZIRing, which contains x, y, z, intensity, and ring index + */ + #ifndef TEST_POINT_CLOUD_FUSION_NODES_HPP_ #define TEST_POINT_CLOUD_FUSION_NODES_HPP_ From c4ba38ea4b51e7c3cee8cabe83410eade86a17df Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Thu, 11 Apr 2024 10:00:47 -0400 Subject: [PATCH 12/19] reverted changes made to use new point type PointXYZIRing in favor of using point_type_adapter to convert ROS2 messages to PointXYZI --- .../include/common/types.hpp | 43 +------------------ .../point_cloud_fusion/point_cloud_fusion.hpp | 11 ++--- .../src/point_cloud_fusion.cpp | 20 +++------ .../point_cloud_fusion_node.hpp | 9 +--- .../src/point_cloud_fusion_node.cpp | 15 +++---- .../test/test_point_cloud_fusion_nodes.hpp | 26 +++-------- .../launch/carma_point_type_adapter.launch.py | 39 +++++++++++++++++ 7 files changed, 62 insertions(+), 101 deletions(-) create mode 100644 AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py diff --git a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp index 04e56348..c9ad2975 100644 --- a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp +++ b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -//    http://www.apache.org/licenses/LICENSE-2.0 +// http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, @@ -16,11 +16,6 @@ /// \file /// \brief This file includes common type definition -/** - * Modification Copyright (C) Leidos 2024 - * - Added new point type PointXYZIRing that contains x, y, z, intensity, and ring index - */ - #ifndef COMMON__TYPES_HPP_ #define COMMON__TYPES_HPP_ @@ -125,42 +120,6 @@ struct COMMON_PUBLIC PointXYZI } }; -// Stores the current packing alignment value and instructs the compiler to not add padding to the PointXYZIRing struct -// This is needed as the package used to process point clouds (point_cloud_msg_wrapper) expects the size of the struct to be 18 bytes -// Without this command, the compiler will add 2 bytes of padding to the end of the struct -#pragma pack(push,1) - -/** - * @brief Point cloud containing x, y, z, intensity, and ring index - * - * The package used to process point clouds (point_cloud_msg_wrapper) expects the variable names of the struct to match with - * the field names in the received ROS message. The velodyne driver used by CARMA platform publishes messages with the fields "x", - * "y", "z", "intensity", and "ring". Thus, although the definition of this point type is similar to PointXYZIF, it is required - * that the final variable is named "ring". -*/ -struct COMMON_PUBLIC PointXYZIRing -{ - float32_t x{0}; - float32_t y{0}; - float32_t z{0}; - float32_t intensity{0}; - uint16_t ring{0}; - friend bool operator==( - const PointXYZIRing & p1, - const PointXYZIRing & p2) noexcept - { - using autoware::common::helper_functions::comparisons::rel_eq; - const auto epsilon = std::numeric_limits::epsilon(); - return rel_eq(p1.x, p2.x, epsilon) && - rel_eq(p1.y, p2.y, epsilon) && - rel_eq(p1.z, p2.z, epsilon) && - rel_eq(p1.intensity, p2.intensity, epsilon) && - (p1.ring == p2.ring); - } -}; -// Restores the previously saved packing alignment value -#pragma pack(pop) - using PointBlock = std::vector; using PointPtrBlock = std::vector; /// \brief Stores basic configuration information, does some simple validity checking diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp index 7aa13e9f..5354b459 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -//    http://www.apache.org/licenses/LICENSE-2.0 +// http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, @@ -14,11 +14,6 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/** - * Modification Copyright (C) Leidos 2024 - * - Changed point type from PointXYZI to PointXYZIRing, which contains x, y, z, intensity, and ring index - */ - #ifndef POINT_CLOUD_FUSION__POINT_CLOUD_FUSION_HPP_ #define POINT_CLOUD_FUSION__POINT_CLOUD_FUSION_HPP_ @@ -37,7 +32,7 @@ namespace filters { namespace point_cloud_fusion { -using autoware::common::types::PointXYZIRing; +using autoware::common::types::PointXYZI; class POINT_CLOUD_FUSION_PUBLIC PointCloudFusion { @@ -72,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 & modifier) const; + point_cloud_msg_wrapper::PointCloud2Modifier & modifier) const; uint32_t m_cloud_capacity; size_t m_input_topics_size; diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp index 840df4b1..8d142c25 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -//    http://www.apache.org/licenses/LICENSE-2.0 +// http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, @@ -14,11 +14,6 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/** - * Modification Copyright (C) Leidos 2024 - * - Changed point type from PointXYZI to PointXYZIRing, which contains x, y, z, intensity, and ring index - */ - #include #include #include @@ -46,8 +41,8 @@ uint32_t PointCloudFusion::fuse_pc_msgs( { uint32_t pc_concat_idx = 0; - using autoware::common::types::PointXYZIRing; - point_cloud_msg_wrapper::PointCloud2Modifier modifier{cloud_concatenated}; + using autoware::common::types::PointXYZI; + point_cloud_msg_wrapper::PointCloud2Modifier modifier{cloud_concatenated}; for (size_t i = 0; i < m_input_topics_size; ++i) { concatenate_pointcloud(*msgs[i], pc_concat_idx, modifier); @@ -59,23 +54,22 @@ 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 & modifier) const + point_cloud_msg_wrapper::PointCloud2Modifier & modifier) const { if ((pc_in.width + concat_idx) > m_cloud_capacity) { throw Error::TOO_LARGE; } - using autoware::common::types::PointXYZIRing; - point_cloud_msg_wrapper::PointCloud2View view{pc_in}; + using autoware::common::types::PointXYZI; + point_cloud_msg_wrapper::PointCloud2View view{pc_in}; auto view_it = view.cbegin(); while (view_it != view.cend()) { - common::types::PointXYZIRing pt; + common::types::PointXYZI pt; pt.x = (*view_it).x; pt.y = (*view_it).y; pt.z = (*view_it).z; pt.intensity = (*view_it).intensity; - pt.ring = (*view_it).ring; modifier.push_back(pt); ++concat_idx; diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp index 1b891a4a..59bec017 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -//    http://www.apache.org/licenses/LICENSE-2.0 +// http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, @@ -14,11 +14,6 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/** - * Modification Copyright (C) Leidos 2024 - * - Changed point type from PointXYZI to PointXYZIRing, which contains x, y, z, intensity, and ring index - */ - #ifndef POINT_CLOUD_FUSION_NODES__POINT_CLOUD_FUSION_NODE_HPP_ #define POINT_CLOUD_FUSION_NODES__POINT_CLOUD_FUSION_NODE_HPP_ @@ -57,7 +52,7 @@ class POINT_CLOUD_FUSION_NODES_PUBLIC PointCloudFusionNode : public rclcpp::Node const rclcpp::NodeOptions & node_options); private: - using PointT = common::types::PointXYZIRing; + using PointT = common::types::PointXYZIF; using PointCloudMsgT = sensor_msgs::msg::PointCloud2; using PointCloudT = sensor_msgs::msg::PointCloud2; using SyncPolicyT = message_filters::sync_policies::ApproximateTime #include #include @@ -62,8 +57,8 @@ void PointCloudFusionNode::init() m_cloud_capacity, m_input_topics.size()); - using autoware::common::types::PointXYZIRing; - point_cloud_msg_wrapper::PointCloud2Modifier{ + using autoware::common::types::PointXYZI; + point_cloud_msg_wrapper::PointCloud2Modifier{ m_cloud_concatenated, m_output_frame_id}.reserve(m_cloud_capacity); if (m_input_topics.size() > 8 || m_input_topics.size() < 2) { @@ -109,8 +104,8 @@ PointCloudFusionNode::pointcloud_callback( msg8}; // reset pointcloud before using - using autoware::common::types::PointXYZIRing; - point_cloud_msg_wrapper::PointCloud2Modifier modifier{m_cloud_concatenated}; + using autoware::common::types::PointXYZI; + point_cloud_msg_wrapper::PointCloud2Modifier modifier{m_cloud_concatenated}; modifier.clear(); modifier.reserve(m_cloud_capacity); diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp index 680bb052..eef6aec7 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -//    http://www.apache.org/licenses/LICENSE-2.0 +// http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, @@ -14,11 +14,6 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/** - * Modification Copyright (C) Leidos 2024 - * - Changed point type from PointXYZI to PointXYZIRing, which contains x, y, z, intensity, and ring index - */ - #ifndef TEST_POINT_CLOUD_FUSION_NODES_HPP_ #define TEST_POINT_CLOUD_FUSION_NODES_HPP_ @@ -52,18 +47,17 @@ sensor_msgs::msg::PointCloud2 make_pc( std::vector seeds, builtin_interfaces::msg::Time stamp) { - using autoware::common::types::PointXYZIRing; + using autoware::common::types::PointXYZI; sensor_msgs::msg::PointCloud2 msg; - point_cloud_msg_wrapper::PointCloud2Modifier modifier{msg, "base_link"}; + point_cloud_msg_wrapper::PointCloud2Modifier modifier{msg, "base_link"}; // modifier.reserve(seeds.size()); for (auto seed : seeds) { - PointXYZIRing pt; + PointXYZI pt; pt.x = seed; pt.y = seed; pt.z = seed; pt.intensity = seed; - pt.ring = 0; modifier.push_back(pt); } @@ -83,44 +77,36 @@ void check_pcl_eq(sensor_msgs::msg::PointCloud2 & msg1, sensor_msgs::msg::PointC sensor_msgs::PointCloud2ConstIterator y_it_1(msg1, "y"); sensor_msgs::PointCloud2ConstIterator z_it_1(msg1, "z"); sensor_msgs::PointCloud2ConstIterator intensity_it_1(msg1, "intensity"); - sensor_msgs::PointCloud2ConstIterator ring_it_1(msg1, "ring"); sensor_msgs::PointCloud2ConstIterator x_it_2(msg2, "x"); sensor_msgs::PointCloud2ConstIterator y_it_2(msg2, "y"); sensor_msgs::PointCloud2ConstIterator z_it_2(msg2, "z"); sensor_msgs::PointCloud2ConstIterator intensity_it_2(msg2, "intensity"); - sensor_msgs::PointCloud2ConstIterator ring_it_2(msg2, "ring"); - while (x_it_1 != x_it_1.end() && y_it_1 != y_it_1.end() && z_it_1 != z_it_1.end() && intensity_it_1 != intensity_it_1.end() && - ring_it_1 != ring_it_1.end() && x_it_2 != x_it_2.end() && y_it_2 != y_it_2.end() && z_it_2 != z_it_2.end() && - intensity_it_2 != intensity_it_2.end() && - ring_it_2 != ring_it_2.end() + intensity_it_2 != intensity_it_2.end() ) { EXPECT_FLOAT_EQ(*x_it_1, *x_it_2); EXPECT_FLOAT_EQ(*y_it_1, *y_it_2); EXPECT_FLOAT_EQ(*z_it_1, *z_it_2); EXPECT_FLOAT_EQ(*intensity_it_1, *intensity_it_2); - EXPECT_EQ(*ring_it_1, *ring_it_2); ++x_it_1; ++y_it_1; ++z_it_1; ++intensity_it_1; - ++ring_it_1; ++x_it_2; ++y_it_2; ++z_it_2; ++intensity_it_2; - ++ring_it_2; } // Operator== is not defined for some reason @@ -128,13 +114,11 @@ void check_pcl_eq(sensor_msgs::msg::PointCloud2 & msg1, sensor_msgs::msg::PointC EXPECT_FALSE(y_it_1 != y_it_1.end()); EXPECT_FALSE(z_it_1 != z_it_1.end()); EXPECT_FALSE(intensity_it_1 != intensity_it_1.end()); - EXPECT_FALSE(ring_it_1 != ring_it_1.end()); EXPECT_FALSE(x_it_2 != x_it_2.end()); EXPECT_FALSE(y_it_2 != y_it_2.end()); EXPECT_FALSE(z_it_2 != z_it_2.end()); EXPECT_FALSE(intensity_it_2 != intensity_it_2.end()); - EXPECT_FALSE(ring_it_2 != ring_it_2.end()); } builtin_interfaces::msg::Time to_msg_time( diff --git a/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py b/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py new file mode 100644 index 00000000..f3bdd420 --- /dev/null +++ b/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py @@ -0,0 +1,39 @@ +# Copyright (C) 2024 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + + + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + """Generate launch description with a single component.""" + container = ComposableNodeContainer( + name='point_type_adapter_container', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='point_type_adapter', + plugin='autoware::tools::point_type_adapter::PointTypeAdapterNode', + name='point_type_adapter_node', + remappings=[('points_raw', 'lidar_front/points_raw'), + ('points_xyzi', 'lidar_front/points_xyzi')], ) + ], + output='screen', + ) + + return launch.LaunchDescription([container]) From 71d1977716131888b2b9daa91a06ed4edaa34faf Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Thu, 11 Apr 2024 10:11:17 -0400 Subject: [PATCH 13/19] reverted whitespace changes --- .../src/common/autoware_auto_common/include/common/types.hpp | 2 +- .../include/point_cloud_fusion/point_cloud_fusion.hpp | 2 +- .../filters/point_cloud_fusion/src/point_cloud_fusion.cpp | 2 +- .../point_cloud_fusion_nodes/point_cloud_fusion_node.hpp | 2 +- .../point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp | 2 +- .../test/test_point_cloud_fusion_nodes.hpp | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp index c9ad2975..f3d49fce 100644 --- a/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp +++ b/AutowareAuto/src/common/autoware_auto_common/include/common/types.hpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -// http://www.apache.org/licenses/LICENSE-2.0 +//    http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp index 5354b459..371ab867 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -// http://www.apache.org/licenses/LICENSE-2.0 +//    http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp index 8d142c25..ae1545ee 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion/src/point_cloud_fusion.cpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -// http://www.apache.org/licenses/LICENSE-2.0 +//    http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp index 59bec017..587f0388 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/include/point_cloud_fusion_nodes/point_cloud_fusion_node.hpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -// http://www.apache.org/licenses/LICENSE-2.0 +//    http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp index 4917e69f..65c6c6a5 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/src/point_cloud_fusion_node.cpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -// http://www.apache.org/licenses/LICENSE-2.0 +//    http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp index eef6aec7..d9ff7759 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/test/test_point_cloud_fusion_nodes.hpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -// http://www.apache.org/licenses/LICENSE-2.0 +//    http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, From 9a5b4228b3463484fd82e431351d0fb16adcd4ff Mon Sep 17 00:00:00 2001 From: carma Date: Fri, 12 Apr 2024 11:23:25 -0400 Subject: [PATCH 14/19] remapped topic names to add point type adapter before fusion node --- .../launch/point_cloud_fusion.launch.py | 4 ++-- .../launch/carma_point_type_adapter.launch.py | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py index 2a6101e4..42b665a9 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py @@ -33,8 +33,8 @@ def generate_launch_description(): parameters=[point_cloud_fusion_node_param_file], remappings=[ ("output_topic", "lidar/points_raw"), - ("input_topic1", "velodyne_1/lidar/points_raw"), - ("input_topic2", "velodyne_2/lidar/points_raw") + ("input_topic1", "velodyne_1/lidar/points_xyzi"), + ("input_topic2", "velodyne_2/lidar/points_xyzi") ] ) diff --git a/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py b/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py index f3bdd420..94b65d85 100644 --- a/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py +++ b/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py @@ -23,6 +23,7 @@ def generate_launch_description(): """Generate launch description with a single component.""" container = ComposableNodeContainer( name='point_type_adapter_container', + namespace='', package='rclcpp_components', executable='component_container', composable_node_descriptions=[ @@ -30,8 +31,8 @@ def generate_launch_description(): package='point_type_adapter', plugin='autoware::tools::point_type_adapter::PointTypeAdapterNode', name='point_type_adapter_node', - remappings=[('points_raw', 'lidar_front/points_raw'), - ('points_xyzi', 'lidar_front/points_xyzi')], ) + remappings=[('points_raw', 'lidar/points_raw'), + ('points_xyzi', 'lidar/points_xyzi')], ) ], output='screen', ) From 60e3f679743e76cf8d3f6decdff03934b92aec06 Mon Sep 17 00:00:00 2001 From: carma Date: Fri, 12 Apr 2024 11:44:49 -0400 Subject: [PATCH 15/19] added namespacing to point_type_adapter_container --- .../launch/carma_point_type_adapter.launch.py | 3 ++- AutowareAuto/src/tools/point_type_adapter/package.xml | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py b/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py index 94b65d85..f9034c85 100644 --- a/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py +++ b/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py @@ -15,6 +15,7 @@ import launch +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -23,7 +24,7 @@ def generate_launch_description(): """Generate launch description with a single component.""" container = ComposableNodeContainer( name='point_type_adapter_container', - namespace='', + namespace=GetCurrentNamespace(), package='rclcpp_components', executable='component_container', composable_node_descriptions=[ diff --git a/AutowareAuto/src/tools/point_type_adapter/package.xml b/AutowareAuto/src/tools/point_type_adapter/package.xml index 6abaf5e3..da38e8cf 100644 --- a/AutowareAuto/src/tools/point_type_adapter/package.xml +++ b/AutowareAuto/src/tools/point_type_adapter/package.xml @@ -15,6 +15,7 @@ sensor_msgs autoware_auto_common point_cloud_msg_wrapper + carma_ros2_utils ament_cmake_gtest ament_lint_auto From d676ad4ed62dccb3ac0ff6995604a19700c2b3a4 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 15 Apr 2024 12:11:44 -0400 Subject: [PATCH 16/19] added point_type_adapter to launch with point_cloud_fusion in single composable node container --- .../launch/point_cloud_fusion.launch.py | 50 ++++++++++++++----- .../point_cloud_fusion_nodes/package.xml | 2 + .../launch/carma_point_type_adapter.launch.py | 41 --------------- 3 files changed, 40 insertions(+), 53 deletions(-) delete mode 100644 AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py index 42b665a9..3d5c4422 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py @@ -1,13 +1,13 @@ # Copyright (C) 2024 LEIDOS. # -# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# Licensed under the Apache License, Version 2.0 (the 'License'); you may not # use this file except in compliance with the License. You may obtain a copy of # the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# distributed under the License is distributed on an 'AS IS' BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations under # the License. @@ -16,7 +16,9 @@ # consisting of two Velodyne LiDARs import launch -from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace from ament_index_python import get_package_share_directory import os @@ -27,15 +29,39 @@ def generate_launch_description(): - point_cloud_fusion_nodes = Node( - package='point_cloud_fusion_nodes', - executable='pointcloud_fusion_node_exe', - parameters=[point_cloud_fusion_node_param_file], - remappings=[ - ("output_topic", "lidar/points_raw"), - ("input_topic1", "velodyne_1/lidar/points_xyzi"), - ("input_topic2", "velodyne_2/lidar/points_xyzi") + + point_cloud_fusion_container = ComposableNodeContainer( + package='carma_ros2_utils', # rclcpp_components + name='point_cloud_fusion_container', + executable='lifecycle_component_wrapper_mt', + namespace=GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='point_cloud_fusion_nodes', + plugin='autoware::perception::filters::point_cloud_fusion_nodes::PointCloudFusionNode', + name='point_cloud_fusion_node', + parameters=[point_cloud_fusion_node_param_file], + remappings=[ + ('output_topic', 'lidar/points_raw'), + ('input_topic1', 'velodyne_1/lidar/points_xyzi'), + ('input_topic2', 'velodyne_2/lidar/points_xyzi') + ] + ), + ComposableNode( + package='point_type_adapter', + plugin='autoware::tools::point_type_adapter::PointTypeAdapterNode', + name='velodyne_1/point_type_adapter_node', + remappings=[('points_raw', 'velodyne_1/lidar/points_raw'), + ('points_xyzi', 'velodyne_1/lidar/points_xyzi')] + ), + ComposableNode( + package='point_type_adapter', + plugin='autoware::tools::point_type_adapter::PointTypeAdapterNode', + name='velodyne_2/point_type_adapter_node', + remappings=[('points_raw', 'velodyne_2/lidar/points_raw'), + ('points_xyzi', 'velodyne_2/lidar/points_xyzi')], + ) ] ) - return launch.LaunchDescription([point_cloud_fusion_nodes]) + return launch.LaunchDescription([point_cloud_fusion_container]) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/package.xml b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/package.xml index 421377b3..9252f91a 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/package.xml +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/package.xml @@ -19,6 +19,8 @@ tf2_ros tf2_geometry_msgs tf2_sensor_msgs + carma_ros2_utils + point_type_adapter eigen autoware_auto_common diff --git a/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py b/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py deleted file mode 100644 index f9034c85..00000000 --- a/AutowareAuto/src/tools/point_type_adapter/launch/carma_point_type_adapter.launch.py +++ /dev/null @@ -1,41 +0,0 @@ -# Copyright (C) 2024 LEIDOS. -# -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations under -# the License. - - - -import launch -from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - - -def generate_launch_description(): - """Generate launch description with a single component.""" - container = ComposableNodeContainer( - name='point_type_adapter_container', - namespace=GetCurrentNamespace(), - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='point_type_adapter', - plugin='autoware::tools::point_type_adapter::PointTypeAdapterNode', - name='point_type_adapter_node', - remappings=[('points_raw', 'lidar/points_raw'), - ('points_xyzi', 'lidar/points_xyzi')], ) - ], - output='screen', - ) - - return launch.LaunchDescription([container]) From 27bba96f10ac9335420c4a096c1e5f27794244ab Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Mon, 15 Apr 2024 15:12:29 -0400 Subject: [PATCH 17/19] added necessary extra args --- .../launch/point_cloud_fusion.launch.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py index 3d5c4422..91079c85 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py @@ -18,11 +18,15 @@ import launch from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace +from carma_ros2_utils.launch.get_log_level import GetLogLevel from ament_index_python import get_package_share_directory import os +env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') + point_cloud_fusion_node_pkg_prefix = get_package_share_directory('point_cloud_fusion_nodes') point_cloud_fusion_node_param_file = os.path.join(point_cloud_fusion_node_pkg_prefix, 'param/point_cloud_fusion.param.yaml') @@ -41,6 +45,11 @@ def generate_launch_description(): plugin='autoware::perception::filters::point_cloud_fusion_nodes::PointCloudFusionNode', name='point_cloud_fusion_node', parameters=[point_cloud_fusion_node_param_file], + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('point_cloud_fusion_nodes', env_log_levels) }, + {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper + ], remappings=[ ('output_topic', 'lidar/points_raw'), ('input_topic1', 'velodyne_1/lidar/points_xyzi'), @@ -51,6 +60,11 @@ def generate_launch_description(): package='point_type_adapter', plugin='autoware::tools::point_type_adapter::PointTypeAdapterNode', name='velodyne_1/point_type_adapter_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('point_type_adapter', env_log_levels) }, + {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper + ], remappings=[('points_raw', 'velodyne_1/lidar/points_raw'), ('points_xyzi', 'velodyne_1/lidar/points_xyzi')] ), @@ -58,6 +72,11 @@ def generate_launch_description(): package='point_type_adapter', plugin='autoware::tools::point_type_adapter::PointTypeAdapterNode', name='velodyne_2/point_type_adapter_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('point_type_adapter', env_log_levels) }, + {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper + ], remappings=[('points_raw', 'velodyne_2/lidar/points_raw'), ('points_xyzi', 'velodyne_2/lidar/points_xyzi')], ) From 00f216b8969acecab4e42e87f8700c3757cfa370 Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Tue, 16 Apr 2024 10:25:23 -0400 Subject: [PATCH 18/19] updated launch file and config to use carma_component_container_mt --- .../launch/point_cloud_fusion.launch.py | 12 +++++------- .../param/point_cloud_fusion.param.yaml | 10 +++------- 2 files changed, 8 insertions(+), 14 deletions(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py index 91079c85..6b197b7e 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): point_cloud_fusion_container = ComposableNodeContainer( package='carma_ros2_utils', # rclcpp_components name='point_cloud_fusion_container', - executable='lifecycle_component_wrapper_mt', + executable='carma_component_container_mt', namespace=GetCurrentNamespace(), composable_node_descriptions=[ ComposableNode( @@ -48,22 +48,21 @@ def generate_launch_description(): extra_arguments=[ {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('point_cloud_fusion_nodes', env_log_levels) }, - {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper ], remappings=[ ('output_topic', 'lidar/points_raw'), ('input_topic1', 'velodyne_1/lidar/points_xyzi'), - ('input_topic2', 'velodyne_2/lidar/points_xyzi') + ('input_topic2', 'velodyne_2/lidar/points_xyzi'), + ] ), ComposableNode( package='point_type_adapter', plugin='autoware::tools::point_type_adapter::PointTypeAdapterNode', - name='velodyne_1/point_type_adapter_node', + name='velodyne_1_point_type_adapter_node', extra_arguments=[ {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('point_type_adapter', env_log_levels) }, - {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper ], remappings=[('points_raw', 'velodyne_1/lidar/points_raw'), ('points_xyzi', 'velodyne_1/lidar/points_xyzi')] @@ -71,11 +70,10 @@ def generate_launch_description(): ComposableNode( package='point_type_adapter', plugin='autoware::tools::point_type_adapter::PointTypeAdapterNode', - name='velodyne_2/point_type_adapter_node', + name='velodyne_2_point_type_adapter_node', extra_arguments=[ {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('point_type_adapter', env_log_levels) }, - {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper ], remappings=[('points_raw', 'velodyne_2/lidar/points_raw'), ('points_xyzi', 'velodyne_2/lidar/points_xyzi')], diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml index c3fb28b9..72d1d522 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/param/point_cloud_fusion.param.yaml @@ -1,7 +1,3 @@ - ---- -/**: - ros__parameters: - number_of_sources: 2 - output_frame_id: "concat_velodyne" - cloud_size: 80000 +number_of_sources: 2 +output_frame_id: "concat_velodyne" +cloud_size: 80000 From be2b701a2f86245c6d06877706cd184e3f41b98e Mon Sep 17 00:00:00 2001 From: John Chrosniak Date: Tue, 16 Apr 2024 16:28:50 -0400 Subject: [PATCH 19/19] removed rclcpp_components comment --- .../launch/point_cloud_fusion.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py index 6b197b7e..9e3441dd 100644 --- a/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py +++ b/AutowareAuto/src/perception/filters/point_cloud_fusion_nodes/launch/point_cloud_fusion.launch.py @@ -35,7 +35,7 @@ def generate_launch_description(): point_cloud_fusion_container = ComposableNodeContainer( - package='carma_ros2_utils', # rclcpp_components + package='carma_ros2_utils', name='point_cloud_fusion_container', executable='carma_component_container_mt', namespace=GetCurrentNamespace(),