diff --git a/open3d_conversions/CMakeLists.txt b/open3d_conversions/CMakeLists.txt index 057d169..0165835 100644 --- a/open3d_conversions/CMakeLists.txt +++ b/open3d_conversions/CMakeLists.txt @@ -1,44 +1,62 @@ -cmake_minimum_required(VERSION 3.5.0) +cmake_minimum_required(VERSION 3.5) project(open3d_conversions) -find_package(catkin REQUIRED COMPONENTS - roscpp - sensor_msgs -) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +# system dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(Eigen3 REQUIRED) find_package(Open3D REQUIRED) -catkin_package( - INCLUDE_DIRS include - LIBRARIES open3d_conversions - CATKIN_DEPENDS roscpp sensor_msgs - DEPENDS EIGEN3 Open3D -) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) include_directories( include - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Open3D_INCLUDE_DIRS} ) -# C++ library add_library(open3d_conversions src/open3d_conversions.cpp) -add_dependencies(open3d_conversions ${open3d_conversions_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(open3d_conversions ${catkin_LIBRARIES} ${Open3D_LIBRARIES}) - -# Install -install(TARGETS open3d_conversions - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) - -install(DIRECTORY include/open3d_conversions/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -# Tests -if (CATKIN_ENABLE_TESTING) - catkin_add_gtest(test_open3d_conversions test/test_open3d_conversions.cpp) - target_link_libraries(test_open3d_conversions open3d_conversions ${catkin_LIBRARIES} ${Open3D_LIBRARIES}) +target_include_directories(open3d_conversions PUBLIC + $ + $) +ament_target_dependencies( + open3d_conversions + "rclcpp" + "sensor_msgs" +) +target_link_libraries(open3d_conversions ${Open3D_LIBRARIES}) +target_compile_definitions(open3d_conversions PUBLIC "OPEN3D_CONVERSIONS_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS open3d_conversions + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_add_gtest(test_open3d_conversions test/test_open3d_conversions.cpp) + target_link_libraries(test_open3d_conversions open3d_conversions ${Open3D_LIBRARIES}) + ament_lint_auto_find_test_dependencies() endif() + +ament_export_include_directories(include) +ament_export_libraries(open3d_conversions) +ament_export_targets(export_${PROJECT_NAME}) +ament_export_dependencies(Open3D rclcpp) + +ament_package() diff --git a/open3d_conversions/README.md b/open3d_conversions/README.md index 2ba9b23..1d6b67d 100644 --- a/open3d_conversions/README.md +++ b/open3d_conversions/README.md @@ -9,7 +9,7 @@ This package provides functions that can convert pointclouds from ROS to Open3D ## System Requirements -* Ubuntu 18.04+: GCC 5+ +* Ubuntu 20.04+: GCC 5+ ## Installation @@ -26,9 +26,9 @@ This package provides functions that can convert pointclouds from ROS to Open3D There are two functions provided in this library: ```cpp -void open3d_ros::open3dToRos(const open3d::geometry::PointCloud& pointcloud, sensor_msgs::PointCloud2& ros_pc2, std::string frame_id = "open3d_pointcloud"); +void open3d_conversions::open3dToRos(const open3d::geometry::PointCloud & pointcloud, sensor_msgs::msg::PointCloud2 & ros_pc2, std::string frame_id = "open3d_pointcloud"); -void open3d_ros::rosToOpen3d(const sensor_msgs::PointCloud2ConstPtr& ros_pc2, open3d::geometry::PointCloud& o3d_pc, bool skip_colors=false); +void open3d_conversions::rosToOpen3d(const sensor_msgs::msg::PointCloud2::SharedPtr & ros_pc2, open3d::geometry::PointCloud & o3d_pc, bool skip_colors=false); ``` * As Open3D pointclouds only contain `points`, `colors` and `normals`, the interface currently supports XYZ, XYZRGB pointclouds. XYZI pointclouds are handled by placing the `intensity` value in the `colors_`. diff --git a/open3d_conversions/include/open3d_conversions/open3d_conversions.h b/open3d_conversions/include/open3d_conversions/open3d_conversions.h deleted file mode 100644 index fc3952c..0000000 --- a/open3d_conversions/include/open3d_conversions/open3d_conversions.h +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2020 Autonomous Robots Lab, University of Nevada, Reno - -// 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. - -#ifndef OPEN3D_CONVERSIONS_HPP_ -#define OPEN3D_CONVERSIONS_HPP_ - -// Open3D -#include - -// ROS -#include -#include - -// Eigen -#include - -// C++ -#include - -namespace open3d_conversions -{ - /** - * @brief Copy data from a open3d::geometry::PointCloud to a sensor_msgs::PointCloud2 - * - * @param pointcloud Reference to the open3d PointCloud - * @param ros_pc2 Reference to the sensor_msgs PointCloud2 - * @param frame_id The string to be placed in the frame_id of the PointCloud2 - */ - void open3dToRos(const open3d::geometry::PointCloud &pointcloud, sensor_msgs::PointCloud2 &ros_pc2, - std::string frame_id = "open3d_pointcloud"); - - /** - * @brief Copy data from a sensor_msgs::PointCloud2 to a open3d::geometry::PointCloud - * - * @param ros_pc2 Reference to the sensor_msgs PointCloud2 - * @param o3d_pc Reference to the open3d PointCloud - * @param skip_colors If true, only xyz fields will be copied - */ - void rosToOpen3d(const sensor_msgs::PointCloud2ConstPtr &ros_pc2, open3d::geometry::PointCloud &o3d_pc, - bool skip_colors = false); -} // namespace open3d_conversions - -#endif // OPEN3D_CONVERSIONS_HPP_ diff --git a/open3d_conversions/include/open3d_conversions/open3d_conversions.hpp b/open3d_conversions/include/open3d_conversions/open3d_conversions.hpp new file mode 100644 index 0000000..74db266 --- /dev/null +++ b/open3d_conversions/include/open3d_conversions/open3d_conversions.hpp @@ -0,0 +1,61 @@ +// Copyright 2020 Autonomous Robots Lab, University of Nevada, Reno + +// 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. +#ifndef OPEN3D_CONVERSIONS__OPEN3D_CONVERSIONS_HPP_ +#define OPEN3D_CONVERSIONS__OPEN3D_CONVERSIONS_HPP_ + +// ROS2 +#include +#include + +// Open3D +#include + +// Eigen +#include + +// C++ +#include + +#include "open3d_conversions/visibility_control.h" + +namespace open3d_conversions +{ +/** + * @brief Copy data from a open3d::geometry::PointCloud to a + * sensor_msgs::msg::PointCloud2 + * + * @param pointcloud Reference to the open3d geometry PointCloud + * @param ros_pc2 Reference to the sensor_msgs PointCloud2 + * @param frame_id The string to be placed in the frame_id of the PointCloud2 + */ +void open3dToRos( + const open3d::geometry::PointCloud & pointcloud, + sensor_msgs::msg::PointCloud2 & ros_pc2, + std::string frame_id = "open3d_pointcloud"); +/** + * @brief Copy data from a sensor_msgs::msg::PointCloud2 to a + * open3d::geometry::PointCloud + * + * @param ros_pc2 Reference to the sensor_msgs PointCloud2 + * @param o3d_pc Reference to the open3d geometry PointCloud + * @param skip_colors If true, only xyz fields will be copied + */ +void rosToOpen3d( + const sensor_msgs::msg::PointCloud2::SharedPtr & ros_pc2, + open3d::geometry::PointCloud & o3d_pc, + bool skip_colors = false); + +} // namespace open3d_conversions + +#endif // OPEN3D_CONVERSIONS__OPEN3D_CONVERSIONS_HPP_ diff --git a/open3d_conversions/include/open3d_conversions/visibility_control.h b/open3d_conversions/include/open3d_conversions/visibility_control.h new file mode 100644 index 0000000..e08c931 --- /dev/null +++ b/open3d_conversions/include/open3d_conversions/visibility_control.h @@ -0,0 +1,48 @@ +// Copyright 2020 Autonomous Robots Lab, University of Nevada, Reno + +// 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. +#ifndef OPEN3D_CONVERSIONS__VISIBILITY_CONTROL_H_ +#define OPEN3D_CONVERSIONS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define OPEN3D_CONVERSIONS_EXPORT __attribute__ ((dllexport)) + #define OPEN3D_CONVERSIONS_IMPORT __attribute__ ((dllimport)) + #else + #define OPEN3D_CONVERSIONS_EXPORT __declspec(dllexport) + #define OPEN3D_CONVERSIONS_IMPORT __declspec(dllimport) + #endif + #ifdef OPEN3D_CONVERSIONS_BUILDING_LIBRARY + #define OPEN3D_CONVERSIONS_PUBLIC OPEN3D_CONVERSIONS_EXPORT + #else + #define OPEN3D_CONVERSIONS_PUBLIC OPEN3D_CONVERSIONS_IMPORT + #endif + #define OPEN3D_CONVERSIONS_PUBLIC_TYPE OPEN3D_CONVERSIONS_PUBLIC + #define OPEN3D_CONVERSIONS_LOCAL +#else + #define OPEN3D_CONVERSIONS_EXPORT __attribute__ ((visibility("default"))) + #define OPEN3D_CONVERSIONS_IMPORT + #if __GNUC__ >= 4 + #define OPEN3D_CONVERSIONS_PUBLIC __attribute__ ((visibility("default"))) + #define OPEN3D_CONVERSIONS_LOCAL __attribute__ ((visibility("default"))) + #else + #define OPEN3D_CONVERSIONS_PUBLIC + #define OPEN3D_CONVERSIONS_LOCAL + #endif + #define OPEN3D_CONVERSIONS_PUBLIC_TYPE +#endif + +#endif // OPEN3D_CONVERSIONS__VISIBILITY_CONTROL_H_ diff --git a/open3d_conversions/package.xml b/open3d_conversions/package.xml index 2c4f0be..69965d9 100644 --- a/open3d_conversions/package.xml +++ b/open3d_conversions/package.xml @@ -1,7 +1,8 @@ - + + open3d_conversions - 0.0.2 + 0.1.0 Provides conversion functions to and from Open3D datatypes Pranay Mathur @@ -12,15 +13,13 @@ Pranay Mathur Nikhil Khedekar + ament_cmake_ros - catkin - roscpp + rclcpp sensor_msgs eigen - - - + ament_cmake diff --git a/open3d_conversions/src/open3d_conversions.cpp b/open3d_conversions/src/open3d_conversions.cpp index efa8b28..c60c972 100644 --- a/open3d_conversions/src/open3d_conversions.cpp +++ b/open3d_conversions/src/open3d_conversions.cpp @@ -12,96 +12,103 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "open3d_conversions/open3d_conversions.h" +// C++ +#include +#include + +#include "open3d_conversions/open3d_conversions.hpp" namespace open3d_conversions { - void open3dToRos(const open3d::geometry::PointCloud &pointcloud, sensor_msgs::PointCloud2 &ros_pc2, std::string frame_id) - { - sensor_msgs::PointCloud2Modifier modifier(ros_pc2); - if (pointcloud.HasColors()) +void open3dToRos( + const open3d::geometry::PointCloud & pointcloud, + sensor_msgs::msg::PointCloud2 & ros_pc2, std::string frame_id) +{ + sensor_msgs::PointCloud2Modifier modifier(ros_pc2); + if (pointcloud.HasColors()) { + modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + } else { + modifier.setPointCloud2FieldsByString(1, "xyz"); + } + modifier.resize(pointcloud.points_.size()); + ros_pc2.header.frame_id = frame_id; + sensor_msgs::PointCloud2Iterator ros_pc2_x(ros_pc2, "x"); + sensor_msgs::PointCloud2Iterator ros_pc2_y(ros_pc2, "y"); + sensor_msgs::PointCloud2Iterator ros_pc2_z(ros_pc2, "z"); + if (pointcloud.HasColors()) { + sensor_msgs::PointCloud2Iterator ros_pc2_r(ros_pc2, "r"); + sensor_msgs::PointCloud2Iterator ros_pc2_g(ros_pc2, "g"); + sensor_msgs::PointCloud2Iterator ros_pc2_b(ros_pc2, "b"); + for (size_t i = 0; i < pointcloud.points_.size(); i++, ++ros_pc2_x, + ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_r, ++ros_pc2_g, + ++ros_pc2_b) { - modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + const Eigen::Vector3d & point = pointcloud.points_[i]; + const Eigen::Vector3d & color = pointcloud.colors_[i]; + *ros_pc2_x = point(0); + *ros_pc2_y = point(1); + *ros_pc2_z = point(2); + *ros_pc2_r = static_cast(255 * color(0)); + *ros_pc2_g = static_cast(255 * color(1)); + *ros_pc2_b = static_cast(255 * color(2)); } - else + } else { + for (size_t i = 0; i < pointcloud.points_.size(); + i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z) { - modifier.setPointCloud2FieldsByString(1, "xyz"); - } - modifier.resize(pointcloud.points_.size()); - ros_pc2.header.frame_id = frame_id; - sensor_msgs::PointCloud2Iterator ros_pc2_x(ros_pc2, "x"); - sensor_msgs::PointCloud2Iterator ros_pc2_y(ros_pc2, "y"); - sensor_msgs::PointCloud2Iterator ros_pc2_z(ros_pc2, "z"); - if (pointcloud.HasColors()) - { - sensor_msgs::PointCloud2Iterator ros_pc2_r(ros_pc2, "r"); - sensor_msgs::PointCloud2Iterator ros_pc2_g(ros_pc2, "g"); - sensor_msgs::PointCloud2Iterator ros_pc2_b(ros_pc2, "b"); - for (size_t i = 0; i < pointcloud.points_.size(); - i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_r, ++ros_pc2_g, ++ros_pc2_b) - { - const Eigen::Vector3d &point = pointcloud.points_[i]; - const Eigen::Vector3d &color = pointcloud.colors_[i]; - *ros_pc2_x = point(0); - *ros_pc2_y = point(1); - *ros_pc2_z = point(2); - *ros_pc2_r = (int)(255 * color(0)); - *ros_pc2_g = (int)(255 * color(1)); - *ros_pc2_b = (int)(255 * color(2)); - } - } - else - { - for (size_t i = 0; i < pointcloud.points_.size(); i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z) - { - const Eigen::Vector3d &point = pointcloud.points_[i]; - *ros_pc2_x = point(0); - *ros_pc2_y = point(1); - *ros_pc2_z = point(2); - } + const Eigen::Vector3d & point = pointcloud.points_[i]; + *ros_pc2_x = point(0); + *ros_pc2_y = point(1); + *ros_pc2_z = point(2); } } +} - void rosToOpen3d(const sensor_msgs::PointCloud2ConstPtr &ros_pc2, open3d::geometry::PointCloud &o3d_pc, bool skip_colors) - { - sensor_msgs::PointCloud2ConstIterator ros_pc2_x(*ros_pc2, "x"); - sensor_msgs::PointCloud2ConstIterator ros_pc2_y(*ros_pc2, "y"); - sensor_msgs::PointCloud2ConstIterator ros_pc2_z(*ros_pc2, "z"); - o3d_pc.points_.reserve(ros_pc2->height * ros_pc2->width); - if (ros_pc2->fields.size() == 3 || skip_colors == true) +void rosToOpen3d( + const sensor_msgs::msg::PointCloud2::SharedPtr & ros_pc2, + open3d::geometry::PointCloud & o3d_pc, bool skip_colors) +{ + sensor_msgs::PointCloud2ConstIterator ros_pc2_x(*ros_pc2, "x"); + sensor_msgs::PointCloud2ConstIterator ros_pc2_y(*ros_pc2, "y"); + sensor_msgs::PointCloud2ConstIterator ros_pc2_z(*ros_pc2, "z"); + o3d_pc.points_.reserve(ros_pc2->height * ros_pc2->width); + if (ros_pc2->fields.size() == 3 || skip_colors == true) { + for (size_t i = 0; i < ros_pc2->height * ros_pc2->width; + ++i, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z) { - for (size_t i = 0; i < ros_pc2->height * ros_pc2->width; ++i, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z) - { - o3d_pc.points_.push_back(Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); - } + o3d_pc.points_.push_back( + Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); } - else - { - o3d_pc.colors_.reserve(ros_pc2->height * ros_pc2->width); - if (ros_pc2->fields[3].name == "rgb") - { - sensor_msgs::PointCloud2ConstIterator ros_pc2_r(*ros_pc2, "r"); - sensor_msgs::PointCloud2ConstIterator ros_pc2_g(*ros_pc2, "g"); - sensor_msgs::PointCloud2ConstIterator ros_pc2_b(*ros_pc2, "b"); + } else { + o3d_pc.colors_.reserve(ros_pc2->height * ros_pc2->width); + if (ros_pc2->fields[3].name == "rgb") { + sensor_msgs::PointCloud2ConstIterator ros_pc2_r(*ros_pc2, "r"); + sensor_msgs::PointCloud2ConstIterator ros_pc2_g(*ros_pc2, "g"); + sensor_msgs::PointCloud2ConstIterator ros_pc2_b(*ros_pc2, "b"); - for (size_t i = 0; i < ros_pc2->height * ros_pc2->width; - ++i, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_r, ++ros_pc2_g, ++ros_pc2_b) - { - o3d_pc.points_.push_back(Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); - o3d_pc.colors_.push_back(Eigen::Vector3d(((int)(*ros_pc2_r)) / 255.0, ((int)(*ros_pc2_g)) / 255.0, - ((int)(*ros_pc2_b)) / 255.0)); - } + for (size_t i = 0; i < ros_pc2->height * ros_pc2->width; ++i, ++ros_pc2_x, + ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_r, ++ros_pc2_g, ++ros_pc2_b) + { + o3d_pc.points_.push_back( + Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); + o3d_pc.colors_.push_back( + Eigen::Vector3d( + (static_cast(*ros_pc2_r)) / 255.0, + (static_cast(*ros_pc2_g)) / 255.0, + (static_cast(*ros_pc2_b)) / 255.0)); } - else if (ros_pc2->fields[3].name == "intensity") + } else if (ros_pc2->fields[3].name == "intensity") { + sensor_msgs::PointCloud2ConstIterator ros_pc2_i(*ros_pc2, + "intensity"); + for (size_t i = 0; i < ros_pc2->height * ros_pc2->width; + ++i, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_i) { - sensor_msgs::PointCloud2ConstIterator ros_pc2_i(*ros_pc2, "intensity"); - for (size_t i = 0; i < ros_pc2->height * ros_pc2->width; - ++i, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_i) - { - o3d_pc.points_.push_back(Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); - o3d_pc.colors_.push_back(Eigen::Vector3d(*ros_pc2_i, *ros_pc2_i, *ros_pc2_i)); - } + o3d_pc.points_.push_back( + Eigen::Vector3d(*ros_pc2_x, *ros_pc2_y, *ros_pc2_z)); + o3d_pc.colors_.push_back( + Eigen::Vector3d(*ros_pc2_i, *ros_pc2_i, *ros_pc2_i)); } } } -} // namespace open3d_conversions +} +} // namespace open3d_conversions diff --git a/open3d_conversions/test/test_open3d_conversions.cpp b/open3d_conversions/test/test_open3d_conversions.cpp index 9548c6f..f14ac10 100644 --- a/open3d_conversions/test/test_open3d_conversions.cpp +++ b/open3d_conversions/test/test_open3d_conversions.cpp @@ -12,53 +12,51 @@ // See the License for the specific language governing permissions and // limitations under the License. +// ROS +#include +#include + // GTest #include -//open3d_conversions -#include "open3d_conversions/open3d_conversions.h" - // Open3D #include +// C++ +#include + // ROS -#include -#include +#include "rclcpp/rclcpp.hpp" -// Boost -#include +// open3d_conversions +#include "open3d_conversions/open3d_conversions.hpp" -TEST(ConversionFunctions, open3dToRos_uncolored) -{ +TEST(ConversionFunctions, open3dToRos2_uncolored) { open3d::geometry::PointCloud o3d_pc; - for (int i = 0; i < 5; ++i) - { + for (int i = 0; i < 5; ++i) { o3d_pc.points_.push_back(Eigen::Vector3d(0.5 * i, i * i, 10.5 * i)); } - sensor_msgs::PointCloud2 ros_pc2; + sensor_msgs::msg::PointCloud2 ros_pc2; open3d_conversions::open3dToRos(o3d_pc, ros_pc2, "o3d_frame"); EXPECT_EQ(ros_pc2.height * ros_pc2.width, o3d_pc.points_.size()); sensor_msgs::PointCloud2Iterator ros_pc2_x(ros_pc2, "x"); sensor_msgs::PointCloud2Iterator ros_pc2_y(ros_pc2, "y"); sensor_msgs::PointCloud2Iterator ros_pc2_z(ros_pc2, "z"); - for (int i = 0; i < 5; i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z) - { - const Eigen::Vector3d &point = o3d_pc.points_[i]; + for (int i = 0; i < 5; i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z) { EXPECT_EQ(*ros_pc2_x, 0.5 * i); EXPECT_EQ(*ros_pc2_y, i * i); EXPECT_EQ(*ros_pc2_z, 10.5 * i); } } -TEST(ConversionFunctions, open3dToRos_colored) -{ +TEST(ConversionFunctions, open3dToRos2_colored) { open3d::geometry::PointCloud o3d_pc; - for (int i = 0; i < 5; ++i) - { + for (int i = 0; i < 5; ++i) { o3d_pc.points_.push_back(Eigen::Vector3d(0.5 * i, i * i, 10.5 * i)); - o3d_pc.colors_.push_back(Eigen::Vector3d(2 * i / 255.0, 5 * i / 255.0, 10 * i / 255.0)); + o3d_pc.colors_.push_back( + Eigen::Vector3d(2 * i / 255.0, 5 * i / 255.0, 10 * i / 255.0)); } - sensor_msgs::PointCloud2 ros_pc2; + sensor_msgs::msg::PointCloud2 ros_pc2; open3d_conversions::open3dToRos(o3d_pc, ros_pc2, "o3d_frame"); EXPECT_EQ(ros_pc2.height * ros_pc2.width, o3d_pc.points_.size()); sensor_msgs::PointCloud2Iterator ros_pc2_x(ros_pc2, "x"); @@ -67,22 +65,20 @@ TEST(ConversionFunctions, open3dToRos_colored) sensor_msgs::PointCloud2Iterator ros_pc2_r(ros_pc2, "r"); sensor_msgs::PointCloud2Iterator ros_pc2_g(ros_pc2, "g"); sensor_msgs::PointCloud2Iterator ros_pc2_b(ros_pc2, "b"); - for (int i = 0; i < 5; i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_r, ++ros_pc2_g, ++ros_pc2_b) + for (int i = 0; i < 5; i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z, + ++ros_pc2_r, ++ros_pc2_g, ++ros_pc2_b) { - const Eigen::Vector3d &point = o3d_pc.points_[i]; EXPECT_EQ(*ros_pc2_x, 0.5 * i); EXPECT_EQ(*ros_pc2_y, i * i); EXPECT_EQ(*ros_pc2_z, 10.5 * i); - const Eigen::Vector3d &color = o3d_pc.points_[i]; EXPECT_EQ(*ros_pc2_r, 2 * i); EXPECT_EQ(*ros_pc2_g, 5 * i); EXPECT_EQ(*ros_pc2_b, 10 * i); } } -TEST(ConversionFunctions, rosToOpen3d_uncolored) -{ - sensor_msgs::PointCloud2 ros_pc2; +TEST(ConversionFunctions, rosToOpen3d_uncolored) { + sensor_msgs::msg::PointCloud2 ros_pc2; ros_pc2.header.frame_id = "ros"; ros_pc2.height = 1; ros_pc2.width = 5; @@ -95,30 +91,28 @@ TEST(ConversionFunctions, rosToOpen3d_uncolored) sensor_msgs::PointCloud2Iterator mod_y(ros_pc2, "y"); sensor_msgs::PointCloud2Iterator mod_z(ros_pc2, "z"); - for (int i = 0; i < 5; ++i, ++mod_x, ++mod_y, ++mod_z) - { + for (int i = 0; i < 5; ++i, ++mod_x, ++mod_y, ++mod_z) { *mod_x = 0.5 * i; *mod_y = i * i; *mod_z = 10.5 * i; } - const sensor_msgs::PointCloud2ConstPtr &ros_pc2_ptr = boost::make_shared(ros_pc2); + const sensor_msgs::msg::PointCloud2::SharedPtr & ros_pc2_ptr = + std::make_shared(ros_pc2); open3d::geometry::PointCloud o3d_pc; open3d_conversions::rosToOpen3d(ros_pc2_ptr, o3d_pc); EXPECT_EQ(ros_pc2_ptr->height * ros_pc2_ptr->width, o3d_pc.points_.size()); EXPECT_EQ(o3d_pc.HasColors(), false); - for (unsigned int i = 0; i < 5; i++) - { - const Eigen::Vector3d &point = o3d_pc.points_[i]; + for (unsigned int i = 0; i < 5; i++) { + const Eigen::Vector3d & point = o3d_pc.points_[i]; EXPECT_EQ(point(0), 0.5 * i); EXPECT_EQ(point(1), i * i); EXPECT_EQ(point(2), 10.5 * i); } } -TEST(ConversionFunctions, rosToOpen3d_colored) -{ - sensor_msgs::PointCloud2 ros_pc2; +TEST(ConversionFunctions, rosToOpen3d_colored) { + sensor_msgs::msg::PointCloud2 ros_pc2; ros_pc2.header.frame_id = "ros"; ros_pc2.height = 1; ros_pc2.width = 5; @@ -135,7 +129,8 @@ TEST(ConversionFunctions, rosToOpen3d_colored) sensor_msgs::PointCloud2Iterator mod_g(ros_pc2, "g"); sensor_msgs::PointCloud2Iterator mod_b(ros_pc2, "b"); - for (int i = 0; i < 5; ++i, ++mod_x, ++mod_y, ++mod_z, ++mod_r, ++mod_g, ++mod_b) + for (int i = 0; i < 5; + ++i, ++mod_x, ++mod_y, ++mod_z, ++mod_r, ++mod_g, ++mod_b) { *mod_x = 0.5 * i; *mod_y = i * i; @@ -145,26 +140,26 @@ TEST(ConversionFunctions, rosToOpen3d_colored) *mod_b = 10 * i; } - const sensor_msgs::PointCloud2ConstPtr &ros_pc2_ptr = boost::make_shared(ros_pc2); + const sensor_msgs::msg::PointCloud2::SharedPtr & ros_pc2_ptr = + std::make_shared(ros_pc2); open3d::geometry::PointCloud o3d_pc; open3d_conversions::rosToOpen3d(ros_pc2_ptr, o3d_pc); EXPECT_EQ(ros_pc2_ptr->height * ros_pc2_ptr->width, o3d_pc.points_.size()); EXPECT_EQ(o3d_pc.HasColors(), true); - for (unsigned int i = 0; i < 5; i++) - { - const Eigen::Vector3d &point = o3d_pc.points_[i]; + for (unsigned int i = 0; i < 5; i++) { + const Eigen::Vector3d & point = o3d_pc.points_[i]; EXPECT_EQ(point(0), 0.5 * i); EXPECT_EQ(point(1), i * i); EXPECT_EQ(point(2), 10.5 * i); - const Eigen::Vector3d &color = o3d_pc.colors_[i]; + const Eigen::Vector3d & color = o3d_pc.colors_[i]; EXPECT_EQ(color(0), 2 * i / 255.0); EXPECT_EQ(color(1), 5 * i / 255.0); EXPECT_EQ(color(2), 10 * i / 255.0); } } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +}