From 7574f1b89e2b47c69f671e0d14f4ba536e71f664 Mon Sep 17 00:00:00 2001 From: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com> Date: Thu, 9 Jan 2025 09:29:06 +0100 Subject: [PATCH] fix: OctoMap and Filtered_Cloud Not Updating During Movement Execution (#3209) * fix: adds MutuallyExclusive to pointcloud subscriber to allow octomap updates during motion * refactor: adjust macro for readability (cherry picked from commit 246aa58e4abf8bb982149a4ab8d579bab747d8b8) # Conflicts: # moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp # moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp --- .../pointcloud_octomap_updater.hpp | 113 ++++++++++++++++++ .../src/pointcloud_octomap_updater.cpp | 14 +++ 2 files changed, 127 insertions(+) create mode 100644 moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp new file mode 100644 index 0000000000..cfe085e8c0 --- /dev/null +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp @@ -0,0 +1,113 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, 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 Willow Garage 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. + *********************************************************************/ + +/* Author: Jon Binney, Ioan Sucan */ + +#pragma once + +#include +#include +#include +#include +#include +#if RCLCPP_VERSION_GTE(28, 3, 3) // Rolling +#include +#else +#include +#endif +#include +#include +#include + +#include + +namespace occupancy_map_monitor +{ +class PointCloudOctomapUpdater : public OccupancyMapUpdater +{ +public: + PointCloudOctomapUpdater(); + ~PointCloudOctomapUpdater() override{}; + + bool setParams(const std::string& name_space) override; + + bool initialize(const rclcpp::Node::SharedPtr& node) override; + void start() override; + void stop() override; + ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape) override; + void forgetShape(ShapeHandle handle) override; + +protected: + virtual void updateMask(const sensor_msgs::msg::PointCloud2& cloud, const Eigen::Vector3d& sensor_origin, + std::vector& mask); + +private: + bool getShapeTransform(ShapeHandle h, Eigen::Isometry3d& transform) const; + void cloudMsgCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud_msg); + + // TODO: Enable private node for publishing filtered point cloud + // ros::NodeHandle root_nh_; + // ros::NodeHandle private_nh_; + rclcpp::Node::SharedPtr node_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Initialize clock type to RCL_ROS_TIME to prevent exception about time sources mismatch + rclcpp::Time last_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + + /* params */ + std::string point_cloud_topic_; + double scale_; + double padding_; + double max_range_; + unsigned int point_subsample_; + double max_update_rate_; + std::string filtered_cloud_topic_; + std::string ns_; + rclcpp::Publisher::SharedPtr filtered_cloud_publisher_; + + message_filters::Subscriber* point_cloud_subscriber_; + tf2_ros::MessageFilter* point_cloud_filter_; + + /* used to store all cells in the map which a given ray passes through during raycasting. + we cache this here because it dynamically pre-allocates a lot of memory in its constructor */ + octomap::KeyRay key_ray_; + + std::unique_ptr shape_mask_; + std::vector mask_; + + rclcpp::Logger logger_; +}; +} // namespace occupancy_map_monitor diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index bf478f4f00..04138f1833 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -115,8 +115,22 @@ void PointCloudOctomapUpdater::start() if (point_cloud_subscriber_) return; + + rclcpp::SubscriptionOptions options; + options.callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); /* subscribe to point cloud topic using tf filter*/ +<<<<<<< HEAD point_cloud_subscriber_ = new message_filters::Subscriber(node_, point_cloud_topic_); +======= + auto qos_profile = +#if RCLCPP_VERSION_GTE(28, 3, 0) + rclcpp::SensorDataQoS(); +#else + rmw_qos_profile_sensor_data; +#endif + point_cloud_subscriber_ = + new message_filters::Subscriber(node_, point_cloud_topic_, qos_profile, options); +>>>>>>> 246aa58e4 (fix: OctoMap and Filtered_Cloud Not Updating During Movement Execution (#3209)) if (tf_listener_ && tf_buffer_ && !monitor_->getMapFrame().empty()) { point_cloud_filter_ = new tf2_ros::MessageFilter(