Refactored amcl code for modularization of motion models(main branch)

nav2_amcl/CHANGELOG.rst

@@ -0,0 +1 @@
+

nav2_amcl/CMakeLists.txt

@@ -16,6 +16,8 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
 
 nav2_package()
 
@@ -44,6 +46,11 @@ add_library(${library_name} SHARED
 )
 
 target_include_directories(${library_name} PRIVATE src/include)
+
+target_include_directories(${executable_name} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
 if(HAVE_DRAND48)
   target_compile_definitions(${library_name} PRIVATE "HAVE_DRAND48")
 endif()
@@ -62,8 +69,17 @@ set(dependencies
   tf2
   nav2_util
   nav2_msgs
+  pluginlib
 )
 
+add_library(motion_models SHARED
+  src/motion_model/differential_motion_model.cpp
+  src/motion_model/omni_motion_model.cpp)
+
+ament_target_dependencies(motion_models
+  ${dependencies}
+)  
+
 ament_target_dependencies(${executable_name}
   ${dependencies}
 )
@@ -77,11 +93,18 @@ ament_target_dependencies(${library_name}
 )
 
 target_link_libraries(${library_name}
-  map_lib motions_lib sensors_lib
+  map_lib motions_lib sensors_lib motion_models
 )
 
 rclcpp_components_register_nodes(${library_name} "nav2_amcl::AmclNode")
 
+
+install(TARGETS motion_models
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
 install(TARGETS ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -105,7 +128,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name} pf_lib sensors_lib motions_lib map_lib)
+ament_export_libraries(${library_name} pf_lib sensors_lib motions_lib map_lib motion_models)
 ament_export_dependencies(${dependencies})
-
+pluginlib_export_plugin_description_file(nav2_amcl plugins.xml)
 ament_package()

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -214,7 +214,7 @@ class AmclNode : public nav2_util::LifecycleNode
    * @brief Initialize odometry
    */
   void initOdometry();
-  std::unique_ptr<nav2_amcl::MotionModel> motion_model_;
+  std::shared_ptr<nav2_amcl::MotionModel> motion_model_;
   geometry_msgs::msg::PoseStamped latest_odom_pose_;
   geometry_msgs::msg::PoseWithCovarianceStamped last_published_pose_;
   double init_pose_[3];  // Initial robot pose

nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp

@@ -20,6 +20,7 @@
 #include <string>
 #include "nav2_amcl/pf/pf.hpp"
 #include "nav2_amcl/pf/pf_pdf.hpp"
+#include <memory>
 
 namespace nav2_amcl
 {
@@ -32,15 +33,7 @@ class MotionModel
 {
 public:
   virtual ~MotionModel() = default;
-
-  /**
-   * @brief Update on new odometry data
-   * @param pf The particle filter to update
-   * @param pose pose of robot in odometry update
-   * @param delta change in pose in odometry update
-   */
-  virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) = 0;
-
+
   /**
    * @brief An factory to create motion models
    * @param type Type of motion model to create in factory
@@ -51,75 +44,31 @@ class MotionModel
    * @param alpha5 error parameters, see documentation
    * @return MotionModel A pointer to the motion model it created
    */
-  static MotionModel * createMotionModel(
-    std::string & type, double alpha1, double alpha2,
-    double alpha3, double alpha4, double alpha5);
-};
-
-/**
- * @class nav2_amcl::OmniMotionModel
- * @brief An Omnidirectional motion model class
- */
-class OmniMotionModel : public MotionModel
-{
-public:
-  /**
-   * @brief An omni constructor
-   * @param alpha1 error parameters, see documentation
-   * @param alpha2 error parameters, see documentation
-   * @param alpha3 error parameters, see documentation
-   * @param alpha4 error parameters, see documentation
-   * @param alpha5 error parameters, see documentation
-   */
-  OmniMotionModel(double alpha1, double alpha2, double alpha3, double alpha4, double alpha5);
+
+  virtual void initialize(double alpha1, double alpha2,double alpha3, double alpha4, double alpha5) = 0;
 
   /**
    * @brief Update on new odometry data
    * @param pf The particle filter to update
    * @param pose pose of robot in odometry update
    * @param delta change in pose in odometry update
    */
-  void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta);
-
-private:
-  double alpha1_;
-  double alpha2_;
-  double alpha3_;
-  double alpha4_;
-  double alpha5_;
-};
+  virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) = 0;
 
-/**
- * @class nav2_amcl::DifferentialMotionModel
- * @brief An differential drive motion model class
- */
-class DifferentialMotionModel : public MotionModel
-{
-public:
   /**
-   * @brief A diff drive constructor
+   * @brief An factory to create motion models
+   * @param type Type of motion model to create in factory
    * @param alpha1 error parameters, see documentation
    * @param alpha2 error parameters, see documentation
    * @param alpha3 error parameters, see documentation
    * @param alpha4 error parameters, see documentation
+   * @param alpha5 error parameters, see documentation
+   * @return MotionModel A pointer to the motion model it created
    */
-  DifferentialMotionModel(double alpha1, double alpha2, double alpha3, double alpha4);
-
-  /**
-   * @brief Update on new odometry data
-   * @param pf The particle filter to update
-   * @param pose pose of robot in odometry update
-   * @param delta change in pose in odometry update
-   */
-  void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta);
-
-private:
-  double alpha1_;
-  double alpha2_;
-  double alpha3_;
-  double alpha4_;
+  static std::shared_ptr<nav2_amcl::MotionModel> createMotionModel(
+    std::string & type, double alpha1, double alpha2,
+    double alpha3, double alpha4, double alpha5);
 };
-
 }  // namespace nav2_amcl
 
 #endif  // NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_

nav2_amcl/package.xml

@@ -35,7 +35,7 @@
   <depend>nav2_msgs</depend>
   <depend>launch_ros</depend>
   <depend>launch_testing</depend>
-
+  <depend>pluginlib</depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
 

nav2_amcl/plugins.xml

@@ -0,0 +1,8 @@
+<library path="motion_models">
+  <class type="nav2_amcl::DifferentialMotionModel" base_class_type="nav2_amcl::MotionModel">
+    <description>This is a diff plugin.</description>
+  </class>
+  <class type="nav2_amcl::OmniMotionModel" base_class_type="nav2_amcl::MotionModel">
+    <description>This is a omni plugin.</description>
+  </class>
+</library>

nav2_amcl/src/amcl_node.cpp

@@ -42,6 +42,7 @@
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
+#include "nav2_amcl/motion_model/motion_model.hpp"
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
@@ -1332,9 +1333,9 @@ AmclNode::initOdometry()
     init_cov_[2] = last_published_pose_.pose.covariance[35];
   }
 
-  motion_model_ = std::unique_ptr<nav2_amcl::MotionModel>(
+  motion_model_ = 
     nav2_amcl::MotionModel::createMotionModel(
-      robot_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_));
+      robot_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
 
   latest_odom_pose_ = geometry_msgs::msg::PoseStamped();
 }