Merge pull request #47 from naturerobots/43-port-mesh_controller-pkg-…

…to-ros2

43 port mesh controller pkg to ros2

mesh_controller/CMakeLists.txt

@@ -1,54 +1,35 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(mesh_controller)
 
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
+find_package(ament_cmake_ros REQUIRED)
+find_package(example_interfaces 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(tf2_geometry_msgs REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  mbf_mesh_core
-  mbf_utility
-  mbf_msgs
-  mesh_map
-  tf2_geometry_msgs
-  dynamic_reconfigure
-)
-
-generate_dynamic_reconfigure_options(
-  cfg/MeshController.cfg
-)
-
-catkin_package(
-  LIBRARIES mesh_controller
-  CATKIN_DEPENDS roscpp mbf_mesh_core mbf_msgs mesh_map tf2_geometry_msgs dynamic_reconfigure
-)
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
-
-add_library(${PROJECT_NAME}
-  src/mesh_controller.cpp
-)
 
-add_dependencies(${PROJECT_NAME}
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  ${PROJECT_NAME}_gencfg
-)
+pluginlib_export_plugin_description_file(mbf_mesh_core mesh_controller.xml)
 
-target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-)
+add_library(${PROJECT_NAME} src/mesh_controller.cpp)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_CONTROLLER_BUILDING_LIBRARY")
+ament_target_dependencies(${PROJECT_NAME} example_interfaces mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs)
 
+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}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
-install(FILES mesh_controller.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_targets(export_${PROJECT_NAME})
+ament_export_dependencies(example_interfaces mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs)
+ament_package()

mesh_controller/include/mesh_controller/mesh_controller.h

@@ -36,10 +36,10 @@
 #define MESH_NAVIGATION__MESH_CONTROLLER_H
 
 #include <mbf_mesh_core/mesh_controller.h>
-#include <mbf_msgs/GetPathResult.h>
-#include <mesh_controller/MeshControllerConfig.h>
+#include <mbf_msgs/action/get_path.hpp>
+#include <example_interfaces/msg/float32.hpp>
 #include <mesh_map/mesh_map.h>
-#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 namespace mesh_controller
 {
@@ -48,7 +48,7 @@ class MeshController : public mbf_mesh_core::MeshController
 public:
 
   //! shared pointer typedef to simplify pointer access of the mesh controller
-  typedef boost::shared_ptr<mesh_controller::MeshController> Ptr;
+  typedef std::shared_ptr<mesh_controller::MeshController> Ptr;
 
   /**
    * @brief Constructor
@@ -69,9 +69,9 @@ class MeshController : public mbf_mesh_core::MeshController
    * @param message Detailed outcome as string message
    * @return An mbf_msgs/ExePathResult outcome code
    */
-  virtual uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose,
-                                           const geometry_msgs::TwistStamped& velocity,
-                                           geometry_msgs::TwistStamped& cmd_vel, std::string& message);
+  virtual uint32_t computeVelocityCommands(const geometry_msgs::msg::PoseStamped& pose,
+                                           const geometry_msgs::msg::TwistStamped& velocity,
+                                           geometry_msgs::msg::TwistStamped& cmd_vel, std::string& message) override;
 
   /**
    * @brief Checks if the robot reached to goal pose
@@ -82,28 +82,40 @@ class MeshController : public mbf_mesh_core::MeshController
    * be partly accepted as reached goal
    * @return true if the goal is reached
    */
-  virtual bool isGoalReached(double dist_tolerance, double angle_tolerance);
+  virtual bool isGoalReached(double dist_tolerance, double angle_tolerance) override;
 
   /**
    * @brief Sets the current plan to follow, it also sets the vector field
    * @param plan The plan to follow
    * @return true if the plan was set successfully, false otherwise
    */
-  virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan);
+  virtual bool setPlan(const std::vector<geometry_msgs::msg::PoseStamped>& plan) 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 controller plugin with a name, a tf pointer and a mesh map pointer
+   * @param plugin_name The controller plugin name, defined by the user. It defines the controller namespace
+   * @param tf_ptr A shared pointer to a transformation buffer
+   * @param mesh_map_ptr A shared pointer to the mesh map
+   * @return true if the plugin has been initialized successfully
+   */
+  virtual bool initialize(const std::string& plugin_name,
+                          const std::shared_ptr<tf2_ros::Buffer>& tf_ptr,
+                          const std::shared_ptr<mesh_map::MeshMap>& mesh_map_ptr,
+                          const rclcpp::Node::SharedPtr& node) override;
 
   /**
    * Converts the orientation of a geometry_msgs/PoseStamped message to a direction vector
    * @param pose      the pose to convert
    * @return          direction normal vector
    */
   mesh_map::Normal poseToDirectionVector(
-      const geometry_msgs::PoseStamped& pose,
+      const geometry_msgs::msg::PoseStamped& pose,
       const tf2::Vector3& axis=tf2::Vector3(1,0,0));
 
 
@@ -113,7 +125,7 @@ class MeshController : public mbf_mesh_core::MeshController
    * @return          position vector
    */
   mesh_map::Vector poseToPositionVector(
-      const geometry_msgs::PoseStamped& pose);
+      const geometry_msgs::msg::PoseStamped& pose);
 
   /**
    * A normal distribution / gaussian function
@@ -141,52 +153,52 @@ class MeshController : public mbf_mesh_core::MeshController
   /**
    * @brief reconfigure callback function which is called if a dynamic reconfiguration were triggered.
    */
-  void reconfigureCallback(mesh_controller::MeshControllerConfig& cfg, uint32_t level);
-
-  /**
-   * @brief Initializes the controller plugin with a name, a tf pointer and a mesh map pointer
-   * @param plugin_name The controller plugin name, defined by the user. It defines the controller namespace
-   * @param tf_ptr A shared pointer to a transformation buffer
-   * @param mesh_map_ptr A shared pointer to the mesh map
-   * @return true if the plugin has been initialized successfully
-   */
-  virtual bool initialize(const std::string& plugin_name, const boost::shared_ptr<tf2_ros::Buffer>& tf_ptr,
-                          const boost::shared_ptr<mesh_map::MeshMap>& mesh_map_ptr);
+  rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector<rclcpp::Parameter> parameters);
 
 private:
+  //! shared pointer to node in which this plugin runs
+  rclcpp::Node::SharedPtr node_;
+
+  //! the user defined plugin name
+  std::string name_;
 
   //! shared pointer to the used mesh map
-  boost::shared_ptr<mesh_map::MeshMap> map_ptr;
+  std::shared_ptr<mesh_map::MeshMap> map_ptr_;
 
   //! the current set plan
-  vector<geometry_msgs::PoseStamped> current_plan;
+  vector<geometry_msgs::msg::PoseStamped> current_plan_;
 
   //! the goal and robot pose
-  mesh_map::Vector goal_pos, robot_pos;
+  mesh_map::Vector goal_pos_, robot_pos_;
 
   //! the goal's and robot's orientation
-  mesh_map::Normal goal_dir, robot_dir;
+  mesh_map::Normal goal_dir_, robot_dir_;
 
   //! The triangle on which the robot is located
-  lvr2::OptionalFaceHandle current_face;
+  lvr2::OptionalFaceHandle current_face_;
 
   //! The vector field to the goal.
-  lvr2::DenseVertexMap<mesh_map::Vector> vector_map;
-
-  //! shared pointer to dynamic reconfigure server
-  boost::shared_ptr<dynamic_reconfigure::Server<mesh_controller::MeshControllerConfig>> reconfigure_server_ptr;
-
-  //! dynamic reconfigure callback function binding
-  dynamic_reconfigure::Server<mesh_controller::MeshControllerConfig>::CallbackType config_callback;
-
-  //! current mesh controller configuration
-  MeshControllerConfig config;
+  lvr2::DenseVertexMap<mesh_map::Vector> vector_map_;
 
   //! publishes the angle between the robots orientation and the goal vector field for debug purposes
-  ros::Publisher angle_pub;
+  rclcpp::Publisher<example_interfaces::msg::Float32>::SharedPtr angle_pub_;
 
   //! flag to handle cancel requests
-  std::atomic_bool cancel_requested;
+  std::atomic_bool cancel_requested_;
+
+  // handle of callback for changing parameters dynamically
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_;
+
+  struct {
+    double max_lin_velocity = 1.0;
+    double max_ang_velocity = 0.5;
+    double arrival_fading = 0.5;
+    double ang_vel_factor = 1.0;
+    double lin_vel_factor = 1.0;
+    double max_angle = 20.0;
+    double max_search_radius = 0.4;
+    double max_search_distance = 0.4;
+  } config_;
 };
 
 } /* namespace mesh_controller */

mesh_controller/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
     <name>mesh_controller</name>
     <version>1.0.1</version>
     <description>The mesh_controller package</description>
@@ -9,17 +9,15 @@
     <author email="spuetz@uos.de">Sebastian Pütz</author>
     <author email="sfrohn@uos.de">Sabrina Frohn</author>
 
-    <buildtool_depend>catkin</buildtool_depend>
-
-    <depend>roscpp</depend>
     <depend>mbf_mesh_core</depend>
-    <depend>mbf_utility</depend>
     <depend>mbf_msgs</depend>
+    <depend>mbf_utility</depend>
     <depend>mesh_map</depend>
+    <depend>rclcpp</depend>
     <depend>tf2_geometry_msgs</depend>
-    <depend>dynamic_reconfigure</depend>
+    <depend>example_interfaces</depend>
 
     <export>
-        <mbf_mesh_core plugin="${prefix}/mesh_controller.xml"/>
+        <build_type>ament_cmake</build_type>
     </export>
 </package>