HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
navigations/sbpl_lattice_planner/CMakeLists.txt Executable file
View File

@@ -0,0 +1,82 @@
cmake_minimum_required(VERSION 3.5.1)
cmake_policy(SET CMP0048 NEW)
project(sbpl_lattice_planner)
##############################################################################
# Find dependencies
##############################################################################
set(THIS_PACKAGE_ROS_DEPS roscpp costmap_2d nav_core pluginlib tf tf2
geometry_msgs nav_msgs)
find_package(catkin REQUIRED COMPONENTS
${THIS_PACKAGE_ROS_DEPS} message_generation)
# Find SBPL
# find_package(SBPL REQUIRED)
find_package(PkgConfig REQUIRED)
pkg_check_modules(SBPL REQUIRED sbpl)
# include_directories(${SBPL_INCLUDE_DIRS})
link_directories(${SBPL_LIBRARY_DIRS})
# include_directories(include ${catkin_INCLUDE_DIRS} ${SBPL_INCLUDE_DIRS})
##############################################################################
# Generate messages
##############################################################################
add_message_files(FILES
SBPLLatticePlannerStats.msg
OccupancyGrid.msg)
generate_messages(DEPENDENCIES geometry_msgs nav_msgs)
##############################################################################
# Define package
##############################################################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS} message_runtime
# DEPENDS SBPL
)
##############################################################################
# Build
##############################################################################
include_directories(
include
${catkin_INCLUDE_DIRS}
${SBPL_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME} src/sbpl_lattice_planner.cpp)
target_link_libraries(${PROJECT_NAME} ${SBPL_LIBRARIES})
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-terminate") # suppress warning from included SBPL header
##############################################################################
# Install
##############################################################################
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
install(FILES bgp_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY
launch
matlab
rviz
worlds
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

8
navigations/sbpl_lattice_planner/bgp_plugin.xml Executable file
View File

@@ -0,0 +1,8 @@
<library path="lib/libsbpl_lattice_planner">
<class name="SBPLLatticePlanner" type="sbpl_lattice_planner::SBPLLatticePlanner" base_class_type="nav_core::BaseGlobalPlanner">
<description>
An implementation of search-based planning using anytime A* and a lattice graph.
</description>
</class>
</library>

118
navigations/sbpl_lattice_planner/include/sbpl_lattice_planner/sbpl_lattice_planner.h Executable file
View File

@@ -0,0 +1,118 @@
#ifndef SBPL_LATTICE_PLANNER_H
#define SBPL_LATTICE_PLANNER_H
#include <iostream>
#include <vector>
using namespace std;
// ROS
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
// Costmap used for the map representation
#include <costmap_2d/costmap_2d_ros.h>
// sbpl headers
#include <sbpl/headers.h>
// global representation
#include <nav_core/base_global_planner.h>
namespace sbpl_lattice_planner{
class SBPLLatticePlanner : public nav_core::BaseGlobalPlanner{
public:
/**
* @brief Default constructor for the NavFnROS object
*/
SBPLLatticePlanner();
/**
* @brief Constructor for the SBPLLatticePlanner object
* @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use
*/
SBPLLatticePlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Initialization function for the SBPLLatticePlanner object
* @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use
*/
virtual void initialize(std::string name,
costmap_2d::Costmap2DROS* costmap_ros);
/**
* @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 geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan);
virtual ~SBPLLatticePlanner(){};
private:
unsigned char costMapCostToSBPLCost(unsigned char newcost);
void publishStats(int solution_cost, int solution_size,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal);
unsigned char computeCircumscribedCost();
static void transformFootprintToEdges(const geometry_msgs::Pose& robot_pose,
const std::vector<geometry_msgs::Point>& footprint,
std::vector<geometry_msgs::Point>& out_footprint);
void getFootprintList(const std::vector<EnvNAVXYTHETALAT3Dpt_t>& sbpl_path, const std::string& path_frame_id,
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_;
std::string name_;
costmap_2d::Costmap2DROS* costmap_ros_; /**< manages the cost map for us */
std::vector<geometry_msgs::Point> footprint_;
unsigned int current_env_width_;
unsigned int current_env_height_;
ros::Publisher plan_pub_;
ros::Publisher stats_publisher_;
ros::Publisher sbpl_plan_footprint_pub_;
};
};
#endif

28
navigations/sbpl_lattice_planner/launch/move_base/base_local_planner_params_close.yaml Executable file
View File

@@ -0,0 +1,28 @@
TrajectoryPlannerROS:
#Independent settings for the local costmap
transform_tolerance: 0.3
sim_time: 1.7
sim_granularity: 0.025
dwa: true
vx_samples: 3
vtheta_samples: 20
max_vel_x: 2.0
min_vel_x: 0.1
max_rotational_vel: 1.0
min_in_place_vel_theta: 0.4
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.1
goal_distance_bias: 0.2
path_distance_bias: .9
occdist_scale: 0.01
heading_lookahead: 0.325
oscillation_reset_dist: 0.05
holonomic_robot: true
acc_lim_theta: 3.2
acc_lim_x: 2.5
acc_lim_y: 2.5
heading_scoring: false
heading_scoring_timestep: 0.8
holonomic_robot: true
simple_attractor: false
meter_scoring: true

21
navigations/sbpl_lattice_planner/launch/move_base/costmap_common_params.yaml Executable file
View File

@@ -0,0 +1,21 @@
#START VOXEL STUFF
map_type: voxel
origin_z: 0.0
z_resolution: 0.2
z_voxels: 10
unknown_threshold: 9
mark_threshold: 0
#END VOXEL STUFF
transform_tolerance: 0.3
obstacle_range: 2.5
max_obstacle_height: 2.0
raytrace_range: 3.0
footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]
#robot_radius: 0.46
footprint_padding: 0.01
inflation_radius: 0.55
cost_scaling_factor: 10.0
lethal_cost_threshold: 100
observation_sources: base_scan
base_scan: {data_type: LaserScan, expected_update_rate: 0.4,
observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}

12
navigations/sbpl_lattice_planner/launch/move_base/global_costmap_params.yaml Executable file
View File

@@ -0,0 +1,12 @@
#Independent settings for the planner's costmap
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 0.0
static_map: true
rolling_window: false
static_layer:
track_unknown_space: true
obstacle_layer:
track_unknown_space: true

13
navigations/sbpl_lattice_planner/launch/move_base/local_costmap_params_close.yaml Executable file
View File

@@ -0,0 +1,13 @@
local_costmap:
publish_voxel_map: true
global_frame: map
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 2.0
height: 2.0
resolution: 0.025
origin_x: 0.0
origin_y: 0.0

44
navigations/sbpl_lattice_planner/launch/move_base/move_base.xml Executable file
View File

@@ -0,0 +1,44 @@
<!-- <launch>
<node ns="local_costmap" name="voxel_grid_throttle" pkg="topic_tools" type="throttle" args="messages voxel_grid 3.0 voxel_grid_throttled" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
<param name="footprint_padding" value="0.01" />
<param name="controller_frequency" value="10.0" />
<param name="controller_patience" value="100.0" />
<param name="base_global_planner" value="SBPLLatticePlanner" />
<param name="base_local_planner" value="DWAPlannerROS" />
<param name="SBPLLatticePlanner/primitive_filename" value="$(find sbpl_lattice_planner)/matlab/mprim/pr2.mprim" />
<rosparam file="$(find sbpl_lattice_planner)/launch/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find sbpl_lattice_planner)/launch/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find sbpl_lattice_planner)/launch/move_base/local_costmap_params_close.yaml" command="load" />
<rosparam file="$(find sbpl_lattice_planner)/launch/move_base/global_costmap_params.yaml" command="load" />
<rosparam file="$(find sbpl_lattice_planner)/launch/move_base/sbpl_global_params.yaml" command="load" />
<rosparam file="$(find sbpl_lattice_planner)/launch/move_base/base_local_planner_params_close.yaml" command="load" />
</node>
</launch> -->
<launch>
<!--node ns="local_costmap" name="voxel_grid_throttle" pkg="topic_tools" type="throttle" args="messages voxel_grid 3.0 voxel_grid_throttled" /-->
<arg name="local_planner" default="dwb" doc="Local planner can be either dwa, base, teb or pose"/>
<arg name="with_virtual_walls" default="true" doc="Enables usage of virtual walls when set. Set to false when running SLAM." />
<arg name="prefix" default="" doc="Prefix used for robot tf frames" /> <!-- used in the config files -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen" clear_params="true">
<param name="SBPLLatticePlanner/primitive_filename" value="$(find sbpl_lattice_planner)/matlab/mprim/unicycle_highcost_5cm.mprim" />
<rosparam file="$(find sbpl_lattice_planner)/config/move_base_common_params.yaml" command="load" />
<rosparam file="$(find sbpl_lattice_planner)/config/sbpl_global_params.yaml" command="load" />
<rosparam file="$(find sbpl_lattice_planner)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />
<!-- global costmap params -->
<rosparam file="$(find sbpl_lattice_planner)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
<rosparam file="$(find sbpl_lattice_planner)/config/costmap_global_params.yaml" command="load" />
<rosparam file="$(find sbpl_lattice_planner)/config/costmap_global_params_plugins_virtual_walls.yaml" command="load" if="$(arg with_virtual_walls)" />
<rosparam file="$(find sbpl_lattice_planner)/config/costmap_global_params_plugins_no_virtual_walls.yaml" command="load" unless="$(arg with_virtual_walls)" />
<!-- local costmap params -->
<rosparam file="$(find sbpl_lattice_planner)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
<rosparam file="$(find sbpl_lattice_planner)/config/costmap_local_params.yaml" command="load" subst_value="true" />
<rosparam file="$(find sbpl_lattice_planner)/config/costmap_local_params_plugins_virtual_walls.yaml" command="load" if="$(arg with_virtual_walls)" />
<rosparam file="$(find sbpl_lattice_planner)/config/costmap_local_params_plugins_no_virtual_walls.yaml" command="load" unless="$(arg with_virtual_walls)" />
<!-- <remap from="map" to="/map" /> -->
<!-- <remap from="marker" to="move_base_node/DWBLocalPlanner/markers" /> -->
</node>
</launch>

10
navigations/sbpl_lattice_planner/launch/move_base/nav_view_sbpl.xml Executable file
View File

@@ -0,0 +1,10 @@
<launch>
<node name="nav_view" pkg="rqt_nav_view" type="rqt_nav_view" respawn="false">
<remap from="goal" to="move_base_simple/goal" />
<remap from="obstacles" to="move_base_node/local_costmap/obstacles" />
<remap from="inflated_obstacles" to="move_base_node/local_costmap/inflated_obstacles" />
<remap from="global_plan" to="move_base_node/SBPLLatticePlanner/plan" />
<remap from="local_plan" to="move_base_node/TrajectoryPlannerROS/local_plan" />
<remap from="robot_footprint" to="move_base_node/local_costmap/robot_footprint"/>
</node>
</launch>

18
navigations/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml Executable file
View File

@@ -0,0 +1,18 @@
# SBPLLatticePlanner:
# environment_type: XYThetaLattice
# planner_type: ARAPlanner
# allocated_time: 5.0
# initial_epsilon: 3.0
# forward_search: false
# allow_unknown: true
base_global_planner: SBPLLatticePlanner
SBPLLatticePlanner:
environment_type: XYThetaLattice
planner_type: ARAPlanner
allocated_time: 10.0
initial_epsilon: 15.0
forward_search: false
nominalvel_mpersecs: 0.8
timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
allow_unknown: true

30
navigations/sbpl_lattice_planner/launch/move_base_sbpl_fake_localization_2.5cm.launch Executable file
View File

@@ -0,0 +1,30 @@
<launch>
<param name="/use_sim_time" value="true"/>
<arg name="local_planner" default="dwb" doc="Local planner can be either dwa, dwb, eband, base, teb or pose" />
<arg name="map_file" default="$(find sbpl_lattice_planner)/worlds/willow.yaml" doc="Path to a map .yaml file (required)." />
<arg name="virtual_walls_map_file" default="$(arg map_file)" doc="Path to a virtual walls map .yaml file (optional)." />
<arg name="with_virtual_walls" default="false" />
<arg name="prefix" default="" />
<!-- <include file="$(find sbpl_lattice_planner)/launch/move_base/move_base_sbpl.xml"/> -->
<include file="$(find sbpl_lattice_planner)/launch/move_base/move_base.xml">
<arg name="local_planner" value="$(arg local_planner)"/>
<arg name="with_virtual_walls" value="$(arg with_virtual_walls)" />
<arg name="prefix" value="$(arg prefix)" />
</include>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" ns="/" output="screen">
<param name="frame_id" type="string" value="map"/>
</node>
<node pkg="stage_ros" type="stageros" name="stageros" args="$(find sbpl_lattice_planner)/worlds/willow.world" respawn="false" >
<param name="base_watchdog_timeout" value="0.2"/>
</node>
<node name="fake_localization" pkg="fake_localization" type="fake_localization" respawn="false" />
<include file="$(find sbpl_lattice_planner)/launch/amcl.launch"> </include>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find sbpl_lattice_planner)/rviz/sbpl.rviz" />
</launch>

280
navigations/sbpl_lattice_planner/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
navigations/sbpl_lattice_planner/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
navigations/sbpl_lattice_planner/matlab/mprim/pr2.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

1683
navigations/sbpl_lattice_planner/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
navigations/sbpl_lattice_planner/matlab/mprim/unicycle_5cm_noreverse_trolley.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

10
navigations/sbpl_lattice_planner/msg/OccupancyGrid.msg Executable file
View File

@@ -0,0 +1,10 @@
# This represents a 2-D grid map, in which each cell represents the probability of
# occupancy.
Header header
#MetaData for the map
nav_msgs/MapMetaData info
# The map data, in row-major order, starting with (0,0). Occupancy
int16[] data

15
navigations/sbpl_lattice_planner/msg/SBPLLatticePlannerStats.msg Executable 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

50
navigations/sbpl_lattice_planner/package.xml Executable file
View File

@@ -0,0 +1,50 @@
<?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>
<build_depend>costmap_2d</build_depend>
<exec_depend>costmap_2d</exec_depend>
<build_depend>geometry_msgs</build_depend>
<exec_depend>geometry_msgs</exec_depend>
<build_depend>nav_core</build_depend>
<exec_depend>nav_core</exec_depend>
<build_depend>nav_msgs</build_depend>
<exec_depend>nav_msgs</exec_depend>
<build_depend>pluginlib</build_depend>
<exec_depend>pluginlib</exec_depend>
<depend>roscpp</depend>
<build_depend>sbpl</build_depend>
<exec_depend>sbpl</exec_depend>
<build_depend>tf</build_depend>
<exec_depend>tf</exec_depend>
<build_depend>tf2</build_depend>
<exec_depend>tf2</exec_depend>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<export>
<nav_core plugin="${prefix}/bgp_plugin.xml" />
</export>
</package>

602
navigations/sbpl_lattice_planner/src/sbpl_lattice_planner.cpp Executable file
View File

@@ -0,0 +1,602 @@
/*********************************************************************
*
* 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 <pluginlib/class_list_macros.hpp>
#include <nav_msgs/Path.h>
#include <sbpl_lattice_planner/SBPLLatticePlannerStats.h>
#include <costmap_2d/inflation_layer.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf/transform_datatypes.h>
using namespace std;
using namespace ros;
PLUGINLIB_EXPORT_CLASS(sbpl_lattice_planner::SBPLLatticePlanner, nav_core::BaseGlobalPlanner)
namespace 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_ros_(NULL)
{
}
SBPLLatticePlanner::SBPLLatticePlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
: initialized_(false), costmap_ros_(NULL)
{
initialize(name, costmap_ros);
}
void SBPLLatticePlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
if (!initialized_)
{
ros::NodeHandle private_nh("~/" + name);
ROS_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);
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)(costmap_2d::INSCRIBED_INFLATED_OBSTACLE / inscribed_inflated_obstacle_ + 1);
ROS_INFO("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));
name_ = name;
costmap_ros_ = costmap_ros;
footprint_ = costmap_ros_->getRobotFootprint();
if ("XYThetaLattice" == environment_type_)
{
ROS_DEBUG("Using a 3D costmap for theta lattice\n");
env_ = new EnvironmentNAVXYTHETALAT();
}
else
{
ROS_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.
ROS_WARN("The costmap value at the robot's circumscribed radius (%f m) is 0.", costmap_ros_->getLayeredCostmap()->getCircumscribedRadius());
ROS_WARN("SBPL performance will suffer.");
ROS_WARN("Please decrease the costmap's cost_scaling_factor.");
}
if (!env_->SetEnvParameter("cost_inscribed_thresh", costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)))
{
ROS_ERROR("Failed to set cost_inscribed_thresh parameter");
exit(1);
}
if (!env_->SetEnvParameter("cost_possibly_circumscribed_thresh", circumscribed_cost_))
{
ROS_ERROR("Failed to set cost_possibly_circumscribed_thresh parameter");
exit(1);
}
int obst_cost_thresh = costMapCostToSBPLCost(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);
}
bool ret;
try
{
ret = env_->InitializeEnv(costmap_ros_->getCostmap()->getSizeInCellsX(), // width
costmap_ros_->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_ros_->getCostmap()->getResolution(), nominalvel_mpersecs,
timetoturn45degsinplace_secs, obst_cost_thresh,
primitive_filename_.c_str());
current_env_width_ = costmap_ros_->getCostmap()->getSizeInCellsX();
current_env_height_ = costmap_ros_->getCostmap()->getSizeInCellsY();
}
catch (SBPL_Exception *e)
{
ROS_ERROR("SBPL encountered a fatal exception: %s", e->what());
ret = false;
}
if (!ret)
{
ROS_ERROR("SBPL initialization failed!");
exit(1);
}
for (ssize_t ix(0); ix < costmap_ros_->getCostmap()->getSizeInCellsX(); ++ix)
for (ssize_t iy(0); iy < costmap_ros_->getCostmap()->getSizeInCellsY(); ++iy)
env_->UpdateCost(ix, iy, costMapCostToSBPLCost(costmap_ros_->getCostmap()->getCost(ix, iy)));
if ("ARAPlanner" == planner_type_)
{
ROS_INFO("Planning with ARA*");
planner_ = new ARAPlanner(env_, forward_search_);
}
else if ("ADPlanner" == planner_type_)
{
ROS_INFO("Planning with AD*");
planner_ = new ADPlanner(env_, forward_search_);
}
else
{
ROS_ERROR("ARAPlanner and ADPlanner are currently the only supported planners!\n");
exit(1);
}
ROS_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<visualization_msgs::Marker>("footprint_markers", 1);
initialized_ = true;
}
}
// Taken from Sachin's sbpl_cart_planner
// This rescales the costmap according to a rosparam which sets the obstacle cost
unsigned char SBPLLatticePlanner::costMapCostToSBPLCost(unsigned char newcost)
{
if (newcost == costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == costmap_2d::NO_INFORMATION))
return lethal_obstacle_;
else if (newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
return inscribed_inflated_obstacle_;
else if (newcost == 0)
return 0;
else if(newcost == costmap_2d::NO_INFORMATION)
return costmap_2d::FREE_SPACE/sbpl_cost_multiplier_;
else
{
unsigned char sbpl_cost = newcost / sbpl_cost_multiplier_;
if (sbpl_cost == 0)
sbpl_cost = 1;
return sbpl_cost;
}
}
void SBPLLatticePlanner::publishStats(int solution_cost, int solution_size,
const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal)
{
// Fill up statistics and publish
sbpl_lattice_planner::SBPLLatticePlannerStats stats;
stats.initial_epsilon = initial_epsilon_;
stats.plan_to_first_solution = false;
stats.final_number_of_expands = planner_->get_n_expands();
stats.allocated_time = allocated_time_;
stats.time_to_first_solution = planner_->get_initial_eps_planning_time();
stats.actual_time = planner_->get_final_eps_planning_time();
stats.number_of_expands_initial_solution = planner_->get_n_expands_init_solution();
stats.final_epsilon = planner_->get_final_epsilon();
stats.solution_cost = solution_cost;
stats.path_size = solution_size;
stats.start = start;
stats.goal = goal;
stats_publisher_.publish(stats);
}
unsigned char SBPLLatticePlanner::computeCircumscribedCost()
{
unsigned char result = 0;
if (!costmap_ros_)
{
ROS_ERROR("Costmap is not initialized");
return 0;
}
// check if the costmap has an inflation layer
for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_ros_->getLayeredCostmap()->getPlugins()->begin();
layer != costmap_ros_->getLayeredCostmap()->getPlugins()->end();
++layer)
{
boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
if (!inflation_layer)
continue;
result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_ros_->getLayeredCostmap()->getCircumscribedRadius() / costmap_ros_->getCostmap()->getResolution()));
}
return result;
}
bool SBPLLatticePlanner::makePlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
std::vector<geometry_msgs::PoseStamped> &plan)
{
if (!initialized_)
{
ROS_ERROR("Global planner is not initialized");
return false;
}
bool do_init = false;
if (current_env_width_ != costmap_ros_->getCostmap()->getSizeInCellsX() ||
current_env_height_ != costmap_ros_->getCostmap()->getSizeInCellsY())
{
ROS_INFO("Costmap dimensions have changed from (%d x %d) to (%d x %d), reinitializing sbpl_lattice_planner.",
current_env_width_, current_env_height_,
costmap_ros_->getCostmap()->getSizeInCellsX(), costmap_ros_->getCostmap()->getSizeInCellsY());
do_init = true;
}
else if (footprint_ != costmap_ros_->getRobotFootprint())
{
ROS_INFO("Robot footprint has changed, reinitializing sbpl_lattice_planner.");
do_init = true;
}
else if (circumscribed_cost_ != computeCircumscribedCost())
{
ROS_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_ros_);
}
plan.clear();
ROS_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);
try
{
int ret = env_->SetStart(start.pose.position.x - costmap_ros_->getCostmap()->getOriginX(), start.pose.position.y - costmap_ros_->getCostmap()->getOriginY(), theta_start);
if (ret < 0 || planner_->set_start(ret) == 0)
{
ROS_ERROR("ERROR: failed to set start state\n");
return false;
}
}
catch (SBPL_Exception *e)
{
ROS_ERROR("SBPL encountered a fatal exception while setting the start state");
return false;
}
try
{
int ret = env_->SetGoal(goal.pose.position.x - costmap_ros_->getCostmap()->getOriginX(), goal.pose.position.y - costmap_ros_->getCostmap()->getOriginY(), theta_goal);
if (ret < 0 || planner_->set_goal(ret) == 0)
{
ROS_ERROR("ERROR: failed to set goal state\n");
return false;
}
}
catch (SBPL_Exception *e)
{
ROS_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_ros_->getCostmap()->getSizeInCellsX(); ix++)
{
for (unsigned int iy = 0; iy < costmap_ros_->getCostmap()->getSizeInCellsY(); iy++)
{
unsigned char oldCost = env_->GetMapCost(ix, iy);
unsigned char newCost = costMapCostToSBPLCost(costmap_ros_->getCostmap()->getCost(ix, iy));
if (oldCost == newCost)
continue;
allCount++;
// first case - off cell goes on
if ((oldCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && oldCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
(newCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || newCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)))
{
offOnCount++;
}
if ((oldCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || oldCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
(newCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && newCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)))
{
onOffCount++;
}
env_->UpdateCost(ix, iy, costMapCostToSBPLCost(costmap_ros_->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)
{
ROS_ERROR("SBPL failed to update the costmap");
return false;
}
// setting planner parameters
ROS_DEBUG("allocated:%f, init eps:%f\n", allocated_time_, initial_epsilon_);
planner_->set_initialsolution_eps(initial_epsilon_);
planner_->set_search_mode(false);
ROS_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)
ROS_DEBUG("Solution is found\n");
else
{
ROS_INFO("Solution not found\n");
publishStats(solution_cost, 0, start, goal);
return false;
}
}
catch (SBPL_Exception *e)
{
ROS_ERROR("SBPL encountered a fatal exception while planning");
return false;
}
ROS_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)
{
ROS_ERROR("SBPL encountered a fatal exception while reconstructing the path");
return false;
}
// 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_ros_->getCostmap()->getOriginX(),
start.pose.position.y - costmap_ros_->getCostmap()->getOriginY(),
theta_start);
sbpl_path.push_back(s);
}
ROS_DEBUG("Plan has %d points.\n", (int)sbpl_path.size());
if (sbpl_path.back().x != goal.pose.position.x - costmap_ros_->getCostmap()->getOriginX() ||
sbpl_path.back().y != goal.pose.position.y - costmap_ros_->getCostmap()->getOriginY() ||
sbpl_path.back().theta != theta_goal)
{
EnvNAVXYTHETALAT3Dpt_t goal_stemp(goal.pose.position.x - costmap_ros_->getCostmap()->getOriginX(),
goal.pose.position.y - costmap_ros_->getCostmap()->getOriginY(),
theta_goal);
sbpl_path.push_back(goal_stemp);
}
ros::Time plan_time = ros::Time::now();
if (publish_footprint_path_)
{
visualization_msgs::Marker sbpl_plan_footprint;
getFootprintList(sbpl_path, costmap_ros_->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_ros_->getGlobalFrameID();
gui_path.header.stamp = plan_time;
for (unsigned int i = 0; i < sbpl_path.size(); i++)
{
geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_ros_->getGlobalFrameID();
pose.pose.position.x = sbpl_path[i].x + costmap_ros_->getCostmap()->getOriginX();
pose.pose.position.y = sbpl_path[i].y + costmap_ros_->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();
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, visualization_msgs::Marker &ma)
{
ma.header.frame_id = path_frame_id;
ma.header.stamp = ros::Time();
ma.ns = "sbpl_robot_footprint";
ma.id = 0;
ma.type = visualization_msgs::Marker::LINE_LIST;
ma.action = 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<geometry_msgs::Point> transformed_rfp;
geometry_msgs::Pose robot_pose;
robot_pose.position.x = sbpl_path[i].x + costmap_ros_->getCostmap()->getOriginX();
;
robot_pose.position.y = sbpl_path[i].y + costmap_ros_->getCostmap()->getOriginY();
;
robot_pose.position.z = 0.0;
tf::Quaternion quat;
quat.setRPY(0.0, 0.0, sbpl_path[i].theta);
tf::quaternionTFToMsg(quat, robot_pose.orientation);
transformFootprintToEdges(robot_pose, footprint_, transformed_rfp);
for (auto &point : transformed_rfp)
ma.points.push_back(point);
}
}
void SBPLLatticePlanner::transformFootprintToEdges(const geometry_msgs::Pose &robot_pose,
const std::vector<geometry_msgs::Point> &footprint,
std::vector<geometry_msgs::Point> &out_footprint)
{
out_footprint.resize(2 * footprint.size());
double yaw = tf::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;
}
}
}
};

0
navigations/sbpl_lattice_planner/test.drawio Executable file
View File