From 7c5a7f23c0e55dbd11b86c906f422712b2a20a14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 4 Jul 2024 17:37:37 +0200 Subject: [PATCH] Removed sensor_msgs::msg::PointCloud MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- sensor_msgs/CMakeLists.txt | 2 - sensor_msgs/README.md | 1 - .../sensor_msgs/point_cloud_conversion.hpp | 200 ------------------ sensor_msgs/msg/PointCloud.msg | 17 -- .../test/test_pointcloud_conversion.cpp | 120 ----------- 5 files changed, 340 deletions(-) delete mode 100644 sensor_msgs/include/sensor_msgs/point_cloud_conversion.hpp delete mode 100644 sensor_msgs/msg/PointCloud.msg delete mode 100644 sensor_msgs/test/test_pointcloud_conversion.cpp diff --git a/sensor_msgs/CMakeLists.txt b/sensor_msgs/CMakeLists.txt index 3c76755f..b64634b3 100644 --- a/sensor_msgs/CMakeLists.txt +++ b/sensor_msgs/CMakeLists.txt @@ -37,7 +37,6 @@ set(msg_files "msg/MultiEchoLaserScan.msg" "msg/NavSatFix.msg" "msg/NavSatStatus.msg" - "msg/PointCloud.msg" "msg/PointCloud2.msg" "msg/PointField.msg" "msg/Range.msg" @@ -88,7 +87,6 @@ if(TARGET "${cpp_typesupport_target}") if(BUILD_TESTING) ament_add_gtest(test_sensor_msgs test/test_image_encodings.cpp - test/test_pointcloud_conversion.cpp test/test_pointcloud_iterator.cpp) target_link_libraries(test_sensor_msgs ${PROJECT_NAME}_library) endif() diff --git a/sensor_msgs/README.md b/sensor_msgs/README.md index 7385aaad..8dc9af24 100644 --- a/sensor_msgs/README.md +++ b/sensor_msgs/README.md @@ -11,7 +11,6 @@ This package provides some common C++ functionality relating to manipulating a c * [fill_image.hpp](include/sensor_msgs/fill_image.hpp): Fill a Image message from type-erased data pointer. * [image_encodings.hpp](include/sensor_msgs/image_encodings.hpp): Definitions and functionality relating to image encodings. -* [point_cloud_conversion.hpp](include/sensor_msgs/point_cloud_conversion.hpp): Functionality for converting between the deprecated PointCloud and PointCloud2 messages. * [point_cloud2_iterator.hpp](include/sensor_msgs/point_cloud2_iterator.hpp): Tools for modifying and parsing PointCloud2 messages. * [point_field_conversion.hpp](include/sensor_msgs/point_field_conversion.hpp): A type to enum mapping for the different PointField types, and methods to read and write in a PointCloud2 buffer for the different PointField types. diff --git a/sensor_msgs/include/sensor_msgs/point_cloud_conversion.hpp b/sensor_msgs/include/sensor_msgs/point_cloud_conversion.hpp deleted file mode 100644 index ec55646b..00000000 --- a/sensor_msgs/include/sensor_msgs/point_cloud_conversion.hpp +++ /dev/null @@ -1,200 +0,0 @@ -// Copyright (c) 2010, Willow Garage -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -// this file is originally ported from ROS1: -// https://raw.githubusercontent.com/ros/common_msgs/ef18af000759bf15c7ea036356dbdce631c75577/sensor_msgs/include/sensor_msgs/point_cloud_conversion.h_ -// -#ifndef SENSOR_MSGS__POINT_CLOUD_CONVERSION_HPP_ -#define SENSOR_MSGS__POINT_CLOUD_CONVERSION_HPP_ - -#include -#include -#include - -#include - -#ifndef SENSOR_MSGS_SKIP_WARNING -# define POINT_CLOUD_DEPRECATION_MESSAGE \ - "PointCloud is deprecated in Foxy for PointCloud2. This whole header will be removed." -# ifdef _MSC_VER -# pragma message(POINT_CLOUD_DEPRECATION_MESSAGE) -# else -# warning POINT_CLOUD_DEPRECATION_MESSAGE -# endif -#endif - -/** - * \brief Convert between the old (sensor_msgs::msg::PointCloud) and the new (sensor_msgs::msg::PointCloud2) format. - * \author Radu Bogdan Rusu - */ -namespace sensor_msgs -{ -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** \brief Get the index of a specified field (i.e., dimension/channel) - * \param cloud the the point cloud message - * \param field_name the string defining the field name - */ -static inline int getPointCloud2FieldIndex( - const sensor_msgs::msg::PointCloud2 & cloud, - const std::string & field_name) -{ - // Get the index we need - for (size_t d = 0; d < cloud.fields.size(); ++d) { - if (cloud.fields[d].name == field_name) { - return static_cast(d); - } - } - return -1; -} - -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** \brief Convert a sensor_msgs::msg::PointCloud message to a sensor_msgs::msg::PointCloud2 message. - * \param input the message in the sensor_msgs::msg::PointCloud format - * \param output the resultant message in the sensor_msgs::msg::PointCloud2 format - */ -[[deprecated("PointCloud is deprecated as of Foxy in favor of sensor_msgs/PointCloud2.")]] -static inline bool convertPointCloudToPointCloud2( - const sensor_msgs::msg::PointCloud & input, - sensor_msgs::msg::PointCloud2 & output) -{ - output.header = input.header; - output.width = static_cast(input.points.size()); - output.height = 1; - output.fields.resize(3 + input.channels.size()); - // Convert x/y/z to fields - output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z"; - int offset = 0; - // All offsets are *4, as all field data types are float32 - for (size_t d = 0; d < output.fields.size(); ++d, offset += 4) { - output.fields[d].offset = offset; - output.fields[d].datatype = sensor_msgs::msg::PointField::FLOAT32; - output.fields[d].count = 1; - } - output.point_step = offset; - output.row_step = output.point_step * output.width; - // Convert the remaining of the channels to fields - for (size_t d = 0; d < input.channels.size(); ++d) { - output.fields[3 + d].name = input.channels[d].name; - } - output.data.resize(input.points.size() * output.point_step); - output.is_bigendian = false; // @todo ? - output.is_dense = false; - - // Copy the data points - for (size_t cp = 0; cp < input.points.size(); ++cp) { - memcpy( - &output.data[cp * output.point_step + output.fields[0].offset], - &input.points[cp].x, sizeof(float)); - memcpy( - &output.data[cp * output.point_step + output.fields[1].offset], - &input.points[cp].y, sizeof(float)); - memcpy( - &output.data[cp * output.point_step + output.fields[2].offset], - &input.points[cp].z, sizeof(float)); - for (size_t d = 0; d < input.channels.size(); ++d) { - if (input.channels[d].values.size() == input.points.size()) { - memcpy( - &output.data[cp * output.point_step + output.fields[3 + d].offset], - &input.channels[d].values[cp], sizeof(float)); - } - } - } - return true; -} - -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** \brief Convert a sensor_msgs::msg::PointCloud2 message to a sensor_msgs::msg::PointCloud message. - * \param input the message in the sensor_msgs::msg::PointCloud2 format - * \param output the resultant message in the sensor_msgs::msg::PointCloud format - */ -[[deprecated("PointCloud is deprecated as of Foxy if favor of sensor_msgs/PointCloud2.")]] -static inline bool convertPointCloud2ToPointCloud( - const sensor_msgs::msg::PointCloud2 & input, - sensor_msgs::msg::PointCloud & output) -{ - output.header = input.header; - output.points.resize(input.width * input.height); - output.channels.resize(input.fields.size() - 3); - // Get the x/y/z field offsets - int x_idx = getPointCloud2FieldIndex(input, "x"); - int y_idx = getPointCloud2FieldIndex(input, "y"); - int z_idx = getPointCloud2FieldIndex(input, "z"); - if (x_idx == -1 || y_idx == -1 || z_idx == -1) { - return false; - } - int x_offset = input.fields[x_idx].offset; - int y_offset = input.fields[y_idx].offset; - int z_offset = input.fields[z_idx].offset; - uint8_t x_datatype = input.fields[x_idx].datatype; - uint8_t y_datatype = input.fields[y_idx].datatype; - uint8_t z_datatype = input.fields[z_idx].datatype; - - // Convert the fields to channels - int cur_c = 0; - for (size_t d = 0; d < input.fields.size(); ++d) { - if (static_cast(input.fields[d].offset) == x_offset || - static_cast(input.fields[d].offset) == y_offset || - static_cast(input.fields[d].offset) == z_offset) - { - continue; - } - output.channels[cur_c].name = input.fields[d].name; - output.channels[cur_c].values.resize(output.points.size()); - cur_c++; - } - - // Copy the data points - for (size_t cp = 0; cp < output.points.size(); ++cp) { - // Copy x/y/z - output.points[cp].x = - sensor_msgs::readPointCloud2BufferValue( - &input.data[cp * input.point_step + x_offset], x_datatype); - output.points[cp].y = - sensor_msgs::readPointCloud2BufferValue( - &input.data[cp * input.point_step + y_offset], y_datatype); - output.points[cp].z = - sensor_msgs::readPointCloud2BufferValue( - &input.data[cp * input.point_step + z_offset], z_datatype); - // Copy the rest of the data - int cur_c = 0; - for (size_t d = 0; d < input.fields.size(); ++d) { - if (static_cast(input.fields[d].offset) == x_offset || - static_cast(input.fields[d].offset) == y_offset || - static_cast(input.fields[d].offset) == z_offset) - { - continue; - } - output.channels[cur_c++].values[cp] = sensor_msgs::readPointCloud2BufferValue( - &input.data[cp * input.point_step + input.fields[d].offset], input.fields[d].datatype); - } - } - return true; -} -} // namespace sensor_msgs - -#endif // SENSOR_MSGS__POINT_CLOUD_CONVERSION_HPP_ diff --git a/sensor_msgs/msg/PointCloud.msg b/sensor_msgs/msg/PointCloud.msg deleted file mode 100644 index a07d4504..00000000 --- a/sensor_msgs/msg/PointCloud.msg +++ /dev/null @@ -1,17 +0,0 @@ -## THIS MESSAGE IS DEPRECATED AS OF FOXY -## Please use sensor_msgs/PointCloud2 - -# This message holds a collection of 3d points, plus optional additional -# information about each point. - -# Time of sensor data acquisition, coordinate frame ID. -std_msgs/Header header - -# Array of 3d points. Each Point32 should be interpreted as a 3d point -# in the frame given in the header. -geometry_msgs/Point32[] points - -# Each channel should have the same number of elements as points array, -# and the data in each channel should correspond 1:1 with each point. -# Channel names in common practice are listed in ChannelFloat32.msg. -ChannelFloat32[] channels diff --git a/sensor_msgs/test/test_pointcloud_conversion.cpp b/sensor_msgs/test/test_pointcloud_conversion.cpp deleted file mode 100644 index 58b97c7f..00000000 --- a/sensor_msgs/test/test_pointcloud_conversion.cpp +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. -// -// 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. - -#include - -#include "geometry_msgs/msg/point32.hpp" -#include "sensor_msgs/msg/point_cloud.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" - -#define SENSOR_MSGS_SKIP_WARNING - -// #warning suppression -// Not working due to preprocessor ignoring pragmas in g++ -// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=53431 -// Clang doesn't support suppressing -Wcpp probably like above. -// And I can't find any windows way to suppress it either. - -#include "sensor_msgs/point_cloud_conversion.hpp" -#undef SENSOR_MSGS_SKIP_WARNING - -#include "sensor_msgs/point_field_conversion.hpp" - -TEST(sensor_msgs, PointCloudConversion) -{ - // Build a PointCloud - sensor_msgs::msg::PointCloud cloud; - - cloud.header.stamp.sec = 100; - cloud.header.stamp.nanosec = 500; - cloud.header.frame_id = "cloud_frame"; - - sensor_msgs::msg::ChannelFloat32 intensity, range; - intensity.name = "intensity"; - range.name = "range"; - - for (size_t ii = 0; ii < 100; ++ii) { - auto pt = geometry_msgs::msg::Point32(); - pt.x = static_cast(1 * ii); - pt.y = static_cast(2 * ii); - pt.z = static_cast(3 * ii); - cloud.points.push_back(pt); - - intensity.values.push_back(static_cast(4 * ii)); - range.values.push_back(static_cast(5 * ii)); - } - cloud.channels.push_back(intensity); - cloud.channels.push_back(range); - - // Convert to PointCloud2 - sensor_msgs::msg::PointCloud2 cloud2; - -#ifdef _MSC_VER -#pragma warning(push) -#pragma warning(disable : 4996) -#else -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif - auto ret_cc2 = sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2); -#ifdef _MSC_VER -#pragma warning(pop) -#else -#pragma GCC diagnostic pop -#endif - ASSERT_TRUE(ret_cc2); - - EXPECT_EQ(1u, cloud2.height); - EXPECT_EQ(100u, cloud2.width); - - EXPECT_EQ(cloud2.fields[0].name, "x"); - EXPECT_EQ(cloud2.fields[1].name, "y"); - EXPECT_EQ(cloud2.fields[2].name, "z"); - EXPECT_EQ(cloud2.fields[3].name, "intensity"); - EXPECT_EQ(cloud2.fields[4].name, "range"); - - EXPECT_EQ(cloud2.fields.size(), 5u); - EXPECT_EQ(cloud2.header.frame_id, "cloud_frame"); - EXPECT_EQ(cloud2.header.stamp.sec, 100); - EXPECT_EQ(cloud2.header.stamp.nanosec, 500u); - - // Convert back to PointCloud - sensor_msgs::msg::PointCloud cloud3; -#ifdef _MSC_VER -#pragma warning(push) -#pragma warning(disable : 4996) -#else -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif - auto ret_c2c = sensor_msgs::convertPointCloud2ToPointCloud(cloud2, cloud3); -#ifdef _MSC_VER -#pragma warning(pop) -#else -#pragma GCC diagnostic pop -#endif - ASSERT_TRUE(ret_c2c); - EXPECT_EQ(cloud3.points.size(), 100u); - EXPECT_EQ(cloud3.channels.size(), 2u); - - EXPECT_EQ(cloud3.channels[0].name, "intensity"); - EXPECT_EQ(cloud3.channels[0].values.size(), 100u); - EXPECT_FLOAT_EQ(cloud3.channels[0].values[0], 0.0); - EXPECT_FLOAT_EQ(cloud3.channels[0].values[10], 40.0); - - EXPECT_EQ(cloud3.channels[1].name, "range"); - EXPECT_EQ(cloud3.channels[1].values.size(), 100u); - EXPECT_FLOAT_EQ(cloud3.channels[1].values[0], 0.0); - EXPECT_FLOAT_EQ(cloud3.channels[1].values[10], 50.0); -}