Merge pull request #38 from naturerobots/humble-port-mbf-mesh-nav

Humble port mbf mesh nav

mbf_mesh_core/include/mbf_mesh_core/mesh_planner.h

@@ -70,7 +70,7 @@ class MeshPlanner : public mbf_abstract_core::AbstractPlanner
    * @param message Optional more detailed outcome as a string
    * @return Result code as described on GetPath action result. (see GetPath.action)
    */
-  virtual uint32_t makePlan(const geometry_msgs::msg::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
+  virtual uint32_t makePlan(const geometry_msgs::msg::PoseStamped& start, const geometry_msgs::msg::PoseStamped& goal,
                             double tolerance, std::vector<geometry_msgs::msg::PoseStamped>& plan, double& cost,
                             std::string& message) = 0;
 

mbf_mesh_nav/CMakeLists.txt

@@ -1,39 +1,36 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(mbf_mesh_nav)
 
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  mbf_mesh_core
+find_package(ament_cmake REQUIRED)
+set(dependencies
+  geometry_msgs
+  LVR2
   mbf_abstract_nav
+  mbf_mesh_core
+  mbf_msgs
   mesh_map
-  dynamic_reconfigure
+  nav_msgs
   pluginlib
+  rclcpp
+  std_srvs
 )
+foreach(dep IN LISTS dependencies)
+  find_package(${dep} REQUIRED)
+endforeach()
 
-find_package(PkgConfig REQUIRED)
-pkg_check_modules(JSONCPP jsoncpp)
-
-find_package(LVR2 2 REQUIRED)
-
-generate_dynamic_reconfigure_options(
-  cfg/MoveBaseFlex.cfg
+include_directories(
+  include
 )
 
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES mbf_mesh_server
-  CATKIN_DEPENDS roscpp mbf_mesh_core mesh_map dynamic_reconfigure mbf_abstract_nav pluginlib
-  DEPENDS LVR2
-)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(JSONCPP jsoncpp)
 
 include_directories(
   include
-  ${catkin_INCLUDE_DIRS}
-  ${LVR2_INCLUDE_DIRS}
 )
 
 add_library(mbf_mesh_server
@@ -42,31 +39,28 @@ add_library(mbf_mesh_server
   src/mesh_planner_execution.cpp
   src/mesh_recovery_execution.cpp
 )
-
-add_dependencies(mbf_mesh_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-target_link_libraries(mbf_mesh_server
-  ${catkin_LIBRARIES}
-  ${JSONCPP_LIBRARIES}
-)
+ament_target_dependencies(mbf_mesh_server ${dependencies})
+target_link_libraries(mbf_mesh_server ${JSONCPP_LIBRARIES})
 
 add_executable(${PROJECT_NAME} src/mbf_mesh_nav.cpp)
-
-add_dependencies(${PROJECT_NAME} mbf_mesh_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
+ament_target_dependencies(${PROJECT_NAME} ${dependencies} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} mbf_mesh_server)
 target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
   ${JSONCPP_LIBRARIES}
   mbf_mesh_server
 )
 
-install(TARGETS ${PROJECT_NAME} mbf_mesh_server
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+install(DIRECTORY include/
+  DESTINATION include
 )
 
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-  FILES_MATCHING PATTERN "*.h"
+install(TARGETS ${PROJECT_NAME} mbf_mesh_server
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
 )
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(${dependencies})
+ament_package()

mbf_mesh_nav/include/mbf_mesh_nav/mesh_controller_execution.h

@@ -38,9 +38,10 @@
 #ifndef MBF_MESH_NAV__MESH_CONTROLLER_EXECUTION_H
 #define MBF_MESH_NAV__MESH_CONTROLLER_EXECUTION_H
 
+#include <memory>
+
 #include <mbf_abstract_nav/abstract_controller_execution.h>
 #include <mbf_mesh_core/mesh_controller.h>
-#include <mbf_mesh_nav/MoveBaseFlexConfig.h>
 #include <mesh_map/mesh_map.h>
 
 namespace mbf_mesh_nav
@@ -55,7 +56,7 @@ namespace mbf_mesh_nav
 class MeshControllerExecution : public mbf_abstract_nav::AbstractControllerExecution
 {
 public:
-  typedef boost::shared_ptr<mesh_map::MeshMap> MeshPtr;
+  typedef std::shared_ptr<mesh_map::MeshMap> MeshPtr;
 
   /**
    * @brief Constructor
@@ -72,9 +73,10 @@ class MeshControllerExecution : public mbf_abstract_nav::AbstractControllerExecu
    * @param setup_fn            A setup function called before execution
    * @param cleanup_fn          A cleanup function called after execution
    */
-  MeshControllerExecution(const std::string name, const mbf_mesh_core::MeshController::Ptr& controller_ptr,
-                          const ros::Publisher& vel_pub, const ros::Publisher& goal_pub, const TFPtr& tf_listener_ptr,
-                          const MeshPtr& mesh_ptr, const MoveBaseFlexConfig& config);
+  MeshControllerExecution(const std::string& name, const mbf_mesh_core::MeshController::Ptr& controller_ptr,
+                          const mbf_utility::RobotInformation::ConstPtr& robot_info,
+                          const rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr& vel_pub, const rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr& goal_pub,
+                          const MeshPtr& mesh_ptr, const rclcpp::Node::SharedPtr& node);
 
   /**
    * @brief Destructor
@@ -93,15 +95,13 @@ class MeshControllerExecution : public mbf_abstract_nav::AbstractControllerExecu
    * @return                   Result code as described in the ExePath action
    * result and plugin's header.
    */
-  virtual uint32_t computeVelocityCmd(const geometry_msgs::PoseStamped& robot_pose,
-                                      const geometry_msgs::TwistStamped& robot_velocity,
-                                      geometry_msgs::TwistStamped& vel_cmd, std::string& message);
+  virtual uint32_t computeVelocityCmd(const geometry_msgs::msg::PoseStamped& robot_pose,
+                                      const geometry_msgs::msg::TwistStamped& robot_velocity,
+                                      geometry_msgs::msg::TwistStamped& vel_cmd, std::string& message);
 
 private:
-  mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig& config);
-
   //! mesh for 3d navigation planning
-  const MeshPtr& mesh_ptr_;
+  const MeshPtr mesh_ptr_;
 
   //! Whether to lock mesh before calling the controller
   bool lock_mesh_;

mbf_mesh_nav/include/mbf_mesh_nav/mesh_navigation_server.h

@@ -38,18 +38,20 @@
 #ifndef MBF_MESH_NAV__MESH_NAVIGATION_SERVER_H
 #define MBF_MESH_NAV__MESH_NAVIGATION_SERVER_H
 
+#include <mutex>
+
 #include <mbf_abstract_nav/abstract_navigation_server.h>
 
 #include "mesh_controller_execution.h"
 #include "mesh_planner_execution.h"
 #include "mesh_recovery_execution.h"
 
-#include <mbf_mesh_nav/MoveBaseFlexConfig.h>
-#include <mbf_msgs/CheckPath.h>
-#include <mbf_msgs/CheckPose.h>
-#include <std_srvs/Empty.h>
+#include <mbf_msgs/srv/check_path.hpp>
+#include <mbf_msgs/srv/check_pose.hpp>
+#include <std_srvs/srv/empty.hpp>
+#include <rclcpp/rclcpp.hpp>
 
-#include <pluginlib/class_loader.h>
+#include <pluginlib/class_loader.hpp>
 
 namespace mbf_mesh_nav
 {
@@ -58,9 +60,6 @@ namespace mbf_mesh_nav
  * @brief Classes belonging to the Move Base Server level.
  */
 
-typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_mesh_nav::MoveBaseFlexConfig>>
-    DynamicReconfigureServerMeshNav;
-
 /**
  * @brief The MeshNavigationServer makes Move Base Flex backwards compatible to
  * the old move_base. It combines the execution classes which use the
@@ -73,15 +72,15 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_mesh_nav::MoveBaseFlex
 class MeshNavigationServer : public mbf_abstract_nav::AbstractNavigationServer
 {
 public:
-  typedef boost::shared_ptr<mesh_map::MeshMap> MeshPtr;
+  typedef std::shared_ptr<mesh_map::MeshMap> MeshPtr;
 
-  typedef boost::shared_ptr<MeshNavigationServer> Ptr;
+  typedef std::shared_ptr<MeshNavigationServer> Ptr;
 
   /**
    * @brief Constructor
    * @param tf_listener_ptr Shared pointer to a common TransformListener
    */
-  MeshNavigationServer(const TFPtr& tf_listener_ptr);
+  MeshNavigationServer(const TFPtr& tf_listener_ptr, const rclcpp::Node::SharedPtr& node);
 
   /**
    * @brief Destructor
@@ -162,39 +161,28 @@ class MeshNavigationServer : public mbf_abstract_nav::AbstractNavigationServer
 
   /**
    * @brief Callback method for the check_pose_cost service
-   * @param request Request object, see the mbf_msgs/CheckPose service
+   * @param request Request object, see the mbf_msgs/srv/CheckPose service
    * definition file.
-   * @param response Response object, see the mbf_msgs/CheckPose service
+   * @param response Response object, see the mbf_msgs/srv/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);
+  void callServiceCheckPoseCost(std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<mbf_msgs::srv::CheckPose::Request> request, std::shared_ptr<mbf_msgs::srv::CheckPose::Response> response);
 
   /**
    * @brief Callback method for the check_path_cost service
-   * @param request Request object, see the mbf_msgs/CheckPath service
+   * @param request Request object, see the mbf_msgs/srv/CheckPath service
    * definition file.
-   * @param response Response object, see the mbf_msgs/CheckPath service
+   * @param response Response object, see the mbf_msgs/srv/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);
+  void callServiceCheckPathCost(std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<mbf_msgs::srv::CheckPath::Request> request, std::shared_ptr<mbf_msgs::srv::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 callServiceClearMesh(std_srvs::Empty::Request& request, std_srvs::Empty::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_mesh_nav::MoveBaseFlexConfig& config, uint32_t level);
+  void callServiceClearMesh(std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
   //! plugin class loader for recovery behaviors plugins
   pluginlib::ClassLoader<mbf_mesh_core::MeshRecovery> recovery_plugin_loader_;
@@ -205,32 +193,20 @@ class MeshNavigationServer : public mbf_abstract_nav::AbstractNavigationServer
   //! plugin class loader for planner plugins
   pluginlib::ClassLoader<mbf_mesh_core::MeshPlanner> planner_plugin_loader_;
 
-  //! Dynamic reconfigure server for the mbf_mesh2d_specific part
-  DynamicReconfigureServerMeshNav dsrv_mesh_;
-
-  //! last configuration save
-  mbf_mesh_nav::MoveBaseFlexConfig last_config_;
-
-  //! the default parameter configuration save
-  mbf_mesh_nav::MoveBaseFlexConfig default_config_;
-
-  //! true, if the dynamic reconfigure has been setup
-  bool setup_reconfigure_;
-
   //! Shared pointer to the common global mesh
   MeshPtr mesh_ptr_;
 
   //! Service Server to clear the mesh
-  ros::ServiceServer clear_mesh_srv_;
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr clear_mesh_srv_;
 
   //! Service Server for the check_pose_cost service
-  ros::ServiceServer check_pose_cost_srv_;
+  rclcpp::Service<mbf_msgs::srv::CheckPose>::SharedPtr check_pose_cost_srv_;
 
   //! Service Server for the check_path_cost service
-  ros::ServiceServer check_path_cost_srv_;
+  rclcpp::Service<mbf_msgs::srv::CheckPath>::SharedPtr check_path_cost_srv_;
 
   //! Start/stop meshs mutex; concurrent calls to start can lead to segfault
-  boost::mutex check_meshs_mutex_;
+  std::mutex check_meshs_mutex_;
 };
 
 } /* namespace mbf_mesh_nav */

mbf_mesh_nav/include/mbf_mesh_nav/mesh_planner_execution.h

@@ -40,7 +40,6 @@
 
 #include <mbf_abstract_nav/abstract_planner_execution.h>
 #include <mbf_mesh_core/mesh_planner.h>
-#include <mbf_mesh_nav/MoveBaseFlexConfig.h>
 #include <mesh_map/mesh_map.h>
 
 namespace mbf_mesh_nav
@@ -56,25 +55,22 @@ namespace mbf_mesh_nav
 class MeshPlannerExecution : public mbf_abstract_nav::AbstractPlannerExecution
 {
 public:
-  typedef boost::shared_ptr<mesh_map::MeshMap> MeshPtr;
+  typedef std::shared_ptr<mesh_map::MeshMap> MeshPtr;
 
   /**
    * @brief Constructor
    * @param condition Thread sleep condition variable, to wake up connected
    * threads
    * @param mesh Shared pointer to the mesh.
    */
-  MeshPlannerExecution(const std::string name, const mbf_mesh_core::MeshPlanner::Ptr& planner_ptr, const MeshPtr& mesh,
-                       const MoveBaseFlexConfig& config);
+  MeshPlannerExecution(const std::string name, const mbf_mesh_core::MeshPlanner::Ptr& planner_ptr, const mbf_utility::RobotInformation::ConstPtr& robot_info, const MeshPtr& mesh, const rclcpp::Node::SharedPtr& node);
 
   /**
    * @brief Destructor
    */
   virtual ~MeshPlannerExecution();
 
 private:
-  mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig& config);
-
   /**
    * @brief Calls the planner plugin to make a plan from the start pose to the
    * goal pose with the given tolerance, if a goal tolerance is enabled in the
@@ -90,15 +86,15 @@ class MeshPlannerExecution : public mbf_abstract_nav::AbstractPlannerExecution
    * GetPath.action file
    */
   virtual uint32_t makePlan(
-      const geometry_msgs::PoseStamped &start,
-      const geometry_msgs::PoseStamped &goal,
+      const geometry_msgs::msg::PoseStamped &start,
+      const geometry_msgs::msg::PoseStamped &goal,
       double tolerance,
-      std::vector<geometry_msgs::PoseStamped> &plan,
+      std::vector<geometry_msgs::msg::PoseStamped> &plan,
       double &cost,
       std::string &message);
 
   //! Shared pointer to the mesh for 3d navigation planning
-  const MeshPtr& mesh_ptr_;
+  const MeshPtr mesh_ptr_;
 
   //! Whether to lock mesh before calling the planner (see issue #4 for details)
   bool lock_mesh_;