first commit

src/sbpl_lattice_planner.cpp

@@ -0,0 +1,648 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Mike Phillips
+*********************************************************************/
+
+#include <sbpl_lattice_planner/sbpl_lattice_planner.h>
+
+#include <cmath>
+
+#include <robot_nav_msgs/Path.h>
+
+#include <robot_costmap_2d/inflation_layer.h>
+#include <tf3/LinearMath/Quaternion.h>
+#include <boost/dll/alias.hpp>
+
+using namespace std;
+
+namespace robot_geometry_msgs {
+  bool operator== (const Point &p1, const Point &p2)
+  {
+    return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z;
+  }
+}
+
+namespace sbpl_lattice_planner{
+class LatticeSCQ : public StateChangeQuery{
+  public:
+    LatticeSCQ(EnvironmentNAVXYTHETALAT* env, std::vector<nav2dcell_t> const & changedcellsV)
+      : env_(env), changedcellsV_(changedcellsV) {
+    }
+
+    // lazy init, because we do not always end up calling this method
+    virtual std::vector<int> const * getPredecessors() const{
+      if(predsOfChangedCells_.empty() && !changedcellsV_.empty())
+        env_->GetPredsofChangedEdges(&changedcellsV_, &predsOfChangedCells_);
+      return &predsOfChangedCells_;
+    }
+
+    // lazy init, because we do not always end up calling this method
+    virtual std::vector<int> const * getSuccessors() const{
+      if(succsOfChangedCells_.empty() && !changedcellsV_.empty())
+        env_->GetSuccsofChangedEdges(&changedcellsV_, &succsOfChangedCells_);
+      return &succsOfChangedCells_;
+    }
+
+    EnvironmentNAVXYTHETALAT * env_;
+    std::vector<nav2dcell_t> const & changedcellsV_;
+    mutable std::vector<int> predsOfChangedCells_;
+    mutable std::vector<int> succsOfChangedCells_;
+};
+
+SBPLLatticePlanner::SBPLLatticePlanner()
+  : initialized_(false), costmap_robot_(NULL){
+}
+
+SBPLLatticePlanner::SBPLLatticePlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) 
+  : initialized_(false), costmap_robot_(NULL)
+  {
+    initialize(name, costmap_robot);
+  }
+    
+bool SBPLLatticePlanner::initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot){
+  if(!initialized_){
+    robot::NodeHandle nh;
+    robot::NodeHandle private_nh = robot::NodeHandle(nh, name);
+
+    robot::log_info("Name is %s", name.c_str());
+
+    private_nh.param("planner_type", planner_type_, string("ARAPlanner"));
+    private_nh.param("allocated_time", allocated_time_, 10.0);
+    private_nh.param("initial_epsilon",initial_epsilon_,3.0);
+    private_nh.param("environment_type", environment_type_, string("XYThetaLattice"));
+    private_nh.param("forward_search", forward_search_, bool(false));
+    private_nh.param("primitive_filename",primitive_filename_,string(""));
+    private_nh.param("force_scratch_limit",force_scratch_limit_,500);
+
+    double nominalvel_mpersecs, timetoturn45degsinplace_secs;
+    private_nh.param("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
+    private_nh.param("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
+    robot::log_error("nominalvel_mpersecs: %f, timetoturn45degsinplace_secs: %f, primitive_filename_: %s",nominalvel_mpersecs,timetoturn45degsinplace_secs,primitive_filename_.c_str());
+
+    int lethal_obstacle;
+    private_nh.param("lethal_obstacle",lethal_obstacle,20);
+    lethal_obstacle_ = (unsigned char) lethal_obstacle;
+    inscribed_inflated_obstacle_ = lethal_obstacle_-1;
+    sbpl_cost_multiplier_ = (unsigned char) (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE/inscribed_inflated_obstacle_ + 1);
+    robot::log_debug("SBPL: lethal: %uz, inscribed inflated: %uz, multiplier: %uz",lethal_obstacle,inscribed_inflated_obstacle_,sbpl_cost_multiplier_);
+
+    private_nh.param("publish_footprint_path", publish_footprint_path_, bool(true));
+    private_nh.param<int>("visualizer_skip_poses", visualizer_skip_poses_, 5);
+
+    private_nh.param("allow_unknown", allow_unknown_, bool(true));
+    // chỉ bật khi local planner có bước quay tại chỗ đầu path (turn_around_priority)
+    private_nh.param("free_start_heading", free_start_heading_, bool(false));
+
+    name_ = name;
+    costmap_robot_ = costmap_robot;
+
+    robot::Time start_time = robot::Time::now();
+    robot::Rate rate(1.0);
+    while(costmap_robot_->getCostmap()->getSizeInCellsX() == 0 || costmap_robot_->getCostmap()->getSizeInCellsY() == 0){
+      robot::log_warning("Waiting for costmap to be initialized...");
+      rate.sleep();
+      if((robot::Time::now() - start_time).toSec() > 2.0){
+        robot::log_error("Costmap not initialized after 10 seconds, exiting...");
+        exit(1);
+      }
+    }
+
+    footprint_ = costmap_robot_->getRobotFootprint();
+    footprint_prev_ = footprint_;
+
+    if ("XYThetaLattice" == environment_type_){
+      robot::log_debug("Using a 3D costmap for theta lattice\n");
+      env_ = new EnvironmentNAVXYTHETALAT();
+    }
+    else{
+      robot::log_error("XYThetaLattice is currently the only supported environment!\n");
+      exit(1);
+    }
+
+    circumscribed_cost_ = computeCircumscribedCost();
+
+    if (circumscribed_cost_ == 0) {
+      // Unfortunately, the inflation_radius is not taken into account by
+      // inflation_layer->computeCost(). If inflation_radius is smaller than
+      // the circumscribed radius, SBPL will ignore some obstacles, but we
+      // cannot detect this problem. If the cost_scaling_factor is too large,
+      // SBPL won't run into obstacles, but will always perform an expensive
+      // footprint check, no matter how far the nearest obstacle is.
+      robot::log_warning("The costmap value at the robot's circumscribed radius (%f m) is 0.", costmap_robot_->getLayeredCostmap()->getCircumscribedRadius());
+      robot::log_warning("SBPL performance will suffer.");
+      robot::log_warning("Please decrease the costmap's cost_scaling_factor.");
+    }
+    if(!env_->SetEnvParameter("cost_inscribed_thresh",costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))){
+      robot::log_error("Failed to set cost_inscribed_thresh parameter");
+      exit(1);
+    }
+    if(!env_->SetEnvParameter("cost_possibly_circumscribed_thresh", circumscribed_cost_)){
+      robot::log_error("Failed to set cost_possibly_circumscribed_thresh parameter");
+      exit(1);
+    }
+    int obst_cost_thresh = costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE);
+    vector<sbpl_2Dpt_t> perimeterptsV;
+    perimeterptsV.reserve(footprint_.size());
+    for (size_t ii(0); ii < footprint_.size(); ++ii) {
+      sbpl_2Dpt_t pt;
+      pt.x = footprint_[ii].x;
+      pt.y = footprint_[ii].y;
+      perimeterptsV.push_back(pt);
+    }
+
+    robot::log_info("footprint size = %zu", perimeterptsV.size());
+    robot::log_info("width=%d",
+        costmap_robot_->getCostmap()->getSizeInCellsX());
+
+    robot::log_info("height=%d",
+        costmap_robot_->getCostmap()->getSizeInCellsY());
+
+    robot::log_info("resolution=%f",
+        costmap_robot_->getCostmap()->getResolution());
+
+    bool ret;
+    try{
+      robot::log_warning("DEBUG 2000");
+      ret = env_->InitializeEnv(costmap_robot_->getCostmap()->getSizeInCellsX(), // width
+                                costmap_robot_->getCostmap()->getSizeInCellsY(), // height
+                                0, // mapdata
+                                0, 0, 0, // start (x, y, theta, t)
+                                0, 0, 0, // goal (x, y, theta)
+                                0, 0, 0, //goal tolerance
+                                perimeterptsV, costmap_robot_->getCostmap()->getResolution(), nominalvel_mpersecs,
+                                timetoturn45degsinplace_secs, obst_cost_thresh,
+                                primitive_filename_.c_str());
+      ret = true;
+      current_env_width_ = costmap_robot_->getCostmap()->getSizeInCellsX();
+      current_env_height_ = costmap_robot_->getCostmap()->getSizeInCellsY();
+      robot::log_warning("DEBUG 3000");
+    }
+    catch(SBPL_Exception *e){
+      robot::log_error("SBPL encountered a fatal exception: %s", e->what());
+      ret = false;
+    }
+    if(!ret){
+      robot::log_error("SBPL initialization failed!");
+      exit(1);
+    }
+    for (ssize_t ix(0); ix < costmap_robot_->getCostmap()->getSizeInCellsX(); ++ix)
+      for (ssize_t iy(0); iy < costmap_robot_->getCostmap()->getSizeInCellsY(); ++iy)
+        env_->UpdateCost(ix, iy, costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(ix,iy)));
+
+    if ("ARAPlanner" == planner_type_){
+      robot::log_info("Planning with ARA*");
+      planner_ = new ARAPlanner(env_, forward_search_);
+    }
+    else if ("ADPlanner" == planner_type_){
+      robot::log_info("Planning with AD*");
+      planner_ = new ADPlanner(env_, forward_search_);
+    }
+    else{
+      robot::log_error("ARAPlanner and ADPlanner are currently the only supported planners!\n");
+      exit(1);
+    }
+
+    robot::log_info("[sbpl_lattice_planner] Initialized successfully");
+//     plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
+//     stats_publisher_ = private_nh.advertise<sbpl_lattice_planner::SBPLLatticePlannerStats>("sbpl_lattice_planner_stats", 1);
+//     sbpl_plan_footprint_pub_ = private_nh.advertise<robot_visualization_msgs::Marker>("footprint_markers", 1);
+
+    initialized_ = true;
+  }
+  return initialized_;
+}
+  
+//Taken from Sachin's sbpl_cart_planner
+//This rescales the costmap according to a robotparam which sets the obstacle cost
+unsigned char SBPLLatticePlanner::costMapCostToSBPLCost(unsigned char newcost){
+  if(newcost == robot_costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == robot_costmap_2d::NO_INFORMATION))
+    return lethal_obstacle_;
+  else if(newcost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+    return inscribed_inflated_obstacle_;
+  else if(newcost == 0 || newcost == robot_costmap_2d::NO_INFORMATION)
+    return 0;
+  else {
+    unsigned char sbpl_cost = newcost / sbpl_cost_multiplier_;
+    if (sbpl_cost == 0)
+      sbpl_cost = 1;
+    return sbpl_cost;
+  }
+}
+
+unsigned char SBPLLatticePlanner::computeCircumscribedCost() {
+  unsigned char result = 0;
+
+  if (!costmap_robot_) {
+    robot::log_error("Costmap is not initialized");
+    return 0;
+  }
+
+  // // check if the costmap has an inflation layer
+  for (const auto &layer : *costmap_robot_->getLayeredCostmap()->getPlugins())
+  {
+    // Kiểm tra layer có đúng type
+    if (layer->getType() == robot_costmap_2d::LayerType::INFLATION_LAYER)
+    {
+      result = costMapCostToSBPLCost(layer->computeCost(
+                                    costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / 
+                                    costmap_robot_->getCostmap()->getResolution()));
+    }
+  }
+  return result;
+}
+
+bool SBPLLatticePlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
+                                 const robot_geometry_msgs::PoseStamped& goal,
+                                 std::vector<robot_geometry_msgs::PoseStamped>& plan){
+  if(!initialized_){
+    robot::log_error("Global planner is not initialized");
+    return false;
+  }
+
+  bool do_init = false;
+  if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
+      current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY()) {
+    robot::log_info("Costmap dimensions have changed from (%d x %d) to (%d x %d), reinitializing sbpl_lattice_planner.",
+             current_env_width_, current_env_height_,
+             costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
+    do_init = true;
+  }
+  else if (footprint_ != costmap_robot_->getRobotFootprint()) {
+    robot::log_info("Robot footprint has changed, reinitializing sbpl_lattice_planner.");
+    do_init = true;
+  }
+  else if (circumscribed_cost_ != computeCircumscribedCost()) {
+    robot::log_info("Cost at circumscribed radius has changed, reinitializing sbpl_lattice_planner.");
+    do_init = true;
+  }
+
+  if (do_init) {
+    initialized_ = false;
+    delete planner_;
+    planner_ = NULL;
+    delete env_;
+    env_ = NULL;
+    initialize(name_, costmap_robot_);
+  }
+
+  plan.clear();
+
+  robot::log_info("[sbpl_lattice_planner] getting start point (%g,%g) goal point (%g,%g)",
+           start.pose.position.x, start.pose.position.y,goal.pose.position.x, goal.pose.position.y);
+  double theta_start = 2 * atan2(start.pose.orientation.z, start.pose.orientation.w);
+  double theta_goal = 2 * atan2(goal.pose.orientation.z, goal.pose.orientation.w);
+
+  // Heading thật của robot — giữ lại để retry nếu heading giả định không ra nghiệm.
+  const double theta_start_actual = theta_start;
+  bool start_heading_overridden = false;
+  if (free_start_heading_) {
+    // Local planner tự quay tại chỗ về hướng path trước khi bám (turn_around_priority),
+    // nên heading xuất phát không phải ràng buộc thật. Giữ nguyên nó thì khi goal nằm
+    // phía sau, SBPL sẽ vẽ cung quay đầu (U-turn) ở đầu path — chuỗi prim quay tại chỗ
+    // đắt hơn cung tiến nhiều lần theo costmult nên không bao giờ được chọn. Gán heading
+    // giả định hướng thẳng tới goal để path bắt đầu thẳng, phần xoay đầu do local planner lo.
+    const double dx = goal.pose.position.x - start.pose.position.x;
+    const double dy = goal.pose.position.y - start.pose.position.y;
+    // Goal quá gần (< ~2 cell): hướng tới goal hết ý nghĩa, dùng luôn heading của goal.
+    theta_start = (std::hypot(dx, dy) > 2.0 * costmap_robot_->getCostmap()->getResolution())
+                      ? atan2(dy, dx)
+                      : theta_goal;
+    start_heading_overridden = true;
+  }
+
+  try{
+    int ret = env_->SetStart(start.pose.position.x - costmap_robot_->getCostmap()->getOriginX(), start.pose.position.y - costmap_robot_->getCostmap()->getOriginY(), theta_start);
+    if(ret < 0 || planner_->set_start(ret) == 0){
+      robot::log_error("ERROR: failed to set start state\n");
+      return false;
+    }
+  }
+  catch(SBPL_Exception *e){
+    robot::log_error("SBPL encountered a fatal exception while setting the start state");
+    return false;
+  }
+
+  try{
+    int ret = env_->SetGoal(goal.pose.position.x - costmap_robot_->getCostmap()->getOriginX(), goal.pose.position.y - costmap_robot_->getCostmap()->getOriginY(), theta_goal);
+    if(ret < 0 || planner_->set_goal(ret) == 0){
+      robot::log_error("ERROR: failed to set goal state\n");
+      return false;
+    }
+  }
+  catch(SBPL_Exception *e){
+    robot::log_error("SBPL encountered a fatal exception while setting the goal state");
+    return false;
+  }
+  
+  int offOnCount = 0;
+  int onOffCount = 0;
+  int allCount = 0;
+  vector<nav2dcell_t> changedcellsV;
+
+  for(unsigned int ix = 0; ix < costmap_robot_->getCostmap()->getSizeInCellsX(); ix++) {
+    for(unsigned int iy = 0; iy < costmap_robot_->getCostmap()->getSizeInCellsY(); iy++) {
+
+      unsigned char oldCost = env_->GetMapCost(ix,iy);
+      unsigned char newCost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(ix,iy));
+
+      if(oldCost == newCost) continue;
+
+      allCount++;
+
+      //first case - off cell goes on
+
+      if((oldCost != costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) && oldCost != costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
+          (newCost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || newCost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))) {
+        offOnCount++;
+      }
+
+      if((oldCost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || oldCost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
+          (newCost != costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) && newCost != costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))) {
+        onOffCount++;
+      }
+      env_->UpdateCost(ix, iy, costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(ix,iy)));
+
+      nav2dcell_t nav2dcell;
+      nav2dcell.x = ix;
+      nav2dcell.y = iy;
+      changedcellsV.push_back(nav2dcell);
+    }
+  }
+
+  try{
+    if(!changedcellsV.empty()){
+      StateChangeQuery* scq = new LatticeSCQ(env_, changedcellsV);
+      planner_->costs_changed(*scq);
+      delete scq;
+    }
+
+    if(allCount > force_scratch_limit_)
+      planner_->force_planning_from_scratch();
+  }
+  catch(SBPL_Exception *e){
+    robot::log_error("SBPL failed to update the costmap");
+    return false;
+  }
+
+  //setting planner parameters
+  robot::log_debug("allocated:%f, init eps:%f\n",allocated_time_,initial_epsilon_);
+  planner_->set_initialsolution_eps(initial_epsilon_);
+  planner_->set_search_mode(false);
+
+  robot::log_debug("[sbpl_lattice_planner] run planner");
+  vector<int> solution_stateIDs;
+  int solution_cost;
+  try{
+    int ret = planner_->replan(allocated_time_, &solution_stateIDs, &solution_cost);
+    if(!ret && start_heading_overridden){
+      // Heading giả định có thể không khả thi (vd hướng tới goal đâm thẳng vào tường
+      // ngay tại cell xuất phát) — thử lại MỘT lần với heading thật trước khi bỏ cuộc.
+      robot::log_warning("[sbpl_lattice_planner] no solution with free start heading, "
+                         "retrying with actual robot heading");
+      int sid = env_->SetStart(start.pose.position.x - costmap_robot_->getCostmap()->getOriginX(),
+                               start.pose.position.y - costmap_robot_->getCostmap()->getOriginY(),
+                               theta_start_actual);
+      if(sid >= 0 && planner_->set_start(sid) != 0){
+        ret = planner_->replan(allocated_time_, &solution_stateIDs, &solution_cost);
+      }
+    }
+    if(ret)
+      robot::log_debug("Solution is found\n");
+    else{
+      robot::log_info("Solution not found\n");
+      // publishStats(solution_cost, 0, start, goal);
+      return false;
+    }
+  }
+  catch(SBPL_Exception *e){
+    robot::log_error("SBPL encountered a fatal exception while planning");
+    return false;
+  }
+
+  robot::log_debug("size of solution=%d", (int)solution_stateIDs.size());
+
+  vector<EnvNAVXYTHETALAT3Dpt_t> sbpl_path;
+  try{
+    env_->ConvertStateIDPathintoXYThetaPath(&solution_stateIDs, &sbpl_path);
+  }
+  catch(SBPL_Exception *e){
+    robot::log_error("SBPL encountered a fatal exception while reconstructing the path");
+    return false;
+  }
+  if (start_heading_overridden && sbpl_path.size() > 1) {
+    // free_start_heading nhắm heading giả định thẳng tới goal; nếu hướng đó bị chặn,
+    // path vẫn mở đầu bằng cung rẽ từ heading giả định sang hướng đi thật. Yêu cầu vận
+    // hành là đoạn đầu path phải THẲNG (robot quay tại chỗ xong là bám thẳng), nên nếu
+    // phát hiện đoạn đầu cong thì căn lại heading xuất phát theo hướng đi thật của path
+    // rồi plan lại đúng MỘT lần (không lặp — lần hai đã xuất phát đúng hướng đi thật
+    // nên primitive thẳng là lựa chọn rẻ nhất).
+    constexpr double kStraightLeadCheckM = 0.5;    // [m] cửa sổ kiểm tra độ thẳng đoạn đầu
+    constexpr double kMaxLeadHeadingDevRad = 0.2;  // [rad] ~ nửa bước góc lưới 16 hướng
+    double lead_len = 0.0;
+    double max_dev = 0.0;
+    size_t iend = 1;
+    for (; iend < sbpl_path.size() && lead_len < kStraightLeadCheckM; ++iend) {
+      lead_len += std::hypot(sbpl_path[iend].x - sbpl_path[iend - 1].x,
+                             sbpl_path[iend].y - sbpl_path[iend - 1].y);
+      max_dev = std::max(max_dev, std::fabs(std::remainder(
+          sbpl_path[iend].theta - sbpl_path[0].theta, 2.0 * M_PI)));
+    }
+    if (max_dev > kMaxLeadHeadingDevRad) {
+      // Tìm HƯỚNG ĐI THẬT của route: đoạn thẳng đầu tiên trên path (heading giữ nguyên
+      // liên tục >= kStraightRunM). Với U-turn dài hơn cửa sổ 0.5m, mốc cố định sẽ rơi
+      // vào giữa cung — phải quét qua hết phần cong. Đoạn thẳng mở đầu đi theo hướng
+      // route tìm được, KHÔNG ép theo hướng tới goal (goal chỉ là seed cho lần plan đầu).
+      constexpr double kStraightRunM = 0.3;  // [m] độ dài tối thiểu để tính là đoạn thẳng
+      constexpr double kScanLimitM = 3.0;    // [m] giới hạn quét phần cong mở đầu
+      double theta_realign = sbpl_path[iend - 1].theta;  // fallback: heading sau cửa sổ đầu
+      double scanned = 0.0;
+      for (size_t i = 0; i + 1 < sbpl_path.size() && scanned < kScanLimitM; ++i) {
+        double run = 0.0;
+        bool straight = true;
+        for (size_t j = i + 1; j < sbpl_path.size() && run < kStraightRunM; ++j) {
+          run += std::hypot(sbpl_path[j].x - sbpl_path[j - 1].x,
+                            sbpl_path[j].y - sbpl_path[j - 1].y);
+          if (std::fabs(std::remainder(sbpl_path[j].theta - sbpl_path[i].theta,
+                                       2.0 * M_PI)) > 1e-3) {
+            straight = false;
+            break;
+          }
+        }
+        if (straight && run >= kStraightRunM) {
+          theta_realign = sbpl_path[i].theta;
+          break;
+        }
+        scanned += std::hypot(sbpl_path[i + 1].x - sbpl_path[i].x,
+                              sbpl_path[i + 1].y - sbpl_path[i].y);
+      }
+      robot::log_debug("[sbpl_lattice_planner] initial segment curved (dev %.2f rad), "
+                       "replanning with start heading realigned to %.2f", max_dev, theta_realign);
+      try{
+        int sid = env_->SetStart(start.pose.position.x - costmap_robot_->getCostmap()->getOriginX(),
+                                 start.pose.position.y - costmap_robot_->getCostmap()->getOriginY(),
+                                 theta_realign);
+        vector<int> realigned_ids;
+        int realigned_cost;
+        if(sid >= 0 && planner_->set_start(sid) != 0 &&
+           planner_->replan(allocated_time_, &realigned_ids, &realigned_cost)){
+          vector<EnvNAVXYTHETALAT3Dpt_t> realigned_path;
+          env_->ConvertStateIDPathintoXYThetaPath(&realigned_ids, &realigned_path);
+          if(!realigned_path.empty()){
+            sbpl_path = realigned_path;
+          }
+        }
+      }
+      catch(SBPL_Exception *e){
+        // giữ path đầu tiên — cong nhưng vẫn hợp lệ, không được trả thất bại ở đây
+        robot::log_warning("[sbpl_lattice_planner] realigned replan failed, keeping first path");
+      }
+    }
+  }
+
+  // if the plan has zero points, add a single point to make move_base happy
+  if( sbpl_path.size() == 0 ) {
+    EnvNAVXYTHETALAT3Dpt_t s(
+        start.pose.position.x - costmap_robot_->getCostmap()->getOriginX(),
+        start.pose.position.y - costmap_robot_->getCostmap()->getOriginY(),
+        theta_start);
+    // sbpl_path.push_back(s);
+  }
+
+  robot::log_debug("Plan has %d points.\n", (int)sbpl_path.size());
+  robot::Time plan_time = robot::Time::now();
+
+  // if (publish_footprint_path_)
+  // {
+  //   robot_visualization_msgs::Marker sbpl_plan_footprint;
+  //   getFootprintList(sbpl_path, costmap_robot_->getGlobalFrameID(), sbpl_plan_footprint);
+  //   sbpl_plan_footprint_pub_.publish(sbpl_plan_footprint);
+  // }
+
+  //create a message for the plan 
+  // nav_msgs::Path gui_path;
+  // gui_path.poses.resize(sbpl_path.size());
+  // gui_path.header.frame_id = costmap_robot_->getGlobalFrameID();
+  // gui_path.header.stamp = plan_time;
+  for(unsigned int i=0; i<sbpl_path.size(); i++){
+    robot_geometry_msgs::PoseStamped pose;
+    pose.header.stamp = plan_time;
+    pose.header.frame_id = costmap_robot_->getGlobalFrameID();
+
+    pose.pose.position.x = sbpl_path[i].x + costmap_robot_->getCostmap()->getOriginX();
+    pose.pose.position.y = sbpl_path[i].y + costmap_robot_->getCostmap()->getOriginY();
+    pose.pose.position.z = start.pose.position.z;
+
+    // tf2::Quaternion temp;
+    // temp.setRPY(0,0,sbpl_path[i].theta);
+    // pose.pose.orientation.x = temp.getX();
+    // pose.pose.orientation.y = temp.getY();
+    // pose.pose.orientation.z = temp.getZ();
+    // pose.pose.orientation.w = temp.getW();
+    pose.pose.orientation = data_convert::getQuaternion(sbpl_path[i].theta);
+
+    plan.push_back(pose);
+
+    // gui_path.poses[i] = plan[i];
+  }
+  // plan_pub_.publish(gui_path);
+  // publishStats(solution_cost, sbpl_path.size(), start, goal);
+
+  return true;
+}
+
+void SBPLLatticePlanner::getFootprintList(const std::vector<EnvNAVXYTHETALAT3Dpt_t>& sbpl_path,
+                                          const std::string& path_frame_id, robot_visualization_msgs::Marker& ma)
+{
+  ma.header.frame_id = path_frame_id;
+  ma.header.stamp = robot::Time();
+  ma.ns = "sbpl_robot_footprint";
+  ma.id = 0;
+  ma.type = robot_visualization_msgs::Marker::LINE_LIST;
+  ma.action = robot_visualization_msgs::Marker::ADD;
+  ma.scale.x = 0.05;
+  ma.color.a = 1.0;
+  ma.color.r = 0.0;
+  ma.color.g = 0.0;
+  ma.color.b = 1.0;
+  ma.pose.orientation.w = 1.0;
+
+  for (unsigned int i = 0; i < sbpl_path.size(); i = i + visualizer_skip_poses_)
+  {
+    std::vector<robot_geometry_msgs::Point> transformed_rfp;
+    robot_geometry_msgs::Pose robot_pose;
+    robot_pose.position.x = sbpl_path[i].x + costmap_robot_->getCostmap()->getOriginX();
+    robot_pose.position.y = sbpl_path[i].y + costmap_robot_->getCostmap()->getOriginY();
+    robot_pose.position.z = 0.0;
+
+    robot_pose.orientation = data_convert::getQuaternion(sbpl_path[i].theta);
+    transformFootprintToEdges(robot_pose, footprint_, transformed_rfp);
+
+    for (auto & point : transformed_rfp)
+      ma.points.push_back(point);
+  }
+}
+
+void SBPLLatticePlanner::transformFootprintToEdges(const robot_geometry_msgs::Pose& robot_pose,
+                                                   const std::vector<robot_geometry_msgs::Point>& footprint,
+                                                   std::vector<robot_geometry_msgs::Point>& out_footprint)
+{
+  out_footprint.resize(2 * footprint.size());
+  double yaw = data_convert::getYaw(robot_pose.orientation);
+  for (unsigned int i = 0; i < footprint.size(); i++)
+  {
+    out_footprint[2 * i].x = robot_pose.position.x + cos(yaw) * footprint[i].x - sin(yaw) * footprint[i].y;
+    out_footprint[2 * i].y = robot_pose.position.y + sin(yaw) * footprint[i].x + cos(yaw) * footprint[i].y;
+    if (i == 0)
+    {
+      out_footprint.back().x = out_footprint[i].x;
+      out_footprint.back().y = out_footprint[i].y;
+    }
+    else
+    {
+      out_footprint[2 * i - 1].x = out_footprint[2 * i].x;
+      out_footprint[2 * i - 1].y = out_footprint[2 * i].y;
+    }
+  }
+}
+// Export factory function
+robot_nav_core::BaseGlobalPlanner::Ptr SBPLLatticePlanner::create() {
+    return std::make_shared<sbpl_lattice_planner::SBPLLatticePlanner>();
+}
+
+
+};
+
+// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
+BOOST_DLL_ALIAS(sbpl_lattice_planner::SBPLLatticePlanner::create, SBPLLatticePlanner)