Merge pull request #46 from naturerobots/41-port-cvp_mesh_planner-pkg…

…-to-ros2

41 port cvp mesh planner pkg to ros2

README.md

@@ -1,18 +1,16 @@
 # Mesh Navigation
 
-The *Mesh Navigation* bundle provides software to perform efficient robot navigation on 2D-manifolds in 3D represented 
-as triangular meshes. It allows to safely navigate in various complex outdoor environments by using a modular extendable
-layerd mesh map. Layers can be loaded as plugins and represent certain geometric or semantic metrics of the terrain.
-The layered mesh map is integrated with Move Base Flex (MBF) which provides a universal ROS action interface for path
-planning and motion control, as well as for recovery behaviours. Thus, additional planner and controller plugins running
-on the layered mesh map are provided.
+The *Mesh Navigation* bundle provides software for efficient robot navigation on 2D manifolds, which are represented in 3D as triangle meshes. It enables safe navigation in various complex outdoor environments by using a modularly extensible
+layered mesh map. Layers can be loaded as plugins representing specific geometric or semantic metrics of the terrain. This allows the incorporation of obstacles in these complex outdoor environments into path and motion motion planning.
+The layered *Mesh Map* is integrated with *Move Base Flex (MBF)*, which provides a universal ROS action interface for path planning, motion control, and for recovery behaviors. We also provide additional planner and controller plugins that run on the layered mesh map.
 
 Maintainer: [Sebastian Pütz](mailto:[email protected])  
 Author: [Sebastian Pütz](mailto:[email protected])
 
 * [Publications](#publications)
 * [Installation](#installation)
 * [Software Stack](#software-stack)
+* [Usage](#usage)
 * [Mesh Map](#mesh-map)
 * [Planners](#planners)
 * [Controllers](#controllers)
@@ -115,9 +113,21 @@ The package structure is as follows:
 
 - `mesh_client` Is an experimental package to additionally load navigation meshes from a server.
 
+## Usage
+
+See the **[pluto_robot](https://github.com/uos/pluto_robot)** bundle for example configurations of the mesh navigatoin stack and usage.
+
+### Mesh map configuratoin
+
+TODO
+
+### Planner and Controller configurations
+
+TODO
+
 ### Path Planning and Motion Control
 
-Use the `MeshGoal` tool to select a goal pose on the shown mesh in RViz. 
+Use the *MeshGoal* tool from the MeshTools bundle to select a goal pose on the shown mesh in *RViz*. This can be added to the top panel *RViz*. 
 
 ## Mesh Map
 

cvp_mesh_planner/CMakeLists.txt

@@ -1,58 +1,32 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(cvp_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/CVPMeshPlanner.cfg
-)
+pluginlib_export_plugin_description_file(mbf_mesh_core cvp_mesh_planner.xml)
 
-catkin_package(
-  LIBRARIES cvp_mesh_planner
-  CATKIN_DEPENDS roscpp mbf_mesh_core mbf_utility mbf_msgs mesh_map dynamic_reconfigure
-)
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
-
-add_library(${PROJECT_NAME}
-  src/cvp_mesh_planner.cpp
-)
-
-add_dependencies(${PROJECT_NAME}
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  ${PROJECT_NAME}_gencfg
-)
-
-target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-  ${JSONCPP_LIBRARIES}
-)
+add_library(${PROJECT_NAME} src/cvp_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 "CVP_MESH_PLANNER_BUILDING_LIBRARY")
+ament_target_dependencies(${PROJECT_NAME} mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp)
 
+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 cvp_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()

cvp_mesh_planner/cvp_mesh_planner.xml

@@ -1,4 +1,4 @@
-<library path="lib/libcvp_mesh_planner">
+<library path="cvp_mesh_planner">
     <class name="cvp_mesh_planner/CVPMeshPlanner" type="cvp_mesh_planner::CVPMeshPlanner"
            base_class_type="mbf_mesh_core::MeshPlanner">
         <description>

cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h

@@ -39,17 +39,17 @@
 #define MESH_NAVIGATION__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 <cvp_mesh_planner/CVPMeshPlannerConfig.h>
-#include <nav_msgs/Path.h>
+#include <nav_msgs/msg/path.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 namespace cvp_mesh_planner
 {
 class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner
 {
 public:
-  typedef boost::shared_ptr<cvp_mesh_planner::CVPMeshPlanner> Ptr;
+  typedef std::shared_ptr<cvp_mesh_planner::CVPMeshPlanner> Ptr;
 
   /**
    * @brief Constructor
@@ -71,23 +71,23 @@ class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner
    * @param message a detailed outcome message
    * @return result outcome code, see the GetPath action definition
    */
-  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 cancel has been successfully requested, false otherwise
    */
-  virtual bool cancel();
+  virtual bool cancel() override;
 
   /**
    * @brief Initializes the planner plugin with a user configured name and a shared pointer to the mesh map
    * @param name The user configured name, which is used as namespace for parameters, etc.
    * @param mesh_map_ptr A shared pointer to the mesh map instance to access attributes and helper functions, etc.
    * @return true if the plugin has been initialized successfully
    */
-  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;
 
 protected:
 
@@ -162,65 +162,66 @@ class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner
   void computeVectorMap();
 
   /**
-   * @brief Dynamic reconfigure callback
+   * @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 cvp mesh planner
    */
-  void reconfigureCallback(cvp_mesh_planner::CVPMeshPlannerConfig& cfg, uint32_t level);
+  rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector<rclcpp::Parameter> parameters);
 
 private:
 
   //! shared pointer to the mesh map
-  mesh_map::MeshMap::Ptr mesh_map;
+  mesh_map::MeshMap::Ptr mesh_map_;
 
   //! the user defined plugin name
-  std::string name;
+  std::string name_;
 
-  //! the private node handle with the user defined namespace (name)
-  ros::NodeHandle private_nh;
+  //! pointer to the node in which this plugin is running
+  rclcpp::Node::SharedPtr node_;
 
   //! flag if cancel has been requested
-  std::atomic_bool cancel_planning;
+  std::atomic_bool cancel_planning_;
 
   //! publisher for the backtracked path
-  ros::Publisher path_pub;
-
-  //! whether to publish the vector field or not
-  bool publish_vector_field;
-
-  //! whether to also publish direction vectors at the triangle centers
-  bool publish_face_vectors;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
 
   //! the map coordinate frame / system id
-  std::string map_frame;
-
-  //! an offset that determines how far beyond the goal (robot's position) is propagated.
-  float goal_dist_offset;
-
-  //! shared pointer to dynamic reconfigure server
-  boost::shared_ptr<dynamic_reconfigure::Server<cvp_mesh_planner::CVPMeshPlannerConfig>> reconfigure_server_ptr;
-
-  //! dynamic reconfigure callback function binding
-  dynamic_reconfigure::Server<cvp_mesh_planner::CVPMeshPlannerConfig>::CallbackType config_callback;
-
-  //! indicates if dynamic reconfigure has been called the first time
-  bool first_config;
-
-  //! the current dynamic reconfigure planner configuration
-  CVPMeshPlannerConfig config;
+  std::string map_frame_;
+
+  // handle of callback for changing parameters dynamically
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_;
+  struct {
+    //! whether to publish the vector field or not
+    bool publish_vector_field = false;
+    //! whether to also publish direction vectors at the triangle centers
+    bool publish_face_vectors = false;
+    //! an offset that determines how far beyond the goal (robot's position) is propagated.
+    double goal_dist_offset = 0.3;
+    //! Defines the vertex cost limit with which it can be accessed.
+    double cost_limit = 1.0;
+    //! The vector field back tracking step width.
+    double step_width = 0.4;
+  } config_;
 
   //! theta angles to the source of the wave front propagation
-  lvr2::DenseVertexMap<float> direction;
+  lvr2::DenseVertexMap<float> direction_;
 
   //! predecessors while wave propagation
-  lvr2::DenseVertexMap<lvr2::VertexHandle> predecessors;
+  lvr2::DenseVertexMap<lvr2::VertexHandle> predecessors_;
 
   //! the face which is cut by the computed 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 seed
-  lvr2::DenseVertexMap<mesh_map::Vector> vector_map;
+  lvr2::DenseVertexMap<mesh_map::Vector> vector_map_;
 
   //! potential field / scalar distance field to the seed
-  lvr2::DenseVertexMap<float> potential;
+  lvr2::DenseVertexMap<float> potential_;
 };
 
 }  // namespace cvp_mesh_planner

cvp_mesh_planner/package.xml

@@ -1,22 +1,19 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
     <name>cvp_mesh_planner</name>
     <version>1.0.1</version>
     <description>The Continuous Vector Field Planner (CVP) mesh planner package</description>
     <maintainer email="[email protected]">Sebastian Pütz</maintainer>
     <license>BSD 3-Clause</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}/cvp_mesh_planner.xml"/>
+        <build_type>ament_cmake</build_type>
     </export>
 </package>