From 272f79e72e388f422d521f74683900f0ddbb1e77 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 4 Jul 2024 17:41:05 +0200 Subject: [PATCH 1/2] Removed sensor_msgs::msg::PointCloud references because is deprecated MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rviz2/test/tools/send_lots_of_points_node.cpp | 52 +++++++---- rviz_default_plugins/CMakeLists.txt | 20 ---- .../pointcloud/point_cloud_common.hpp | 1 - .../pointcloud/point_cloud_display.hpp | 93 ------------------- .../point_cloud_to_point_cloud2.hpp | 48 ---------- .../pointcloud/point_cloud_common.cpp | 6 -- .../pointcloud/point_cloud_display.cpp | 83 ----------------- .../point_cloud_to_point_cloud2.cpp | 85 ----------------- .../camera/camera_display_visual_test.cpp | 18 ++-- .../point_cloud_display_visual_test.cpp | 67 ------------- .../pointcloud_messages.cpp | 25 ----- .../pointcloud_messages.hpp | 3 - .../publishers/point_cloud_publisher.hpp | 90 ------------------ 13 files changed, 45 insertions(+), 546 deletions(-) delete mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_display.hpp delete mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.hpp delete mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp delete mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.cpp delete mode 100644 rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp delete mode 100644 rviz_default_plugins/test/rviz_default_plugins/publishers/point_cloud_publisher.hpp diff --git a/rviz2/test/tools/send_lots_of_points_node.cpp b/rviz2/test/tools/send_lots_of_points_node.cpp index e52fc92fd..571b12a5c 100644 --- a/rviz2/test/tools/send_lots_of_points_node.cpp +++ b/rviz2/test/tools/send_lots_of_points_node.cpp @@ -31,7 +31,8 @@ #include #include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/point_cloud.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" #include "geometry_msgs/msg/point32.hpp" int main(int argc, char ** argv) @@ -54,34 +55,53 @@ int main(int argc, char ** argv) auto node = rclcpp::Node::make_shared("send_lots_of_points"); - auto pub = node->create_publisher("pointcloud", 100); + auto pub = node->create_publisher("pointcloud", 100); rclcpp::Rate loop_rate(rate); - sensor_msgs::msg::PointCloud msg; + sensor_msgs::msg::PointCloud2 msg; int width = size; int length = 2 * size; - msg.points.resize(width * length); msg.header.frame_id = "world"; + // Fill 2 using an iterator + sensor_msgs::PointCloud2Modifier modifier(msg); + modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); int count = 0; + printf("cloud_msg_.point_step %d\n", msg.point_step); while (rclcpp::ok() ) { width++; - msg.points.resize(width * length + (count % 2) ); - + modifier.resize(width, length + (count % 2)); + sensor_msgs::PointCloud2Iterator iter_x(msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(msg, "z"); + sensor_msgs::PointCloud2Iterator iter_r(msg, "r"); + sensor_msgs::PointCloud2Iterator iter_g(msg, "g"); + sensor_msgs::PointCloud2Iterator iter_b(msg, "b"); for (int x = 0; x < width; x++) { for (int y = 0; y < length; y++) { - geometry_msgs::msg::Point32 & point = msg.points[x + y * width]; - point.x = static_cast(x / 100.0); - point.y = static_cast(y / 100.0); -// point.z = sinf( x / 100.0 + y / 100.0 + count / 100.0 ); - point.z = ((x + y + count) % 100) / 100.0f; + if (count % 2) { + *iter_x = -.1f; + *iter_y = -.1f; + *iter_z = 1.1f; + } else { + *iter_x = static_cast(x / 100.0); + *iter_y = static_cast(y / 100.0); + *iter_z = ((x + y + count) % 100) / 100.0f; + } + + *iter_r = rand() % 256; + *iter_g = rand() % 256; + *iter_b = rand() % 256; + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_r; + ++iter_g; + ++iter_b; } } - if (count % 2) { - msg.points[width * length + 1].x = -.1f; - msg.points[width * length + 1].y = -.1f; - msg.points[width * length + 1].z = 1.1f; - } + msg.header.stamp = node->now(); printf( diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index b845f9d8c..b40000edd 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -187,10 +187,8 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/pointcloud/transformers/xyz_pc_transformer.cpp src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp - src/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.cpp src/rviz_default_plugins/displays/pointcloud/point_cloud_transformer_factory.cpp src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.cpp - src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp src/rviz_default_plugins/displays/pointcloud/point_cloud2_display.cpp src/rviz_default_plugins/displays/polygon/polygon_display.cpp src/rviz_default_plugins/displays/pose/pose_display.cpp @@ -1025,24 +1023,6 @@ if(BUILD_TESTING) ) endif() - ament_add_gtest(point_cloud_display_visual_test - test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp - ${SKIP_VISUAL_TESTS} - TIMEOUT 180) - if(TARGET point_cloud_display_visual_test) - target_include_directories(point_cloud_display_visual_test PRIVATE test) - target_link_libraries(point_cloud_display_visual_test - Qt5::Test - rviz_visual_testing_framework::rviz_visual_testing_framework - ${geometry_msgs_TARGETS} - tf2::tf2 - rclcpp::rclcpp - ${std_msgs_TARGETS} - ${sensor_msgs_TARGETS} - point_cloud_common_page_object - ) - endif() - ament_add_gtest(point_cloud2_display_visual_test test/rviz_default_plugins/displays/pointcloud/point_cloud2_display_visual_test.cpp ${SKIP_VISUAL_TESTS} diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp index 6f5d4709e..89d062f3d 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp @@ -144,7 +144,6 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC PointCloudCommon : public QObject void reset(); void update(float wall_dt, float ros_dt); - void addMessage(sensor_msgs::msg::PointCloud::ConstSharedPtr cloud); void addMessage(sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud); rviz_common::Display * getDisplay() {return display_;} diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_display.hpp deleted file mode 100644 index 19a09528a..000000000 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_display.hpp +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * 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 Willow Garage, Inc. 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 OWNER 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. - */ - -#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_DISPLAY_HPP_ -#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_DISPLAY_HPP_ - -#include -#include -#include -#include - -#include "sensor_msgs/msg/point_cloud.hpp" - -#include "rviz_common/message_filter_display.hpp" - -#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" -#include "rviz_default_plugins/visibility_control.hpp" - -namespace rviz_common -{ -namespace properties -{ - -class IntProperty; - -} // namespace properties -} // namespace rviz_common - -namespace rviz_default_plugins -{ -namespace displays -{ - -/** - * \class PointCloudDisplay - * \brief Displays a point cloud of type sensor_msgs::PointCloud - * - * By default it will assume channel 0 of the cloud is an intensity value, and will color them by intensity. - * If you set the channel's name to "rgb", it will interpret the channel as an integer rgb value, with r, g and b - * all being 8 bits. - */ -class RVIZ_DEFAULT_PLUGINS_PUBLIC PointCloudDisplay : public - rviz_common::MessageFilterDisplay -{ -public: - PointCloudDisplay(); - - void reset() override; - - void update(float wall_dt, float ros_dt) override; - - void onDisable() override; - -protected: - /** @brief Do initialization. Overridden from MessageFilterDisplay. */ - void onInitialize() override; - - /** @brief Process a single message. Overridden from MessageFilterDisplay. */ - void processMessage(sensor_msgs::msg::PointCloud::ConstSharedPtr cloud) override; - - std::unique_ptr point_cloud_common_; -}; - -} // namespace displays -} // namespace rviz_default_plugins - -#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_DISPLAY_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.hpp deleted file mode 100644 index af9c7af65..000000000 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.hpp +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * Copyright (c) 2017, Bosch Software Innovations GmbH. - * All rights reserved. - * - * 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 Willow Garage, Inc. 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 OWNER 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. - */ - -#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TO_POINT_CLOUD2_HPP_ -#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TO_POINT_CLOUD2_HPP_ - -#include "sensor_msgs/msg/point_cloud.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include "rviz_default_plugins/visibility_control.hpp" - -namespace rviz_default_plugins -{ - -RVIZ_DEFAULT_PLUGINS_PUBLIC -sensor_msgs::msg::PointCloud2::ConstSharedPtr -convertPointCloudToPointCloud2(sensor_msgs::msg::PointCloud::ConstSharedPtr input); - -} // namespace rviz_default_plugins - -#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TO_POINT_CLOUD2_HPP_ diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp index 172c3eaec..5d58d13da 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp @@ -40,7 +40,6 @@ #include "rclcpp/clock.hpp" -#include "rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.hpp" #include "rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp" #include "rviz_common/display.hpp" #include "rviz_common/display_context.hpp" @@ -666,11 +665,6 @@ void PointCloudCommon::setProblematicPointsToInfinity(V_PointCloudPoint & cloud_ } } -void PointCloudCommon::addMessage(const sensor_msgs::msg::PointCloud::ConstSharedPtr cloud) -{ - addMessage(convertPointCloudToPointCloud2(cloud)); -} - void PointCloudCommon::addMessage(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud) { processMessage(cloud); diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp deleted file mode 100644 index c7fae2aab..000000000 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * 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 Willow Garage, Inc. 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 OWNER 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. - */ - -#include "rviz_default_plugins/displays/pointcloud/point_cloud_display.hpp" - -#include -#include - -#include - -#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" -#include "rviz_common/display_context.hpp" -#include "rviz_common/frame_manager_iface.hpp" -#include "rviz_common/properties/int_property.hpp" - -namespace rviz_default_plugins -{ -namespace displays -{ - -PointCloudDisplay::PointCloudDisplay() -: point_cloud_common_(std::make_unique(this)) -{} - -void PointCloudDisplay::onInitialize() -{ - MFDClass::onInitialize(); - point_cloud_common_->initialize(context_, scene_node_); -} - -void PointCloudDisplay::processMessage(const sensor_msgs::msg::PointCloud::ConstSharedPtr cloud) -{ - point_cloud_common_->addMessage(cloud); -} - -void PointCloudDisplay::update(float wall_dt, float ros_dt) -{ - point_cloud_common_->update(wall_dt, ros_dt); -} - -void PointCloudDisplay::reset() -{ - MFDClass::reset(); - point_cloud_common_->reset(); -} - -void PointCloudDisplay::onDisable() -{ - MFDClass::onDisable(); - point_cloud_common_->onDisable(); -} - -} // namespace displays -} // namespace rviz_default_plugins - -#include // NOLINT -PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::PointCloudDisplay, rviz_common::Display) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.cpp deleted file mode 100644 index e109f4794..000000000 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * Copyright (c) 2017, Bosch Software Innovations GmbH. - * All rights reserved. - * - * 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 Willow Garage, Inc. 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 OWNER 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. - */ - -#include "rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.hpp" - -#include - -uint32_t size(size_t value) -{ - return static_cast(value); -} - -sensor_msgs::msg::PointCloud2::ConstSharedPtr rviz_default_plugins::convertPointCloudToPointCloud2( - const sensor_msgs::msg::PointCloud::ConstSharedPtr input) -{ - auto output = std::make_shared(); - output->header = input->header; - output->width = size(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"; - - size_t offset = 0; - for (size_t d = 0; d < output->fields.size(); ++d, offset += sizeof(float)) { - output->fields[d].offset = size(offset); - output->fields[d].datatype = sensor_msgs::msg::PointField::FLOAT32; - } - output->point_step = size(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; - output->is_dense = false; - - // Copy the data points - auto floatData = reinterpret_cast(output->data.data()); - for (size_t cp = 0; cp < input->points.size(); ++cp) { - floatData[(cp * output->point_step + output->fields[0].offset) / sizeof(float)] = - input->points[cp].x; - floatData[(cp * output->point_step + output->fields[1].offset) / sizeof(float)] = - input->points[cp].y; - floatData[(cp * output->point_step + output->fields[2].offset) / sizeof(float)] = - input->points[cp].z; - - for (size_t d = 0; d < input->channels.size(); ++d) { - if (input->channels[d].values.size() == input->points.size()) { - floatData[(cp * output->point_step + output->fields[3 + d].offset) / sizeof(float)] = - input->channels[d].values[cp]; - } - } - } - return output; -} diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp index c722935d0..d3a0b826b 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp @@ -40,7 +40,7 @@ #include "../../publishers/camera_info_publisher.hpp" #include "../../publishers/image_publisher.hpp" #include "../../page_objects/point_cloud_common_page_object.hpp" -#include "../../publishers/point_cloud_publisher.hpp" +#include "../../publishers/point_cloud2_publisher.hpp" class PointCloudDisplayPageObject : public PointCloudCommonPageObject @@ -52,11 +52,11 @@ class PointCloudDisplayPageObject }; TEST_F(VisualTestFixture, test_camera_display_with_published_image) { - auto points = {nodes::createPoint(0, 0, 10)}; + // auto points = {nodes::createPoint(0, 0, 10)}; std::vector publishers = { PublisherWithFrame(std::make_shared(), "image"), - PublisherWithFrame(std::make_shared(), "image_frame"), - PublisherWithFrame(std::make_shared(points), "pointcloud_frame") + PublisherWithFrame(std::make_shared(), "image_frame") + // PublisherWithFrame(std::make_shared(points), "pointcloud_frame") }; auto cam_publisher = std::make_unique(publishers); @@ -67,11 +67,11 @@ TEST_F(VisualTestFixture, test_camera_display_with_published_image) { camera_display->setTopic("/image"); camera_display->collapse(); - auto pointcloud_display = addDisplay(); - pointcloud_display->setTopic("/pointcloud"); - pointcloud_display->setStyle("Spheres"); - pointcloud_display->setSizeMeters(11); - pointcloud_display->setColor(0, 0, 255); + // auto pointcloud_display = addDisplay(); + // pointcloud_display->setTopic("/pointcloud"); + // pointcloud_display->setStyle("Spheres"); + // pointcloud_display->setSizeMeters(11); + // pointcloud_display->setColor(0, 0, 255); captureRenderWindow(camera_display); diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp deleted file mode 100644 index 840c0e935..000000000 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/pointcloud/point_cloud_display_visual_test.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright (c) 2017, Bosch Software Innovations GmbH. - * All rights reserved. - * - * 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 OWNER 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. - */ - -#include -#include - -#include "rviz_visual_testing_framework/visual_test_fixture.hpp" -#include "rviz_visual_testing_framework/visual_test_publisher.hpp" - -#include "../../page_objects/point_cloud_common_page_object.hpp" -#include "../../publishers/point_cloud_publisher.hpp" - -class PointCloudDisplayPageObject - : public PointCloudCommonPageObject -{ -public: - PointCloudDisplayPageObject() - : PointCloudCommonPageObject("PointCloud") - {} -}; - -TEST_F(VisualTestFixture, pointcloud_containing_one_big_point) { - auto pointcloud_publisher = - std::make_unique( - std::make_shared(), "pointcloud_frame"); - - // Set the position of the camera and its sight vector: - setCamPose(Ogre::Vector3(0, 0, 16)); - setCamLookAt(Ogre::Vector3(0, 0, 0)); - - auto pointcloud_display = addDisplay(); - pointcloud_display->setTopic("/pointcloud"); - pointcloud_display->setStyle("Spheres"); - pointcloud_display->setSizeMeters(11); - pointcloud_display->setColor(0, 255, 0); - /// Take the screenshots of the desired render windows: - captureMainWindow(); - - /// Compare test screenshots with the reference ones (if in TEST mode): - assertScreenShotsIdentity(); -} diff --git a/rviz_default_plugins/test/rviz_default_plugins/pointcloud_messages.cpp b/rviz_default_plugins/test/rviz_default_plugins/pointcloud_messages.cpp index 9ee956694..545c22d25 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/pointcloud_messages.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/pointcloud_messages.cpp @@ -213,35 +213,10 @@ sensor_msgs::msg::PointCloud2::ConstSharedPtr create8BitColoredPointCloud2( return cloud; } -sensor_msgs::msg::PointCloud::ConstSharedPtr createPointCloudWithPoints(std::vector points) -{ - auto message = sensor_msgs::msg::PointCloud(); - message.header = std_msgs::msg::Header(); - message.header.stamp = rclcpp::Clock().now(); - message.header.frame_id = "base_link"; - - std::vector points32; - for (auto const & point : points) { - geometry_msgs::msg::Point32 p; - p.x = point.x; - p.y = point.y; - p.z = point.z; - message.points.push_back(p); - } - - return std::make_shared(message); -} - static std::vector points = {{1, 1, 1}, {-1, -1, 1}, {-1, 1, 1}, {1, -1, 1}}; sensor_msgs::msg::PointCloud2::ConstSharedPtr createPointCloud2WithSquare() { return createPointCloud2WithPoints(points); } - -sensor_msgs::msg::PointCloud::ConstSharedPtr createPointCloudWithSquare() -{ - return createPointCloudWithPoints(points); -} - } // namespace rviz_default_plugins diff --git a/rviz_default_plugins/test/rviz_default_plugins/pointcloud_messages.hpp b/rviz_default_plugins/test/rviz_default_plugins/pointcloud_messages.hpp index ad19cfc60..f7603d7a4 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/pointcloud_messages.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/pointcloud_messages.hpp @@ -60,9 +60,6 @@ struct ColoredPoint : Point : Point(x, y, z), r(r), g(g), b(b) {} }; -sensor_msgs::msg::PointCloud::ConstSharedPtr createPointCloudWithSquare(); -sensor_msgs::msg::PointCloud::ConstSharedPtr createPointCloudWithPoints(std::vector points); - sensor_msgs::msg::PointCloud2::ConstSharedPtr createPointCloud2WithSquare(); sensor_msgs::msg::PointCloud2::SharedPtr createPointCloud2WithPoints( const std::vector & points); diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/point_cloud_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/point_cloud_publisher.hpp deleted file mode 100644 index dbb4d2701..000000000 --- a/rviz_default_plugins/test/rviz_default_plugins/publishers/point_cloud_publisher.hpp +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright (c) 2018, Bosch Software Innovations GmbH. - * All rights reserved. - * - * 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 Willow Garage, Inc. 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 OWNER 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. - */ - -#ifndef RVIZ_DEFAULT_PLUGINS__PUBLISHERS__POINT_CLOUD_PUBLISHER_HPP_ -#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__POINT_CLOUD_PUBLISHER_HPP_ - -#include -#include - -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2/LinearMath/Quaternion.h" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/header.hpp" -#include "sensor_msgs/msg/point_cloud.hpp" - -using namespace std::chrono_literals; // NOLINT - -namespace nodes -{ - -geometry_msgs::msg::Point32 createPoint(float x, float y, float z) -{ - geometry_msgs::msg::Point32 point; - point.x = x; - point.y = y; - point.z = z; - - return point; -} - -class PointCloudPublisher : public rclcpp::Node -{ -public: - explicit PointCloudPublisher( - std::vector points = {createPoint(0, 0, 0)}) - : Node("pointcloud_publisher"), - timer_(nullptr), - publisher_(nullptr), - points_(points) - { - publisher_ = this->create_publisher("pointcloud", 10); - timer_ = this->create_wall_timer(500ms, std::bind(&PointCloudPublisher::timer_callback, this)); - } - -private: - void timer_callback() - { - auto message = sensor_msgs::msg::PointCloud(); - message.header = std_msgs::msg::Header(); - message.header.frame_id = "pointcloud_frame"; - message.header.stamp = rclcpp::Clock().now(); - message.points = points_; - - publisher_->publish(message); - } - - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - std::vector points_; -}; - -} // namespace nodes - -#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__POINT_CLOUD_PUBLISHER_HPP_ From d250e12735466a2c8ef00a70a8048fbada3fe126 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 10 Jul 2024 11:02:58 +0200 Subject: [PATCH 2/2] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rviz2/test/tools/send_lots_of_points_node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rviz2/test/tools/send_lots_of_points_node.cpp b/rviz2/test/tools/send_lots_of_points_node.cpp index 571b12a5c..e3789dfaa 100644 --- a/rviz2/test/tools/send_lots_of_points_node.cpp +++ b/rviz2/test/tools/send_lots_of_points_node.cpp @@ -89,9 +89,9 @@ int main(int argc, char ** argv) *iter_z = ((x + y + count) % 100) / 100.0f; } - *iter_r = rand() % 256; - *iter_g = rand() % 256; - *iter_b = rand() % 256; + *iter_r = 255; + *iter_g = 0; + *iter_b = 0; ++iter_x; ++iter_y;