OccupancyMapMonitor tests using Dependency Injection

moveit_ros/occupancy_map_monitor/CMakeLists.txt

@@ -45,6 +45,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 
 add_library(${MOVEIT_LIB_NAME} SHARED
   src/occupancy_map_monitor.cpp
+  src/occupancy_map_monitor_middleware_handle.cpp
   src/occupancy_map_updater.cpp
 )
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
@@ -86,6 +87,15 @@ if(BUILD_TESTING)
 
   # Run all lint tests in package.xml except those listed above
   ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gmock REQUIRED)
+
+  ament_add_gmock(occupancy_map_monitor_tests
+    test/occupancy_map_monitor_tests.cpp
+  )
+  target_link_libraries(occupancy_map_monitor_tests
+    ${MOVEIT_LIB_NAME}
+  )
 endif()
 
 ament_package(CONFIG_EXTRAS ConfigExtras.cmake)

moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h

@@ -37,10 +37,13 @@
 #pragma once
 
 #include <octomap/octomap.h>
+
 #include <boost/thread/locks.hpp>
 #include <boost/thread/shared_mutex.hpp>
 #include <boost/function.hpp>
+
 #include <memory>
+#include <string>
 
 namespace occupancy_map_monitor
 {

moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h

@@ -36,127 +36,307 @@
 
 #pragma once
 
-#include <vector>
-#include <string>
-#include <rclcpp/rclcpp.hpp>
-#include <tf2_ros/buffer.h>
-#include <pluginlib/class_loader.hpp>
-
-#include <moveit_msgs/srv/save_map.hpp>
-#include <moveit_msgs/srv/load_map.hpp>
 #include <moveit/occupancy_map_monitor/occupancy_map.h>
 #include <moveit/occupancy_map_monitor/occupancy_map_updater.h>
+#include <moveit_msgs/srv/load_map.hpp>
+#include <moveit_msgs/srv/save_map.hpp>
 
-#include <boost/thread/mutex.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_ros/buffer.h>
 
+#include <functional>
 #include <memory>
+#include <mutex>
+#include <string>
+#include <utility>
+#include <vector>
 
 namespace occupancy_map_monitor
 {
 class OccupancyMapMonitor
 {
 public:
+  /**
+   * @brief      This class contains the rcl interfaces for easier testing
+   */
+  class MiddlewareHandle
+  {
+  public:
+    using SaveMapServiceCallback = std::function<bool(const std::shared_ptr<rmw_request_id_t> request_header,
+                                                      const std::shared_ptr<moveit_msgs::srv::SaveMap::Request> request,
+                                                      std::shared_ptr<moveit_msgs::srv::SaveMap::Response> response)>;
+    using LoadMapServiceCallback = std::function<bool(const std::shared_ptr<rmw_request_id_t> request_header,
+                                                      const std::shared_ptr<moveit_msgs::srv::LoadMap::Request> request,
+                                                      std::shared_ptr<moveit_msgs::srv::LoadMap::Response> response)>;
+
+    /**
+     * @brief      Destroys the object.  Needed because this is pure virtual interface.
+     */
+    virtual ~MiddlewareHandle() = default;
+
+    /**
+     * @brief      Getter for the map resolution parameter.
+     *
+     * @return     The map resolution parameter.
+     */
+    virtual double getMapResolutionParameter() const = 0;
+
+    /**
+     * @brief      Getter for the map frame parameter.
+     *
+     * @return     The map frame parameter.
+     */
+    virtual std::string getMapFrameParameter() const = 0;
+
+    /**
+     * @brief      Getter for the list of sensor plugins that can be loaded.
+     *
+     * @return     A list of pairs of sensor names and plugin types for use by loadOccupancyMapUpdater.
+     */
+    virtual std::vector<std::pair<std::string, std::string>> getSensorPluginsParameter() const = 0;
+
+    /**
+     * @brief      Loads an occupancy map updater based on string.
+     *
+     * @param[in]  sensor_plugin  The sensor plugin string.
+     *
+     * @return     The occupancy map updater pointer.
+     */
+    virtual OccupancyMapUpdaterPtr loadOccupancyMapUpdater(const std::string& sensor_plugin) = 0;
+
+    /**
+     * @brief      Initializes the occupancy map updater.  Needed because of interface to OccupancyMapUpdater
+     *
+     * @param[in]  occupancy_map_updater  The occupancy map updater
+     */
+    virtual void initializeOccupancyMapUpdater(OccupancyMapUpdaterPtr occupancy_map_updater) = 0;
+
+    /**
+     * @brief      Creates a save map service.
+     *
+     * @param[in]  callback  The callback
+     */
+    virtual void createSaveMapService(SaveMapServiceCallback callback) = 0;
+
+    /**
+     * @brief      Creates a load map service.
+     *
+     * @param[in]  callback  The callback
+     */
+    virtual void createLoadMapService(LoadMapServiceCallback callback) = 0;
+  };
+
+  /**
+   * @brief      Occupancy map monitor constructor with the MiddlewareHandle
+   *
+   * @param[in]  middleware_handle   The rcl interface
+   * @param[in]  tf_buffer           The tf buffer
+   */
+  OccupancyMapMonitor(std::unique_ptr<MiddlewareHandle> middleware_handle,
+                      const std::shared_ptr<tf2_ros::Buffer>& tf_buffer);
+
+  /**
+   * @brief      Occupancy map monitor constructor with Node
+   *
+   * @param[in]  node            The node
+   * @param[in]  tf_buffer       The tf buffer
+   * @param[in]  map_frame       The map frame
+   * @param[in]  map_resolution  The map resolution
+   */
   OccupancyMapMonitor(const rclcpp::Node::SharedPtr& node, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
                       const std::string& map_frame = "", double map_resolution = 0.0);
+
+  /**
+   * @brief      Occupancy map monitor constructor with Node
+   *
+   * @param[in]  node            The node
+   * @param[in]  map_resolution  The map resolution
+   */
   OccupancyMapMonitor(const rclcpp::Node::SharedPtr& node, double map_resolution = 0.0);
 
+  /**
+   * @brief      Destroys the object.
+   */
   ~OccupancyMapMonitor();
 
-  /** @brief start the monitor (will begin updating the octomap */
+  /**
+   * @brief start the monitor (will begin updating the octomap
+   */
   void startMonitor();
 
+  /**
+   * @brief      Stops the monitor, this also stops the updaters.
+   */
   void stopMonitor();
 
-  /** @brief Get a pointer to the underlying octree for this monitor. Lock the tree before reading or writing using this
-   *  pointer. The value of this pointer stays the same throughout the existance of the monitor instance. */
+  /**
+   * @brief Get a pointer to the underlying octree for this monitor. Lock the tree before reading or writing using this
+   *  pointer. The value of this pointer stays the same throughout the existance of the monitor instance.
+   */
   const OccMapTreePtr& getOcTreePtr()
   {
     return tree_;
   }
 
-  /** @brief Get a const pointer to the underlying octree for this monitor. Lock the
-   *  tree before reading this pointer */
+  /**
+   * @brief Get a const pointer to the underlying octree for this monitor. Lock the
+   *  tree before reading this pointer
+   */
   const OccMapTreeConstPtr& getOcTreePtr() const
   {
     return tree_const_;
   }
 
+  /**
+   * @brief      Gets the map frame (this is set either by the constor or a parameter).
+   *
+   * @return     The map frame.
+   */
   const std::string& getMapFrame() const
   {
     return map_frame_;
   }
 
+  /**
+   * @brief      Sets the map frame.
+   *
+   * @param[in]  frame  The frame
+   */
   void setMapFrame(const std::string& frame);
 
+  /**
+   * @brief      Gets the map resolution.
+   *
+   * @return     The map resolution.
+   */
   double getMapResolution() const
   {
     return map_resolution_;
   }
 
+  /**
+   * @brief      Gets the tf client.
+   *
+   * @return     The tf client.
+   */
   const std::shared_ptr<tf2_ros::Buffer>& getTFClient() const
   {
     return tf_buffer_;
   }
 
+  /**
+   * @brief      Adds an OccupancyMapUpdater to be monitored.
+   *
+   * @param[in]  updater  The OccupancyMapUpdater
+   */
   void addUpdater(const OccupancyMapUpdaterPtr& updater);
 
-  /** \brief Add this shape to the set of shapes to be filtered out from the octomap */
+  /**
+   * @brief      Add this shape to the set of shapes to be filtered out from the octomap
+   *
+   * @param[in]  shape  The shape to be exclueded from the updaters.
+   *
+   * @return     The shape handle.  Can be used to forget the shape later.
+   */
   ShapeHandle excludeShape(const shapes::ShapeConstPtr& shape);
 
-  /** \brief Forget about this shape handle and the shapes it corresponds to */
+  /**
+   * \brief Forget about this shape handle and the shapes it corresponds to
+   *
+   * @param[in]  handle  The handle to forget.
+   */
   void forgetShape(ShapeHandle handle);
 
-  /** @brief Set the callback to trigger when updates to the maintained octomap are received */
+  /**
+   * @brief      Set the callback to trigger when updates to the maintained octomap are received
+   *
+   * @param[in]  update_callback  The update callback function
+   */
   void setUpdateCallback(const boost::function<void()>& update_callback)
   {
     tree_->setUpdateCallback(update_callback);
   }
 
+  /**
+   * @brief      Sets the transform cache callback.
+   *
+   * @param[in]  transform_cache_callback  The transform cache callback
+   */
   void setTransformCacheCallback(const TransformCacheProvider& transform_cache_callback);
 
+  /**
+   * @brief      Set the debug flag on the updaters.
+   *
+   * @param[in]  flag  The flag
+   */
   void publishDebugInformation(bool flag);
 
+  /**
+   * @brief      Determines if active.
+   *
+   * @return     True if active, False otherwise.
+   */
   bool isActive() const
   {
     return active_;
   }
 
 private:
-  void initialize();
-
-  /** @brief Save the current octree to a binary file */
+  /**
+   * @brief      Save the current octree to a binary file
+   *
+   * @param[in]  request_header  The request header
+   * @param[in]  request         The request
+   * @param[in]  response        The response
+   *
+   * @return     True on success, False otherwise.
+   */
   bool saveMapCallback(const std::shared_ptr<rmw_request_id_t> request_header,
                        const std::shared_ptr<moveit_msgs::srv::SaveMap::Request> request,
                        std::shared_ptr<moveit_msgs::srv::SaveMap::Response> response);
 
-  /** @brief Load octree from a binary file (gets rid of current octree data) */
+  /**
+   * @brief      Load octree from a binary file (gets rid of current octree data)
+   *
+   * @param[in]  request_header  The request header
+   * @param[in]  request         The request
+   * @param[in]  response        The response
+   *
+   * @return     True on success, False otherwise.
+   */
   bool loadMapCallback(const std::shared_ptr<rmw_request_id_t> request_header,
                        const std::shared_ptr<moveit_msgs::srv::LoadMap::Request> request,
                        std::shared_ptr<moveit_msgs::srv::LoadMap::Response> response);
 
+  /**
+   * @brief      Gets the shape transform cache.
+   *
+   * @param[in]  index         The index
+   * @param[in]  target_frame  The target frame
+   * @param[in]  target_time   The target time
+   * @param[out] cache         The cache
+   *
+   * @return     True on success, False otherwise.
+   */
   bool getShapeTransformCache(std::size_t index, const std::string& target_frame, const rclcpp::Time& target_time,
                               ShapeTransformCache& cache) const;
 
-  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
-  std::string map_frame_;
-  double map_resolution_;
-  boost::mutex parameters_lock_;
-
-  OccMapTreePtr tree_;
-  OccMapTreeConstPtr tree_const_;
+  std::unique_ptr<MiddlewareHandle> middleware_handle_; /*!< The abstract interface to ros */
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;          /*!< TF buffer */
+  std::string map_frame_;                               /*!< Map frame parameter */
+  double map_resolution_;                               /*!< Map resolution parameter */
+  std::mutex parameters_lock_;                          /*!< Mutex for synchronizing access to parameters */
 
-  std::unique_ptr<pluginlib::ClassLoader<OccupancyMapUpdater> > updater_plugin_loader_;
-  std::vector<OccupancyMapUpdaterPtr> map_updaters_;
-  std::vector<std::map<ShapeHandle, ShapeHandle> > mesh_handles_;
-  TransformCacheProvider transform_cache_callback_;
-  bool debug_info_;
+  OccMapTreePtr tree_;            /*!< Oct map tree */
+  OccMapTreeConstPtr tree_const_; /*!< Shared pointer to a const oct map tree */
 
-  std::size_t mesh_handle_count_;
+  std::vector<OccupancyMapUpdaterPtr> map_updaters_;             /*!< The Occupancy map updaters */
+  std::vector<std::map<ShapeHandle, ShapeHandle>> mesh_handles_; /*!< The mesh handles */
+  TransformCacheProvider transform_cache_callback_;              /*!< Callback for the transform cache */
+  bool debug_info_;                                              /*!< Enable/disable debug output */
 
-  rclcpp::Node::SharedPtr node_;
-  rclcpp::Service<moveit_msgs::srv::SaveMap>::SharedPtr save_map_srv_;
-  rclcpp::Service<moveit_msgs::srv::LoadMap>::SharedPtr load_map_srv_;
+  std::size_t mesh_handle_count_; /*!< Count of mesh handles */
 
-  bool active_;
+  bool active_; /*!< True when actively monitoring updaters */
 };
 }  // namespace occupancy_map_monitor