Add search helper for goal angle tolerance (#317)

Author: renan028

mbf_costmap_nav/CMakeLists.txt

@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(mbf_costmap_nav)
 
+set(CMAKE_CXX_STANDARD 17)
+
 find_package(catkin REQUIRED
   COMPONENTS
   costmap_2d
@@ -76,6 +78,8 @@ add_library(${MBF_COSTMAP_2D_SERVER_LIB}
   src/mbf_costmap_nav/costmap_recovery_execution.cpp
   src/mbf_costmap_nav/costmap_wrapper.cpp
   src/mbf_costmap_nav/footprint_helper.cpp
+  src/mbf_costmap_nav/free_pose_search.cpp
+  src/mbf_costmap_nav/free_pose_search_viz.cpp
 )
 add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${MBF_COSTMAP_2D_SERVER_LIB} ${MBF_NAV_CORE_WRAPPER_LIB})
@@ -108,3 +112,20 @@ install(DIRECTORY include/${PROJECT_NAME}/
 catkin_install_python(PROGRAMS scripts/move_base_legacy_relay.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+
+#############
+## Test    ##
+#############
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  find_package(map_server REQUIRED)
+
+  add_rostest_gtest(free_pose_search_test
+    test/free_pose_search.test
+    test/free_pose_search_test.cpp
+  )
+  target_link_libraries(free_pose_search_test
+    ${MBF_COSTMAP_2D_SERVER_LIB}
+  )
+endif()

mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h

@@ -47,6 +47,7 @@
 #include <mbf_msgs/CheckPath.h>
 #include <mbf_msgs/CheckPose.h>
 #include <mbf_msgs/CheckPoint.h>
+#include <mbf_msgs/FindValidPose.h>
 
 #include <nav_core/base_global_planner.h>
 #include <nav_core/base_local_planner.h>
@@ -71,8 +72,8 @@ namespace mbf_costmap_nav
  * @brief Classes belonging to the Move Base Server level.
  */
 
-
-typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> > DynamicReconfigureServerCostmapNav;
+typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> >
+    DynamicReconfigureServerCostmapNav;
 
 /// @brief A mapping from a string to a map-ptr.
 typedef boost::unordered_map<std::string, CostmapWrapper::Ptr> StringToMap;
@@ -88,14 +89,13 @@ typedef boost::unordered_map<std::string, CostmapWrapper::Ptr> StringToMap;
 class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServer
 {
 public:
-
   typedef boost::shared_ptr<CostmapNavigationServer> Ptr;
 
   /**
    * @brief Constructor
    * @param tf_listener_ptr Shared pointer to a common TransformListener
    */
-  CostmapNavigationServer(const TFPtr &tf_listener_ptr);
+  CostmapNavigationServer(const TFPtr& tf_listener_ptr);
 
   /**
    * @brief Destructor
@@ -105,62 +105,56 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe
   virtual void stop();
 
 private:
-
   /**
    * @brief Create a new planner execution.
    * @param plugin_name Name of the planner to use.
    * @param plugin_ptr Shared pointer to the plugin to use.
    * @return Shared pointer to a new @ref planner_execution "PlannerExecution".
    */
-  virtual mbf_abstract_nav::AbstractPlannerExecution::Ptr newPlannerExecution(
-      const std::string &plugin_name,
-      const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr);
+  virtual mbf_abstract_nav::AbstractPlannerExecution::Ptr
+  newPlannerExecution(const std::string& plugin_name, const mbf_abstract_core::AbstractPlanner::Ptr& plugin_ptr);
 
   /**
    * @brief Create a new controller execution.
    * @param plugin_name Name of the controller to use.
    * @param plugin_ptr Shared pointer to the plugin to use.
    * @return Shared pointer to a new @ref controller_execution "ControllerExecution".
    */
-  virtual mbf_abstract_nav::AbstractControllerExecution::Ptr newControllerExecution(
-      const std::string &plugin_name,
-      const mbf_abstract_core::AbstractController::Ptr &plugin_ptr);
+  virtual mbf_abstract_nav::AbstractControllerExecution::Ptr
+  newControllerExecution(const std::string& plugin_name, const mbf_abstract_core::AbstractController::Ptr& plugin_ptr);
 
   /**
    * @brief Create a new recovery behavior execution.
    * @param plugin_name Name of the recovery behavior to run.
    * @param plugin_ptr Shared pointer to the plugin to use
    * @return Shared pointer to a new @ref recovery_execution "RecoveryExecution".
    */
-  virtual mbf_abstract_nav::AbstractRecoveryExecution::Ptr newRecoveryExecution(
-      const std::string &plugin_name,
-      const mbf_abstract_core::AbstractRecovery::Ptr &plugin_ptr);
+  virtual mbf_abstract_nav::AbstractRecoveryExecution::Ptr
+  newRecoveryExecution(const std::string& plugin_name, const mbf_abstract_core::AbstractRecovery::Ptr& plugin_ptr);
 
   /**
    * @brief Loads the plugin associated with the given planner_type parameter.
    * @param planner_type The type of the planner plugin to load.
    * @return true, if the local planner plugin was successfully loaded.
    */
-  virtual mbf_abstract_core::AbstractPlanner::Ptr loadPlannerPlugin(const std::string &planner_type);
+  virtual mbf_abstract_core::AbstractPlanner::Ptr loadPlannerPlugin(const std::string& planner_type);
 
   /**
    * @brief Initializes the controller plugin with its name and pointer to the costmap
    * @param name The name of the planner
    * @param planner_ptr pointer to the planner object which corresponds to the name param
    * @return true if init succeeded, false otherwise
    */
-  virtual bool initializePlannerPlugin(
-      const std::string &name,
-      const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr
-  );
+  virtual bool initializePlannerPlugin(const std::string& name,
+                                       const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr);
 
   /**
    * @brief Loads the plugin associated with the given controller type parameter
    * @param controller_type The type of the controller plugin
    * @return A shared pointer to a new loaded controller, if the controller plugin was loaded successfully,
    *         an empty pointer otherwise.
    */
-  virtual mbf_abstract_core::AbstractController::Ptr loadControllerPlugin(const std::string &controller_type);
+  virtual mbf_abstract_core::AbstractController::Ptr loadControllerPlugin(const std::string& controller_type);
 
   /**
    * @brief Initializes the controller plugin with its name, a pointer to the TransformListener
@@ -169,69 +163,82 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe
    * @param controller_ptr pointer to the controller object which corresponds to the name param
    * @return true if init succeeded, false otherwise
    */
-  virtual bool initializeControllerPlugin(
-      const std::string &name,
-      const mbf_abstract_core::AbstractController::Ptr &controller_ptr
-  );
+  virtual bool initializeControllerPlugin(const std::string& name,
+                                          const mbf_abstract_core::AbstractController::Ptr& controller_ptr);
 
   /**
    * @brief Loads a Recovery plugin associated with given recovery type parameter
    * @param recovery_name The name of the Recovery plugin
    * @return A shared pointer to a Recovery plugin, if the plugin was loaded successfully, an empty pointer otherwise.
    */
-  virtual mbf_abstract_core::AbstractRecovery::Ptr loadRecoveryPlugin(const std::string &recovery_type);
+  virtual mbf_abstract_core::AbstractRecovery::Ptr loadRecoveryPlugin(const std::string& recovery_type);
 
   /**
    * @brief Initializes a recovery behavior plugin with its name and pointers to the global and local costmaps
    * @param name The name of the recovery behavior
    * @param behavior_ptr pointer to the recovery behavior object which corresponds to the name param
    * @return true if init succeeded, false otherwise
    */
-  virtual bool initializeRecoveryPlugin(
-      const std::string &name,
-      const mbf_abstract_core::AbstractRecovery::Ptr &behavior_ptr);
+  virtual bool initializeRecoveryPlugin(const std::string& name,
+                                        const mbf_abstract_core::AbstractRecovery::Ptr& behavior_ptr);
+
+  /**
+   * @brief If mbf_msgs::CheckPose::Request::LOCAL_COSTMAP the local costmap is returned
+   * if mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP, the global costmap is returned.
+   * Otherwise, it returns an empty pointer.
+   * @param costmap_type The type of the costmap to return
+   * @return A pair: string which is the type of the costmap; and the shared pointer to the requested costmap.
+   * If costmap is not valid, it returns an empty pointer and an empty string.
+   */
+  std::pair<std::string, CostmapWrapper::Ptr>
+  requestedCostmap(mbf_msgs::CheckPose::Request::_costmap_type costmap_type) const;
 
   /**
    * @brief Callback method for the check_point_cost service
    * @param request Request object, see the mbf_msgs/CheckPoint service definition file.
    * @param response Response object, see the mbf_msgs/CheckPoint service definition file.
    * @return true, if the service completed successfully, false otherwise
    */
-  bool callServiceCheckPointCost(mbf_msgs::CheckPoint::Request &request,
-                                 mbf_msgs::CheckPoint::Response &response);
+  bool callServiceCheckPointCost(mbf_msgs::CheckPoint::Request& request, mbf_msgs::CheckPoint::Response& response);
 
   /**
    * @brief Callback method for the check_pose_cost service
    * @param request Request object, see the mbf_msgs/CheckPose service definition file.
    * @param response Response object, see the mbf_msgs/CheckPose service definition file.
    * @return true, if the service completed successfully, false otherwise
    */
-  bool callServiceCheckPoseCost(mbf_msgs::CheckPose::Request &request,
-                                mbf_msgs::CheckPose::Response &response);
+  bool callServiceCheckPoseCost(mbf_msgs::CheckPose::Request& request, mbf_msgs::CheckPose::Response& response);
 
   /**
    * @brief Callback method for the check_path_cost service
    * @param request Request object, see the mbf_msgs/CheckPath service definition file.
    * @param response Response object, see the mbf_msgs/CheckPath service definition file.
    * @return true, if the service completed successfully, false otherwise
    */
-  bool callServiceCheckPathCost(mbf_msgs::CheckPath::Request &request,
-                                mbf_msgs::CheckPath::Response &response);
+  bool callServiceCheckPathCost(mbf_msgs::CheckPath::Request& request, mbf_msgs::CheckPath::Response& response);
 
   /**
    * @brief Callback method for the make_plan service
    * @param request Empty request object.
    * @param response Empty response object.
    * @return true, if the service completed successfully, false otherwise
    */
-  bool callServiceClearCostmaps(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response);
+  bool callServiceClearCostmaps(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
+
+  /**
+   * @brief Callback method for the find valid pose service
+   * @param request FindValidPose request object.
+   * @param response FindValidPose response object.
+   * @return true, if the service completed successfully, false otherwise
+   */
+  bool callServiceFindValidPose(mbf_msgs::FindValidPose::Request& request, mbf_msgs::FindValidPose::Response& response);
 
   /**
    * @brief Reconfiguration method called by dynamic reconfigure.
    * @param config Configuration parameters. See the MoveBaseFlexConfig definition.
    * @param level bit mask, which parameters are set.
    */
-  void reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &config, uint32_t level);
+  void reconfigure(mbf_costmap_nav::MoveBaseFlexConfig& config, uint32_t level);
 
   pluginlib::ClassLoader<mbf_costmap_core::CostmapRecovery> recovery_plugin_loader_;
   pluginlib::ClassLoader<nav_core::RecoveryBehavior> nav_core_recovery_plugin_loader_;
@@ -275,6 +282,11 @@ class CostmapNavigationServer : public mbf_abstract_nav::AbstractNavigationServe
 
   //! Service Server for the clear_costmap service
   ros::ServiceServer clear_costmaps_srv_;
+
+  //! Service Server for the find_valid_pose service
+  ros::ServiceServer find_valid_pose_srv_;
+
+  static constexpr double ANGLE_INCREMENT = 5.0 * M_PI / 180.0;  // 5 degrees
 };
 
 } /* namespace mbf_costmap_nav */

mbf_costmap_nav/include/mbf_costmap_nav/footprint_helper.h

@@ -48,7 +48,7 @@ namespace mbf_costmap_nav
 
 struct Cell
 {
-  unsigned int x, y;
+  unsigned int x, y, cost;
 };
 
 class FootprintHelper
@@ -66,6 +66,17 @@ class FootprintHelper
                                              const std::vector<geometry_msgs::Point>& footprint_spec,
                                              const costmap_2d::Costmap2D&, bool fill);
 
+    /**
+   * @brief  Supercover algorithm is a modified Bresenham which prints ALL the points (not only one point per axis) the ideal line contains
+   * ref: http://eugen.dedu.free.fr/projects/bresenham/
+   * @param  x0 The x coordinate of the first point
+   * @param  x1 The x coordinate of the second point
+   * @param  y0 The y coordinate of the first point
+   * @param  y1 The y coordinate of the second point
+   * @param  pts Will be filled with the cells that lie on the line in the grid
+   */
+  static void supercover(int x0, int x1, int y0, int y1, std::vector<Cell>& pts);
+
   /**
    * @brief  Use Bresenham's algorithm to trace a line between two points in a grid
    * @param  x0 The x coordinate of the first point