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 246aa58)

# 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

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 <rclcpp/rclcpp.hpp>
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/version.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/message_filter.h>
+#if RCLCPP_VERSION_GTE(28, 3, 3)  // Rolling
+#include <message_filters/subscriber.hpp>
+#else
+#include <message_filters/subscriber.h>
+#endif
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <moveit/occupancy_map_monitor/occupancy_map_updater.hpp>
+#include <moveit/point_containment_filter/shape_mask.hpp>
+
+#include <memory>
+
+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<int>& 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<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> 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<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_cloud_publisher_;
+
+  message_filters::Subscriber<sensor_msgs::msg::PointCloud2>* point_cloud_subscriber_;
+  tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>* 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<point_containment_filter::ShapeMask> shape_mask_;
+  std::vector<int> mask_;
+
+  rclcpp::Logger logger_;
+};
+}  // namespace occupancy_map_monitor

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<sensor_msgs::msg::PointCloud2>(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<sensor_msgs::msg::PointCloud2>(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<sensor_msgs::msg::PointCloud2>(