Merge pull request #44 from naturerobots/40-port-dijkstra_mesh_planne…

…r-pkg-to-ros2

40 port dijkstra mesh planner pkg to ros2

dijkstra_mesh_planner/CMakeLists.txt

@@ -1,58 +1,36 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(dijkstra_mesh_planner)
 
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
+find_package(ament_cmake_ros REQUIRED)
+find_package(mbf_mesh_core REQUIRED)
+find_package(mbf_msgs REQUIRED)
+find_package(mbf_utility REQUIRED)
+find_package(mesh_map REQUIRED)
+find_package(rclcpp REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  mbf_mesh_core
-  mbf_utility
-  mbf_msgs
-  mesh_map
-  dynamic_reconfigure
-)
-
-find_package(PkgConfig REQUIRED)
-pkg_check_modules(JSONCPP jsoncpp)
-
-generate_dynamic_reconfigure_options(
-  cfg/DijkstraMeshPlanner.cfg
-)
-
-catkin_package(
-  LIBRARIES dijkstra_mesh_planner
-  CATKIN_DEPENDS roscpp mbf_mesh_core mbf_msgs mesh_map dynamic_reconfigure
-)
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
+pluginlib_export_plugin_description_file(mbf_mesh_core dijkstra_mesh_planner.xml)
 
 add_library(${PROJECT_NAME}
   src/dijkstra_mesh_planner.cpp
 )
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_definitions(${PROJECT_NAME} PRIVATE "DIJKSTRA_MESH_PLANNER_BUILDING_LIBRARY")
+ament_target_dependencies(${PROJECT_NAME} mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp)
 
-add_dependencies(${PROJECT_NAME}
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  ${PROJECT_NAME}_gencfg
-)
-
-target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-  ${JSONCPP_LIBRARIES}
+install(DIRECTORY include/
+ DESTINATION include
 )
-
 install(TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(FILES dijkstra_mesh_planner.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_targets(export_${PROJECT_NAME})
+ament_export_dependencies(mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp)
+ament_package()

dijkstra_mesh_planner/dijkstra_mesh_planner.xml

@@ -1,4 +1,4 @@
-<library path="lib/libdijkstra_mesh_planner">
+<library path="dijkstra_mesh_planner">
     <class name="dijkstra_mesh_planner/DijkstraMeshPlanner" type="dijkstra_mesh_planner::DijkstraMeshPlanner"
            base_class_type="mbf_mesh_core::MeshPlanner">
         <description>

dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h

@@ -39,17 +39,17 @@
 #define MESH_NAVIGATION__DIJKSTRA_MESH_PLANNER_H
 
 #include <mbf_mesh_core/mesh_planner.h>
-#include <mbf_msgs/GetPathResult.h>
+#include <mbf_msgs/action/get_path.hpp>
 #include <mesh_map/mesh_map.h>
-#include <dijkstra_mesh_planner/DijkstraMeshPlannerConfig.h>
-#include <nav_msgs/Path.h>
+#include <nav_msgs/msg/path.hpp>
 
 namespace dijkstra_mesh_planner
 {
+
 class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner
 {
 public:
-  typedef boost::shared_ptr<dijkstra_mesh_planner::DijkstraMeshPlanner> Ptr;
+  typedef std::shared_ptr<dijkstra_mesh_planner::DijkstraMeshPlanner> Ptr;
 
   DijkstraMeshPlanner();
 
@@ -84,17 +84,17 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner
    *         INTERNAL_ERROR  = 60
    *         71..99 are reserved as plugin specific errors
    */
-  virtual uint32_t makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
-                            double tolerance, std::vector<geometry_msgs::PoseStamped>& plan, double& cost,
-                            std::string& message);
+  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) override;
 
   /**
    * @brief Requests the planner to cancel, e.g. if it takes too much time.
    *
    * @return True if a cancel has been successfully requested, false if not
    * implemented.
    */
-  virtual bool cancel();
+  virtual bool cancel() override;
 
   /**
    * @brief initializes this planner with the given plugin name and map
@@ -104,7 +104,7 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner
    *
    * @return true if initialization was successul; else false
    */
-  virtual bool initialize(const std::string& name, const boost::shared_ptr<mesh_map::MeshMap>& mesh_map_ptr);
+  virtual bool initialize(const std::string& plugin_name, const std::shared_ptr<mesh_map::MeshMap>& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) override;
 
   /**
    * @brief delivers vector field which has been generated during the latest planning
@@ -152,48 +152,49 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner
   void computeVectorMap();
 
   /**
-   * @brief gets called on new incoming reconfigure parameters
-   *
-   * @param cfg new configuration
-   * @param level level
+   * @brief gets called whenever the node's parameters change
+   
+   * @param parameters vector of changed parameters.
+   *                   Note that this vector will also contain parameters not related to the dijkstra mesh planner.
    */
-  void reconfigureCallback(dijkstra_mesh_planner::DijkstraMeshPlannerConfig& cfg, uint32_t level);
+  rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector<rclcpp::Parameter> parameters);
 
 private:
   // current map
-  mesh_map::MeshMap::Ptr mesh_map;
+  mesh_map::MeshMap::Ptr mesh_map_;
   // name of this plugin
-  std::string name;
+  std::string name_;
   // node handle
-  ros::NodeHandle private_nh;
+  rclcpp::Node::SharedPtr node_;
   // true if the abort of the current planning was requested; else false
-  std::atomic_bool cancel_planning;
+  std::atomic_bool cancel_planning_;
   // publisher of resulting path
-  ros::Publisher path_pub;
-  // publisher of resulting vector fiels
-  bool publish_vector_field;
-  // publisher of per face vectorfield
-  bool publish_face_vectors;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
   // tf frame of the map
-  std::string map_frame;
-  // offset of maximum distance from goal position
-  float goal_dist_offset;
-  // Server for Reconfiguration
-  boost::shared_ptr<dynamic_reconfigure::Server<dijkstra_mesh_planner::DijkstraMeshPlannerConfig>>
-      reconfigure_server_ptr;
-  dynamic_reconfigure::Server<dijkstra_mesh_planner::DijkstraMeshPlannerConfig>::CallbackType config_callback;
-  bool first_config;
-  DijkstraMeshPlannerConfig config;
+  std::string map_frame_;
+  // handle of callback for changing parameters dynamically
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_;
+  // config determined by ROS params; Init values defined here are used as default ROS param value
+  struct {
+    // publisher of resulting vector fiels
+    bool publish_vector_field = false;
+    // publisher of per face vectorfield
+    bool publish_face_vectors = false;
+    // offset of maximum distance from goal position
+    double goal_dist_offset = 0.3;
+    // defines the vertex cost limit with which it can be accessed
+    double cost_limit = 1.0;
+  } config_;
 
   // predecessors while wave propagation
-  lvr2::DenseVertexMap<lvr2::VertexHandle> predecessors;
+  lvr2::DenseVertexMap<lvr2::VertexHandle> predecessors_;
   // the face which is cut by line to the source
-  lvr2::DenseVertexMap<lvr2::FaceHandle> cutting_faces;
+  lvr2::DenseVertexMap<lvr2::FaceHandle> cutting_faces_;
   // stores the current vector map containing vectors pointing to the source
   // (path goal)
-  lvr2::DenseVertexMap<mesh_map::Vector> vector_map;
+  lvr2::DenseVertexMap<mesh_map::Vector> vector_map_;
   // potential field or distance values to the source (path goal)
-  lvr2::DenseVertexMap<float> potential;
+  lvr2::DenseVertexMap<float> potential_;
 };
 
 }  // namespace dijkstra_mesh_planner

dijkstra_mesh_planner/package.xml

@@ -1,21 +1,19 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
     <name>dijkstra_mesh_planner</name>
     <version>1.0.1</version>
     <description>The dijkstra_mesh_planner package</description>
     <maintainer email="[email protected]">Sebastian Pütz</maintainer>
     <license>BSD-3</license>
     <author email="[email protected]">Sebastian Pütz</author>
 
-    <buildtool_depend>catkin</buildtool_depend>
-    <depend>roscpp</depend>
+    <depend>rclcpp</depend>
     <depend>mbf_mesh_core</depend>
     <depend>mbf_utility</depend>
     <depend>mbf_msgs</depend>
     <depend>mesh_map</depend>
-    <depend>dynamic_reconfigure</depend>
 
     <export>
-        <mbf_mesh_core plugin="${prefix}/dijkstra_mesh_planner.xml"/>
+        <build_type>ament_cmake</build_type>
     </export>
 </package>