first commit

CMakeLists.txt

@@ -0,0 +1,177 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(sbpl_lattice_planner VERSION 1.0.0 LANGUAGES CXX)
+
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building sbpl_lattice_planner with Catkin")
+
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building sbpl_lattice_planner with Standalone CMake")
+endif()
+
+# C++ Standard - must be set before find_package
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# Find dependencies
+find_package(SBPL REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system thread filesystem)
+
+if (NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # Cấu hình RPATH để tránh cycle trong runtime search path
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+  set(PACKAGES_DIR
+    robot_visualization_msgs
+    robot_nav_msgs
+    robot_std_msgs
+    robot_geometry_msgs
+    tf3
+    robot_tf3_geometry_msgs
+    robot_time
+    data_convert
+    robot_costmap_2d
+    robot_nav_core
+    robot_protocol_msgs
+    robot_cpp
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS
+    robot_visualization_msgs
+    robot_nav_msgs
+    robot_std_msgs
+    robot_geometry_msgs
+    robot_tf3_geometry_msgs
+    robot_time
+    data_convert
+    robot_costmap_2d
+    robot_nav_core
+    robot_protocol_msgs
+    robot_cpp
+  )
+
+  find_library(TF3_LIBRARY NAMES tf3)
+  
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS robot_visualization_msgs robot_nav_msgs robot_std_msgs robot_geometry_msgs robot_tf3_geometry_msgs robot_time data_convert robot_costmap_2d robot_nav_core robot_protocol_msgs robot_cpp
+    DEPENDS Eigen3 Boost SBPL
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+  )
+endif()
+
+# ========================================================
+# Libraries
+# ========================================================
+add_library(${PROJECT_NAME} SHARED
+  src/sbpl_lattice_planner.cpp
+)
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${catkin_LIBRARIES} ${SBPL_LIBRARIES}
+    PRIVATE Eigen3::Eigen Boost::system Boost::thread Boost::filesystem 
+  )
+
+else()
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC
+      ${PACKAGES_DIR}
+    PRIVATE
+      Eigen3::Eigen Boost::system Boost::thread Boost::filesystem
+  )
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+endif()
+
+# ========================================================
+# Install
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # Print configuration info
+  message(STATUS "=================================")
+  message(STATUS "Project: ${PROJECT_NAME}")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Dependencies: robot_visualization_msgs, robot_nav_msgs, robot_std_msgs, robot_geometry_msgs, tf3, robot_tf3_geometry_msgs, robot_time, data_convert, robot_costmap_2d, robot_nav_core, robot_protocol_msgs, robot_cpp, Eigen3, Boost")
+  message(STATUS "=================================")
+endif()

README.md

@@ -0,0 +1,184 @@
+sbpl_lattice_planner
+====================
+
+The `sbpl_lattice_planner` is a global planner plugin for
+[move_base](https://wiki.ros.org/move_base) and wraps the
+[SBPL search-based planning library](https://wiki.ros.org/sbpl).
+
+Paths are generated by combining a series of "motion primitives" which are
+short, kinematically feasible motions. Planning is therefore done in x, y, and
+theta dimensions, resulting in smooth paths that take robot orientation into
+account, which is especially important if the robot is not assumed to be
+circular or has nonholonomic constraints (e.g., the robot cannot move
+sideways). Plans can be found using the `ARA*` planner or `AD*` planner from the
+SBPL library.
+
+## Video
+
+[![SBPL lattice planner on PR2 robot](https://img.youtube.com/vi/WeXdCmEpRW0/0.jpg)](https://www.youtube.com/watch?v=WeXdCmEpRW0)
+
+## How to use
+
+This global planner can be used with `move_base` simply by setting the
+`base_global_planner` parameter to `SBPLLatticePlanner`.  Additionally, at the
+very least the path to a motion primitive file must be specified shown below in
+the list of parameters.
+
+### Example in Stage
+
+The package contains a launch file for testing the `sbpl_lattice_planner` as
+the global planner for `move_base` using stage for 2D simulation:
+
+```bash
+roslaunch sbpl_lattice_planner move_base_sbpl_fake_localization_2.5cm.launch
+```
+
+## ROS API
+
+### Published Topics
+
+`~/SBPLLatticePlanner/plan` ([nav\_msgs/Path](http://docs.ros.org/api/nav_msgs/html/msg/Path.html))
+
+- The last plan computed by SBPL, published every time the planner computes a
+  new path, and used primarily for visualization purposes.
+
+`~/SBPLLatticePlanner/sbpl_lattice_planner_stats` ([sbpl\_lattice\_planner/SBPLLatticePlannerStats](http://docs.ros.org/api/sbpl_lattice_planner/html/msg/SBPLLatticePlannerStats.html))
+
+- Statistics from the last planning request. Stats include: time taken to get
+  to the first and final solutions, number of state expansions taken to get the
+  first and final solutions, the epsilon (bound on the sub-optimality of the
+  solution) of the first and final solutions, and the size of the final
+  solution.
+
+`~/SBPLLatticePlanner/footprint_markers` ([visualization\_msgs/Marker](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html))
+
+- The footprint markers along the planned path (for visualization in RViz).
+
+### Subscribed Topics
+
+None
+
+### Services
+
+None
+
+### Parameters
+
+`~/SBPLLatticePlanner/planner_type` (`string`, default: "ARAPlanner")
+
+- Specifies which planner to use. It can either be "ARAPlanner" for `ARA*` or
+  "ADPlanner" for `AD*`.
+
+`~/SBPLLatticePlanner/allocated_time` (`double`, default: 10.0)
+
+- The amount of time given to the planner to find a solution. If there is still
+  time remaining after the planner finds its sub-optimal initial solution
+  (specified by "initial_epsilon"), the planner will use up remaining time
+  improving the solution until it is optimal or until time runs out (whichever
+  comes first).
+
+`~/SBPLLatticePlanner/initial_epsilon` (`double`, default: 3.0)
+
+- The value the heuristic is scaled by for the first search. This value must
+  be greater or equal to 1. The larger this value is, the faster the search
+  tends to find a solution (likely sub-optimal if epsilon is larger than 1).
+  After the first search, the planner will continue to reduce the epsilon value
+  until it is 1 (optimal search).
+
+`~/SBPLLatticePlanner/environment_type` (`string`, default: "XYThetaLattice"))
+
+- The type of environment being searched. Currently, XYThetaLattice is the only
+  supported environment.
+
+`~/SBPLLatticePlanner/forward_search` (`bool`, default: false)
+
+- The direction the search is done in. If you are using `AD*`, you should use
+  backward search for fast replanning times.  For `ARA*` it doesn't matter too
+  much which direction you use.
+
+`~/SBPLLatticePlanner/primitive_filename` (`string`, default: "")
+
+- The path to a motion primitive file. This MUST be specified by the user for
+  the planner to work. There is an example motion primitive file that can be
+  used in `matlab/mprim/pr2.mprim` in the SBPL package. If you want to generate
+  your own motion primitive file to match the kinematics of your robot or your
+  map resolution, there is are several `genmprim*.m` scripts in `matlab/mprim/`
+  in the SBPL package to help you.
+
+`~/SBPLLatticePlanner/force_scratch_limit` (`int`, default: 500)
+
+- The parameter only matters if you are using `AD*`. If at least this many map
+  cells have changed since the last plan was generated, the planner will not
+  reuse previous search information and instead plan from scratch.
+
+`~/SBPLLatticePlanner/nominalvel_mpersecs` (`double`, default: 0.4)
+
+- The linear velocity of the robot in meters/sec.
+
+`~/SBPLLatticePlanner/timetoturn45degsinplace_secs` (`double`, 0.6)
+
+- The time it takes the robot to turn 45 degrees in place in seconds.
+
+`~/SBPLLatticePlanner/lethal_obstacle` (`unsigned char`, default: 20)
+
+- The cost of an obstacle in the planner's version of the costmap. All other
+  values in the costmap are scaled accordingly. The obstacle cost is the
+  largest in the costmap_2d so by setting this parameter to something below its
+  obstacle thresh, we can get obstacle padding that is less harsh and more
+  reasonable. This will make the planner more likely to choose to go through
+  more narrow areas such as doorways.
+
+`~/SBPLLatticePlanner/publish_footprint_path` (`bool`, default: true)
+
+- Whether or not to publish the `footprint_markers` topic.
+
+`~/SBPLLatticePlanner/visualizer_skip_poses` (`int`, default: 5)
+
+- Only publish every nth pose on the `footprint_markers` topic.
+
+`~/SBPLLatticePlanner/allow_unknown` (`bool`, default: true)
+
+- Whether or not to allow planning through unknown space.
+
+
+## Customizing your Motion Primitives
+
+Please refer to the [SBPL documentation](https://wiki.ros.org/sbpl) for
+pre-made motion primitives for the PR2 (and other robots) as well as
+instructions on how to generate your own custom motions.
+
+
+## Choosing good costmap_2d parameters
+
+If the costmap_2d parameters are set incorrectly, sbpl_lattice_planner will
+ignore the robot's footprint and incorrectly plan paths that lead into
+obstacles. For this reason, the following two parameters of the global
+costmap_2d have to be fine-tuned to your robot's footprint:
+
+* **inflation_radius:** Maximum distance from an obstacle at which costs are
+  incurred for planning paths. Must be greater or equal to the robot's
+  circumscribed radius.
+* **cost_scaling_factor:** Exponential rate at which the obstacle cost drops
+  off (default: 10). Must be chosen so that the cost value is greater than 0 at
+  the robot's circumscribed radius.
+
+The most important of these two parameters is inflation_radius. If this
+parameter is less than the robot's circumscribed radius, SBPL will skip the
+detailed footprint check and plan into obstacles. If cost_scaling_factor is too
+large for the robot footprint (i.e., the inflation around the obstacles is too
+small), SBPL will always perform a detailed footprint check, even for poses
+that are far away from obstacles. This will incur a performance penalty, but no
+infeasible paths.
+
+The best way to fine-tune these parameters is by using `rqt_reconfigure` and
+observing the resulting costmap with inflated obstacles in RViz. First you
+should set inflation_radius to something bigger than the circumscribed radius.
+This parameter is a cutoff, so if the costmap value has not yet reached 0 at
+this distance, it is clamped to 0. This parameter isn't that important as long
+as it is greater than the robot's circumscribed radius. The parameter
+cost_scaling_factor is an exponential dropoff, so lowering it increases the
+inflation radius. Lower it until the inflation radius is at least the robot's
+circumscribed radius. Once you have found good parameters, put them into your
+move_base launch file. Also see: https://wiki.ros.org/costmap_2d#Inflation .
+
+For more details, see [issue #33](https://github.com/ros-planning/navigation_experimental/issues/33).

cfg/sbpl_global_params.yaml

@@ -0,0 +1,15 @@
+SBPLLatticePlanner:
+  library_path: libsbpl_lattice_planner
+  environment_type: XYThetaLattice
+  planner_type: ARAPlanner
+  allocated_time: 10.0
+  initial_epsilon: 1.0
+  force_scratch_limit: 10000
+  forward_search: true
+  # Bỏ ràng buộc heading xuất phát: local planner đã có bước quay tại chỗ đầu path
+  # (turn_around_priority) nên không cần SBPL vẽ cung quay đầu khi goal ở phía sau.
+  # Nếu không ra nghiệm, planner tự retry một lần với heading thật của robot.
+  free_start_heading: true
+  nominalvel_mpersecs: 0.3
+  timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
+  primitive_filename: /home/duongtd/T800_ws/src/AMR_T800/Test/mprim_gen/diff_5cm.mprim

include/sbpl_lattice_planner/sbpl_lattice_planner.h

@@ -0,0 +1,123 @@
+#ifndef SBPL_LATTICE_PLANNER_H
+#define SBPL_LATTICE_PLANNER_H
+
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+// ROBOT
+#include <robot/robot.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_visualization_msgs/Marker.h>
+
+// Costmap used for the map representation
+#include <robot_costmap_2d/costmap_2d_robot.h>
+
+// sbpl headers
+#include <sbpl/headers.h>
+
+// global representation
+#include <robot_nav_core/base_global_planner.h>
+
+namespace sbpl_lattice_planner{
+
+class SBPLLatticePlanner : public robot_nav_core::BaseGlobalPlanner{
+public:
+  
+  /**
+   * @brief  Default constructor for the NavFnROBOT object
+   */
+  SBPLLatticePlanner();
+
+  
+  /**
+   * @brief  Constructor for the SBPLLatticePlanner object
+   * @param  name The name of this planner
+   * @param  costmap_robot A pointer to the ROBOT wrapper of the costmap to use
+   */
+  SBPLLatticePlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);
+
+
+  /**
+   * @brief  Initialization function for the SBPLLatticePlanner object
+   * @param  name The name of this planner
+   * @param  costmap_robot A pointer to the ROBOT wrapper of the costmap to use
+   */
+  virtual bool initialize(std::string name, 
+                          robot_costmap_2d::Costmap2DROBOT* costmap_robot);
+  
+  /**
+   * @brief Given a goal pose in the world, compute a plan
+   * @param start The start pose 
+   * @param goal The goal pose 
+   * @param plan The plan... filled by the planner
+   * @return True if a valid plan was found, false otherwise
+   */
+  virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start, 
+                        const robot_geometry_msgs::PoseStamped& goal, 
+                        std::vector<robot_geometry_msgs::PoseStamped>& plan);
+
+  virtual ~SBPLLatticePlanner(){};
+  
+  static robot_nav_core::BaseGlobalPlanner::Ptr create();
+
+private:
+  unsigned char costMapCostToSBPLCost(unsigned char newcost);
+
+  unsigned char computeCircumscribedCost();
+
+  static void transformFootprintToEdges(const robot_geometry_msgs::Pose& robot_pose,
+                                        const std::vector<robot_geometry_msgs::Point>& footprint,
+                                        std::vector<robot_geometry_msgs::Point>& out_footprint);
+
+  void getFootprintList(const std::vector<EnvNAVXYTHETALAT3Dpt_t>& sbpl_path, const std::string& path_frame_id,
+                        robot_visualization_msgs::Marker& ma);
+
+  bool initialized_;
+
+  SBPLPlanner* planner_;
+  EnvironmentNAVXYTHETALAT* env_;
+  
+  std::string planner_type_; /**< sbpl method to use for planning.  choices are ARAPlanner and ADPlanner */
+
+  double allocated_time_; /**< amount of time allowed for search */
+  double initial_epsilon_; /**< initial epsilon for beginning the anytime search */
+
+  std::string environment_type_; /** what type of environment in which to plan.  choices are 2D and XYThetaLattice. */ 
+  std::string cost_map_topic_; /** what topic is being used for the costmap topic */
+
+  bool forward_search_; /** whether to use forward or backward search */
+  std::string primitive_filename_; /** where to find the motion primitives for the current robot */
+  int force_scratch_limit_; /** the number of cells that have to be changed in the costmap to force the planner to plan from scratch even if its an incremental planner */
+
+  unsigned char lethal_obstacle_;
+  unsigned char inscribed_inflated_obstacle_;
+  unsigned char circumscribed_cost_;
+  unsigned char sbpl_cost_multiplier_;
+
+  bool publish_footprint_path_;
+  int visualizer_skip_poses_;
+
+  bool allow_unknown_;
+
+  /** Bỏ ràng buộc heading tại điểm xuất phát, đảm bảo path MỞ ĐẦU BẰNG ĐOẠN THẲNG
+   *  theo hướng đi thật của route (không nhất thiết hướng tới goal). Dùng khi local
+   *  planner tự quay tại chỗ về hướng path trước khi bám (turn_around_priority) — khi
+   *  đó heading xuất phát không phải ràng buộc thật, giữ nó chỉ ép SBPL vẽ cung quay
+   *  đầu lúc goal nằm phía sau robot. Cơ chế: plan lần 1 với heading seed hướng tới
+   *  goal; nếu đoạn đầu vẫn cong thì căn heading theo đoạn thẳng đầu tiên của path và
+   *  plan lại một lần. Không ra nghiệm thì retry với heading thật. */
+  bool free_start_heading_;
+
+  std::string name_;
+  robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
+  std::vector<robot_geometry_msgs::Point> footprint_;
+  std::vector<robot_geometry_msgs::Point> footprint_prev_;
+  unsigned int current_env_width_;
+  unsigned int current_env_height_;
+};
+};
+
+#endif
+

matlab/mprim/genmprim_unicycle_highcost_5cm.m

@@ -0,0 +1,280 @@
+% /*
+%  * Copyright (c) 2008, Maxim Likhachev
+%  * 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 Carnegie Mellon University 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.
+%  */
+
+function[] = genmprim_unicycle_highcost_5cm(outfilename)
+
+%
+%generates motion primitives and saves them into file
+%
+%written by Maxim Likhachev
+%---------------------------------------------------
+%
+
+%defines
+
+UNICYCLE_MPRIM_16DEGS = 1;
+
+
+if UNICYCLE_MPRIM_16DEGS == 1
+    resolution = 0.05;
+    numberofangles = 16; %preferably a power of 2, definitely multiple of 8
+    numberofprimsperangle = 7;
+
+    %multipliers (multiplier is used as costmult*cost)
+    forwardcostmult = 1;
+    backwardcostmult = 40;
+    forwardandturncostmult = 2;
+    sidestepcostmult = 10;
+    turninplacecostmult = 20;
+
+    %note, what is shown x,y,theta changes (not absolute numbers)
+
+    %0 degreees
+    basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
+    %x aligned with the heading of the robot, angles are positive
+    %counterclockwise
+    %0 theta change
+    basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
+    basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
+    basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
+    %1/16 theta change
+    basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
+    basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
+    %turn in place
+    basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
+    basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
+
+    %45 degrees
+    basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
+    %x aligned with the heading of the robot, angles are positive
+    %counterclockwise
+    %0 theta change
+    basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
+    basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
+    basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
+    %1/16 theta change
+    basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
+    basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
+    %turn in place
+    basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
+    basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
+
+    %22.5 degrees
+    basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
+    %x aligned with the heading of the robot, angles are positive
+    %counterclockwise
+    %0 theta change
+    basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
+    basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
+    basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
+    %1/16 theta change
+    basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
+    basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
+    %turn in place
+    basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
+    basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
+
+else
+    fprintf(1, 'ERROR: undefined mprims type\n');
+    return;
+end;
+
+
+fout = fopen(outfilename, 'w');
+
+
+%write the header
+fprintf(fout, 'resolution_m: %f\n', resolution);
+fprintf(fout, 'numberofangles: %d\n', numberofangles);
+fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
+
+%iterate over angles
+for angleind = 1:numberofangles
+
+    figure(1);
+    hold off;
+
+    text(0, 0, int2str(angleind));
+
+    %iterate over primitives
+    for primind = 1:numberofprimsperangle
+        fprintf(fout, 'primID: %d\n', primind-1);
+        fprintf(fout, 'startangle_c: %d\n', angleind-1);
+
+        %current angle
+        currentangle = (angleind-1)*2*pi/numberofangles;
+        currentangle_36000int = round((angleind-1)*36000/numberofangles);
+
+        %compute which template to use
+        if (rem(currentangle_36000int, 9000) == 0)
+            basemprimendpts_c = basemprimendpts0_c(primind,:);
+            angle = currentangle;
+        elseif (rem(currentangle_36000int, 4500) == 0)
+            basemprimendpts_c = basemprimendpts45_c(primind,:);
+            angle = currentangle - 45*pi/180;
+        elseif (rem(currentangle_36000int-7875, 9000) == 0)
+            basemprimendpts_c = basemprimendpts33p75_c(primind,:);
+            basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
+            basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
+            basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
+            angle = currentangle - 78.75*pi/180;
+            fprintf(1, '78p75\n');
+        elseif (rem(currentangle_36000int-6750, 9000) == 0)
+            basemprimendpts_c = basemprimendpts22p5_c(primind,:);
+            basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
+            basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
+            basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
+            %fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
+            %    basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
+            angle = currentangle - 67.5*pi/180;
+            fprintf(1, '67p5\n');
+        elseif (rem(currentangle_36000int-5625, 9000) == 0)
+            basemprimendpts_c = basemprimendpts11p25_c(primind,:);
+            basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
+            basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
+            basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
+            angle = currentangle - 56.25*pi/180;
+            fprintf(1, '56p25\n');
+        elseif (rem(currentangle_36000int-3375, 9000) == 0)
+            basemprimendpts_c = basemprimendpts33p75_c(primind,:);
+            angle = currentangle - 33.75*pi/180;
+            fprintf(1, '33p75\n');
+        elseif (rem(currentangle_36000int-2250, 9000) == 0)
+            basemprimendpts_c = basemprimendpts22p5_c(primind,:);
+            angle = currentangle - 22.5*pi/180;
+            fprintf(1, '22p5\n');
+        elseif (rem(currentangle_36000int-1125, 9000) == 0)
+            basemprimendpts_c = basemprimendpts11p25_c(primind,:);
+            angle = currentangle - 11.25*pi/180;
+            fprintf(1, '11p25\n');
+        else
+            fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
+            return;
+        end;
+
+        %now figure out what action will be
+        baseendpose_c = basemprimendpts_c(1:3);
+        additionalactioncostmult = basemprimendpts_c(4);
+        endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
+        endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
+        endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
+        endpose_c = [endx_c endy_c endtheta_c];
+
+        fprintf(1, 'rotation angle=%f\n', angle*180/pi);
+
+        if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
+            %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
+        end;
+
+        %generate intermediate poses (remember they are w.r.t 0,0 (and not
+        %centers of the cells)
+        numofsamples = 10;
+        intermcells_m = zeros(numofsamples,3);
+        if UNICYCLE_MPRIM_16DEGS == 1
+            startpt = [0 0 currentangle];
+            endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
+                rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
+            intermcells_m = zeros(numofsamples,3);
+            if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
+                for iind = 1:numofsamples
+                    intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
+                                            startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
+                                            0];
+                    rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
+                    intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
+
+                end;
+            else %unicycle-based move forward or backward
+                R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
+                    sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
+                S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
+                l = S(1);
+                tvoverrv = S(2);
+                rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
+                tv = tvoverrv*rv;
+
+                if l < 0
+                    fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
+                    l = 0;
+                end;
+                %compute rv
+                %rv = baseendpose_c(3)*2*pi/numberofangles;
+                %compute tv
+                %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
+                %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
+                %tv = (tvx + tvy)/2.0;
+                %generate samples
+                for iind = 1:numofsamples
+                    dt = (iind-1)/(numofsamples-1);
+
+                    %dtheta = rv*dt + startpt(3);
+                    %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
+                    %                        startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
+                    %                        dtheta];
+
+                    if(dt*tv < l)
+                        intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
+                                                 startpt(2) + dt*tv*sin(startpt(3)) ...
+                                                 startpt(3)];
+                    else
+                        dtheta = rv*(dt - l/tv) + startpt(3);
+                        intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
+                                                 startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
+                                                 dtheta];
+                    end;
+                end;
+                %correct
+                errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
+                           endpt(2) - intermcells_m(numofsamples,2)];
+                fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
+                interpfactor = [0:1/(numofsamples-1):1];
+                intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
+                intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
+            end;
+        end;
+
+        %write out
+        fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
+        fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
+        fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
+        for interind = 1:size(intermcells_m, 1)
+            fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
+        end;
+
+        plot(intermcells_m(:,1), intermcells_m(:,2));
+        axis([-0.3 0.3 -0.3 0.3]);
+        text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
+        hold on;
+
+    end;
+    grid;
+    pause;
+end;
+
+fclose('all');

matlab/mprim/genmprim_unicycle_highcost_5cm.py

@@ -0,0 +1,416 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2016, David Conner (Christopher Newport University)
+# Based on genmprim_unicycle.m
+# Copyright (c) 2008, Maxim Likhachev
+# All rights reserved.
+# converted by libermate utility (https://github.com/awesomebytes/libermate)
+#
+# 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 Carnegie Mellon University 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.
+
+import numpy as np
+import rospkg
+
+# if available import pylab (from matlibplot)
+matplotlib_found = False
+try:
+    import matplotlib.pylab as plt
+
+    matplotlib_found = True
+except ImportError:
+    pass
+
+
+def matrix_size(mat, elem=None):
+    if not elem:
+        return mat.shape
+    else:
+        return mat.shape[int(elem) - 1]
+
+
+def genmprim_unicycle(outfilename, visualize=False, separate_plots=False):
+    visualize = matplotlib_found and visualize  # Plot the primitives
+
+    # Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c,
+    #   baseendpose_c, additionalactioncostmult, fout, numofsamples,
+    #   basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename,
+    #   numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult,
+    #   rotation_angle, basemprimendpts_c, forwardandturncostmult,
+    #   forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult,
+    #   interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt,
+    #   currentangle, numberofprimsperangle, resolution, currentangle_36000int,
+    #   l, iind, errorxy, interind, endy_c, angleind, endpt
+    # Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text,
+    #   int2str, pause, axis, sin, pinv, fprintf, fclose, rem, zeros, fopen,
+    #   round, size
+    # %
+    # %generates motion primitives and saves them into file
+    # %
+    # %written by Maxim Likhachev
+    # %---------------------------------------------------
+    # %
+    # %defines
+    UNICYCLE_MPRIM_16DEGS = 1.0
+    if UNICYCLE_MPRIM_16DEGS == 1.0:
+        resolution = 0.05
+        numberofangles = 16
+        # %preferably a power of 2, definitely multiple of 8
+        numberofprimsperangle = 7
+        # %multipliers (multiplier is used as costmult*cost)
+        forwardcostmult = 1.0
+        backwardcostmult = 40.0
+        forwardandturncostmult = 2.0
+        # sidestepcostmult = 10.0
+        turninplacecostmult = 20.0
+        # %note, what is shown x,y,theta changes (not absolute numbers)
+        # %0 degreees
+        basemprimendpts0_c = np.zeros((numberofprimsperangle, 4))
+        # %x,y,theta,costmult
+        # %x aligned with the heading of the robot, angles are positive
+        # %counterclockwise
+        # %0 theta change
+        basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult)))
+        basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult)))
+        basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult)))
+        # %1/16 theta change
+        basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult)))
+        basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult)))
+        # %turn in place
+        basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
+        basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
+        # %45 degrees
+        basemprimendpts45_c = np.zeros((numberofprimsperangle, 4))
+        # %x,y,theta,costmult (multiplier is used as costmult*cost)
+        # %x aligned with the heading of the robot, angles are positive
+        # %counterclockwise
+        # %0 theta change
+        basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult)))
+        basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult)))
+        basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult)))
+        # %1/16 theta change
+        basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult)))
+        basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult)))
+        # %turn in place
+        basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
+        basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
+        # %22.5 degrees
+        basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4))
+        # %x,y,theta,costmult (multiplier is used as costmult*cost)
+        # %x aligned with the heading of the robot, angles are positive
+        # %counterclockwise
+        # %0 theta change
+        basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult)))
+        basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult)))
+        basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult)))
+        # %1/16 theta change
+        basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult)))
+        basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult)))
+        # %turn in place
+        basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
+        basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
+    else:
+        print('ERROR: undefined mprims type\n')
+        return []
+
+    fout = open(outfilename, 'w')
+    # %write the header
+    fout.write('resolution_m: %f\n' % (resolution))
+    fout.write('numberofangles: %d\n' % (numberofangles))
+    fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles))
+    # %iterate over angles
+    for angleind in np.arange(1.0, (numberofangles) + 1):
+        currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles
+        currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles)
+        if visualize:
+            if separate_plots:
+                fig = plt.figure(angleind)
+                plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0))
+            else:
+                fig = plt.figure(1)
+
+            plt.axis('equal')
+            plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution])
+            ax = fig.add_subplot(1, 1, 1)
+            major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution)
+            minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution)
+            ax.set_xticks(major_ticks)
+            ax.set_xticks(minor_ticks, minor=True)
+            ax.set_yticks(major_ticks)
+            ax.set_yticks(minor_ticks, minor=True)
+            ax.grid(which='minor', alpha=0.5)
+            ax.grid(which='major', alpha=0.9)
+
+        # %iterate over primitives
+        for primind in np.arange(1.0, (numberofprimsperangle) + 1):
+            fout.write('primID: %d\n' % (primind - 1))
+            fout.write('startangle_c: %d\n' % (angleind - 1))
+            # %current angle
+            # %compute which template to use
+            if (currentangle_36000int % 9000) == 0:
+                basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :]
+                angle = currentangle
+            elif (currentangle_36000int % 4500) == 0:
+                basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :]
+                angle = currentangle - 45.0 * np.pi / 180.0
+
+            # commented out because basemprimendpts33p75_c is undefined
+            # elif ((currentangle_36000int - 7875) % 9000) == 0:
+            #     basemprimendpts_c = (
+            #         1 * basemprimendpts33p75_c[primind, :]
+            #     )  # 1* to force deep copy to avoid reference update below
+            #     basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1]
+            #     # %reverse x and y
+            #     basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0]
+            #     basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2]
+            #     # %reverse the angle as well
+            #     angle = currentangle - (78.75 * np.pi) / 180.0
+            #     print('78p75\n')
+
+            elif ((currentangle_36000int - 6750) % 9000) == 0:
+                basemprimendpts_c = (
+                    1 * basemprimendpts22p5_c[int(primind) - 1, :]
+                )  # 1* to force deep copy to avoid reference update below
+                basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1]
+                # %reverse x and y
+                basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0]
+                basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2]
+                # %reverse the angle as well
+                # print(
+                #     '%d : %d %d %d onto %d %d %d\n'
+                #     % (
+                #         primind - 1,
+                #         basemprimendpts22p5_c[int(primind) - 1, 0],
+                #         basemprimendpts22p5_c[int(primind) - 1, 1],
+                #         basemprimendpts22p5_c[int(primind) - 1, 2],
+                #         basemprimendpts_c[0],
+                #         basemprimendpts_c[1],
+                #         basemprimendpts_c[2],
+                #     )
+                # )
+                angle = currentangle - (67.5 * np.pi) / 180.0
+                print('67p5\n')
+
+            # commented out because basemprimendpts11p25_c is undefined
+            # elif ((currentangle_36000int - 5625) % 9000) == 0:
+            #     basemprimendpts_c = (
+            #         1 * basemprimendpts11p25_c[primind, :]
+            #     )  # 1* to force deep copy to avoid reference update below
+            #     basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1]
+            #     # %reverse x and y
+            #     basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0]
+            #     basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2]
+            #     # %reverse the angle as well
+            #     angle = currentangle - (56.25 * np.pi) / 180.0
+            #     print('56p25\n')
+
+            # commented out because basemprimendpts33p75_c is undefined
+            # elif ((currentangle_36000int - 3375) % 9000) == 0:
+            #     basemprimendpts_c = basemprimendpts33p75_c[int(primind), :]
+            #     angle = currentangle - (33.75 * np.pi) / 180.0
+            #     print('33p75\n')
+
+            elif ((currentangle_36000int - 2250) % 9000) == 0:
+                basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :]
+                angle = currentangle - (22.5 * np.pi) / 180.0
+                print('22p5\n')
+
+            # commented out because basemprimendpts11p25_c is undefined
+            # elif ((currentangle_36000int - 1125) % 9000) == 0:
+            #     basemprimendpts_c = basemprimendpts11p25_c[int(primind), :]
+            #     angle = currentangle - (11.25 * np.pi) / 180.0
+            #     print('11p25\n')
+
+            else:
+                print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int)
+                return []
+
+            # %now figure out what action will be
+            baseendpose_c = basemprimendpts_c[0:3]
+            additionalactioncostmult = basemprimendpts_c[3]
+            endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle)))
+            endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle)))
+            endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)
+            endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c)))
+            print("endpose_c=", endpose_c)
+            print(('rotation angle=%f\n' % (angle * 180.0 / np.pi)))
+            # if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.):
+            # %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
+
+            # %generate intermediate poses (remember they are w.r.t 0,0 (and not
+            # %centers of the cells)
+            numofsamples = 10
+            intermcells_m = np.zeros((numofsamples, 3))
+            if UNICYCLE_MPRIM_16DEGS == 1.0:
+                startpt = np.array(np.hstack((0.0, 0.0, currentangle)))
+                endpt = np.array(
+                    np.hstack(
+                        (
+                            (endpose_c[0] * resolution),
+                            (endpose_c[1] * resolution),
+                            (
+                                ((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi)
+                                / numberofangles
+                            ),
+                        )
+                    )
+                )
+
+                print("startpt =", startpt)
+                print("endpt   =", endpt)
+                intermcells_m = np.zeros((numofsamples, 3))
+                if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0):
+                    # %turn in place or move forward
+                    for iind in np.arange(1.0, (numofsamples) + 1):
+                        fraction = float(iind - 1) / (numofsamples - 1)
+                        intermcells_m[int(iind) - 1, :] = np.array(
+                            (
+                                startpt[0] + (endpt[0] - startpt[0]) * fraction,
+                                startpt[1] + (endpt[1] - startpt[1]) * fraction,
+                                0,
+                            )
+                        )
+                        rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles)
+                        intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi))
+                        # print " ",iind,"  of ",numofsamples," fraction=",fraction," rotation=",rotation_angle
+
+                else:
+                    # %unicycle-based move forward or backward  (http://sbpl.net/node/53)
+                    R = np.array(
+                        np.vstack(
+                            (
+                                np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))),
+                                np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))),
+                            )
+                        )
+                    )
+
+                    S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1]))))
+                    l = S[0]
+                    tvoverrv = S[1]
+                    rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv
+                    tv = tvoverrv * rv
+
+                    # print "R=\n",R
+                    # print "Rpi=\n",np.linalg.pinv(R)
+                    # print "S=\n",S
+                    # print "l=",l
+                    # print "tvoverrv=",tvoverrv
+                    # print "rv=",rv
+                    # print "tv=",tv
+
+                    if l < 0.0:
+                        print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l)))
+                        l = 0.0
+
+                    # %compute rv
+                    # %rv = baseendpose_c(3)*2*pi/numberofangles;
+                    # %compute tv
+                    # %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
+                    # %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
+                    # %tv = (tvx + tvy)/2.0;
+                    # %generate samples
+                    for iind in np.arange(1, numofsamples + 1):
+                        dt = (iind - 1) / (numofsamples - 1)
+                        # %dtheta = rv*dt + startpt(3);
+                        # %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
+                        # %                        startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
+                        # %                        dtheta];
+                        if (dt * tv) < l:
+                            intermcells_m[int(iind) - 1, :] = np.array(
+                                np.hstack(
+                                    (
+                                        startpt[0] + dt * tv * np.cos(startpt[2]),
+                                        startpt[1] + dt * tv * np.sin(startpt[2]),
+                                        startpt[2],
+                                    )
+                                )
+                            )
+                        else:
+                            dtheta = rv * (dt - l / tv) + startpt[2]
+                            intermcells_m[int(iind) - 1, :] = np.array(
+                                np.hstack(
+                                    (
+                                        startpt[0]
+                                        + l * np.cos(startpt[2])
+                                        + tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])),
+                                        startpt[1]
+                                        + l * np.sin(startpt[2])
+                                        - tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])),
+                                        dtheta,
+                                    )
+                                )
+                            )
+
+                    # %correct
+                    errorxy = np.array(
+                        np.hstack(
+                            (
+                                endpt[0] - intermcells_m[int(numofsamples) - 1, 0],
+                                endpt[1] - intermcells_m[int(numofsamples) - 1, 1],
+                            )
+                        )
+                    )
+                    # print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1]))
+                    interpfactor = np.array(
+                        np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1))))
+                    )
+
+                    # print "intermcells_m=",intermcells_m
+                    # print "interp'=",interpfactor.conj().T
+
+                    intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T
+                    intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T
+
+            # %write out
+            fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2]))
+            fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult))
+            fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0)))
+            for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1):
+                fout.write(
+                    '%.4f %.4f %.4f\n'
+                    % (
+                        intermcells_m[int(interind) - 1, 0],
+                        intermcells_m[int(interind) - 1, 1],
+                        intermcells_m[int(interind) - 1, 2],
+                    )
+                )
+
+            if visualize:
+                plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o")
+                plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2]))
+        # if (visualize):
+        #    plt.waitforbuttonpress()  # uncomment to plot each primitive set one at a time
+
+    fout.close()
+    if visualize:
+        #    plt.waitforbuttonpress()  # hold until buttom pressed
+        plt.show()  # Keep windows open until the program is terminated
+    return []
+
+
+if __name__ == "__main__":
+    rospack = rospkg.RosPack()
+    outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim'
+    genmprim_unicycle(outfilename, visualize=True)

msg/SBPLLatticePlannerStats.msg

@@ -0,0 +1,15 @@
+#planner stats
+float64 initial_epsilon
+float64 final_epsilon
+bool plan_to_first_solution
+float64 allocated_time
+float64 actual_time
+float64 time_to_first_solution
+float64 solution_cost
+float64 path_size
+int64 final_number_of_expands
+int64 number_of_expands_initial_solution
+
+#problem stats
+geometry_msgs/PoseStamped start
+geometry_msgs/PoseStamped goal

package.xml

@@ -0,0 +1,93 @@
+<!-- <?xml version="1.0"?>
+<package format="2">
+  <name>sbpl_lattice_planner</name>
+  <version>0.4.1</version>
+  <description>
+    The sbpl_lattice_planner is a global planner plugin for move_base and wraps
+    the SBPL search-based planning library.
+  </description>
+  <maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
+  <author>Michael Phillips</author>
+  <license>BSD</license>
+  <url type="website">http://wiki.ros.org/sbpl_lattice_planner</url>
+  <url type="repository">https://github.com/ros-planning/navigation_experimental.git</url>
+  <url type="bugtracker">https://github.com/ros-planning/navigation_experimental/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  
+  <depend>costmap_2d</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_core</depend>
+  <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>sbpl</depend>
+  <depend>tf</depend>
+  <depend>tf2</depend>
+
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <export>
+    <nav_core plugin="${prefix}/bgp_plugin.xml" />
+  </export>
+</package> -->
+
+<package>
+  <name>sbpl_lattice_planner</name>
+  <version>0.7.10</version>
+  <description>
+    sbpl_lattice_planner is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. sbpl_lattice_planner
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/sbpl_lattice_planner</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>robot_costmap_2d</build_depend>
+  <run_depend>robot_costmap_2d</run_depend>
+
+  <build_depend>robot_nav_core</build_depend>
+  <run_depend>robot_nav_core</run_depend>
+
+  <build_depend>robot_geometry_msgs</build_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+
+  <build_depend>robot_nav_msgs</build_depend>
+  <run_depend>robot_nav_msgs</run_depend>
+
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+
+  <build_depend>robot_tf3_geometry_msgs</build_depend>
+  <run_depend>robot_tf3_geometry_msgs</run_depend>
+
+  <build_depend>robot_cpp</build_depend>
+  <run_depend>robot_cpp</run_depend>
+
+  <build_depend>robot_protocol_msgs</build_depend>
+  <run_depend>robot_protocol_msgs</run_depend>
+
+  <build_depend>robot_time</build_depend>
+  <run_depend>robot_time</run_depend>
+
+  <build_depend>robot_visualization_msgs</build_depend>
+  <run_depend>robot_visualization_msgs</run_depend>
+
+  <build_depend>data_convert</build_depend>
+  <run_depend>data_convert</run_depend>
+
+  <build_depend>sbpl</build_depend>
+  <run_depend>sbpl</run_depend>
+
+</package>

rviz/sbpl.rviz

@@ -0,0 +1,192 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map2
+      Splitter Ratio: 0.5
+    Tree Height: 565
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.7
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.1
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.3
+      Head Radius: 0.1
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Value: true
+    - Arrow Length: 0.3
+      Class: rviz/PoseArray
+      Color: 255; 25; 0
+      Enabled: true
+      Name: PoseArray
+      Topic: /particlecloud
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1
+      Min Color: 0; 0; 0
+      Min Intensity: 1
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Flat Squares
+      Topic: /base_scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Path
+      Topic: /move_base_node/SBPLLatticePlanner/plan
+      Value: true
+    - Alpha: 0.7
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base_node/local_costmap/costmap
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /move_base_node/SBPLLatticePlanner/footprint_markers
+      Name: Footprint Markers
+      Namespaces:
+        sbpl_robot_footprint: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 54.0128
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 27.4168
+        Y: 22.0811
+        Z: 0.0219529
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.5698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.70358
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000013c000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000036e000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 390
+  Y: 65

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)