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

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,7 @@ add_library(${library_name} SHARED
 )
 
 target_include_directories(${library_name} PRIVATE src/include)
+
 if(HAVE_DRAND48)
   target_compile_definitions(${library_name} PRIVATE "HAVE_DRAND48")
 endif()
@@ -62,6 +65,15 @@ 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}
@@ -77,11 +89,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 +124,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/differential_motion_model.hpp

@@ -0,0 +1,47 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      [email protected]    [email protected]
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef DIFFERENTIAL_MOTION_MODEL_HPP
+#define DIFFERENTIAL_MOTION_MODEL_HPP
+
+#include <sys/types.h>
+#include <math.h>
+#include <algorithm>
+#include "nav2_amcl/motion_model/motion_model.hpp"
+#include "nav2_amcl/angleutils.hpp"
+
+
+namespace nav2_amcl
+{
+
+class DifferentialMotionModel : public nav2_amcl::MotionModel
+{
+public:
+  virtual void initialize(
+    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);
+
+private:
+  double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
+};
+}  // namespace nav2_amcl
+#endif

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
 {
@@ -33,14 +34,6 @@ 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,32 @@ 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/include/nav2_amcl/motion_model/omni_motion_model.hpp

@@ -0,0 +1,47 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      [email protected]    [email protected]
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef DIFFERENTIAL_MOTION_MODEL_HPP
+#define DIFFERENTIAL_MOTION_MODEL_HPP
+
+#include <sys/types.h>
+#include <math.h>
+#include <algorithm>
+#include "nav2_amcl/motion_model/motion_model.hpp"
+#include "nav2_amcl/angleutils.hpp"
+
+
+namespace nav2_amcl
+{
+
+class OmniMotionModel : public nav2_amcl::MotionModel
+{
+public:
+  virtual void initialize(
+    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);
+
+private:
+  double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
+};
+}  // namespace nav2_amcl
+#endif

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,8 @@ AmclNode::initOdometry()
     init_cov_[2] = last_published_pose_.pose.covariance[35];
   }
 
-  motion_model_ = std::unique_ptr<nav2_amcl::MotionModel>(
-    nav2_amcl::MotionModel::createMotionModel(
-      robot_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_));
+  motion_model_ = nav2_amcl::MotionModel::createMotionModel(
+    robot_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
 
   latest_odom_pose_ = geometry_msgs::msg::PoseStamped();
 }

nav2_amcl/src/motion_model/CMakeLists.txt

@@ -5,6 +5,7 @@ add_library(motions_lib SHARED
 )
 target_link_libraries(motions_lib pf_lib)
 ament_target_dependencies(motions_lib
+  pluginlib
   nav2_util
 )
 

nav2_amcl/src/motion_model/differential_motion_model.cpp

@@ -19,25 +19,21 @@
  *
  */
 
-
-#include <sys/types.h>
-#include <math.h>
-#include <algorithm>
-
-#include "nav2_amcl/motion_model/motion_model.hpp"
-#include "nav2_amcl/angleutils.hpp"
+#include "nav2_amcl/motion_model/differential_motion_model.hpp"
 
 namespace nav2_amcl
 {
 
-DifferentialMotionModel::DifferentialMotionModel(
-  double alpha1, double alpha2, double alpha3,
-  double alpha4)
+void
+DifferentialMotionModel::initialize(
+  double alpha1, double alpha2, double alpha3, double alpha4,
+  double alpha5)
 {
   alpha1_ = alpha1;
   alpha2_ = alpha2;
   alpha3_ = alpha3;
   alpha4_ = alpha4;
+  alpha5_ = alpha5;
 }
 
 void
@@ -116,3 +112,6 @@ DifferentialMotionModel::odometryUpdate(
 }
 
 }  // namespace nav2_amcl
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(nav2_amcl::DifferentialMotionModel, nav2_amcl::MotionModel)

nav2_amcl/src/motion_model/motion_model.cpp

@@ -15,24 +15,24 @@
 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 
 #include "nav2_amcl/motion_model/motion_model.hpp"
-
+#include <pluginlib/class_loader.hpp>
 #include <string>
 
 namespace nav2_amcl
 {
-
-MotionModel *
+std::shared_ptr<nav2_amcl::MotionModel>
 MotionModel::createMotionModel(
   std::string & type, double alpha1, double alpha2,
   double alpha3, double alpha4, double alpha5)
 {
-  if (type == "differential") {
-    return new DifferentialMotionModel(alpha1, alpha2, alpha3, alpha4);
-  } else if (type == "omnidirectional") {
-    return new OmniMotionModel(alpha1, alpha2, alpha3, alpha4, alpha5);
-  }
+  pluginlib::ClassLoader<nav2_amcl::MotionModel> poly_loader("nav2_amcl", "nav2_amcl::MotionModel");
 
-  return nullptr;
+  try {
+    std::shared_ptr<nav2_amcl::MotionModel> motion_model_ = poly_loader.createSharedInstance(type);
+    motion_model_->initialize(alpha1, alpha2, alpha3, alpha4, alpha5);
+    return motion_model_;
+  } catch (pluginlib::PluginlibException & ex) {
+    return nullptr;
+  }
 }
-
-}  // namespace nav2_amcl
+} // namespace nav2_amcl