update

CMakeLists.txt

@@ -15,7 +15,9 @@ if(NOT CMAKE_BUILD_TYPE)
 endif()
 
 # Flags chung
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 
@@ -64,10 +66,6 @@ if (NOT TARGET voxel_grid)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/voxel_grid) 
 endif()
 
-if (NOT TARGET nav_grid)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid) 
-endif()
-
 if (NOT TARGET nav_2d_msgs)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_msgs) 
 endif()
@@ -84,11 +82,14 @@ if (NOT TARGET nav_core)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core)
 endif()
 
+if (NOT TARGET nav_grid)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid) 
+endif()
+
 if (NOT TARGET nav_core2)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2)
 endif()
 
-
 if (NOT TARGET nav_core_adapter)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter)
 endif()
@@ -113,9 +114,9 @@ if (NOT TARGET score_algorithm)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm)
 endif()
 
-#if (NOT TARGET mkt_algorithm)
+if (NOT TARGET mkt_algorithm)
-#    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
-#endif()
+endif()
 
 if (NOT TARGET two_points_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner)

config/pnkx_local_planner_params.yaml

@@ -4,7 +4,7 @@ docking_planner_name: PNKXDockingLocalPlanner
 go_straight_planner_name: PNKXGoStraightLocalPlanner
 rotate_planner_name: PNKXRotateLocalPlanner
 
-base_local_planner: nav_core_adapter::LocalPlannerAdapter
+base_local_planner: LocalPlannerAdapter
 yaw_goal_tolerance: 0.017  
 xy_goal_tolerance: 0.03 
 min_approach_linear_velocity: 0.06   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)

src/APIs/c_api/CMakeLists.txt

@@ -69,7 +69,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 option(BUILD_TESTS "Build test programs" OFF)
 
 # Flags biên dịch
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 

src/Algorithms/Cores/score_algorithm/CMakeLists.txt

@@ -25,6 +25,8 @@ add_library(score_algorithm src/score_algorithm.cpp)
 target_link_libraries(score_algorithm 
   PUBLIC
     ${PACKAGES_DIR}
+    robot::node_handle
+    robot::console
   PRIVATE
     Boost::filesystem
     Boost::system
@@ -55,7 +57,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 option(BUILD_TESTS "Build test programs" OFF)
 
 # Flags biên dịch
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 

src/Algorithms/Cores/score_algorithm/include/angles/angles.h

@@ -1,26 +0,0 @@
-#ifndef ANGLES_ANGLES_H
-#define ANGLES_ANGLES_H
-
-#include <cmath>
-
-namespace angles
-{
-
-/**
- * @brief Normalize an angle to the range [-π, π]
- * @param angle The angle in radians to normalize
- * @return The normalized angle in the range [-π, π]
- */
-inline double normalize_angle(double angle)
-{
-  while (angle > M_PI)
-    angle -= 2.0 * M_PI;
-  while (angle < -M_PI)
-    angle += 2.0 * M_PI;
-  return angle;
-}
-
-} // namespace angles
-
-#endif // ANGLES_ANGLES_H
-

src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h

@@ -1,7 +1,7 @@
 #ifndef _SCORE_ALGORITHM_H_INCLUDED__
 #define _SCORE_ALGORITHM_H_INCLUDED__
 
-#include <yaml-cpp/yaml.h>
+#include <robot/node_handle.h>
 #include <iostream>
 #include <nav_2d_msgs/Twist2DStamped.h>
 #include <nav_2d_msgs/Path2D.h>
@@ -24,7 +24,7 @@ namespace score_algorithm
   class ScoreAlgorithm
   {
   public:
-    using Ptr = boost::shared_ptr<score_algorithm::ScoreAlgorithm>;
+    using Ptr = std::shared_ptr<score_algorithm::ScoreAlgorithm>;
 
     inline double smoothstep(double x)
     {
@@ -38,7 +38,7 @@ namespace score_algorithm
      * @param tf TFListener pointer
      * @param costmap_ros Costmap pointer
      */
-    virtual void initialize(YAML::Node &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_ros,
+    virtual void initialize(robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
                             const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0;
   
     /**

src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h

@@ -1,7 +1,7 @@
 #ifndef _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
 #define _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
 
-#include <yaml-cpp/yaml.h>
+#include <robot/node_handle.h>
 #include <mkt_msgs/Trajectory2D.h>
 #include <nav_2d_msgs/Path2D.h>
 #include <nav_2d_msgs/Twist2D.h>
@@ -39,7 +39,7 @@ namespace score_algorithm
      * @brief Initialize parameters as needed
      * @param nh NodeHandle to read parameters from
      */
-    virtual void initialize(const YAML::Node &nh) = 0;
+    virtual void initialize(const robot::NodeHandle &nh) = 0;
 
     /**
      * @brief Reset the state (if any) when the planner gets a new goal
@@ -128,7 +128,8 @@ namespace score_algorithm
                                                       const nav_2d_msgs::Twist2D &start_vel,
                                                       const nav_2d_msgs::Twist2D &cmd_vel) = 0;
 
-    virtual YAML::Node getNodeHandle() = 0;
+    virtual robot::NodeHandle getNodeHandle() = 0;
+
   };
 
 } // namespace score_algorithm

src/Algorithms/Libraries/angles/include/angles/angles.h

@@ -20,6 +20,28 @@ inline double normalize_angle(double angle)
   return angle;
 }
 
+/**
+ * @brief Calculate the shortest angular distance between two angles
+ * 
+ * Computes the shortest angular distance from angle 'from' to angle 'to'.
+ * The result is in the range [-π, π], where positive values indicate
+ * counter-clockwise rotation and negative values indicate clockwise rotation.
+ * 
+ * @param from The starting angle in radians
+ * @param to The target angle in radians
+ * @return The shortest angular distance in radians, in the range [-π, π]
+ * 
+ * @code
+ * double diff = angles::shortest_angular_distance(0.0, M_PI/2);  // Returns π/2
+ * double diff2 = angles::shortest_angular_distance(M_PI, 0.0);  // Returns -π (or π, normalized)
+ * @endcode
+ */
+inline double shortest_angular_distance(double from, double to)
+{
+  double diff = to - from;
+  return normalize_angle(diff);
+}
+
 } // namespace angles
 
 #endif // ANGLES_ANGLES_H

src/Algorithms/Libraries/kalman/CMakeLists.txt

@@ -78,7 +78,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 option(BUILD_TESTS "Build test programs" OFF)
 
 # Flags biên dịch
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 

src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt

@@ -22,16 +22,15 @@ include_directories(
 
 # Dependencies packages
 set(PACKAGES_DIR
-  geometry_msgs
+    geometry_msgs
-  score_algorithm
+    score_algorithm
-  nav_2d_msgs
+    nav_2d_msgs
-  nav_2d_utils
+    nav_2d_utils
-  kalman
+    kalman
     angles
     nav_grid
-  costmap_2d
+    costmap_2d
     sensor_msgs
-  visualization_msgs
     std_msgs
 )
 
@@ -53,6 +52,8 @@ add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS})
 target_link_libraries(mkt_algorithm_diff
     PUBLIC
         ${PACKAGES_DIR}
+        robot::node_handle
+        robot::console
         Boost::system
         Eigen3::Eigen
 )
@@ -97,7 +98,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 option(BUILD_TESTS "Build test programs" OFF)
 
 # Flags biên dịch
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h

@@ -24,7 +24,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Calculating algorithm
@@ -35,6 +35,12 @@ namespace mkt_algorithm
       virtual mkt_msgs::Trajectory2D calculator(
           const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
 
+      /**
+       * @brief Create a new GoStraight instance
+       * @return A pointer to the new GoStraight instance
+       */
+      static score_algorithm::ScoreAlgorithm::Ptr create();
+      
     }; // class GoStraight
   } // namespace diff
 

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h

@@ -1,26 +1,18 @@
 #ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
 #define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
 
+#include <robot/node_handle.h>
 #include <score_algorithm/score_algorithm.h>
 
-#include <boost/dll/import.hpp>
+#include <boost/dll/alias.hpp>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <nav_2d_msgs/Pose2DStamped.h>
-// #include <visualization_msgs/Marker.h>
-// #include <ddynamic_reconfigure/ddynamic_reconfigure.h>
-
-// #include <base_local_planner/costmap_model.h>
-// #include <base_local_planner/world_model.h>
-// #include <base_local_planner/trajectory_planner.h>
-// #include <base_local_planner/map_grid_visualizer.h>
-// #include <base_local_planner/BaseLocalPlannerConfig.h>
-// #include <teb_local_planner/pose_se2.h>
-
 #include <nav_core2/exceptions.h>
 #include <nav_core2/costmap.h>
 #include <nav_grid/coordinate_conversion.h>
 #include <angles/angles.h>
+#include <tf3/exceptions.h>
 #include <tf3/utils.h>
 #include <nav_msgs/Path.h>
 #include <kalman/kalman.h>
@@ -52,7 +44,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
@@ -92,6 +84,12 @@ namespace mkt_algorithm
        */
       virtual void resume() override;
 
+      /**
+       * @brief Create a new PredictiveTrajectory instance
+       * @return A pointer to the new PredictiveTrajectory instance
+       */
+      static score_algorithm::ScoreAlgorithm::Ptr create();
+
     protected:
       inline double sign(double x)
       {
@@ -107,7 +105,7 @@ namespace mkt_algorithm
        * @brief Initialize dynamic reconfigure
        * @param nh NodeHandle to read parameters from
        */
-      virtual void initDynamicReconfigure(const ros::NodeHandle &nh);
+      virtual void initDynamicReconfigure(const robot::NodeHandle &nh);
 
       /**
        * @brief Dynamically adjust look ahead distance based on the speed
@@ -160,7 +158,7 @@ namespace mkt_algorithm
        */
       bool transformGlobalPlan(
           TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose,
-          const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length,
+          const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
           nav_2d_msgs::Path2D &transformed_plan);
 
       /**
@@ -241,17 +239,15 @@ namespace mkt_algorithm
 
       bool initialized_;
       bool nav_stop_, avoid_obstacles_;
-      ros::NodeHandle nh_, nh_priv_;
+      robot::NodeHandle nh_, nh_priv_;
       std::string frame_id_path_;
       std::string robot_base_frame_;
-      ros::Publisher carrot_pub_;
 
       nav_2d_msgs::Pose2DStamped goal_;
       nav_2d_msgs::Path2D global_plan_;
       nav_2d_msgs::Path2D compute_plan_;
       nav_2d_msgs::Path2D transform_plan_;
       nav_2d_msgs::Twist2D prevous_drive_cmd_;
-      ros::Publisher drive_pub_;
 
       double x_direction_, y_direction_, theta_direction_;
       double max_robot_pose_search_dist_;
@@ -296,16 +292,9 @@ namespace mkt_algorithm
 
       // Control frequency
       double control_duration_;
-
-      std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure> ddr_;
-      base_local_planner::BaseLocalPlannerConfig config_;
-
-      base_local_planner::WorldModel *world_model_; ///< @brief The world model that the controller will use
-      base_local_planner::TrajectoryPlanner *tc_;   ///< @brief The trajectory controller
-      base_local_planner::MapGridVisualizer map_viz_;
       std::vector<geometry_msgs::Point> footprint_spec_;
 
-      ros::Time last_actuator_update_;
+      robot::Time last_actuator_update_;
       boost::shared_ptr<KalmanFilter> kf_;
       int m_, n_;
       Eigen::MatrixXd A;
@@ -314,8 +303,6 @@ namespace mkt_algorithm
       Eigen::MatrixXd R;
       Eigen::MatrixXd P;
 
-      ros::Publisher cost_right_goals_pub_, cost_left_goals_pub_;
-      // visualization_msgs::Marker L_, R_;
     }; // class PredictiveTrajectory
 
   } // namespace diff

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h

@@ -23,7 +23,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
@@ -53,6 +53,12 @@ namespace mkt_algorithm
       virtual mkt_msgs::Trajectory2D calculator(
           const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
 
+      /**
+       * @brief Create a new RotateToGoal instance
+       * @return A pointer to the new RotateToGoal instance
+       */
+      static score_algorithm::ScoreAlgorithm::Ptr create();
+
     protected:
       /**
        * @brief Initialize parameters

src/Algorithms/Libraries/mkt_algorithm/package.xml

@@ -59,7 +59,6 @@
   <build_depend>ddynamic_reconfigure</build_depend>
   <build_depend>costmap_2d</build_depend>
   <build_depend>base_local_planner</build_depend>
-  <build_depend>visualization_msgs</build_depend>
 
   <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>score_algorithm</build_export_depend>
@@ -71,7 +70,6 @@
   <build_export_depend>ddynamic_reconfigure</build_export_depend>
   <build_export_depend>costmap_2d</build_export_depend>
   <build_export_depend>base_local_planner</build_export_depend>
-  <build_export_depend>visualization_msgs</build_export_depend>
 
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>score_algorithm</exec_depend>
@@ -83,7 +81,6 @@
   <exec_depend>ddynamic_reconfigure</exec_depend>
   <exec_depend>costmap_2d</exec_depend>
   <exec_depend>base_local_planner</exec_depend>
-  <exec_depend>visualization_msgs</exec_depend>
 
   <depend>pluginlib</depend>
 

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp

@@ -1,35 +1,24 @@
 #include <mkt_algorithm/diff/diff_go_straight.h>
-
-// other
-// #include <pluginlib/class_list_macros.h>
 #include <boost/dll/alias.hpp>
+#include <robot/console.h>
 
 void mkt_algorithm::diff::GoStraight::initialize(
-    ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
-    nh_ = ros::NodeHandle("~/");
+    nh_ = robot::NodeHandle("~");
-    nh_priv_ = ros::NodeHandle("~/" + name);
+    nh_priv_ = robot::NodeHandle(nh, name);
     name_ = name;
     tf_ = tf;
     traj_ = traj;
-    costmap_ros_ = costmap_ros;
+    costmap_robot_ = costmap_robot;
 
     this->getParams();
-    // this->initDynamicReconfigure(nh_priv_);
     nh_.param("publish_topic", enable_publish_, false);
     nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
-
+    std::vector<geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
-    carrot_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("lookahead_point", 1);
-    reached_intermediate_goals_pub_ = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
-    current_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("sub_goal", 1);
-    closet_robot_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
-    transformed_plan_pub_ = nh_.advertise<nav_msgs::Path>("transformed_plan", 1);
-    compute_plan_pub_ = nh_.advertise<nav_msgs::Path>("compute_plan", 1);
-
-    std::vector<geometry_msgs::Point> footprint = costmap_ros_? costmap_ros_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
     if (footprint.size() > 1)
     {
       double min_length = 1e10;
@@ -53,7 +42,7 @@ void mkt_algorithm::diff::GoStraight::initialize(
     }
 
     // kalman
-    last_actuator_update_ = ros::Time::now();
+    last_actuator_update_ = robot::Time::now();
     n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
     m_ = 2; // measurements: x, y, theta
     double dt = control_duration_;
@@ -87,11 +76,11 @@ void mkt_algorithm::diff::GoStraight::initialize(
     kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
     Eigen::VectorXd x0(n_);
     x0 << 0, 0, 0, 0, 0, 0;
-    kf_->init(ros::Time::now().toSec(), x0);
+    kf_->init(robot::Time::now().toSec(), x0);
 
     x_direction_ = y_direction_ = theta_direction_ = 0;
     initialized_ = true;
-    ROS_INFO("GoStraight Initialized successfully");
+    robot::log_info("GoStraight Initialized successfully");
   }
 }
 
@@ -104,11 +93,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     return result;
   if (compute_plan_.poses.size() < 2)
   {
-    ROS_WARN_THROTTLE(2.0, "Local compute plan is available");
+    robot::log_warning("Local compute plan is available");
     return result;
   }
 
-  ros::Time current_time = ros::Time::now();
+  robot::Time current_time = robot::Time::now();
   double dt = (current_time - last_actuator_update_).toSec();
   last_actuator_update_ = current_time;
 
@@ -117,8 +106,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
 
   nav_2d_msgs::Twist2D twist;
   traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom
-  ros::Rate r(50);
+  robot::Rate r(50);
-  while (ros::ok() && traj_->hasMoreTwists())
+  while (traj_->hasMoreTwists())
   {
     twist = traj_->nextTwist();
     // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
@@ -129,13 +118,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   nav_2d_msgs::Path2D transformed_plan = transform_plan_;
   if (transformed_plan.poses.empty())
   {
-    ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");
+    robot::log_warning("Transformed plan is empty. Cannot determine a local plan.");
     return result;
   }
 
-  if (enable_publish_)
-    transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan));
-
   // Dynamically adjust look ahead distance based on the speed
   double lookahead_dist = this->getLookAheadDistance(twist);
   // Return false if the transformed global plan is empty
@@ -145,9 +131,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   // Get lookahead point and publish for visualization
   auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
 
-  if (enable_publish_)
-    carrot_pub_.publish(this->createCarrotMsg(carrot_pose));
-
   // Carrot distance squared
   const double carrot_dist2 =
       (carrot_pose.pose.x * carrot_pose.pose.x) +
@@ -240,4 +223,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   return result;
 }
 
-PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::GoStraight, score_algorithm::ScoreAlgorithm)
+score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::GoStraight::create()
+{
+  return std::make_shared<mkt_algorithm::diff::GoStraight>();
+}
+
+BOOST_DLL_ALIAS(mkt_algorithm::diff::GoStraight::create, MKTAlgorithmDiffGoStraight)

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -1,93 +1,27 @@
 #include <mkt_algorithm/diff/diff_predictive_trajectory.h>
-// other
+#include <boost/dll/import.hpp>
-#include <pluginlib/class_list_macros.h>
+#include <robot/console.h>
+
 #define LIMIT_VEL_THETA 0.33
 
 void mkt_algorithm::diff::PredictiveTrajectory::initialize(
-    ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
-    nh_ = ros::NodeHandle("~/");
+    nh_ = robot::NodeHandle("~");
-    nh_priv_ = ros::NodeHandle("~/" + name);
+    nh_priv_ = robot::NodeHandle(nh, name);
     name_ = name;
     tf_ = tf;
     traj_ = traj;
-    costmap_ros_ = costmap_ros;
+    costmap_robot_ = costmap_robot;
     this->getParams();
     // this->initDynamicReconfigure(nh_priv_);
     nh_.param("publish_topic", enable_publish_, false);
     nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
-    std::vector<double> y_vels = {};
+    footprint_spec_ = costmap_robot_->getRobotFootprint();
-    footprint_spec_ = costmap_ros_->getRobotFootprint();
+    std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
-    world_model_ = new base_local_planner::CostmapModel(*costmap_ros->getCostmap());
-
-    this->config_.acc_lim_x = fabs(acc_lim_x_);
-    this->config_.acc_lim_y = fabs(acc_lim_y_);
-    this->config_.acc_lim_theta = fabs(acc_lim_theta_);
-
-    this->config_.max_vel_x = max_vel_x_;
-    this->config_.min_vel_x = fabs(min_vel_x_);
-
-    this->config_.max_vel_theta = max_vel_theta_;
-    this->config_.min_vel_theta = (-1.0) * max_vel_theta_;
-    this->config_.min_in_place_vel_theta = 0.4;
-
-    this->config_.sim_time = 1.5;
-    this->config_.sim_granularity = 0.025;
-    this->config_.vx_samples = 10;
-    this->config_.vtheta_samples = 40;
-    this->config_.path_distance_bias = 0.6;
-    this->config_.goal_distance_bias = 0.4;
-    this->config_.occdist_scale = 0.2;
-    this->config_.heading_lookahead = 1.0;
-    this->config_.oscillation_reset_dist = 0.05;
-    this->config_.escape_reset_dist = 0.1;
-    this->config_.escape_reset_theta = 1.570796;
-    this->config_.escape_vel = -0.1;
-    this->config_.holonomic_robot = false;
-    this->config_.heading_scoring_timestep = 0.1;
-    this->config_.dwa = true;
-    this->config_.simple_attractor = true;
-    this->config_.heading_scoring = false;
-    this->config_.angular_sim_granularity = 0.025;
-    bool meter_scoring = false;
-    double stop_time_buffer = 0.2;
-
-    tc_ = new base_local_planner::TrajectoryPlanner(
-        *world_model_, *costmap_ros->getCostmap(), footprint_spec_,
-        this->config_.acc_lim_x, this->config_.acc_lim_y, this->config_.acc_lim_theta,
-        this->config_.sim_time, this->config_.sim_granularity,
-        this->config_.vx_samples, this->config_.vtheta_samples,
-        this->config_.path_distance_bias, this->config_.goal_distance_bias, this->config_.occdist_scale,
-        this->config_.heading_lookahead, this->config_.oscillation_reset_dist,
-        this->config_.escape_reset_dist, this->config_.escape_reset_theta,
-        this->config_.holonomic_robot,
-        this->config_.max_vel_x, this->config_.min_vel_x,
-        this->config_.max_vel_theta, this->config_.min_vel_theta, this->config_.min_in_place_vel_theta,
-        this->config_.escape_vel,
-        this->config_.dwa, this->config_.heading_scoring, this->config_.heading_scoring_timestep, meter_scoring, this->config_.simple_attractor,
-        y_vels, stop_time_buffer, control_duration_, this->config_.angular_sim_granularity);
-
-    map_viz_.initialize(name,
-                        costmap_ros_->getGlobalFrameID(),
-                        [this](int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)
-                        {
-                          return tc_->getCellCosts(cx, cy, path_cost, goal_cost, occ_cost, total_cost);
-                        });
-
-    carrot_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("lookahead_point", 1);
-    reached_intermediate_goals_pub_ = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
-    current_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("sub_goal", 1);
-    closet_robot_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
-    transformed_plan_pub_ = nh_.advertise<nav_msgs::Path>("transformed_plan", 1);
-    compute_plan_pub_ = nh_.advertise<nav_msgs::Path>("compute_plan", 1);
-    cost_right_goals_pub_ = nh_.advertise<visualization_msgs::Marker>("cost_right_goals", 1);
-    cost_left_goals_pub_ = nh_.advertise<visualization_msgs::Marker>("cost_left_goals", 1);
-    drive_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_raw", 1);
-
-    std::vector<geometry_msgs::Point> footprint = costmap_ros_ ? costmap_ros_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
     if (footprint.size() > 1)
     {
       double min_length = 1e10;
@@ -111,7 +45,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
     }
 
     // kalman
-    last_actuator_update_ = ros::Time::now();
+    last_actuator_update_ = robot::Time::now();
     n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
     m_ = 2; // measurements: x, y, theta
     double dt = control_duration_;
@@ -145,47 +79,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
     kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
     Eigen::VectorXd x0(n_);
     x0 << 0, 0, 0, 0, 0, 0;
-    kf_->init(ros::Time::now().toSec(), x0);
+    kf_->init(robot::Time::now().toSec(), x0);
-
-    L_.header.frame_id = R_.header.frame_id = costmap_ros_->getGlobalFrameID(); // hoặc frame phù hợp
-    L_.ns = "left_cost_points";
-    R_.ns = "right_cost_points";
-    L_.id = 0;
-    R_.id = 1;
-    L_.type = R_.type = visualization_msgs::Marker::POINTS;
-    L_.action = R_.action = visualization_msgs::Marker::ADD;
-    L_.scale.x = L_.scale.y = R_.scale.x = R_.scale.y = 0.05; // kích thước điểm
-    // màu: L = xanh lá, R = đỏ
-    L_.color.r = 0.0;
-    L_.color.g = 1.0;
-    L_.color.b = 0.0;
-    L_.color.a = 1.0;
-    R_.color.r = 1.0;
-    R_.color.g = 0.0;
-    R_.color.b = 0.0;
-    R_.color.a = 1.0;
-
     x_direction_ = y_direction_ = theta_direction_ = 0;
     this->initialized_ = true;
-    ROS_INFO("PredictiveTrajectory Initialized successfully");
+    robot::log_info("[%s:%d] PredictiveTrajectory Initialized successfully", __FILE__, __LINE__);
   }
 }
 
 mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory()
 {
-  if (ddr_)
-    ddr_.reset();
-
-  if (tc_ != NULL)
-    delete tc_;
-
-  if (world_model_ != NULL)
-    delete world_model_;
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::getParams()
 {
-  robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
+  robot_base_frame_ = nh_priv_.param<std::string>("robot_base_frame", std::string("base_link"));
   nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
   nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
   nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
@@ -220,7 +127,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
   if (inflation_cost_scaling_factor_ <= 0.0)
   {
-    ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, "
+    robot::log_warning("[%s:%d] The value inflation_cost_scaling_factor is incorrectly set, "
              "it should be >0. Disabling cost regulated linear velocity scaling.");
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
@@ -247,59 +154,25 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   }
 }
 
-void mkt_algorithm::diff::PredictiveTrajectory::initDynamicReconfigure(const ros::NodeHandle &nh)
-{
-  // Ddynamic Reconfigure
-  ddr_ = std::make_shared<ddynamic_reconfigure::DDynamicReconfigure>(nh);
-  ddr_->registerVariable<bool>("avoid_obstacles", &this->avoid_obstacles_, "", true);
-  ddr_->registerVariable<double>("lookahead_time", &this->lookahead_time_, "", 0.0, 20.0);
-  ddr_->registerVariable<double>("lookahead_dist", &this->lookahead_dist_, "", 0.0, 20.0);
-
-  ddr_->registerVariable<bool>("use_velocity_scaled_lookahead_dist", &this->use_velocity_scaled_lookahead_dist_);
-  ddr_->registerVariable<double>("min_lookahead_dist", &this->min_lookahead_dist_, "", 0.0, 5.0);
-  ddr_->registerVariable<double>("max_lookahead_dist", &this->max_lookahead_dist_, "", 0.0, 10.0);
-  ddr_->registerVariable<double>("min_journey_squared", &this->min_journey_squared_, "", 0.0, 1.0);
-  ddr_->registerVariable<double>("max_journey_squared", &this->max_journey_squared_, "", 0.0, 1.0);
-  // Rotate to heading param
-  ddr_->registerVariable<bool>("use_rotate_to_heading", &this->use_rotate_to_heading_);
-  ddr_->registerVariable<double>("rotate_to_heading_min_angle", &this->rotate_to_heading_min_angle_, "", 0.0, 15.0);
-
-  // Speed
-  ddr_->registerVariable<double>("min_approach_linear_velocity", &this->min_approach_linear_velocity_, "", 0.0, 10.0);
-
-  // Regulated linear velocity scaling
-  ddr_->registerVariable<bool>("use_regulated_linear_velocity_scaling", &this->use_regulated_linear_velocity_scaling_);
-  ddr_->registerVariable<double>("regulated_linear_scaling_min_radius", &this->regulated_linear_scaling_min_radius_, "", 0.0, 5.0);
-  ddr_->registerVariable<double>("regulated_linear_scaling_min_speed", &this->regulated_linear_scaling_min_speed_, "", 0.0, 5.0);
-
-  // Inflation cost scaling (Limit velocity by proximity to obstacles)
-  ddr_->registerVariable<bool>("use_cost_regulated_linear_velocity_scaling", &this->use_cost_regulated_linear_velocity_scaling_);
-  ddr_->registerVariable<double>("inflation_cost_scaling_factor", &this->inflation_cost_scaling_factor_, "", 0.0, 10.0);
-  ddr_->registerVariable<double>("cost_scaling_dist", &this->cost_scaling_dist_, "", 0.0, 10.0);
-  ddr_->registerVariable<double>("cost_scaling_gain", &this->cost_scaling_gain_, "", 0.0, 10.0);
-
-  ddr_->publishServicesTopics();
-}
-
 void mkt_algorithm::diff::PredictiveTrajectory::reset()
 {
   if (this->initialized_)
   {
-    ROS_INFO("PredictiveTrajectory is reset");
+    robot::log_info("[%s:%d] PredictiveTrajectory is reset", __FILE__, __LINE__);
     reached_intermediate_goals_.clear();
     start_index_saved_vt_.clear();
     this->clear();
     x_direction_ = y_direction_ = theta_direction_ = 0;
     this->follow_step_path_ = false;
     this->nav_stop_ = false;
-    last_actuator_update_ = ros::Time::now();
+    last_actuator_update_ = robot::Time::now();
     prevous_drive_cmd_ = nav_2d_msgs::Twist2D();
 
     if (kf_)
     {
       Eigen::VectorXd x0(n_);
       x0 << 0, 0, 0, 0, 0, 0;
-      kf_->init(ros::Time::now().toSec(), x0);
+      kf_->init(robot::Time::now().toSec(), x0);
     }
   }
 }
@@ -316,13 +189,13 @@ void mkt_algorithm::diff::PredictiveTrajectory::resume()
     if(!this->nav_stop_)
       return;
     prevous_drive_cmd_ = nav_2d_msgs::Twist2D();
-    last_actuator_update_ = ros::Time::now();
+    last_actuator_update_ = robot::Time::now();
 
     if (kf_)
     {
       Eigen::VectorXd x0(n_);
       x0 << 0, 0, 0, 0, 0, 0;
-      kf_->init(ros::Time::now().toSec(), x0);
+      kf_->init(robot::Time::now().toSec(), x0);
     }
     this->nav_stop_ = false;
   }
@@ -334,11 +207,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
 {
   if (!initialized_)
   {
-    ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
+    robot::log_error("[%s:%d] This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__);
     return false;
   }
 
-  std::vector<geometry_msgs::Point> footprint = costmap_ros_ ? costmap_ros_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
+  std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
   if (footprint.size() > 1)
   {
     double min_length = 1e10;
@@ -363,12 +236,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
 
   if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
   {
-    ROS_ERROR("The Local plan is empty or less than 1 points %d", (unsigned int)global_plan.poses.size());
+    robot::log_error("[%s:%d] The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
     return false;
   }
   this->getParams();
-  if (avoid_obstacles_)
-    map_viz_.publishCostCloud(costmap_ros_->getCostmap());
 
   frame_id_path_ = global_plan.header.frame_id;
   goal_ = goal;
@@ -377,13 +248,13 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
   // prune global plan to cut off parts of the past (spatially before the robot)
   if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0))
   {
-    ROS_ERROR("pruneGlobalPlan Failed");
+    robot::log_error("[%s:%d] pruneGlobalPlan Failed", __FILE__, __LINE__);
     return false;
   }
 
   double S = std::numeric_limits<double>::infinity();
-  S = std::max(costmap_ros_->getCostmap()->getSizeInCellsX() * costmap_ros_->getCostmap()->getResolution() / 2.0,
+  S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0,
-               costmap_ros_->getCostmap()->getSizeInCellsY() * costmap_ros_->getCostmap()->getResolution() / 2.0);
+               costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0);
   const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_;
   S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S);
   compute_plan_.poses.clear();
@@ -392,13 +263,13 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
   {
     double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x;
     double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y;
-    if (hypot(dx, dy) <= 2.0 * costmap_ros_->getCostmap()->getResolution())
+    if (hypot(dx, dy) <= 2.0 * costmap_robot_->getCostmap()->getResolution())
     {
       compute_plan_ = global_plan_;
     }
     else
     {
-      ROS_ERROR("The Local Plan has 2 points and hypot between more than is %.3f m", costmap_ros_->getCostmap()->getResolution());
+      robot::log_error("[%s:%d] The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution());
       return false;
     }
   }
@@ -407,24 +278,21 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
     unsigned int start_index, goal_index;
     if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_))
     {
-      ROS_ERROR("computePlanCommand is Failed");
+      robot::log_error("[%s:%d] computePlanCommand is Failed", __FILE__, __LINE__);
       return false;
     }
   }
 
-  if (enable_publish_)
-    compute_plan_pub_.publish(nav_2d_utils::pathToPath(compute_plan_));
-
   double lookahead_dist = this->getLookAheadDistance(velocity);
   transform_plan_.poses.clear();
-  if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_ros_, robot_base_frame_, lookahead_dist, transform_plan_))
+  if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_))
   {
-    ROS_WARN("Could not transform the global plan to the frame of the controller");
+    robot::log_warning("[%s:%d] Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
     return false;
   }
   // else
   // {
-  //   ROS_INFO_THROTTLE(0.5, "Transform plan journey: %f %f %f", journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_);
+  //   robot::log_info("[%s:%d] Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_);
   // }
 
   x_direction = x_direction_;
@@ -462,29 +330,30 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
         double dx = it->pose.x - carrot_pose_it->pose.x;
         double dy = it->pose.x - carrot_pose_it->pose.y;
         distance_it += std::hypot(dx, dy);
-        if (distance_it > costmap_ros_->getCostmap()->getResolution())
+        if (distance_it > costmap_robot_->getCostmap()->getResolution())
         {
           prev_carrot_pose_it = it;
           break;
         }
       }
 
-      geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_ros_->getCostmap()->getResolution()
+      geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
                                       ? nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
                                       : nav_2d_utils::pose2DToPose(geometry_msgs::Pose2D());
 
       geometry_msgs::Pose back = nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
 
-      teb_local_planner::PoseSE2 start_pose(front);
+      // teb_local_planner::PoseSE2 start_pose(front);
-      teb_local_planner::PoseSE2 goal_pose(back);
+      // teb_local_planner::PoseSE2 goal_pose(back);
-      const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
+      // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
+      const double dir_path = 0.0;
 
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
     }
     catch (std::exception &e)
     {
-      ROS_ERROR("getLookAheadPoint throw an exception: %s", e.what());
+      robot::log_error("[%s:%d] getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
       return false;
     }
   }
@@ -504,11 +373,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     return result;
   if (compute_plan_.poses.size() < 2)
   {
-    ROS_WARN("Local compute plan is available");
+    robot::log_warning("[%s:%d] Local compute plan is available", __FILE__, __LINE__);
     return result;
   }
 
-  ros::Time current_time = ros::Time::now();
+  robot::Time current_time = robot::Time::now();
   double dt = (current_time - last_actuator_update_).toSec();
   last_actuator_update_ = current_time;
 
@@ -516,77 +385,29 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   double sign_x = sign(x_direction_);
   nav_2d_msgs::Twist2D twist;
   traj_->startNewIteration(velocity);
-  while (ros::ok() && traj_->hasMoreTwists())
+  while (traj_->hasMoreTwists())
   {
     twist = traj_->nextTwist();
   }
   drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
   double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
 
-  if (avoid_obstacles_)
-  {
-    if ((cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE) ||
-        (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE))
-    {
-      nav_2d_msgs::Twist2D cmd;
-      cmd.x = 0.2;
-      moveWithAccLimits(velocity, cmd, drive_cmd);
-      config_.max_vel_x = drive_cmd.x;
-      config_.sim_time = 1.9;
-      config_.path_distance_bias = 0.7;
-      config_.goal_distance_bias = 0.6;
-      config_.occdist_scale = 0.05;
-      config_.dwa = true;
-    }
-    else if (cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE ||
-             cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE)
-    {
-      nav_2d_msgs::Twist2D cmd;
-      cmd.x = 0.3;
-      moveWithAccLimits(velocity, cmd, drive_cmd);
-      config_.max_vel_x = drive_cmd.x;
-      config_.sim_time = 1.9;
-      config_.path_distance_bias = 0.7;
-      config_.goal_distance_bias = 0.6;
-      config_.occdist_scale = 0.12;
-      config_.dwa = false;
-    }
-    else
-    {
-      config_.max_vel_x = std::max(drive_cmd.x, 0.2);
-      config_.sim_time = 1.5;
-      config_.path_distance_bias = 0.8;
-      config_.goal_distance_bias = 0.6;
-      config_.occdist_scale = 0.01;
-      config_.dwa = false;
-    }
-    tc_->reconfigure(config_);
-    this->publishMarkers(pose);
-  }
-
   nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
   {
-    ROS_WARN("Transformed plan is empty. Cannot determine a localglobal_plan.");
+    robot::log_warning("[%s:%d] Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
     return result;
   }
-  if (enable_publish_)
-    transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan));
 
   double lookahead_dist = getLookAheadDistance(velocity);
   double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y);
-  // lookahead_dist = std::clamp(lookahead_dist - tolerance * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_);
 
   if (transformed_plan.poses.empty())
   {
-    ROS_WARN("Transformed plan is empty after compute lookahead point");
+    robot::log_warning("[%s:%d] Transformed plan is empty after compute lookahead point", __FILE__, __LINE__);
     return result;
   }
   auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
-
-  if (enable_publish_)
-    carrot_pub_.publish(nav_2d_utils::pose2DToPoseStamped(carrot_pose.pose, carrot_pose.header.frame_id, ros::Time::now()));
-
   bool allow_rotate = false;
   nh_priv_.param("allow_rotate", allow_rotate, false);
 
@@ -598,7 +419,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   if (avoid_obstacles_)
     allow_rotate = journey_plan >= distance_allow_rotate &&
                    fabs(front.y) <= 0.2 &&
-                   (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_ros_->getCostmap()->getResolution());
+                   (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution());
   else
     allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
 
@@ -607,7 +428,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   {
     if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_))
     {
-      tc_->updatePlan(nav_2d_utils::pathToPath(compute_plan_).poses, true);
       if (!stopWithAccLimits(pose, velocity, drive_cmd))
         return result;
     }
@@ -618,78 +438,75 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   else
   {
-    bool narrow_road = (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE);
+    const double vel_x_reduce = std::min(fabs(v_max), 0.3);
-    if (!avoid_obstacles_ || sign_x < 0 || journey_plan < max_journey_squared_ || narrow_road)
+    double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
+    carrot_dist2 = std::max(carrot_dist2, 0.05);
+    double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
+
+    const auto &plan_back_pose = compute_plan_.poses.back();
+    double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
+    post_cost = std::max(post_cost, center_cost_);
+    this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
+
+    const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
+    const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
+    double d_reduce = std::clamp(journey_plan, min_S, max_S);
+    double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
+    double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
+    double v_min =
+        journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
+    v_min *= sign_x;
+
+    double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
+    double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
+    double vel_reduce = sign_x > 0
+                            ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
+                            : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
+    drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
+
+    double v_theta = drive_cmd.x * curvature;
+    double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
+    if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
     {
-      const double vel_x_reduce = std::min(fabs(v_max), 0.3);
+      carrot_dist2 *= 0.6;
-      double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
+      curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
-      carrot_dist2 = std::max(carrot_dist2, 0.05);
+      v_theta = drive_cmd.x * curvature;
-      double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
+    }
+    if (fabs(v_theta) > LIMIT_VEL_THETA)
+    {
+      nav_2d_msgs::Twist2D cmd_vel, cmd_result;
+      cmd_vel.x = sign_x > 0
+                      ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1))
+                      : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1));
+      cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5);
+      this->moveWithAccLimits(velocity, cmd_vel, cmd_result);
+      drive_cmd.x = std::copysign(cmd_result.x, sign_x);
+      v_theta = drive_cmd.x * curvature;
+    }
 
-      const auto &plan_back_pose = compute_plan_.poses.back();
+    if (journey_plan < min_journey_squared_)
-      double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
+    {
-      post_cost = std::max(post_cost, center_cost_);
+      if (transform_plan_.poses.size() > 2)
-      this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
-
-      const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
-      const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
-      double d_reduce = std::clamp(journey_plan, min_S, max_S);
-      double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
-      double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
-      double v_min =
-          journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
-      v_min *= sign_x;
-
-      double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
-      double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
-      double vel_reduce = sign_x > 0
-                              ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
-                              : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
-      drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
-
-      double v_theta = drive_cmd.x * curvature;
-      double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
-      if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
       {
-        carrot_dist2 *= 0.6;
+        nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
-        curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
+        nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
-        v_theta = drive_cmd.x * curvature;
+        double dx = end.pose.x - start.pose.x;
+        double dy = end.pose.y - start.pose.y;
+        v_theta = atan2(dy, dx);
+        if (v_theta > M_PI_2)
+          v_theta -= M_PI;
+        else if (v_theta < -M_PI_2)
+          v_theta += M_PI;
+        // v_theta *= 0.5;
+        v_theta = std::clamp(v_theta, -0.02, 0.02);
       }
-      if (fabs(v_theta) > LIMIT_VEL_THETA)
+      else
-      {
+        v_theta = 0.0;
-        nav_2d_msgs::Twist2D cmd_vel, cmd_result;
+    }
-        cmd_vel.x = sign_x > 0
+    double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8;
-                        ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1))
+    double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
-                        : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1));
+    double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
-        cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5);
+    drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
-        this->moveWithAccLimits(velocity, cmd_vel, cmd_result);
-        drive_cmd.x = std::copysign(cmd_result.x, sign_x);
-        v_theta = drive_cmd.x * curvature;
-      }
-
-      if (journey_plan < min_journey_squared_)
-      {
-        if (transform_plan_.poses.size() > 2)
-        {
-          nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
-          nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
-          double dx = end.pose.x - start.pose.x;
-          double dy = end.pose.y - start.pose.y;
-          v_theta = atan2(dy, dx);
-          if (v_theta > M_PI_2)
-            v_theta -= M_PI;
-          else if (v_theta < -M_PI_2)
-            v_theta += M_PI;
-          // v_theta *= 0.5;
-          v_theta = std::clamp(v_theta, -0.02, 0.02);
-        }
-        else
-          v_theta = 0.0;
-      }
-      double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8;
-      double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
-      double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
-      drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
 
       if (this->nav_stop_)
       {
@@ -703,78 +520,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
         result.velocity = drive_cmd;
         return result;
       }
-    }
+  
-    else
-    {
-      geometry_msgs::PoseStamped traj_cmd, robot_vel;
-      robot_vel.pose.position.x = velocity.x;
-      robot_vel.pose.position.y = velocity.y;
-      tf2::Quaternion q;
-
-      q.setRPY(0, 0, velocity.theta);
-      tf2::convert(q, robot_vel.pose.orientation);
-      tc_->updatePlan(nav_2d_utils::pathToPath(compute_plan_).poses, true);
-      auto path = tc_->findBestPath(nav_2d_utils::pose2DToPoseStamped(pose), robot_vel, traj_cmd);
-
-      if (path.cost_ < 0)
-      {
-        ROS_DEBUG_NAMED("trajectory_planner_ros",
-                        "The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories.");
-
-        throw nav_core2::PlannerTFException("The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories.");
-      }
-
-      if (path.cost_ < 0 || nav_stop_)
-      {
-        if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
-        {
-          if (!stopWithAccLimits(pose, velocity, drive_cmd))
-            return result;
-        }
-        else
-          drive_cmd = {};
-        result.velocity = drive_cmd;
-        return result;
-      }
-      else
-      {
-        drive_cmd.x = std::max(drive_cmd.x, min_approach_linear_velocity_);
-        drive_cmd.x = std::clamp(traj_cmd.pose.position.x, -fabs(drive_cmd.x), fabs(drive_cmd.x));
-        drive_cmd.y = traj_cmd.pose.position.y;
-        drive_cmd.theta = tf2::getYaw(traj_cmd.pose.orientation);
-
-        if ((cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE ||
-             cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE) &&
-            fabs(traj_cmd.pose.position.x) < 0.02 && fabs(drive_cmd.theta) > 0.1)
-        {
-          nav_2d_msgs::Twist2D cmd, cmd_result;
-          cmd.x = sign_x * min_approach_linear_velocity_;
-          moveWithAccLimits(velocity, cmd, cmd_result);
-
-          if (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE)
-            drive_cmd.theta = 0.0;
-
-          else if (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE)
-            drive_cmd.theta = -min_vel_theta_ * sign_x;
-
-          else
-            drive_cmd.theta = min_vel_theta_ * sign_x;
-          drive_cmd.x = sign_x * cmd_result.x;
-        }
-
-        for (unsigned int i = 0; i < path.getPointsSize(); ++i)
-        {
-          double p_x, p_y, p_th;
-          path.getPoint(i, p_x, p_y, p_th);
-          geometry_msgs::Pose2D pose;
-          pose.x = p_x;
-          pose.y = p_y;
-          pose.theta = p_th;
-          result.poses.push_back(pose);
-        }
-      }
-    }
-
     Eigen::VectorXd y(2);
     y << drive_cmd.x, drive_cmd.theta;
 
@@ -1063,9 +809,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
     if (erase_end != global_plan.poses.begin())
       global_plan.poses.erase(global_plan.poses.begin(), erase_end);
   }
-  catch (const tf::TransformException &ex)
+  catch (const tf3::TransformException &ex)
   {
-    ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what());
+    robot::log_debug("[%s:%d] Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what());
     return false;
   }
   return true;
@@ -1073,7 +819,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
 
 bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
     TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose,
-    const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length,
+    const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
     nav_2d_msgs::Path2D &transformed_plan)
 {
   // this method is a slightly modified version of base_local_planner/goal_functions.h
@@ -1094,7 +840,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
   {
     if (global_plan.poses.empty())
     {
-      ROS_ERROR("Received plan with zero length");
+      robot::log_error("[%s:%d] Received plan with zero length", __FILE__, __LINE__);
       return false;
     }
 
@@ -1179,25 +925,25 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
       ++i;
     }
   }
-  catch (tf::LookupException &ex)
+  catch (tf3::LookupException &ex)
   {
-    ROS_ERROR("No Transform available Error: %s\n", ex.what());
+    robot::log_error("[%s:%d] No Transform available Error: %s", __FILE__, __LINE__, ex.what());
     return false;
   }
-  catch (tf::ConnectivityException &ex)
+  catch (tf3::ConnectivityException &ex)
   {
-    ROS_ERROR("Connectivity Error: %s\n", ex.what());
+    robot::log_error("[%s:%d] Connectivity Error: %s", __FILE__, __LINE__, ex.what());
     return false;
   }
-  catch (tf::ExtrapolationException &ex)
+  catch (tf3::ExtrapolationException &ex)
   {
-    ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+    robot::log_error("[%s:%d] Extrapolation Error: %s", __FILE__, __LINE__, ex.what());
     if (global_plan.poses.size() > 0)
-      ROS_ERROR("Robot Frame: %s Plan Frame size %d: %s\n", robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
+      robot::log_error("[%s:%d] Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
 
     return false;
   }
-  return true;
+  return true;  
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits(
@@ -1241,29 +987,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const nav_2d_m
   double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt));
 
   // we do want to check whether or not the command is valid
-  if (avoid_obstacles_)
-  {
-    try
-    {
-      if (tc_ != NULL)
-      {
-        bool valid_cmd = tc_->checkTrajectory(pose.pose.x, pose.pose.y, pose.pose.theta, velocity.x, velocity.y, vel_yaw, vx, vy, vth);
-        // if we have a valid command, we'll pass it on, otherwise we'll command all zeros
-        if (valid_cmd)
-        {
-          ROS_DEBUG("Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
-          cmd_vel.x = vx;
-          cmd_vel.y = vy;
-          cmd_vel.theta = vth;
-          return true;
-        }
-      }
-    }
-    catch (const std::exception &e)
-    {
-      ROS_ERROR_STREAM(e.what() << '\n');
-    }
-  }
   cmd_vel.x = vx;
   cmd_vel.y = vy;
   cmd_vel.theta = vth;
@@ -1302,7 +1025,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
       pose_cost != static_cast<double>(costmap_2d::NO_INFORMATION) &&
       pose_cost != static_cast<double>(costmap_2d::FREE_SPACE))
   {
-    const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius();
+    const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius();
     const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
                                                 std::log(pose_cost / (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
                                             inscribed_radius;
@@ -1327,8 +1050,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
   //      (2) using t with the actual lookahead
   //          distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t).
 
-  double dist_error_limit = costmap_ros_ != nullptr && costmap_ros_->getCostmap() != nullptr
+  double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr
-                                ? 2.0 * costmap_ros_->getCostmap()->getResolution()
+                                ? 2.0 * costmap_robot_->getCostmap()->getResolution()
                                 : 0.1;
   if (dist_error > dist_error_limit)
   {
@@ -1414,9 +1137,9 @@ double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, co
 {
   unsigned int mx, my;
   unsigned char cost;
-  if (costmap_ros_->getCostmap()->worldToMap(x, y, mx, my))
+  if (costmap_robot_->getCostmap()->worldToMap(x, y, mx, my))
   {
-    cost = costmap_ros_->getCostmap()->getCost(mx, my);
+    cost = costmap_robot_->getCostmap()->getCost(mx, my);
   }
   return static_cast<double>(cost);
 }
@@ -1490,10 +1213,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v
 void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose2DStamped pose)
 {
   const auto &plan_back_pose = compute_plan_.poses.back();
-  // const double offset_max = this->min_path_distance_ + costmap_ros_->getCostmap()->getResolution() * 2.0;
+  // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0;
   // const double offset_min = this->min_path_distance_;
 
-  auto points_rb = interpolateFootprint(pose.pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0);
+  auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
   for (const auto &point : points_rb)
   {
     double cost_goal = costAtPose(point.x, point.y);
@@ -1506,19 +1229,17 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
     if (y_rel > epsilon)
     {
       cost_left_side_ = std::max(cost_left_side_, cost_goal);
-      L_.points.push_back(point);
     }
     else if (y_rel < -epsilon)
     {
       cost_right_side_ = std::max(cost_right_side_, cost_goal);
-      R_.points.push_back(point);
     }
   }
 
   unsigned int step_footprint = 10;
   if ((unsigned int)(compute_plan_.poses.size() - 1) < 10)
   {
-    auto points = interpolateFootprint(plan_back_pose.pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0);
+    auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
     for (const auto &point : points)
     {
       double cost_goal = costAtPose(point.x, point.y);
@@ -1531,12 +1252,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
       if (y_rel > epsilon)
       {
         cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
-        L_.points.push_back(point);
       }
       else if (y_rel < -epsilon)
       {
         cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
-        R_.points.push_back(point);
       }
       else
         center_cost_ = std::max(center_cost_, cost_goal);
@@ -1546,7 +1265,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
   {
     for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint)
     {
-      auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0);
+      auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
       for (const auto &point : points)
       {
         double cost_goal = costAtPose(point.x, point.y);
@@ -1559,12 +1278,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
         if (y_rel > epsilon)
         {
           cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
-          L_.points.push_back(point);
         }
         else if (y_rel < -epsilon)
         {
           cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
-          R_.points.push_back(point);
         }
         else
           center_cost_ = std::max(center_cost_, cost_goal);
@@ -1575,13 +1292,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
         break;
     }
   }
-  if (enable_publish_)
-  {
-    cost_left_goals_pub_.publish(L_);
-    cost_right_goals_pub_.publish(R_);
-  }
-  R_.points.clear();
-  L_.points.clear();
 }
 
-PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::PredictiveTrajectory, score_algorithm::ScoreAlgorithm)
+score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
+{
+  return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();
+}
+
+// Register this planner as a GlobalPlanner plugin
+BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory)

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp

@@ -1,25 +1,24 @@
+#include <robot/console.h>
 #include <mkt_algorithm/diff/diff_rotate_to_goal.h>
-
-// other
 #include <nav_2d_utils/parameters.h>
-#include <pluginlib/class_list_macros.h>
+#include <boost/dll/alias.hpp>
 #include <angles/angles.h>
 
 void mkt_algorithm::diff::RotateToGoal::initialize(
-    ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
-    nh_ = ros::NodeHandle(nh, name);
+    nh_ = robot::NodeHandle(nh, name);
     name_ = name;
     tf_ = tf;
     traj_ = traj;
-    costmap_ros_ = costmap_ros;
+    costmap_robot_ = costmap_robot;
     this->getParams();
 
     x_direction_ = y_direction_ = theta_direction_ = 0;
     this->initialized_ = true;
-    ROS_INFO("RotateToGoal Initialized successfully");
+    robot::log_info("[%s:%d] RotateToGoal Initialized successfully", __FILE__, __LINE__);
   }
 }
 
@@ -59,8 +58,7 @@ void mkt_algorithm::diff::RotateToGoal::getParams()
   nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
   if (inflation_cost_scaling_factor_ <= 0.0)
   {
-    ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, "
+    robot::log_warning("[%s:%d] The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
-             "it should be >0. Disabling cost regulated linear velocity scaling.");
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
   double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
@@ -96,7 +94,7 @@ bool mkt_algorithm::diff::RotateToGoal::prepare(const nav_2d_msgs::Pose2DStamped
 
   if (!initialized_)
   {
-    ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
+    robot::log_error("This planner has not been initialized, please call initialize() before using this planner");
     return false;
   }
   this->getParams();
@@ -128,4 +126,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator(
   return result;
 }
 
-PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::RotateToGoal, score_algorithm::ScoreAlgorithm)
+score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::RotateToGoal::create()
+{
+  return std::make_shared<mkt_algorithm::diff::RotateToGoal>();
+}
+
+BOOST_DLL_ALIAS(mkt_algorithm::diff::RotateToGoal::create, MKTAlgorithmDiffRotateToGoal)

src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt

@@ -78,7 +78,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 option(BUILD_TESTS "Build test programs" OFF)
 
 # Flags biên dịch
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -58,23 +58,23 @@ namespace two_points_planner
     unsigned char result = 0;
 
     // Bug
-    // if (!costmap_robot_)
+    if (!costmap_robot_)
-    // {
+    {
-    //   std::cerr << "TwoPointsPlanner: Costmap is not initialized" << std::endl;
+      std::cerr << "TwoPointsPlanner: Costmap is not initialized" << std::endl;
-    //   return 0;
+      return 0;
-    // }
+    }
 
-    // // check if the costmap has an inflation layer
+    // check if the costmap has an inflation layer
-    // for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
+    for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
-    //   layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
+      layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
-    //   ++layer)
+      ++layer)
-    // {
+    {
-    //   boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
+      boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
-    //   if (!inflation_layer)
+      if (!inflation_layer)
-    //     continue;
+        continue;
 
-    //   result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution()));
+      result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution()));
-    // }
+    }
     return result;
   }
 
@@ -109,104 +109,104 @@ namespace two_points_planner
       return false;
     }
 
-    // bool do_init = false;
+    bool do_init = false;
-    // if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
+    if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
-    //   current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY())
+      current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY())
-    // {
+    {
-    //   printf("TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.\n",
+      printf("TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.\n",
-    //     current_env_width_, current_env_height_,
+        current_env_width_, current_env_height_,
-    //     costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
+        costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
-    //   do_init = true;
+      do_init = true;
-    // }
+    }
-    // else if (footprint_ != costmap_robot_->getRobotFootprint())
+    else if (footprint_ != costmap_robot_->getRobotFootprint())
-    // {
+    {
-    //   printf("TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.\n");
+      printf("TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.\n");
-    //   do_init = true;
+      do_init = true;
-    // }
+    }
 
-    // if (do_init)
+    if (do_init)
-    // {
+    {
-    //   initialized_ = false;
+      initialized_ = false;
-    //   initialize(name_, costmap_robot_);
+      initialize(name_, costmap_robot_);
-    // }
+    }
 
-    // plan.clear();
+    plan.clear();
-    // plan.push_back(start);
+    plan.push_back(start);
-    // printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n",
+    printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n",
-    //   start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
+      start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
 
-    // unsigned int mx_start, my_start;
+    unsigned int mx_start, my_start;
-    // unsigned int mx_end, my_end;
+    unsigned int mx_end, my_end;
-    // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
+    if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
-    //   start.pose.position.y,
+      start.pose.position.y,
-    //   mx_start, my_start)
+      mx_start, my_start)
 
-    //   || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
+      || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
-    //     goal.pose.position.y,
+        goal.pose.position.y,
-    //     mx_end, my_end))
+        mx_end, my_end))
-    // {
+    {
-    //   std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl;
+      std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl;
-    //   return false;
+      return false;
-    // }
+    }
-    // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
+    unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
-    // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
+    unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
-    // if (start_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+    if (start_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
-    //   || end_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
+      || end_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
-    // {
+    {
-    //   std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
+      std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
-    //   return false;
+      return false;
-    // }
+    }
-    // robot::Time plan_time = robot::Time::now();
+    robot::Time plan_time = robot::Time::now();
-    // // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
+    // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
-    // const double dx = goal.pose.position.x - start.pose.position.x;
+    const double dx = goal.pose.position.x - start.pose.position.x;
-    // const double dy = goal.pose.position.y - start.pose.position.y;
+    const double dy = goal.pose.position.y - start.pose.position.y;
-    // double distance = std::sqrt(dx * dx + dy * dy);
+    double distance = std::sqrt(dx * dx + dy * dy);
-    // double theta;
+    double theta;
-    // if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
+    if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
-    // {
+    {
-    //   theta = atan2(dy, dx);
+      theta = atan2(dy, dx);
-    //   tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y, 
+      tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y, 
-    //                             goal.pose.orientation.z, goal.pose.orientation.w);
+                                goal.pose.orientation.z, goal.pose.orientation.w);
-    //   double goal_theta = tf3::getYaw(goal_quat);
+      double goal_theta = tf3::getYaw(goal_quat);
-    //   if(cos(goal_theta - theta) < 0) theta += M_PI;
+      if(cos(goal_theta - theta) < 0) theta += M_PI;
-    // }
+    }
-    // else 
+    else 
-    // {
+    {
-    //   std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl;
+      std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl;
-    //   return false;
+      return false;
-    // }
+    }
 
-    // // Lấy độ phân giải của costmap
+    // Lấy độ phân giải của costmap
-    // double resolution = costmap_robot_->getCostmap()->getResolution();
+    double resolution = costmap_robot_->getCostmap()->getResolution();
 
-    // // Tính số điểm cần chia
+    // Tính số điểm cần chia
-    // int num_points = std::ceil(distance / resolution);
+    int num_points = std::ceil(distance / resolution);
 
-    // // Chia nhỏ tuyến đường
+    // Chia nhỏ tuyến đường
-    // for (int i = 0; i <= num_points; i++)
+    for (int i = 0; i <= num_points; i++)
-    // {
+    {
-    //   double fraction = static_cast<double>(i) / num_points;
+      double fraction = static_cast<double>(i) / num_points;
 
-    //   geometry_msgs::PoseStamped pose;
+      geometry_msgs::PoseStamped pose;
-    //   pose.header.stamp = plan_time;
+      pose.header.stamp = plan_time;
-    //   pose.header.frame_id = costmap_robot_->getGlobalFrameID();
+      pose.header.frame_id = costmap_robot_->getGlobalFrameID();
-    //   pose.pose.position.x = start.pose.position.x + fraction * dx;
+      pose.pose.position.x = start.pose.position.x + fraction * dx;
-    //   pose.pose.position.y = start.pose.position.y + fraction * dy;
+      pose.pose.position.y = start.pose.position.y + fraction * dy;
-    //   pose.pose.position.z = start.pose.position.z;
+      pose.pose.position.z = start.pose.position.z;
 
-    //   // Nội suy hướng
+      // Nội suy hướng
-    //   tf3::Quaternion interpolated_quat;
+      tf3::Quaternion interpolated_quat;
-    //   interpolated_quat.setRPY(0, 0, theta);
+      interpolated_quat.setRPY(0, 0, theta);
 
-    //   // Chuyển đổi trực tiếp từ tf3::Quaternion sang geometry_msgs::Quaternion
+      // Chuyển đổi trực tiếp từ tf3::Quaternion sang geometry_msgs::Quaternion
-    //   pose.pose.orientation.x = interpolated_quat.x();
+      pose.pose.orientation.x = interpolated_quat.x();
-    //   pose.pose.orientation.y = interpolated_quat.y();
+      pose.pose.orientation.y = interpolated_quat.y();
-    //   pose.pose.orientation.z = interpolated_quat.z();
+      pose.pose.orientation.z = interpolated_quat.z();
-    //   pose.pose.orientation.w = interpolated_quat.w();
+      pose.pose.orientation.w = interpolated_quat.w();
 
-    //   plan.push_back(pose);
+      plan.push_back(pose);
-    // }
+    }
-    // plan.push_back(goal);
+    plan.push_back(goal);
     return true;
   }
 

src/Libraries/common_msgs

@@ -1 +1 @@
-Subproject commit bf1fc3df34fa79be25cb0e99b1bdb441ec3c7a9d
+Subproject commit 6bac68429824cab4bbb1df3e259a24a8bf741b59

src/Libraries/costmap_2d

@@ -1 +1 @@
-Subproject commit ddff75465ca49b04bf22d2fbe26f61aa4bff7acb
+Subproject commit fdfba18bde0662ab5f042b938ecd011c5382ca7a

src/Libraries/nav_2d_utils/CMakeLists.txt

@@ -32,6 +32,8 @@ target_link_libraries(conversions
         nav_msgs
         nav_grid
         nav_core2
+        robot::node_handle
+        robot::console
     PRIVATE
         console_bridge::console_bridge
         Boost::system
@@ -48,6 +50,8 @@ target_link_libraries(path_ops
     PUBLIC
         nav_2d_msgs
         geometry_msgs
+        robot::node_handle
+        robot::console
 )
 
 add_library(polygons src/polygons.cpp src/footprint.cpp)
@@ -60,6 +64,8 @@ target_link_libraries(polygons
     PUBLIC
         nav_2d_msgs
         geometry_msgs
+        robot::node_handle
+        robot::console
     PRIVATE
         ${xmlrpcpp_LIBRARIES}
 )
@@ -81,6 +87,8 @@ target_link_libraries(bounds
     PUBLIC
         nav_grid
         nav_core2
+        robot::node_handle
+        robot::console
 )
 
 add_library(tf_help src/tf_help.cpp)
@@ -95,6 +103,8 @@ target_link_libraries(tf_help
         geometry_msgs
         nav_core2
         tf3
+        robot::node_handle
+        robot::console
 )
 
 # Create an INTERFACE library that represents all nav_2d_utils libraries
@@ -111,6 +121,8 @@ target_link_libraries(nav_2d_utils
         polygons
         bounds
         tf_help
+        robot::node_handle
+        robot::console
 )
 
 # Install header files

src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h

@@ -35,7 +35,7 @@
 #ifndef NAV_2D_UTILS_FOOTPRINT_H
 #define NAV_2D_UTILS_FOOTPRINT_H
 
-#include <yaml-cpp/yaml.h>
+#include <robot/node_handle.h>
 #include <nav_2d_msgs/Polygon2D.h>
 
 namespace nav_2d_utils
@@ -47,7 +47,7 @@ namespace nav_2d_utils
  * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
  * is present.
  */
-nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true);
+nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle& nh, bool write = true);
 
 }  // namespace nav_2d_utils
 

src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h

@@ -41,7 +41,7 @@
 #include <boost/thread/mutex.hpp>
 #include <string>
 #include <iostream>
-#include <yaml-cpp/yaml.h>
+#include <robot/node_handle.h>
 
 namespace nav_2d_utils
 {
@@ -59,7 +59,7 @@ public:
    * @param nh NodeHandle for creating subscriber
    * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
    */
-  explicit OdomSubscriber(YAML::Node& nh, std::string default_topic = "odom")
+  explicit OdomSubscriber(robot::NodeHandle& nh, std::string default_topic = "odom")
   {
     std::string odom_topic;
     // nh.param("odom_topic", odom_topic, default_topic);

src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h

@@ -52,7 +52,7 @@ namespace nav_2d_utils
  * @return Value of parameter if found, otherwise the default_value
  */
 template<class param_t>
-param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, const param_t& default_value)
+param_t searchAndGetParam(const robot::NodeHandle& nh, const std::string& param_name, const param_t& default_value)
 {
   std::string resolved_name;
   // if (nh.searchParam(param_name, resolved_name))
@@ -73,19 +73,19 @@ param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, c
  * @return The value of the parameter or the default value
  */
 template<class param_t>
-param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string current_name,
+param_t loadParameterWithDeprecation(const robot::NodeHandle& nh, const std::string current_name,
                                      const std::string old_name, const param_t& default_value)
 {
   param_t value;
-  if (nh[current_name] && nh[current_name].IsDefined())
+  if (nh.hasParam(current_name))
   {
-    value = nh[current_name].as<param_t>();
+    nh.getParam(current_name, value, default_value);
     return value;
   }
-  if (nh[old_name] && nh[old_name].IsDefined())
+  if (nh.hasParam(old_name))
   {
     std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
-    value = nh[old_name].as<param_t>();
+    nh.getParam(old_name, value, default_value);
     return value;
   }
   return default_value;
@@ -98,14 +98,14 @@ param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string cur
  * @param old_name Deprecated parameter name
  */
 template<class param_t>
-void moveDeprecatedParameter(const YAML::Node& nh, const std::string current_name, const std::string old_name)
+void moveDeprecatedParameter(const robot::NodeHandle& nh, const std::string current_name, const std::string old_name)
 {
-  if (!nh[old_name] || !nh[old_name].IsDefined()) return;
+  if (!nh.hasParam(old_name)) return;
 
   param_t value;
   std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
-  value = nh[old_name].as<param_t>();
+  value = nh.param<param_t>(old_name);
-  nh[current_name] = value;
+  nh.setParam(current_name, value);
 }
 
 /**
@@ -122,7 +122,7 @@ void moveDeprecatedParameter(const YAML::Node& nh, const std::string current_nam
  * @param should_delete If true, whether to delete the parameter from the old name
  */
 template<class param_t>
-void moveParameter(const YAML::Node& nh, std::string old_name,
+void moveParameter(const robot::NodeHandle& nh, std::string old_name,
                    std::string current_name, param_t default_value, bool should_delete = true)
 {
   // if (nh.hasParam(current_name))

src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h

@@ -207,7 +207,7 @@ protected:
   // ROS Interface
   ros::ServiceServer switch_plugin_srv_;
   ros::Publisher current_plugin_pub_;
-  YAML::Node private_nh_;
+  robot::NodeHandle private_nh_;
   std::string ros_name_;
 
   // Switch Callback

src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h

@@ -89,7 +89,7 @@ nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
 //  * @param search Whether to search up the namespace for the parameter name
 //  * @return Loaded polygon
 //  */
-// nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name,
+// nav_2d_msgs::Polygon2D polygonFromParams(const robot::NodeHandle& nh, const std::string parameter_name,
 //                                          bool search = true);
 
 /**
@@ -116,7 +116,7 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool
 //  * @param parameter_name Name of the parameter
 //  * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays
 //  */
-// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name,
+// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const robot::NodeHandle& nh, const std::string parameter_name,
 //                      bool array_of_arrays = true);
 
 /**

src/Libraries/robot_cpp

@@ -1 +1 @@
-Subproject commit e3df6935432bcfdb14367bc99c2d114c385992a0
+Subproject commit 1974d0ddeea08a05b83363614d1a946db5dd07ee

src/Navigations/Cores/nav_core2/CMakeLists.txt

@@ -28,6 +28,8 @@ target_link_libraries(nav_core2 INTERFACE
         tf3 
         nav_grid 
         nav_2d_msgs
+        robot::node_handle
+        robot::console
 )
 
 # Set include directories

src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h

@@ -35,13 +35,13 @@
 #ifndef NAV_CORE2_COSTMAP_H
 #define NAV_CORE2_COSTMAP_H
 
+#include <robot/node_handle.h>
 #include <nav_grid/nav_grid.h>
 #include <nav_core2/common.h>
 #include <nav_core2/bounds.h>
 #include <boost/thread.hpp>
 #include <memory>
 #include <string>
-#include <yaml-cpp/yaml.h>
 
 namespace nav_core2
 {
@@ -71,7 +71,7 @@ public:
    * @param  name The namespace for the costmap
    * @param  tf A pointer to a transform listener
    */
-  virtual void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) {}
+  virtual void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) {}
 
   inline unsigned char getCost(const unsigned int x, const unsigned int y)
   {

src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h

@@ -68,7 +68,7 @@ public:
    * @param  tf A pointer to a transform listener
    * @param  costmap A pointer to the costmap
    */
-  virtual void initialize(const YAML::Node& parent, const std::string& name,
+  virtual void initialize(const robot::NodeHandle& parent, const std::string& name,
                           TFListenerPtr tf, Costmap::Ptr costmap) = 0;
 
   /**

src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h

@@ -76,7 +76,7 @@ namespace nav_core2
     // virtual void initialize(YAML::Node &parent, const std::string &name,
     //                         TFListenerPtr tf, Costmap::Ptr costmap) = 0;
 
-    virtual void initialize(YAML::Node &parent, const std::string &name,
+    virtual void initialize(robot::NodeHandle &parent, const std::string &name,
                             TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap) = 0;
 
     /**

src/Navigations/Cores/nav_core_adapter/CMakeLists.txt

@@ -75,7 +75,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 option(BUILD_TESTS "Build test programs" OFF)
 
 # Flags biên dịch
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 

src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h

@@ -60,7 +60,7 @@ public:
   void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
 
   // Standard Costmap Interface
-  void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) override;
+  void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override;
   nav_core2::Costmap::mutex_t* getMutex() override;
 
   // NavGrid Interface

src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h

@@ -55,7 +55,7 @@ public:
   GlobalPlannerAdapter();
 
   // Nav Core 2 Global Planner Interface
-  void initialize(const YAML::Node& parent, const std::string& name,
+  void initialize(const robot::NodeHandle& parent, const std::string& name,
                   TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
   nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
                                const nav_2d_msgs::Pose2DStamped& goal) override;

src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h

@@ -35,6 +35,7 @@
 #ifndef NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
 #define NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
 
+#include <robot/node_handle.h>
 #include <nav_core/base_local_planner.h>
 #include <nav_core2/local_planner.h>
 #include <nav_core_adapter/costmap_adapter.h>
@@ -197,8 +198,8 @@ namespace nav_core_adapter
     boost::recursive_mutex configuration_mutex_;
     // std::shared_ptr<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>> dsrv_;
 
-    YAML::Node private_nh_;
+    robot::NodeHandle private_nh_;
-    YAML::Node adapter_nh_;
+    robot::NodeHandle adapter_nh_;
     // void reconfigureCB(nav_core_adapter::NavCoreAdapterConfig &config, uint32_t level);
     nav_core_adapter::NavCoreAdapterConfig last_config_;
     nav_core_adapter::NavCoreAdapterConfig default_config_;

src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp

@@ -72,10 +72,10 @@ void CostmapAdapter::initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool
   costmap_ = costmap_robot_->getCostmap();
 }
 
-void CostmapAdapter::initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf)
+void CostmapAdapter::initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf)
 {
   // TODO: Implement this if needed
-  throw nav_core2::CostmapException("initialize with YAML::Node not implemented");
+  throw nav_core2::CostmapException("initialize with robot::NodeHandle not implemented");
 }
 
 nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex()

src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp

@@ -53,7 +53,7 @@ namespace nav_core_adapter
   /**
   * @brief Load the nav_core global planner and initialize it
   */
-  void GlobalPlannerAdapter::initialize(const YAML::Node& parent, const std::string& name,
+  void GlobalPlannerAdapter::initialize(const robot::NodeHandle& parent, const std::string& name,
                                         TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
   {
     costmap_ = costmap;

src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp

@@ -32,6 +32,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <robot/console.h>
 #include <nav_core_adapter/local_planner_adapter.h>
 #include <nav_core_adapter/costmap_adapter.h>
 #include <nav_core_adapter/shared_pointers.h>
@@ -79,23 +80,22 @@ namespace nav_core_adapter
     costmap_adapter_ = std::make_shared<CostmapAdapter>();
     costmap_adapter_->initialize(costmap_robot);
 
-    YAML::Node nh;
+    robot::NodeHandle nh;
-    private_nh_ = YAML::Node("~");
+    private_nh_ = robot::NodeHandle("~");
-    adapter_nh_ = YAML::Node("~/" + name);
+    adapter_nh_ = robot::NodeHandle(private_nh_, name);
-    std::cout << "Adapter namespace: " << adapter_nh_["~"].as<std::string>() << std::endl;
+    robot::log_info("Adapter namespace: %s", private_nh_.getNamespace().c_str());
-
     std::string planner_name;
-    if (adapter_nh_["planner_name"] && adapter_nh_["planner_name"].IsDefined())
+    if (adapter_nh_.hasParam("planner_name"))
     {
-      planner_name = adapter_nh_["planner_name"].as<std::string>();
+      adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
     }
     else
     {
       planner_name = nav_2d_utils::loadParameterWithDeprecation(
         adapter_nh_, "planner_name", "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
-      adapter_nh_["planner_name"] = planner_name;
+      adapter_nh_.setParam("planner_name", planner_name);
     }
-    std::cout << "Loading plugin " << planner_name << std::endl;
+    robot::log_info("Loading plugin %s", planner_name.c_str());
 
     // planner_ = planner_loader_.createInstance(planner_name);
     std::string path_file_so;
@@ -104,10 +104,10 @@ namespace nav_core_adapter
     planner_ = planner_loader_();
     if (!planner_)
     {
-      std::cerr << "Failed to load planner " << planner_name << std::endl;
+      robot::log_error("Failed to load planner %s", planner_name.c_str());
       exit(EXIT_FAILURE);
     }
-    planner_->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(), tf_, costmap_robot_);
+    planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
     
     // odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh);
     // dsrv_ = std::make_shared<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>>(configuration_mutex_, adapter_nh_);
@@ -140,26 +140,17 @@ namespace nav_core_adapter
           std::cerr << "Failed to load planner " << planner_name << std::endl;
           exit(EXIT_FAILURE);
         }
-        new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(),
+        new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
-                                tf_, costmap_robot_);
-        if (!new_planner)
-        {
-          std::cerr << "Failed to load planner " << planner_name << std::endl;
-          exit(EXIT_FAILURE);
-        }
-        new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(),
-                                tf_, costmap_robot_);
-
         if (planner_)
           planner_.reset();
         planner_ = new_planner;
 
         last_config_.planner_name = planner_name;
-        std::cout << "Loaded new planner: " << planner_name << std::endl;
+        robot::log_info("Loaded new planner: %s", planner_name.c_str());
       }
       catch (const std::exception &ex)
       {
-        std::cerr << "Failed to load planner " << planner_name << ": " << ex.what() << std::endl;
+        robot::log_error("Failed to load planner %s: %s", planner_name.c_str(), ex.what());
         return false;
       }
     }

src/Navigations/Packages/move_base/CMakeLists.txt

@@ -237,7 +237,9 @@ option(BUILD_TESTS "Build test programs" OFF)
 # ========================================================
 # Compiler Flags
 # ========================================================
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 

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

@@ -73,201 +73,196 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
     // NodeHandle("~") will automatically load YAML files from config directory
     robot::NodeHandle nh("~");
     private_nh_ = nh;
+    private_nh_.printAllParams();
     recovery_trigger_ = PLANNING_R;
-
-    robot::NodeHandle nh1 = robot::NodeHandle(nh);
-    nh1.setParam("local_costmap/obstacles/enabled", false);
-    // nh.printAllParams();
-    // nh1.printAllParams();
-    // nh.printAllParams();
   
-    // // get some parameters that will be global to the move base node
+    // get some parameters that will be global to the move base node
-    // std::string global_planner, local_planner;
+    std::string global_planner, local_planner;
-    // private_nh_.getParam("base_global_planner", global_planner, std::string(""));
+    private_nh_.getParam("base_global_planner", global_planner, std::string(""));
-    // printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str());
+    printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str());
-    // private_nh_.getParam("base_local_planner", local_planner, std::string(""));
+    private_nh_.getParam("base_local_planner", local_planner, std::string(""));
-    // printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str());
+    printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str());
 
-    // // Handle nested YAML nodes for costmap config
+    // Handle nested YAML nodes for costmap config
-    // std::string robot_base_frame;
+    std::string robot_base_frame;
-    // private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint"));
+    private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint"));
-    // std::string global_frame;
+    std::string global_frame;
-    // private_nh_.getParam("global_frame", global_frame, std::string("map"));
+    private_nh_.getParam("global_frame", global_frame, std::string("map"));
-    // robot_base_frame_ = robot_base_frame;
+    robot_base_frame_ = robot_base_frame;
-    // global_frame_ = global_frame;
+    global_frame_ = global_frame;
-    // double planner_frequency;
+    double planner_frequency;
-    // private_nh_.getParam("planner_frequency", planner_frequency, 0.0);
+    private_nh_.getParam("planner_frequency", planner_frequency, 0.0);
-    // planner_frequency_ = planner_frequency;
+    planner_frequency_ = planner_frequency;
-    // double controller_frequency;
+    double controller_frequency;
-    // private_nh_.getParam("controller_frequency", controller_frequency, 20.0);
+    private_nh_.getParam("controller_frequency", controller_frequency, 20.0);
-    // controller_frequency_ = controller_frequency;
+    controller_frequency_ = controller_frequency;
-    // double planner_patience;
+    double planner_patience;
-    // private_nh_.getParam("planner_patience", planner_patience, 5.0);
+    private_nh_.getParam("planner_patience", planner_patience, 5.0);
-    // planner_patience_ = planner_patience;
+    planner_patience_ = planner_patience;
-    // private_nh_.getParam("controller_patience", controller_patience_, 15.0);
+    private_nh_.getParam("controller_patience", controller_patience_, 15.0);
-    // double max_planning_retries;
+    double max_planning_retries;
-    // private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0);
+    private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0);
-    // max_planning_retries_ = max_planning_retries;
+    max_planning_retries_ = max_planning_retries;
-    // double oscillation_timeout;
+    double oscillation_timeout;
-    // private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0);
+    private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0);
-    // oscillation_timeout_ = oscillation_timeout;
+    oscillation_timeout_ = oscillation_timeout;
-    // double oscillation_distance;
+    double oscillation_distance;
-    // private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5);
+    private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5);
-    // oscillation_distance_ = oscillation_distance;
+    oscillation_distance_ = oscillation_distance;
 
-    // double original_xy_goal_tolerance;
+    double original_xy_goal_tolerance;
-    // private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2);
+    private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2);
-    // original_xy_goal_tolerance_ = original_xy_goal_tolerance;
+    original_xy_goal_tolerance_ = original_xy_goal_tolerance;
-    // double original_yaw_goal_tolerance;
+    double original_yaw_goal_tolerance;
-    // private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2);
+    private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2);
-    // original_yaw_goal_tolerance_ = original_yaw_goal_tolerance;
+    original_yaw_goal_tolerance_ = original_yaw_goal_tolerance;
 
-    // // defined local planner name
+    // defined local planner name
-    // private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner"));
+    private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner"));
-    // private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner"));
+    private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner"));
-    // private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner"));
+    private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner"));
-    // private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner"));
+    private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner"));
 
-    // // parameters of make_plan service
+    // parameters of make_plan service
-    // private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false);
+    private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false);
-    // private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false);
+    private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false);
-    // private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
+    private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
-    // min_approach_linear_velocity_ *= 1.2;
+    min_approach_linear_velocity_ *= 1.2;
-    // // set up plan triple buffer
+    // set up plan triple buffer
-    // planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
+    planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
-    // latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
+    latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
-    // controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
+    controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
 
-    // // set up the planner's thread
+    // set up the planner's thread
-    // planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
+    planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
 
-    // // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
+    // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
-    // //  From config param
+    //  From config param
-    // double inscribed_radius;
+    double inscribed_radius;
-    // private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
+    private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
-    // double circumscribed_radius; 
+    double circumscribed_radius; 
-    // private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
+    private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
-    // inscribed_radius_ = inscribed_radius;
+    inscribed_radius_ = inscribed_radius;
-    // circumscribed_radius_ = circumscribed_radius;
+    circumscribed_radius_ = circumscribed_radius;
-    // private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
+    private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
-    // double conservative_reset_dist;
+    double conservative_reset_dist;
-    // private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
+    private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
-    // conservative_reset_dist_ = conservative_reset_dist;
+    conservative_reset_dist_ = conservative_reset_dist;
-    // private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
+    private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
-    // private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
+    private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
-    // private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
+    private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
 
-    // // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
+    // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
-    // planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
+    planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
-    // planner_costmap_robot_->pause();
+    planner_costmap_robot_->pause();
-    // // initialize the global planner
+    // initialize the global planner
-    // try
+    try
-    // {
+    {
-    //   planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
+      planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
-    //       "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", 
+          "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", 
-    //       global_planner, 
+          global_planner, 
-    //       boost::dll::load_mode::append_decorations);
+          boost::dll::load_mode::append_decorations);
 
-    //   planner_ = planner_loader_();
+      planner_ = planner_loader_();
-    //   if (!planner_)
+      if (!planner_)
-    //   {
+      {
-    //     robot::printf_red("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
+        robot::log_error("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
-    //     throw std::runtime_error("Failed to load global planner " + global_planner);
+        throw std::runtime_error("Failed to load global planner " + global_planner);
-    //   }
+      }
-    //   if(planner_->initialize(global_planner, planner_costmap_robot_))
+      if(planner_->initialize(global_planner, planner_costmap_robot_))
-    //     printf("[%s:%d] Global planner initialized successfully\n", __FILE__, __LINE__);
+        robot::log_info("[%s:%d] Global planner initialized successfully", __FILE__, __LINE__);
-    //   else 
+      else 
-    //     robot::printf_red("[%s:%d] Global planner initialized failed\n", __FILE__, __LINE__);
+        robot::log_error("[%s:%d] Global planner initialized failed", __FILE__, __LINE__);
-    // }
+    }
-    // catch (const std::exception &ex)
+    catch (const std::exception &ex)
-    // {
+    {
-    //   printf("[%s:%d] EXCEPTION in global planner: %s\n", __FILE__, __LINE__, ex.what());
+      robot::log_error("[%s:%d] EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what());
-    //   throw std::runtime_error("Failed to create the " + global_planner + " planner");
+      throw std::runtime_error("Failed to create the " + global_planner + " planner");
-    // }
+    }
-    // // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
+    // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
-    // controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
+    controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
-    // controller_costmap_robot_->pause();
+    controller_costmap_robot_->pause();
-    // // create a local planner
+    // create a local planner
-    // try
+    try
-    // {
+    {
-    //   std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so";
-    //   controller_loader_ = 
+      controller_loader_ = 
-    //     boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
+        boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
-    //       path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations);
+          path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations);
-    //   tc_ = controller_loader_();
+      tc_ = controller_loader_();
-    //   if (!tc_)
+      if (!tc_)
-    //   {
+      {
-    //     robot::printf_red("[%s:%d] ERROR: controller_loader_() returned nullptr\n", __FILE__, __LINE__);
+        robot::log_error("[%s:%d] ERROR: controller_loader_() returned nullptr", __FILE__, __LINE__);
-    //     throw std::runtime_error("Failed to load local planner " + local_planner);
+        throw std::runtime_error("Failed to load local planner " + local_planner);
-    //   }
+      }
-    //   // tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
+      // tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
-    // }
+    }
-    // catch (const std::exception &ex)
+    catch (const std::exception &ex)
-    // {
+    {
-    //   printf("[%s:%d] EXCEPTION in local planner: %s\n", __FILE__, __LINE__, ex.what());
+      robot::log_error("[%s:%d] EXCEPTION in local planner: %s", __FILE__, __LINE__, ex.what());
-    //   throw std::runtime_error("Failed to create the " + local_planner + " planner");
+      throw std::runtime_error("Failed to create the " + local_planner + " planner");
-    // }
+    }
 
-    // // Start actively updating costmaps based on sensor data
+    // Start actively updating costmaps based on sensor data
-    // planner_costmap_robot_->start();
+    planner_costmap_robot_->start();
-    // controller_costmap_robot_->start();
+    controller_costmap_robot_->start();
 
 
-    // try
+    try
-    // {
+    {
-    //   old_linear_fwd_ = tc_->getTwistLinear(true);
+      old_linear_fwd_ = tc_->getTwistLinear(true);
-    //   old_linear_bwd_ = tc_->getTwistLinear(false);
+      old_linear_bwd_ = tc_->getTwistLinear(false);
 
-    //   old_angular_fwd_ = tc_->getTwistAngular(true);
+      old_angular_fwd_ = tc_->getTwistAngular(true);
-    //   old_angular_rev_ = tc_->getTwistAngular(false);
+      old_angular_rev_ = tc_->getTwistAngular(false);
-    // }
+    }
-    // catch (const std::exception &e)
+    catch (const std::exception &e)
-    // {
+    {
-    //   std::cerr << e.what() << '\n';
+      std::cerr << e.what() << '\n';
-    // }
+    }
 
-    // // // advertise a service for getting a plan
+    // // advertise a service for getting a plan
-    // // make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this);
+    // make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this);
 
-    // // // advertise a service for clearing the costmaps
+    // // advertise a service for clearing the costmaps
-    // // clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this);
+    // clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this);
 
-    // // if we shutdown our costmaps when we're deactivated... we'll do that now
+    // if we shutdown our costmaps when we're deactivated... we'll do that now
-    // if (shutdown_costmaps_)
+    if (shutdown_costmaps_)
-    // {
+    {
-    //   planner_costmap_robot_->stop();
+      planner_costmap_robot_->stop();
-    //   controller_costmap_robot_->stop();
+      controller_costmap_robot_->stop();
-    // }
+    }
 
-    // // load any user specified recovery behaviors, and if that fails load the defaults
+    // load any user specified recovery behaviors, and if that fails load the defaults
-    // if (!loadRecoveryBehaviors(private_nh_))
+    if (!loadRecoveryBehaviors(private_nh_))
-    // {
+    {
-    //   robot::printf_yellow("[%s:%d] Loading default recovery behaviors\n", __FILE__, __LINE__);
+      robot::log_warning("[%s:%d] Loading default recovery behaviors", __FILE__, __LINE__);
-    //   loadDefaultRecoveryBehaviors();
+      loadDefaultRecoveryBehaviors();
-    // }
+    }
 
-    // // initially, we'll need to make a plan
+    // initially, we'll need to make a plan
-    // state_ = move_base::PLANNING;
+    state_ = move_base::PLANNING;
 
-    // // we'll start executing recovery behaviors at the beginning of our list
+    // we'll start executing recovery behaviors at the beginning of our list
-    // recovery_index_ = 0;
+    recovery_index_ = 0;
-    // nav_feedback_ = std::make_shared<move_base_core::NavFeedback>();
+    nav_feedback_ = std::make_shared<move_base_core::NavFeedback>();
-    // if (nav_feedback_)
+    if (nav_feedback_)
-    // {
+    {
-    //   nav_feedback_->navigation_state = move_base_core::State::PLANNING;
+      nav_feedback_->navigation_state = move_base_core::State::PLANNING;
-    //   nav_feedback_->is_ready = true;
+      nav_feedback_->is_ready = true;
-    // }
+    }
-    // else
+    else
-    // {
+    {
-    //   robot::printf_red("[%s:%d] ERROR: nav_feedback_ is nullptr\n", __FILE__, __LINE__);
+      robot::log_error("[%s:%d] ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
-    // }
+    }
     
-    // initialized_ = true;
+    initialized_ = true;
-    // printf("[%s:%d] ========== End: initialize() - SUCCESS ==========\n", __FILE__, __LINE__);
+    robot::log_info("[%s:%d] ========== End: initialize() - SUCCESS ==========", __FILE__, __LINE__);
   }
   else
   {
-    robot::printf_yellow("[%s:%d] Already initialized, skipping\n", __FILE__, __LINE__);
+    robot::log_warning("[%s:%d] Already initialized, skipping", __FILE__, __LINE__);
   }
 }