DuongTD/sbpl_lattice_planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd
2026-07-28 23:44:14 +07:00
commit 84d4becbdf
21 changed files with 21856 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

177
CMakeLists.txt Normal file
View File

@@ -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()

184
README.md Normal file
View File

@@ -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).

15
cfg/sbpl_global_params.yaml Normal file
View File

@@ -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

123
include/sbpl_lattice_planner/sbpl_lattice_planner.h Normal file
View File

@@ -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

2643
matlab/mprim/diff.mprim Normal file
View File

File diff suppressed because it is too large Load Diff

280
matlab/mprim/genmprim_unicycle_highcost_5cm.m Executable file
View File

@@ -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');

416
matlab/mprim/genmprim_unicycle_highcost_5cm.py Executable file
View File

@@ -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)

1683
matlab/mprim/pr2.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1203
matlab/mprim/unicycle_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
matlab/mprim/unicycle_5cm_expensive_turn_in_place.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
matlab/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

2403
matlab/mprim/unicycle_5cm_noreverse_trolley.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
matlab/mprim/unicycle_highcost_10cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
matlab/mprim/unicycle_highcost_1cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
matlab/mprim/unicycle_highcost_2_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
matlab/mprim/unicycle_highcost_2cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
matlab/mprim/unicycle_highcost_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

15
msg/SBPLLatticePlannerStats.msg Normal file
View File

@@ -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

93
package.xml Normal file
View File

@@ -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>

192
rviz/sbpl.rviz Normal file
View File

@@ -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

648
src/sbpl_lattice_planner.cpp Normal file
View File

@@ -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)