fix bug

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

@@ -789,16 +789,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   //   (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
 
   bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
-#ifdef BUILD_WITH_ROS
+// #ifdef BUILD_WITH_ROS
-  if (result)
+  // if (result)
-    ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+  //   ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
 
   // else if(fabs(velocity.x) < min_speed_xy_)
   // {
   //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
   //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
   // }
-#endif
+// #endif
   return result;
 }
 

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

@@ -36,8 +36,6 @@ namespace two_points_planner
     if (!initialized_)
     {
       robot::NodeHandle nh_priv_("~/" + name);
-      robot::log_info("TwoPointsPlanner: Name is %s", name.c_str());
-
       int lethal_obstacle;
       nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20);
       lethal_obstacle_ = (unsigned char)lethal_obstacle;
@@ -121,6 +119,7 @@ namespace two_points_planner
       robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
       return false;
     }
+    
     robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start);
     robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);
     robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", 
@@ -148,21 +147,21 @@ namespace two_points_planner
     }
 
     plan.clear();
-    plan.push_back(start);
+    // plan.push_back(start);
     
-    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))
-    {
+    // {
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
+    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
-      return false;
+    //   return false;
-    }
+    // }
     // 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));
     // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)

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

@@ -250,11 +250,16 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
   if(rotate_algorithm_) rotate_algorithm_->reset();
   ret_nav_ = ret_angle_ = false;
 
+  robot::log_info_at(__FILE__, __LINE__, "Debug");
+  parent_.printParams();
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+  // parent_.setParam(algorithm_nav_name, original_papams_);
   robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
-  parent_.setParam(algorithm_nav_name, original_papams_);
   nh_algorithm.setParam("allow_rotate", false);
+
+  robot::log_info_at(__FILE__, __LINE__, "Debug ở đây");
+  parent_.printParams();
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -567,18 +572,51 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name)
-    : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false),
+    : initialized_(false),
-      is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("")
+      is_detected_(false),
+      is_goal_reached_(false),
+      following_(false),
+      allow_rotate_(false),
+      xy_goal_tolerance_(0.05),
+      yaw_goal_tolerance_(0.05),
+      min_lookahead_dist_(0.4),
+      max_lookahead_dist_(1.0),
+      lookahead_time_(1.5),
+      angle_threshold_(0.4),
+      name_(name),
+      docking_planner_name_(),
+      docking_nav_name_(),
+      docking_planner_(nullptr),
+      docking_nav_(nullptr),
+      linear_(),
+      angular_(),
+      nh_(),
+      nh_priv_(),
+      tf_(nullptr),
+      costmap_robot_(nullptr),
+      traj_generator_(),
+      docking_planner_loader_(),
+      docking_nav_loader_(),
+      detected_timeout_wt_(),
+      delayed_wt_(),
+      delayed_(false),
+      detected_timeout_(false),
+      robot_base_frame_(),
+      maker_goal_frame_()
 {
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
 {
-  if (docking_planner_)
+  detected_timeout_wt_.stop();
+  delayed_wt_.stop();
+  if (docking_planner_ != nullptr) {
     docking_planner_.reset();
-  if (docking_nav_)
+  }
+  if (docking_nav_ != nullptr) {
     docking_nav_.reset();
   }
+}
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                                                                              score_algorithm::TrajectoryGenerator::Ptr traj_generator)

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

@@ -107,6 +107,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str());
       nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
       nav_algorithm_ = nav_algorithm_loader_();

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -651,7 +651,8 @@ namespace robot
           std::string key = it->first.as<std::string>();
           const YAML::Node &value = it->second;
           std::string full_key = prefix.empty() ? key : prefix + "/" + key;
-
+          if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos)
+            continue;
           if (value.IsMap())
           {
             std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl;

src/Libraries/robot_cpp/src/plugin_loader_helper.cpp

@@ -66,6 +66,7 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
   }
   // Try to read from NodeHandle
   std::string library_path;
+  robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str());
   if (nh_.hasParam(param_path)) {
     nh_.getParam(param_path, library_path, std::string(""));
     if (!library_path.empty()) {

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h

@@ -41,6 +41,7 @@
 #include <robot_geometry_msgs/Twist.h>
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <tf3/buffer_core.h>
+#include <robot_nav_msgs/Odometry.h>
 #include <memory>
 
 namespace robot_nav_core
@@ -148,6 +149,12 @@ namespace robot_nav_core
      */
     virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
 
+    /**
+     * @brief Set the odometry of the robot
+     * @param odom The odometry of the robot
+     */
+    virtual void setOdom(robot_nav_msgs::Odometry *odom) { odom_ = odom; }
+
     /**
      * @brief  Virtual destructor for the interface
      */
@@ -155,6 +162,11 @@ namespace robot_nav_core
 
   protected:
     BaseLocalPlanner() {}
+
+    /**
+     * @brief The odometry of the robot
+     */
+     robot_nav_msgs::Odometry *odom_;
   };
 } // namespace robot_nav_core
 

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

@@ -189,11 +189,6 @@ namespace robot_nav_core_adapter
     robot_nav_2d_msgs::Pose2DStamped last_goal_;
     bool has_active_goal_;
 
-    /**
-     * @brief The odometry of the robot
-     */
-    robot_nav_msgs::Odometry odom_;
-
     // Plugin handling
     boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
     robot_nav_core2::LocalPlanner::Ptr planner_;

src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp

@@ -291,7 +291,7 @@ namespace robot_nav_core_adapter
 
   robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
   {
-    return odom_.twist.twist;
+    return odom_->twist.twist;
   }
 
   bool LocalPlannerAdapter::islock()
@@ -356,7 +356,7 @@ namespace robot_nav_core_adapter
     if (!getRobotPose(pose2d))
       return false;
 
-    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_.twist.twist);
+    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_->twist.twist);
     bool ret = planner_->isGoalReached(pose2d, velocity);
     if (ret)
     {

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

@@ -324,6 +324,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
         throw std::runtime_error("Failed to load local planner " + local_planner);
       }
       tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
+      tc_->setOdom(&odometry_);
     }
     catch (const std::exception &ex)
     {
@@ -1781,32 +1782,6 @@ void move_base::MoveBase::cancel()
   boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   cancel_ctr_ = true;
   robot::log_info("[MoveBase::cancel] cancel_ctr_ set to true");
-  
-  if (as_)
-  {
-    // Get current goal ID from action server to cancel specific goal
-    // If we want to cancel all goals, use empty string ""
-    robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
-    
-    // Use empty string to cancel current goal (processCancel accepts empty string to cancel all)
-    msg->id = "";  // Empty string cancels current goal
-    msg->stamp = robot::Time::now();
-    
-    robot::log_info("[MoveBase::cancel] Sending cancel request to action server");
-    robot::log_info("[MoveBase::cancel] Cancel message: id='%s', stamp=%ld.%09ld", 
-                    msg->id.c_str(), msg->stamp.sec, msg->stamp.nsec);
-    
-    // Convert to ConstPtr for processCancel
-    robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
-    as_->processCancel(cancel_msg);
-    robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
-  }
-  else
-  {
-    robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
-  }
-  
-  robot::log_info("[MoveBase::cancel] ===== EXIT =====");
 }
 
 bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
@@ -2381,8 +2356,9 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
       robot::log_info("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
     }
   }
-
+  robot::log_warning("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
   robot_geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
+  robot::log_warning("Received goalToGlobalFrame: x=%.2f, y=%.2f", goal.pose.position.x, goal.pose.position.y);
   publishZeroVelocity();
   // we have a goal so start the planner
   boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2602,7 +2578,6 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
     robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
     return false;
   }
-
   tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);
 
   // next, we need to check if the length of the quaternion is close to zero
@@ -2764,6 +2739,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       recovery_index_ = 0;
   }
 
+
   // Cancle executeCycle
   if (cancel_ctr_ && tc_)
   {
@@ -2772,13 +2748,25 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     tc_->setTwistLinear(linear);
     try
     {
-      if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
+      double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
+      if (actual_linear_velocity <= min_approach_linear_velocity_)
       {
         robot::log_info("MoveTo is canceled.");
         resetState();
+        cancel_ctr_ = false;
         // if(default_config_.base_global_planner != last_config_.base_global_planner)
+        robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+        if(nh_planner.hasParam("base_global_planner"))
+        {
+          std::string base_global_planner;
+          nh_planner.getParam("base_global_planner", base_global_planner);
+          swapPlanner(base_global_planner);
+        }
+        else
+        {
           swapPlanner(default_config_.base_global_planner);
-
+        }
+        cancel_ctr_ = false;
         // disable the planner thread
         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
         runPlanner_ = false;
@@ -2789,7 +2777,21 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
           nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
         }
         // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
-        cancel_ctr_ = false;
+
+        if (as_)
+        {
+          robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
+          msg->id = "";  // Empty string cancels current goal
+          msg->stamp = robot::Time::now();
+          // Convert to ConstPtr for processCancel
+          robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
+          as_->processCancel(cancel_msg);
+          robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
+        }
+        else
+        {
+          robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
+        }
         return true;
       }
     }
@@ -2798,7 +2800,17 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       robot::log_info("MoveTo is canceled.");
       resetState();
       // if(default_config_.base_global_planner != last_config_.base_global_planner)
+      robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+      if(nh_planner.hasParam("base_global_planner"))
+      {
+        std::string base_global_planner;
+        nh_planner.getParam("base_global_planner", base_global_planner);
+        swapPlanner(base_global_planner);
+      }
+      else
+      {
         swapPlanner(default_config_.base_global_planner);
+      }
 
       // disable the planner thread
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2810,6 +2822,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
         nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
       }
       // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
+      if (as_)
+      {
+        robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
+        msg->id = "";  // Empty string cancels current goal
+        msg->stamp = robot::Time::now();
+        // Convert to ConstPtr for processCancel
+        robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
+        as_->processCancel(cancel_msg);
+        robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
+      }
+      else
+      {
+        robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
+      }
       cancel_ctr_ = false;
       return true;
     }
@@ -2830,7 +2856,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
   // if we're controlling, we'll attempt to find valid velocity commands
   case move_base::CONTROLLING:
-    // robot::log_debug("In controlling state.");
 
     // check to see if we've reached our goal
     try
@@ -3004,7 +3029,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
         tc_->setTwistLinear(linear);
         try
         {
-          if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
+          double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
+          if (actual_linear_velocity <= min_approach_linear_velocity_)
           {
             paused_ = true;
           }
@@ -3108,16 +3134,49 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
 robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg)
 {
-  std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
-  robot_geometry_msgs::PoseStamped goal_pose, global_pose;
-  goal_pose = goal_pose_msg;
 
-  goal_pose.header.stamp = robot::Time(); // latest available
+  if (!tf_)
+  {
+    return goal_pose_msg;
+  }
+
+  std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
+  // robot_geometry_msgs::PoseStamped goal_pose, global_pose;
+  // goal_pose = goal_pose_msg;
+
+  // goal_pose.header.stamp = robot::Time(); // latest available
+  // try
+  // {
+  //   tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
+  //   tf3::doTransform(goal_pose, global_pose, transform);
+  // }
+  robot_geometry_msgs::PoseStamped global_pose;
+  tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
+  robot_geometry_msgs::PoseStamped goal_pose;
+  tf3::toMsg(tf3::Transform::getIdentity(), goal_pose.pose);
+  goal_pose = goal_pose_msg;
+  robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
+  tf3::Time tf3_current_time = data_convert::convertTime(current_time);
+  tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
+  
   try
   {
-    tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
+    // use current time if possible (makes sure it's not in the future)
+    std::string error_msg;
+    if (tf_->canTransform(global_frame, goal_pose.header.frame_id, tf3_current_time, &error_msg))
+    {
+      // Transform is available at current time
+      tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_current_time);
       tf3::doTransform(goal_pose, global_pose, transform);
     }
+    // use the latest otherwise (tf3::Time() means latest available)
+    else
+    {
+      // Try to get latest transform
+      tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_zero_time);
+      tf3::doTransform(goal_pose, global_pose, transform);
+    }
+  }
   catch (tf3::LookupException &ex)
   {
     robot::log_error("Move Base No Transform available Error looking up robot pose: %s", ex.what());