update

examples/NavigationExample/Program.cs

@@ -404,17 +404,19 @@ namespace NavigationExample
             goal.pose.orientation.z = 0.0;
             goal.pose.orientation.w = 1.0;
 
-            NavigationAPI.navigation_dock_to_order(navHandle, order, "docking_point", goal);
 
-            // NavigationAPI.navigation_move_to_order(navHandle, order, goal);`
+            Console.WriteLine("Docking to docking_point");
+            NavigationAPI.navigation_dock_to_order(navHandle, order, "charger", goal);
+
+            // NavigationAPI.navigation_move_to_order(navHandle, order, goal);
             NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);
             NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2);
 
             // NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
 
-            // while (true) 
-            // {
-            //     System.Threading.Thread.Sleep(100);
+            while (true) 
+            {
+                System.Threading.Thread.Sleep(100);
             //     NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
             //     if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
             //     {
@@ -474,7 +476,7 @@ namespace NavigationExample
             //             Console.WriteLine("Local Costmap is not updated");
             //         }
             //     }
-            // }
+            }
             // Cleanup (destroy nav first, then tf3 buffer)
 
             NavigationAPI.navigation_destroy(navHandle);

src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt

@@ -101,6 +101,7 @@ add_library(${PROJECT_NAME} SHARED
   src/pnkx_local_planner.cpp
   src/pnkx_go_straight_local_planner.cpp
   src/pnkx_rotate_local_planner.cpp
+  src/pnkx_docking_local_planner.cpp
 )
 
 if(BUILDING_WITH_CATKIN)

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h

@@ -1,8 +1,9 @@
 #ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 #define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 
-#include <robot_nav_core2/base_global_planner.h>
+#include <robot_nav_core/base_global_planner.h>
 #include <pnkx_local_planner/pnkx_local_planner.h>
+#include <robot_xmlrpcpp/XmlRpcValue.h>
 
 namespace pnkx_local_planner
 {
@@ -137,7 +138,7 @@ namespace pnkx_local_planner
       std::string docking_planner_name_;
       std::string docking_nav_name_;
 
-      robot_nav_core2::BaseGlobalPlanner::Ptr docking_planner_;
+      robot_nav_core::BaseGlobalPlanner::Ptr docking_planner_;
       score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
 
       robot_geometry_msgs::Vector3 linear_;
@@ -158,7 +159,7 @@ namespace pnkx_local_planner
       TFListenerPtr tf_;
       robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
       score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
-      std::function<robot_nav_core2::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
+      std::function<robot_nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
       std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
 
       robot::WallTimer detected_timeout_wt_, delayed_wt_;
@@ -169,7 +170,7 @@ namespace pnkx_local_planner
 
     bool start_docking_;
     double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
-    XmlRpc::XmlRpcValue original_papams_;
+    robot_xmlrpcpp::XmlRpcValue original_papams_;
     std::vector<DockingPlanner*> dkpl_;
 
     bool dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity);

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp

@@ -56,19 +56,18 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     if (!costmap_robot_->isCurrent())
       throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
 
-    nh_ = robot::NodeHandle("~");
     parent_ = parent;
     planner_nh_ = robot::NodeHandle(parent_, name);
-    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s", planner_nh_.getNamespace().c_str());
-
+    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.getNamespace().c_str());
     this->getParams(planner_nh_);
 
     std::string traj_generator_name;
-    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator"));
+    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("StandardTrajectoryGenerator"));
     robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(traj_generator_name);
       traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
         path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
       traj_generator_ = traj_gen_loader_();
@@ -77,7 +76,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
         exit(1);
       }
-      robot::NodeHandle nh_traj_gen = robot::NodeHandle(nh_, traj_generator_name);
+      robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str());
     }
@@ -88,20 +87,21 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     }
 
     std::string algorithm_nav_name; 
-    planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+    planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
     robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
-      nav_algorithm_ = algorithm_loader_();
+      nav_algorithm_ = nav_algorithm_loader_();
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
         exit(1);
       }
-      nav_algorithm_->initialize(nh_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
+      nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception &ex)
     {
@@ -110,19 +110,20 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     }
 
     std::string algorithm_rotate_name;
-    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("pnkx_local_planner::RotateToGoalDiff"));
+    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
+      rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
-      rotate_algorithm_ = algorithm_loader_();
+      rotate_algorithm_ = rotate_algorithm_loader_();
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
         exit(1);
       }
-      rotate_algorithm_->initialize(nh_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
+      rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
     }
     catch (const std::exception &ex)
@@ -133,10 +134,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
 
     std::string goal_checker_name;
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
+    robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(goal_checker_name);
+      goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
         path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
       goal_checker_ = goal_checker_loader_();
       if (!goal_checker_)
@@ -144,7 +147,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
         exit(1);
       }
-      goal_checker_->initialize(nh_);
+      goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
     }
     catch (const std::exception &ex)
@@ -152,6 +155,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
       robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
       exit(1);
     }
+
     this->initializeOthers();
     this->getMaker();
     robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
@@ -162,8 +166,10 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
 void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
 {
   std::string maker_name, maker_sources;
-  nh_.param("maker_name", maker_name, std::string(""));
-  nh_.param("maker_sources", maker_sources, std::string(""));
+  parent_.param("maker_name", maker_name, std::string(""));
+  parent_.param("maker_sources", maker_sources, std::string(""));
+  robot::log_info_at(__FILE__, __LINE__, "maker_name: %s", maker_name.c_str());
+  robot::log_info_at(__FILE__, __LINE__, "maker_sources: %s", maker_sources.c_str());
   std::stringstream ss(maker_sources);
   std::string source;
 
@@ -171,24 +177,23 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
   {
     if (maker_name == source)
     {
-      robot::NodeHandle source_node(nh_, source);
+      robot::NodeHandle source_node(parent_, source);
 
       if (source_node.hasParam("plugins"))
       {
-        XmlRpc::XmlRpcValue plugins;
+        robot_xmlrpcpp::XmlRpcValue plugins;
         source_node.getParam("plugins", plugins);
 
-        if (plugins.getType() == XmlRpc::XmlRpcValue::TypeArray)
+        if (plugins.getType() == robot_xmlrpcpp::XmlRpcValue::TypeArray)
         {
           for (int i = 0; i < plugins.size(); ++i)
           {
-            if (plugins[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
+            if (plugins[i].getType() == robot_xmlrpcpp::XmlRpcValue::TypeStruct)
             {
               std::stringstream name;
               name << source << "/" << static_cast<std::string>(plugins[i]["name"]);
               std::string docking_planner_name = static_cast<std::string>(plugins[i]["docking_planner"]);
               std::string docking_nav_name = static_cast<std::string>(plugins[i]["docking_nav"]);
-
               // std::shared_ptr<DockingPlanner> dkpl = std::make_shared<DockingPlanner>(name.str());
               DockingPlanner* dkpl = new DockingPlanner(name.str());
               dkpl->docking_planner_name_ = docking_planner_name;
@@ -215,7 +220,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initializeOthers()
 {
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-  if (!nh_.getParam(algorithm_nav_name, original_papams_))
+  if (!parent_.getParam(algorithm_nav_name, original_papams_))
     robot::log_warning_at(__FILE__, __LINE__, "No found in %s in yaml-file, please check configuration", algorithm_nav_name.c_str());
   original_xy_goal_tolerance_ = xy_goal_tolerance_;
   original_yaw_goal_tolerance_ = yaw_goal_tolerance_;
@@ -247,8 +252,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
 
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-  robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
-  nh_.setParam(algorithm_nav_name, original_papams_);
+  robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
+  parent_.setParam(algorithm_nav_name, original_papams_);
   nh_algorithm.setParam("allow_rotate", false);
 }
 
@@ -279,7 +284,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
   try
   {
-    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
   }
   catch(const robot_nav_core2::LocalPlannerException& e)
@@ -290,7 +295,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   double x_direction, y_direction, theta_direction;
   if (!ret_nav_)
   {
-    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@@ -309,15 +314,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
       std::string algorithm_nav_name;
       planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-      robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
+      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
       nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_);
       nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_);
       nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_);
       nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
       nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);
 
-      nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
-      nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
+      planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+      planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
 
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
       {
@@ -330,7 +335,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
             local_goal_pose = follow_pose;
           }
         }
-        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
         {
           throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
           robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
@@ -340,7 +345,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   }
   else
   {
-    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
@@ -418,7 +423,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
   robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
-  robot_nav_2d_msgs::Path2D plan = transformed_plan_;
+  robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
   if (start_docking_)
   {
     local_goal = goal_pose_;
@@ -462,8 +467,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     // nh_.setParam("yaw_goal_tolerance", original_yaw_goal_tolerance_);
     std::string algorithm_nav_name;
     planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-    robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
-    nh_.setParam(algorithm_nav_name, original_papams_);
+    robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
+    parent_.setParam(algorithm_nav_name, original_papams_);
     nh_algorithm.setParam("allow_rotate", false);
   }
   return ret_nav_ && ret_angle_ && dock_ok;
@@ -479,7 +484,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
       {
         if (!dkpl_.front()->initialized_)
         {
-          dkpl_.front()->initialize(nh_, tf_, costmap_robot_, traj_generator_);
+          dkpl_.front()->initialize(parent_, tf_, costmap_robot_, traj_generator_);
           dkpl_.front()->initialized_ = true;
         }
         else
@@ -589,7 +594,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
     try
     {
       std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      docking_planner_loader_ = boost::dll::import_alias<robot_nav_core2::BaseGlobalPlanner::Ptr()>(
+      docking_planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
         path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations);
       docking_planner_ = docking_planner_loader_();
       if (!docking_planner_)
@@ -620,7 +625,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
         robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str());
         exit(1);
       }
-      robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_, docking_nav_name_);
+      robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_priv_, docking_nav_name_);
       docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Created docking nav %s", docking_nav_name_.c_str());
     }
@@ -657,13 +662,22 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   nh_priv_.param("lookahead_time", lookahead_time_, 1.5);
   nh_priv_.param("angle_threshold", angle_threshold_, 0.4);
 
-  detected_timeout_wt_ =
-      nh_priv_.createWallTimer(::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this);
+  robot::log_info("delay: %f", delay_time);
+  robot::log_info("timeout: %f", time_out);
+  robot::log_info("xy_goal_tolerance: %f", xy_goal_tolerance_);
+  robot::log_info("yaw_goal_tolerance: %f", yaw_goal_tolerance_);
+  robot::log_info("min_lookahead_dist: %f", min_lookahead_dist_);
+  robot::log_info("max_lookahead_dist: %f", max_lookahead_dist_);
+  robot::log_info("lookahead_time: %f", lookahead_time_);
+  robot::log_info("angle_threshold: %f", angle_threshold_);
+
+  detected_timeout_wt_ = ::robot::WallTimer(
+      ::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this, false, false);
   detected_timeout_wt_.start();
   robot::log_warning_at(__FILE__, __LINE__, "%s %f time_out start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
 
-  delayed_wt_ =
-      nh_priv_.createWallTimer(::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this);
+  delayed_wt_ = ::robot::WallTimer(
+      ::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this, false, false);
   delayed_wt_.start();
   robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
 

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp

@@ -120,7 +120,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      // exit(1);
+      exit(1);
     }
 
     std::string algorithm_rotate_name;
@@ -143,7 +143,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      // exit(1);
+      exit(1);
     }
 
     std::string goal_checker_name;

src/Libraries/robot_cpp/include/robot/node_handle.h

@@ -283,6 +283,14 @@ namespace robot
        */
       bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const;
 
+      /**
+       * @brief Get a parameter as robot_xmlrpcpp::XmlRpcValue (converted from YAML).
+       * @param key The parameter key (supports nested keys with '/' separator).
+       * @param v Storage for the retrieved value. Left unchanged if key not found.
+       * @return true if the parameter was retrieved and converted successfully, false otherwise.
+       */
+      bool getParam (const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const;
+
       /**
        * @brief Template method to get a parameter value (without default).
        *

src/Libraries/robot_cpp/include/robot/robot.h

@@ -2,11 +2,17 @@
 #define ROBOT_ROBOT_H
 
 #include <robot/init.h>
-#include <robot/time.h>
-#include <robot/timer.h>
-#include <robot/rate.h>
 #include <robot/console.h>
 #include <robot/node_handle.h>
 #include <robot/plugin_loader_helper.h>
 
+
+#include <robot/time.h>
+#include <robot/timer.h>
+#include <robot/duration.h>
+#include <robot/wall_timer.h>
+#include <robot/rate.h>
+#include <robot/exception.h>
+#include <robot/macros.h>
+#include <robot/platform.h>
 #endif

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -1727,6 +1727,87 @@ namespace robot
     return true;
   }
 
+  namespace
+  {
+    bool yamlToXmlRpc(const YAML::Node &y, robot_xmlrpcpp::XmlRpcValue &out)
+    {
+      if (!y.IsDefined())
+        return false;
+      if (y.IsScalar())
+      {
+        try
+        {
+          std::string s = y.as<std::string>();
+          std::string lower = s;
+          std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower);
+          if (lower == "true" || lower == "1" || lower == "yes" || lower == "on")
+          {
+            out = robot_xmlrpcpp::XmlRpcValue(true);
+            return true;
+          }
+          if (lower == "false" || lower == "0" || lower == "no" || lower == "off")
+          {
+            out = robot_xmlrpcpp::XmlRpcValue(false);
+            return true;
+          }
+          try
+          {
+            int i = y.as<int>();
+            out = robot_xmlrpcpp::XmlRpcValue(i);
+            return true;
+          }
+          catch (...) {}
+          try
+          {
+            double d = y.as<double>();
+            out = robot_xmlrpcpp::XmlRpcValue(d);
+            return true;
+          }
+          catch (...) {}
+          out = robot_xmlrpcpp::XmlRpcValue(s);
+          return true;
+        }
+        catch (...)
+        {
+          return false;
+        }
+      }
+      if (y.IsSequence())
+      {
+        out = robot_xmlrpcpp::XmlRpcValue();
+        out.setSize(static_cast<int>(y.size()));
+        for (size_t i = 0; i < y.size(); ++i)
+        {
+          robot_xmlrpcpp::XmlRpcValue item;
+          if (yamlToXmlRpc(y[i], item))
+            out[static_cast<int>(i)] = item;
+        }
+        return true;
+      }
+      if (y.IsMap())
+      {
+        out = robot_xmlrpcpp::XmlRpcValue();
+        for (YAML::const_iterator it = y.begin(); it != y.end(); ++it)
+        {
+          std::string k = it->first.as<std::string>();
+          robot_xmlrpcpp::XmlRpcValue item;
+          if (yamlToXmlRpc(it->second, item))
+            out[k] = item;
+        }
+        return true;
+      }
+      return false;
+    }
+  }
+
+  bool NodeHandle::getParam(const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const
+  {
+    YAML::Node value = getNestedValue(key);
+    if (!value.IsDefined())
+      return false;
+    return yamlToXmlRpc(value, v);
+  }
+
   template <typename T>
   T NodeHandle::param(const std::string &param_name) const
   {

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -527,8 +527,7 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   // for (size_t i = 0; i < laser_scan.intensities.size(); i++) {
   //   intensities_str << laser_scan.intensities[i] << " ";
   // }
-  // robot::log_info("intensities: %s", intensities_str.str().c_str());
-  // robot::log_info("--------------------------------");
+  // robot::log_error("intensities: %s", intensities_str.str().c_str());
 
   updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
   updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
@@ -2198,7 +2197,6 @@ void move_base::MoveBase::planThread()
     // if we didn't get a plan and we are in the planning state (the robot isn't moving)
     else if (state_ == move_base::PLANNING)
     {
-      robot::log_info("No Plan...");
       robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_);
 
       // check if we've tried to make a plan for over our time limit or our maximum number of retries