@ -0,0 +1,18 @@
|
||||
{
|
||||
"configurations": [
|
||||
{
|
||||
"name": "Linux",
|
||||
"browse": {
|
||||
"databaseFilename": "",
|
||||
"limitSymbolsToIncludedHeaders": true
|
||||
},
|
||||
"includePath": [
|
||||
"${workspaceFolder}/**",
|
||||
"/opt/ros/melodic/include",
|
||||
"/opt/ros/melodic/include/ompl-1.4"
|
||||
]
|
||||
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
}
|
||||
@ -0,0 +1,73 @@
|
||||
{
|
||||
"python.autoComplete.extraPaths": [
|
||||
"/home/dengpw/catkin_ws/devel/lib/python2.7/dist-packages",
|
||||
"/opt/ros/melodic/lib/python2.7/dist-packages"
|
||||
],
|
||||
"files.associations": {
|
||||
"cctype": "cpp",
|
||||
"clocale": "cpp",
|
||||
"cmath": "cpp",
|
||||
"csignal": "cpp",
|
||||
"cstdarg": "cpp",
|
||||
"cstddef": "cpp",
|
||||
"cstdio": "cpp",
|
||||
"cstdlib": "cpp",
|
||||
"cstring": "cpp",
|
||||
"ctime": "cpp",
|
||||
"cwchar": "cpp",
|
||||
"cwctype": "cpp",
|
||||
"array": "cpp",
|
||||
"atomic": "cpp",
|
||||
"strstream": "cpp",
|
||||
"*.tcc": "cpp",
|
||||
"bitset": "cpp",
|
||||
"chrono": "cpp",
|
||||
"complex": "cpp",
|
||||
"cstdint": "cpp",
|
||||
"deque": "cpp",
|
||||
"list": "cpp",
|
||||
"unordered_map": "cpp",
|
||||
"unordered_set": "cpp",
|
||||
"vector": "cpp",
|
||||
"exception": "cpp",
|
||||
"hash_map": "cpp",
|
||||
"hash_set": "cpp",
|
||||
"fstream": "cpp",
|
||||
"functional": "cpp",
|
||||
"initializer_list": "cpp",
|
||||
"iomanip": "cpp",
|
||||
"iosfwd": "cpp",
|
||||
"iostream": "cpp",
|
||||
"istream": "cpp",
|
||||
"limits": "cpp",
|
||||
"new": "cpp",
|
||||
"ostream": "cpp",
|
||||
"numeric": "cpp",
|
||||
"ratio": "cpp",
|
||||
"sstream": "cpp",
|
||||
"stdexcept": "cpp",
|
||||
"streambuf": "cpp",
|
||||
"system_error": "cpp",
|
||||
"thread": "cpp",
|
||||
"cfenv": "cpp",
|
||||
"cinttypes": "cpp",
|
||||
"tuple": "cpp",
|
||||
"type_traits": "cpp",
|
||||
"utility": "cpp",
|
||||
"typeindex": "cpp",
|
||||
"typeinfo": "cpp",
|
||||
"algorithm": "cpp",
|
||||
"iterator": "cpp",
|
||||
"map": "cpp",
|
||||
"memory": "cpp",
|
||||
"memory_resource": "cpp",
|
||||
"optional": "cpp",
|
||||
"random": "cpp",
|
||||
"set": "cpp",
|
||||
"string": "cpp",
|
||||
"string_view": "cpp",
|
||||
"*.ipp": "cpp",
|
||||
"*.txx": "cpp",
|
||||
"valarray": "cpp"
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,66 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(hybrid_astar_planner)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 11)
|
||||
set(CMAKE_CXX_COMPILER "g++")
|
||||
|
||||
# add_subdirectory(test_the_plugin)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
geometry_msgs
|
||||
pluginlib
|
||||
costmap_2d
|
||||
roscpp
|
||||
nav_core
|
||||
tf2_ros
|
||||
ompl
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES ${PROJECT_NAME}
|
||||
geometry_msgs
|
||||
pluginlib
|
||||
roscpp
|
||||
nav_core
|
||||
tf2_ros
|
||||
ompl
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
test_the_plugin/include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${OMPL_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# find_package(ompl REQUIRED)
|
||||
|
||||
# if(NOT OMPL_FOUND)
|
||||
# message(AUTHOR_WARNING,"Open Motion Planning Library not found")
|
||||
# endif(NOT OMPL_FOUND)
|
||||
|
||||
|
||||
add_executable(test_planner test_the_plugin/src/test.cpp test_the_plugin/src/test_plugins.cpp)
|
||||
target_link_libraries(test_planner ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(tf_test_broadcaster test_the_plugin/src/tf_broadcaster.cpp)
|
||||
target_link_libraries(tf_test_broadcaster ${catkin_LIBRARIES})
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
src/planner_core.cpp
|
||||
src/hybrid_astar.cpp
|
||||
src/node2d.cpp
|
||||
src/a_start.cpp
|
||||
src/visualize.cpp
|
||||
src/node3d.cpp
|
||||
src/hybrid_astar.cpp
|
||||
src/algorithm.cpp
|
||||
src/dubins.cpp
|
||||
src/ReedsShepp.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME} ${OMPL_LIBRARIES})
|
||||
|
||||
install(FILES bgp_plugin.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
@ -0,0 +1,29 @@
|
||||
BSD 3-Clause License
|
||||
|
||||
Copyright (c) 2021, dengpw
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
2. 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.
|
||||
|
||||
3. Neither the name of the copyright holder 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 HOLDER 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.
|
||||
@ -0,0 +1,41 @@
|
||||
# hybrid_astar_planner
|
||||
|
||||
This is a global planner plugin of ROS move_base package.
|
||||
|
||||
## Introduction
|
||||
Traditional robot navigation algorithms (such as the logistics storage robot used by Jingdong or Amazon)
|
||||
mostly use a * algorithm or Dijkstra search algorithm to planning its trajectory after the map is gridded.
|
||||
Most of them are differential models. Obviously, this kind of path is still difficult to realize for traditional
|
||||
four-wheel vehicles whitch has nonholonomic constraint (vehicles with Ackerman steering mechanism).
|
||||
So I designed a hybrid a * plug-in for ROS as my graduation project
|
||||
|
||||
## How to Use
|
||||
|
||||
### <a name="dependencies"></a>Dependencies
|
||||
Install all dependencies
|
||||
* [Open Motion Planning Library (OMPL)](http://ompl.kavrakilab.org/)
|
||||
* [ros_map_server](http://wiki.ros.org/map_server)
|
||||
|
||||
### <a name="setup"></a><font color=Darkorange size=5 >Setup</font>
|
||||
|
||||
Run the following command to clone, build, and launch the package (requires a sources ROS environment):
|
||||
|
||||
```
|
||||
sudo apt install libompl-dev \
|
||||
&& mkdir -p ~/catkin_ws/src \
|
||||
&& cd ~/catkin_ws/src \
|
||||
&& git clone https://github.com/dengpw/hybrid_astar_planner.git \
|
||||
&& cd .. \
|
||||
&& catkin_make \
|
||||
&& source devel/setup.bash \
|
||||
&& rospack profile \
|
||||
```
|
||||
|
||||
You can run the folling command to test whether the plug-in is install propely
|
||||
```
|
||||
&& roslaunch hybrid_astar_planner test.launch
|
||||
```
|
||||
It can run a static teajectory planner using hybrid A*
|
||||
If everything run fine,you can use this plugin in ROS naviagation package by making a special declaration before starting the launch file:
|
||||
< `param name="base_global_planner" value="global_planner/GlobalPlanner"` / >
|
||||
< `param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"` / >
|
||||
@ -0,0 +1,7 @@
|
||||
<library path="lib/libhybrid_astar_planner">
|
||||
<class name="hybrid_astar_planner/HybridAStarPlanner" type="hybrid_astar_planner::HybridAStarPlanner" base_class_type="nav_core::BaseGlobalPlanner">
|
||||
<description>
|
||||
A implementation of a based planner using Hybrid A*
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@ -0,0 +1,90 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016, Guan-Horng Liu.
|
||||
* 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 the copyright holder 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: Guan-Horng Liu
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef SPACES_REEDS_SHEPP_STATE_SPACE_
|
||||
#define SPACES_REEDS_SHEPP_STATE_SPACE_
|
||||
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#include <cassert>
|
||||
|
||||
class ReedsSheppStateSpace
|
||||
{
|
||||
public:
|
||||
|
||||
/** \brief The Reeds-Shepp path segment types */
|
||||
enum ReedsSheppPathSegmentType { RS_NOP=0, RS_LEFT=1, RS_STRAIGHT=2, RS_RIGHT=3 };
|
||||
|
||||
/** \brief Reeds-Shepp path types */
|
||||
static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
|
||||
|
||||
/** \brief Complete description of a ReedsShepp path */
|
||||
class ReedsSheppPath
|
||||
{
|
||||
public:
|
||||
ReedsSheppPath(const ReedsSheppPathSegmentType* type=reedsSheppPathType[0],
|
||||
double t=std::numeric_limits<double>::max(), double u=0., double v=0.,
|
||||
double w=0., double x=0.);
|
||||
|
||||
double length() const { return totalLength_; }
|
||||
|
||||
/** Path segment types */
|
||||
const ReedsSheppPathSegmentType* type_;
|
||||
/** Path segment lengths */
|
||||
double length_[5];
|
||||
/** Total length */
|
||||
double totalLength_;
|
||||
};
|
||||
|
||||
ReedsSheppStateSpace(double turningRadius) : rho_(turningRadius) {}
|
||||
|
||||
double distance(double q0[3], double q1[3]);
|
||||
|
||||
std::vector<ReedsSheppPathSegmentType> type(double q0[3], double q1[3]);
|
||||
|
||||
void sample(double q0[3], double q1[3], double step_size, double &length, std::vector<std::vector<double> > &points);
|
||||
|
||||
/** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */
|
||||
ReedsSheppPath reedsShepp(double q0[3], double q1[3]);
|
||||
|
||||
protected:
|
||||
void interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[3]);
|
||||
|
||||
/** \brief Turning radius */
|
||||
double rho_;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,32 @@
|
||||
#ifndef _ALGORITHM_H
|
||||
#define _ALGORITHM_H
|
||||
|
||||
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
|
||||
#include <ompl/base/spaces/DubinsStateSpace.h>
|
||||
#include <ompl/base/spaces/SE2StateSpace.h>
|
||||
#include <ompl/base/State.h>
|
||||
#include <ompl/base/State.h>
|
||||
#include "node3d.h"
|
||||
#include "node2d.h"
|
||||
#include <boost/heap/binomial_heap.hpp>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
typedef ompl::base::SE2StateSpace::StateType State;
|
||||
namespace hybrid_astar_planner {
|
||||
// OPEN LIST AS BOOST IMPLEMENTATION
|
||||
struct CompareNodes {
|
||||
// Sorting 3D nodes by increasing C value - the total estimated cost
|
||||
bool operator()(const Node3D* lhs, const Node3D* rhs) const {
|
||||
return lhs->getF() > rhs->getF();
|
||||
}
|
||||
// Sorting 2D nodes by increasing C value - the total estimated cost
|
||||
bool operator()(const Node2D* lhs, const Node2D* rhs) const {
|
||||
return lhs->getF() > rhs->getF();
|
||||
}
|
||||
};
|
||||
Node3D* dubinsShot(Node3D& start, Node3D& goal, costmap_2d::Costmap2D* costmap);
|
||||
Node3D* reedsSheppShot(Node3D& start, Node3D& goal, costmap_2d::Costmap2D* costmap);
|
||||
void updateH(Node3D& start, const Node3D& goal, Node2D* nodes2D, float* dubinsLookup, int width, int height, float inspireAstar);
|
||||
}
|
||||
|
||||
#endif //end of algorithm.h
|
||||
@ -0,0 +1,58 @@
|
||||
#ifndef _ASTAR_H
|
||||
#define _ASTAR_H
|
||||
|
||||
#include <vector>
|
||||
#include "algorithm.h"
|
||||
#include "expander.h"
|
||||
#include "node2d.h"
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <ros/publisher.h>
|
||||
namespace hybrid_astar_planner {
|
||||
class Index {
|
||||
public:
|
||||
Index(int a,float b) {
|
||||
i = a;
|
||||
cost = b;
|
||||
}
|
||||
int i;
|
||||
float cost;
|
||||
};
|
||||
|
||||
class astar : public Expander
|
||||
{
|
||||
|
||||
public:
|
||||
astar(std::string frame_id, costmap_2d::Costmap2D* _costmap)
|
||||
:Expander(frame_id, _costmap) {
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Find the path between the start pose and goal pose
|
||||
* @param start the reference of start pose
|
||||
* @param goal the reference of goal pose
|
||||
* @param cells_x the number of the cells of the costmap in x axis
|
||||
* @param cells_y the number of the cells of the costmap in y axis
|
||||
* @param plan the refrence of plan;
|
||||
* @return true if a valid plan was found.
|
||||
*/
|
||||
bool calculatePath(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
|
||||
int cells_x, int cells_y, std::vector<geometry_msgs::PoseStamped>& plan ,ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes );
|
||||
~astar(){ }
|
||||
private:
|
||||
|
||||
std::vector<Node2D*> gatAdjacentPoints(int cells_x, int cells_y, const unsigned char* charMap, Node2D* pathNode2D, Node2D *point );
|
||||
|
||||
/**
|
||||
* @brief transform the 2Dnode to geometry_msgs::PoseStamped
|
||||
* @param node the ptr of node
|
||||
* @param plan the refrence of plan
|
||||
*/
|
||||
void nodeToPlan(Node2D* node, std::vector<geometry_msgs::PoseStamped>& plan);
|
||||
/* data */
|
||||
};
|
||||
|
||||
|
||||
}//end of namespace hybrid_astar_planner
|
||||
|
||||
#endif //the end of astar.h
|
||||
@ -0,0 +1,35 @@
|
||||
#ifndef _EXPANDER_H
|
||||
#define _EXPANDER_H
|
||||
#include <vector>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <boost/heap/binomial_heap.hpp>
|
||||
#include <ros/publisher.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
namespace hybrid_astar_planner {
|
||||
|
||||
/**
|
||||
* @brief the base class of astar or hybrid astar planner
|
||||
*/
|
||||
class Expander
|
||||
{
|
||||
|
||||
public:
|
||||
Expander(std::string frame_id, costmap_2d::Costmap2D* _costmap)
|
||||
:frame_id_(frame_id), costmap(_costmap) {
|
||||
|
||||
}
|
||||
|
||||
virtual bool calculatePath(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
|
||||
int cells_x, int cells_y, std::vector<geometry_msgs::PoseStamped>& plan ,ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes) = 0;
|
||||
virtual ~Expander() {}
|
||||
protected:
|
||||
std::string frame_id_;
|
||||
costmap_2d::Costmap2D* costmap;
|
||||
/* data */
|
||||
};
|
||||
|
||||
|
||||
} //end of namespace hybrid_astar_planner
|
||||
#endif //end of expander.h
|
||||
@ -0,0 +1,89 @@
|
||||
#ifndef _HYBRID_ASTAR_H
|
||||
#define _HYBRID_ASTAR_H
|
||||
|
||||
#include <vector>
|
||||
#include "algorithm.h"
|
||||
#include "expander.h"
|
||||
#include "node3d.h"
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <ros/publisher.h>
|
||||
// #define TEST
|
||||
#define point_accuracy 0.5
|
||||
#define theta_accuracy 2
|
||||
namespace hybrid_astar_planner {
|
||||
|
||||
class hybridAstar : public Expander
|
||||
{
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Default constructor for the HybridAStarPlanner object
|
||||
*/
|
||||
hybridAstar(std::string frame_id, costmap_2d::Costmap2D* _costmap)
|
||||
:Expander(frame_id, _costmap) {
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Find the path between the start pose and goal pose
|
||||
* @param start the reference of start pose
|
||||
* @param goal the reference of goal pose
|
||||
* @param cells_x the number of the cells of the costmap in x axis
|
||||
* @param cells_y the number of the cells of the costmap in y axis
|
||||
* @param plan the refrence of plan;
|
||||
* @return true if a valid plan was found.
|
||||
*/
|
||||
bool calculatePath(
|
||||
const geometry_msgs::PoseStamped& start,
|
||||
const geometry_msgs::PoseStamped& goal,
|
||||
int cellsX, int cellsY, std::vector<geometry_msgs::PoseStamped>& plan,
|
||||
ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes);
|
||||
|
||||
/**
|
||||
* @brief Default deconstructor for the HybridAStarPlanner object
|
||||
*/
|
||||
~hybridAstar(){ }
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* @brief Get the adjacent pose of a given pose
|
||||
* @param cells_x the number of the cells of the costmap in x axis
|
||||
* @param cells_y the number of the cells of the costmap in y axis
|
||||
* @param charMap
|
||||
*/
|
||||
std::vector<Node3D*> gatAdjacentPoints(int dir, int cells_x, int cells_y, const unsigned char* charMap, Node3D *point);
|
||||
|
||||
/**
|
||||
* @brief judge whether is reach the goal pose
|
||||
* @param node the refrence of the node
|
||||
* @param goalPose the goal of the planner
|
||||
* @return true if reach the goal
|
||||
*/
|
||||
bool reachGoal(Node3D* node, Node3D* goalPose);
|
||||
|
||||
/**
|
||||
* @brief get the index of node
|
||||
* @param x the x position axis of the node
|
||||
* @param y the y position axis of the node
|
||||
* @param cells_x the scale of cells in x axis
|
||||
* @param t the depth of the 3D nodes
|
||||
* @return the index of node
|
||||
*/
|
||||
int calcIndix(float x, float y, int cells_x, float t);
|
||||
|
||||
/**
|
||||
* @brief transform the 2Dnode to geometry_msgs::PoseStamped
|
||||
* @param node the ptr of node
|
||||
* @param plan the refrence of plan
|
||||
*/
|
||||
void nodeToPlan(Node3D* node, std::vector<geometry_msgs::PoseStamped>& plan);
|
||||
std::unique_ptr<GridSearch> grid_a_star_heuristic_generator_;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
}//end of namespace hybrid_astar_planner
|
||||
|
||||
#endif //the end of astar.h
|
||||
@ -0,0 +1,99 @@
|
||||
#ifndef _NODE2D_H
|
||||
#define _NODE2D_H
|
||||
|
||||
#include <cmath>
|
||||
#include <unordered_map>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
namespace hybrid_astar_planner {
|
||||
|
||||
class Node2D
|
||||
{
|
||||
public:
|
||||
Node2D(): Node2D(0, 0, 999, 0, nullptr) {}
|
||||
Node2D(int _x , int _y, float _g = 999, float _h = 0, Node2D* _perd = nullptr):
|
||||
x(_x), y(_y), g(_g), h(_h), perd(_perd), o(false), c(false)
|
||||
{
|
||||
index = -1;
|
||||
}
|
||||
~Node2D(){}
|
||||
|
||||
void setX(int _x) { x = _x; }
|
||||
void setY(int _y) { y = _y; }
|
||||
void setCost(unsigned int C) { cost = C; }
|
||||
void setG(float _g) { g = _g; }
|
||||
void setH(float _h) { h = _h; }
|
||||
void setClosedSet() { c = true; o = false; }
|
||||
void setOpenSet() { o = true; }
|
||||
void setPerd(Node2D* _perd) { perd = _perd; }
|
||||
void setPerd_(std::shared_ptr<Node2D> _perd) { perd_node = _perd; }
|
||||
int getX(void) { return x; }
|
||||
int getY(void) { return y; }
|
||||
float getF(void) const { return g + h; }
|
||||
float calcG(Node2D const *partent);
|
||||
float getG() const { return g; }
|
||||
float calcH(Node2D const *goal);
|
||||
int getindex(int width) { return (y * width + x); }
|
||||
int getCost() {return cost;}
|
||||
bool isOpenSet() { return o; }
|
||||
bool isClosedSet() { return c; }
|
||||
Node2D* getPerd() { return perd; }
|
||||
void SetPathCost(const float path_cost) {
|
||||
g = path_cost;
|
||||
}
|
||||
private:
|
||||
// float cost_ = 999;
|
||||
/// the x position
|
||||
int x;
|
||||
/// the y position
|
||||
int y;
|
||||
/// the cost of current node
|
||||
unsigned int cost;
|
||||
/// the cost-so-far
|
||||
float g;
|
||||
/// the cost-to-go
|
||||
float h;
|
||||
/// the index of the node in the 2D array
|
||||
int index;
|
||||
/// the open value
|
||||
bool o;
|
||||
/// the closed value
|
||||
bool c;
|
||||
/// the discovered value
|
||||
bool d;
|
||||
/// the predecessor pointer
|
||||
Node2D* perd;
|
||||
std::shared_ptr<Node2D> perd_node = nullptr;
|
||||
};
|
||||
|
||||
class GridSearch {
|
||||
public:
|
||||
GridSearch()
|
||||
{
|
||||
|
||||
}
|
||||
~GridSearch()
|
||||
{
|
||||
|
||||
}
|
||||
std::vector<std::shared_ptr<Node2D>> getAdjacentPoints(int cells_x,
|
||||
int cells_y, const unsigned char* charMap, std::shared_ptr<Node2D> point);
|
||||
|
||||
std::unordered_map<int, std::shared_ptr<Node2D>> GenerateDpMap(
|
||||
const double goal_x, const double goal_y,
|
||||
costmap_2d::Costmap2D* costmap);
|
||||
// std::unordered_map<int, std::shared_ptr<Node2D>> dp_map_;
|
||||
private:
|
||||
|
||||
// std::unordered_map<int, std::shared_ptr<Node2D>> dp_map_;
|
||||
struct cmp {
|
||||
// Sorting 3D nodes by increasing C value - the total estimated cost
|
||||
bool operator()(const std::pair<int, double>& left,
|
||||
const std::pair<int, double>& right) const {
|
||||
return left.second >= right.second;
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
}//end of namespace hybrid_astar_planner
|
||||
|
||||
#endif //end of node2d.h
|
||||
@ -0,0 +1,87 @@
|
||||
#ifndef _NODE3D_H
|
||||
#define _NODE3D_H
|
||||
#include "constants.h"
|
||||
#include <cmath>
|
||||
namespace hybrid_astar_planner {
|
||||
class Node3D
|
||||
{
|
||||
public:
|
||||
Node3D(): Node3D(0, 0, 0, 999, 0, false, nullptr) {}
|
||||
Node3D(float _x , float _y, float t, float _g = 999, float _h = 0, bool _reverse = false, Node3D* _perd = nullptr):
|
||||
x(_x), y(_y), g(_g), h(_h), reverse(_reverse), perd(_perd), o(false), c(false)
|
||||
{
|
||||
setT(t);
|
||||
index = -1;
|
||||
}
|
||||
~Node3D(){}
|
||||
|
||||
void setX(float _x) { x = _x; }
|
||||
void setY(float _y) { y = _y; }
|
||||
void setCost(unsigned int C) { cost = C; }
|
||||
void setT(float _t);
|
||||
void setG(float _g) { g = _g; }
|
||||
void setH(float _h) { h = _h; }
|
||||
void setClosedSet() { c = true; o = false; }
|
||||
void setOpenSet() { o = true; }
|
||||
void setPerd(Node3D* _perd) { perd = _perd; }
|
||||
void setReverse(bool r) { reverse = r; }
|
||||
float getX(void) const { return x; }
|
||||
float getY(void) const { return y; }
|
||||
float getT(void) const { return t * Constants::deltaHeadingRad; }
|
||||
float getTheta(void) const { return theta; }
|
||||
float getF(void) const { return g + h; }
|
||||
int getCost() {return cost;}
|
||||
/**
|
||||
* @brief caculate the G value of the node
|
||||
* @return the G value of the node
|
||||
* G值为状态空间中从初始节点到当前此节点的实际代价
|
||||
*
|
||||
*/
|
||||
float calcG(void);
|
||||
float getG() const { return g; }
|
||||
float calcH(Node3D const *goal);
|
||||
int getindex(int width, int depth, float resolution, unsigned int dx, unsigned int dy) { this->index = (int(x/resolution + dx) * width + int(y/resolution + dy))*depth + t; return index;}//这里的resolution变动了
|
||||
bool isOpenSet() { return o; }
|
||||
bool isClosedSet() { return c; }
|
||||
bool isReverse() { return reverse; }
|
||||
Node3D* getPerd() { return perd; }
|
||||
// RANGE CHECKING
|
||||
/// Determines whether it is appropriate to find a analytical solution.
|
||||
bool isInRange(const Node3D& goal) const;//检测是否可以分析方法找到解
|
||||
|
||||
// CUSTOM OPERATORS
|
||||
/// Custom operator to compare nodes. Nodes are equal if their x and y position as well as heading is similar.
|
||||
bool operator == (const Node3D& rhs) const;//位置相同且朝向相似则认为是同一节点
|
||||
|
||||
private:
|
||||
/// the x position
|
||||
float x;
|
||||
/// the y position
|
||||
float y;
|
||||
/// the t position
|
||||
int t;
|
||||
/// the cost of this node in costmap
|
||||
unsigned int cost;
|
||||
/// the theta of this node
|
||||
float theta;
|
||||
/// the cost-so-far
|
||||
float g;
|
||||
/// the cost-to-go
|
||||
float h;
|
||||
/// the index of the node in the 3D array
|
||||
int index;
|
||||
/// the open value
|
||||
bool o;
|
||||
/// the closed value
|
||||
bool c;
|
||||
/// the discovered value
|
||||
bool d;
|
||||
/// the flag of node,Indicates whether the vehicle is in reverse
|
||||
bool reverse;
|
||||
/// the predecessor pointer
|
||||
Node3D* perd;
|
||||
};
|
||||
|
||||
}//end of namespace hybrid_astar_planner
|
||||
|
||||
#endif //end of node3d.h
|
||||
@ -0,0 +1,142 @@
|
||||
#ifndef _PLANNERCORE_H
|
||||
#define _PLANNERCORE_H
|
||||
/*********************************************************************
|
||||
*
|
||||
* BSD 3-Clause License
|
||||
*
|
||||
* Copyright (c) 2021, dengpw
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1 Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2 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.
|
||||
* 3 Neither the name of the copyright holder 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.
|
||||
*
|
||||
*********************************************************************/
|
||||
// #define POT_HIGH 1.0e10 // unassigned cell potential
|
||||
#include <vector>
|
||||
#include <nav_msgs/GetPlan.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <nav_core/base_global_planner.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include "node2d.h"
|
||||
#include "node3d.h"
|
||||
namespace hybrid_astar_planner {
|
||||
//类插件实现的多态的时候，若在头文件中定义了函数，那么就必须有这个函数的实现，否则会报错！！！
|
||||
/**
|
||||
* @class HybridAStarPlanner
|
||||
* @brief Provides a ROS wrapper for the HybridAStarPlanner planner which runs a fast, interpolated navigation function on a costmap.
|
||||
*/
|
||||
|
||||
class HybridAStarPlanner : public nav_core::BaseGlobalPlanner {
|
||||
public:
|
||||
/**
|
||||
* @brief Default constructor for the HybridAStarPlanner object
|
||||
*/
|
||||
HybridAStarPlanner();
|
||||
|
||||
/**
|
||||
* @brief Default deconstructor for the HybridAStarPlanner object
|
||||
*/
|
||||
~HybridAStarPlanner();
|
||||
|
||||
/**
|
||||
* @brief Initialization function for the HybridAStarPlanner
|
||||
* @param name The name of this planner
|
||||
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning.And the costmap is get from the topic name /map
|
||||
*/
|
||||
void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
|
||||
|
||||
void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
|
||||
|
||||
/**
|
||||
* @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
|
||||
*/
|
||||
bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
|
||||
std::vector<geometry_msgs::PoseStamped>& plan);
|
||||
|
||||
/**
|
||||
* @brief Publish the plan to RVIZ
|
||||
* @param path the vector contain the path
|
||||
*/
|
||||
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
|
||||
|
||||
/**
|
||||
* @brief Publish the path node to RVIZ
|
||||
* @param path the vector contain the path
|
||||
*/
|
||||
void publishPathNodes(const std::vector<geometry_msgs::PoseStamped>& path);
|
||||
|
||||
/**
|
||||
* @brief The call back function of makeplan service
|
||||
* @param req
|
||||
* @param resp the plan the planner made
|
||||
* @return True if a valid plan was found, false otherwise
|
||||
*/
|
||||
bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp);
|
||||
protected:
|
||||
|
||||
bool initialized_;
|
||||
std::string frame_id_;
|
||||
ros::Publisher plan_pub_;
|
||||
ros::Publisher path_vehicles_pub_;//用于在路径上发布车子位置
|
||||
costmap_2d::Costmap2D* costmap;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Clarn the visualization_msgs::Marker 清理可视化信息的标记点
|
||||
*/
|
||||
void clearPathNodes(void);
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Check whethe the start pose is available
|
||||
* @param start A reference to start pose
|
||||
* @return True if the start pose is available
|
||||
*/
|
||||
bool checkStartPose(const geometry_msgs::PoseStamped &start);
|
||||
|
||||
/**
|
||||
* @brief Check whethe the goal pose is available
|
||||
* @param goal A reference to goal pose
|
||||
* @return True if the goal pose is available
|
||||
*/
|
||||
bool checkgoalPose(const geometry_msgs::PoseStamped &goal);
|
||||
visualization_msgs::MarkerArray pathNodes;//节点数据结构，用于可视化
|
||||
double resolution;
|
||||
ros::ServiceServer make_plan_srv_;
|
||||
bool use_hybrid_astar;
|
||||
|
||||
};
|
||||
|
||||
|
||||
} //end namespace hybrid_astar_planner
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,10 @@
|
||||
#ifndef _VISUALIZE_H
|
||||
#define _VISUALIZE_H
|
||||
#include "node3d.h"
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <ros/publisher.h>
|
||||
namespace hybrid_astar_planner {
|
||||
void publishSearchNodes(Node3D node,ros::Publisher& pub, visualization_msgs::MarkerArray& pathNodes, int i);
|
||||
}
|
||||
|
||||
#endif//end of visualize,h
|
||||
@ -0,0 +1,49 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>hybrid_astar_planner</name>
|
||||
<version>1.1.2</version>
|
||||
<description>A hybrid_astar_planner package for move_base</description>
|
||||
<maintainer email="dengpw@qq.com">dengpw</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>BSD</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>pluginlib</build_depend>
|
||||
<build_export_depend>pluginlib</build_export_depend>
|
||||
<exec_depend>pluginlib</exec_depend>
|
||||
|
||||
|
||||
<build_depend>nav_core</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>visualization_msgs</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>ompl</build_depend>
|
||||
<build_depend>map_server</build_depend>
|
||||
|
||||
<exec_depend>nav_core</exec_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>visualization_msgs</exec_depend>
|
||||
<exec_depend>tf</exec_depend>
|
||||
<exec_depend>ompl</exec_depend>
|
||||
<exec_depend>map_server</exec_depend>
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<nav_core plugin="${prefix}/hybrid_astar_planner_plugin.xml" />
|
||||
</export>
|
||||
|
||||
|
||||
</package>
|
||||
@ -0,0 +1,623 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016, Guan-Horng Liu.
|
||||
* 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 the copyright holder 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: Guan-Horng Liu
|
||||
*********************************************************************/
|
||||
|
||||
|
||||
#include "ReedsShepp.h"
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
|
||||
|
||||
namespace
|
||||
{
|
||||
// The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
|
||||
|
||||
const double pi = boost::math::constants::pi<double>();
|
||||
const double twopi = 2. * pi;
|
||||
const double RS_EPS = 1e-6;
|
||||
const double ZERO = 10*std::numeric_limits<double>::epsilon();
|
||||
|
||||
inline double mod2pi(double x)
|
||||
{
|
||||
double v = fmod(x, twopi);
|
||||
if (v < -pi)
|
||||
v += twopi;
|
||||
else
|
||||
if (v > pi)
|
||||
v -= twopi;
|
||||
return v;
|
||||
}
|
||||
inline void polar(double x, double y, double &r, double &theta)
|
||||
{
|
||||
r = sqrt(x*x + y*y);
|
||||
theta = atan2(y, x);
|
||||
}
|
||||
inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
|
||||
{
|
||||
double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
|
||||
double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
|
||||
tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
|
||||
omega = mod2pi(tau - u + v - phi) ;
|
||||
}
|
||||
|
||||
// formula 8.1 in Reeds-Shepp paper
|
||||
inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
|
||||
{
|
||||
polar(x - sin(phi), y - 1. + cos(phi), u, t);
|
||||
if (t >= -ZERO)
|
||||
{
|
||||
v = mod2pi(phi - t);
|
||||
if (v >= -ZERO)
|
||||
{
|
||||
assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
|
||||
assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
|
||||
assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// formula 8.2
|
||||
inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
|
||||
{
|
||||
double t1, u1;
|
||||
polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
|
||||
u1 = u1*u1;
|
||||
if (u1 >= 4.)
|
||||
{
|
||||
double theta;
|
||||
u = sqrt(u1 - 4.);
|
||||
theta = atan2(2., u);
|
||||
t = mod2pi(t1 + theta);
|
||||
v = mod2pi(t - phi);
|
||||
assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
|
||||
assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
|
||||
assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
|
||||
return t>=-ZERO && v>=-ZERO;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
|
||||
{
|
||||
double t, u, v, Lmin = path.length(), L;
|
||||
if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
|
||||
if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
|
||||
}
|
||||
// formula 8.3 / 8.4 *** TYPO IN PAPER ***
|
||||
inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
|
||||
{
|
||||
double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
|
||||
polar(xi, eta, u1, theta);
|
||||
if (u1 <= 4.)
|
||||
{
|
||||
u = -2.*asin(.25 * u1);
|
||||
t = mod2pi(theta + .5 * u + pi);
|
||||
v = mod2pi(phi - t + u);
|
||||
assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
|
||||
assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
|
||||
assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
|
||||
return t>=-ZERO && u<=ZERO;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
|
||||
{
|
||||
double t, u, v, Lmin = path.length(), L;
|
||||
if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
|
||||
// backwards
|
||||
double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
|
||||
if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
|
||||
}
|
||||
// formula 8.7
|
||||
inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
|
||||
{
|
||||
double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
|
||||
if (rho <= 1.)
|
||||
{
|
||||
u = acos(rho);
|
||||
tauOmega(u, -u, xi, eta, phi, t, v);
|
||||
assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
|
||||
assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
|
||||
assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
|
||||
return t>=-ZERO && v<=ZERO;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// formula 8.8
|
||||
inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
|
||||
{
|
||||
double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
|
||||
if (rho>=0 && rho<=1)
|
||||
{
|
||||
u = -acos(rho);
|
||||
if (u >= -.5 * pi)
|
||||
{
|
||||
tauOmega(u, u, xi, eta, phi, t, v);
|
||||
assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
|
||||
assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
|
||||
assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
|
||||
return t>=-ZERO && v>=-ZERO;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
|
||||
{
|
||||
double t, u, v, Lmin = path.length(), L;
|
||||
if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
|
||||
if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
|
||||
}
|
||||
// formula 8.9
|
||||
inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
|
||||
{
|
||||
double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
|
||||
polar(xi, eta, rho, theta);
|
||||
if (rho >= 2.)
|
||||
{
|
||||
double r = sqrt(rho*rho - 4.);
|
||||
u = 2. - r;
|
||||
t = mod2pi(theta + atan2(r, -2.));
|
||||
v = mod2pi(phi - .5*pi - t);
|
||||
assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
|
||||
assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
|
||||
assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
|
||||
return t>=-ZERO && u<=ZERO && v<=ZERO;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// formula 8.10
|
||||
inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
|
||||
{
|
||||
double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
|
||||
polar(-eta, xi, rho, theta);
|
||||
if (rho >= 2.)
|
||||
{
|
||||
t = theta;
|
||||
u = 2. - rho;
|
||||
v = mod2pi(t + .5*pi - phi);
|
||||
assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
|
||||
assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
|
||||
assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
|
||||
return t>=-ZERO && u<=ZERO && v<=ZERO;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
|
||||
{
|
||||
double t, u, v, Lmin = path.length() - .5*pi, L;
|
||||
if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
|
||||
if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
|
||||
// backwards
|
||||
double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
|
||||
if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
|
||||
Lmin = L;
|
||||
}
|
||||
|
||||
if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
|
||||
}
|
||||
// formula 8.11 *** TYPO IN PAPER ***
|
||||
inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
|
||||
{
|
||||
double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
|
||||
polar(xi, eta, rho, theta);
|
||||
if (rho >= 2.)
|
||||
{
|
||||
u = 4. - sqrt(rho*rho - 4.);
|
||||
if (u <= ZERO)
|
||||
{
|
||||
t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
|
||||
v = mod2pi(t - phi);
|
||||
assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
|
||||
assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
|
||||
assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
|
||||
return t>=-ZERO && v>=-ZERO;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
|
||||
{
|
||||
double t, u, v, Lmin = path.length() - pi, L;
|
||||
if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
|
||||
{
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
|
||||
Lmin = L;
|
||||
}
|
||||
if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
|
||||
path = ReedsSheppStateSpace::ReedsSheppPath(
|
||||
ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
|
||||
}
|
||||
|
||||
ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
|
||||
{
|
||||
ReedsSheppStateSpace::ReedsSheppPath path;
|
||||
CSC(x, y, phi, path);
|
||||
CCC(x, y, phi, path);
|
||||
CCCC(x, y, phi, path);
|
||||
CCSC(x, y, phi, path);
|
||||
CCSCC(x, y, phi, path);
|
||||
return path;
|
||||
}
|
||||
}
|
||||
|
||||
const ReedsSheppStateSpace::ReedsSheppPathSegmentType
|
||||
ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
|
||||
{ RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 0
|
||||
{ RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP }, // 1
|
||||
{ RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 2
|
||||
{ RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP }, // 3
|
||||
{ RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 4
|
||||
{ RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 5
|
||||
{ RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 6
|
||||
{ RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 7
|
||||
{ RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 8
|
||||
{ RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 9
|
||||
{ RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 10
|
||||
{ RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 11
|
||||
{ RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 12
|
||||
{ RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 13
|
||||
{ RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 14
|
||||
{ RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 15
|
||||
{ RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT }, // 16
|
||||
{ RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT } // 17
|
||||
};
|
||||
|
||||
ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
|
||||
double t, double u, double v, double w, double x)
|
||||
: type_(type)
|
||||
{
|
||||
length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
|
||||
totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
|
||||
}
|
||||
|
||||
|
||||
double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
|
||||
{
|
||||
return rho_ * reedsShepp(q0, q1).length();
|
||||
}
|
||||
|
||||
ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
|
||||
{
|
||||
double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
|
||||
double c = cos(q0[2]), s = sin(q0[2]);
|
||||
double x = c*dx + s*dy, y = -s*dx + c*dy;
|
||||
return ::reedsShepp(x/rho_, y/rho_, dth);
|
||||
}
|
||||
|
||||
std::vector<ReedsSheppStateSpace::ReedsSheppPathSegmentType> ReedsSheppStateSpace::type(double q0[3], double q1[3])
|
||||
{
|
||||
ReedsSheppPath path = reedsShepp(q0, q1);
|
||||
std::vector<ReedsSheppStateSpace::ReedsSheppPathSegmentType> type_list;
|
||||
for (int i=0;i<5;++i)
|
||||
type_list.push_back(path.type_[i]);
|
||||
return type_list;
|
||||
}
|
||||
|
||||
void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, double &length, std::vector<std::vector<double> > &points)
|
||||
{
|
||||
ReedsSheppPath path = reedsShepp(q0, q1);
|
||||
length = rho_ * path.length();
|
||||
|
||||
for (double seg=0.0; seg<=length; seg+=step_size) {
|
||||
double qnew[3] = {};
|
||||
interpolate(q0, path, seg/rho_, qnew);
|
||||
std::vector<double> v(qnew, qnew + sizeof qnew / sizeof qnew[0]);
|
||||
points.push_back(v);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[3])
|
||||
{
|
||||
|
||||
if (seg < 0.0) seg = 0.0;
|
||||
if (seg > path.length()) seg = path.length();
|
||||
|
||||
double phi, v;
|
||||
|
||||
s[0] = s[1] = 0.0;
|
||||
s[2] = q0[2];
|
||||
|
||||
for (unsigned int i=0; i<5 && seg>0; ++i)
|
||||
{
|
||||
if (path.length_[i]<0)
|
||||
{
|
||||
v = std::max(-seg, path.length_[i]);
|
||||
seg += v;
|
||||
}
|
||||
else
|
||||
{
|
||||
v = std::min(seg, path.length_[i]);
|
||||
seg -= v;
|
||||
}
|
||||
phi = s[2];
|
||||
switch(path.type_[i])
|
||||
{
|
||||
case RS_LEFT:
|
||||
s[0] += ( sin(phi+v) - sin(phi));
|
||||
s[1] += (-cos(phi+v) + cos(phi));
|
||||
s[2] = phi + v;
|
||||
break;
|
||||
case RS_RIGHT:
|
||||
s[0] += (-sin(phi-v) + sin(phi));
|
||||
s[1] += ( cos(phi-v) - cos(phi));
|
||||
s[2] = phi - v;
|
||||
break;
|
||||
case RS_STRAIGHT:
|
||||
s[0] += (v * cos(phi));
|
||||
s[1] += (v * sin(phi));
|
||||
break;
|
||||
case RS_NOP:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
s[0] = s[0] * rho_ + q0[0];
|
||||
s[1] = s[1] * rho_ + q0[1];
|
||||
}
|
||||
@ -0,0 +1,133 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* BSD 3-Clause License
|
||||
*
|
||||
* Copyright (c) 2021, dengpw
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1 Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2 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.
|
||||
* 3 Neither the name of the copyright holder 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: dengpw
|
||||
*********************************************************************/
|
||||
#include "node2d.h"
|
||||
#include <iostream>
|
||||
namespace hybrid_astar_planner {
|
||||
|
||||
float Node2D::calcG(Node2D const *partent) {
|
||||
float g;
|
||||
if((abs(x - partent->x) + abs(y - partent->y)) == 2) {
|
||||
g = 1.4142;
|
||||
}
|
||||
else g = 1;
|
||||
return g + partent->getG() + 0.1*cost;
|
||||
}
|
||||
|
||||
float Node2D::calcH(Node2D const *goal) {
|
||||
float dx, dy;
|
||||
dx = abs(x - goal->x);
|
||||
dy = abs(y - goal->y);
|
||||
h = dx + dy;
|
||||
}
|
||||
|
||||
std::unordered_map<int, std::shared_ptr<Node2D>> GridSearch::GenerateDpMap(
|
||||
const double goal_x, const double goal_y,
|
||||
costmap_2d::Costmap2D* costmap) {
|
||||
const unsigned char* charMap = costmap->getCharMap();
|
||||
int counter = 0;
|
||||
int cells_x, cells_y;
|
||||
cells_x = costmap->getSizeInCellsX();
|
||||
cells_y = costmap->getSizeInCellsY();
|
||||
float resolution = costmap->getResolution();
|
||||
std::priority_queue<std::pair<int, double>,
|
||||
std::vector<std::pair<int, double>>, cmp>
|
||||
open_pq;
|
||||
std::unordered_map<int, std::shared_ptr<Node2D>> open_set;
|
||||
std::unordered_map<int, std::shared_ptr<Node2D>> dp_map_;
|
||||
std::shared_ptr<Node2D> goal_node = std::make_shared<Node2D>(goal_x, goal_y);
|
||||
goal_node->setX( goal_x );
|
||||
goal_node->setY( goal_y );
|
||||
goal_node->setG(0);
|
||||
open_set.emplace(goal_node->getindex(cells_x), goal_node );
|
||||
open_pq.emplace(goal_node->getindex(cells_x), goal_node->getG());
|
||||
while(!open_pq.empty()) {
|
||||
++counter;
|
||||
int id = open_pq.top().first;
|
||||
open_pq.pop();
|
||||
std::shared_ptr<Node2D> current_node = open_set[id];
|
||||
dp_map_.emplace(current_node->getindex(cells_x), current_node);
|
||||
std::vector<std::shared_ptr<Node2D>> adjacent_nodes =
|
||||
getAdjacentPoints(cells_x, cells_y, charMap, current_node );
|
||||
for (std::vector<std::shared_ptr<Node2D>>::iterator
|
||||
it = adjacent_nodes.begin(); it != adjacent_nodes.end(); ++it) {
|
||||
std::shared_ptr<Node2D> next_node = *it;
|
||||
if (dp_map_.find(next_node->getindex(cells_x)) != dp_map_.end()) {
|
||||
continue;
|
||||
}
|
||||
if (open_set.find(next_node->getindex(cells_x)) != open_set.end()) {
|
||||
if (open_set[next_node->getindex(cells_x)]->getG() > next_node->getG()) {
|
||||
open_set[next_node->getindex(cells_x)]->setCost(next_node->getG());
|
||||
open_set[next_node->getindex(cells_x)]->setPerd_(current_node);
|
||||
}
|
||||
} else {
|
||||
// ++counter;
|
||||
next_node->setPerd_(current_node);
|
||||
open_set.emplace(next_node->getindex(cells_x), next_node );
|
||||
open_pq.emplace(next_node->getindex(cells_x), next_node->getG());
|
||||
}
|
||||
}
|
||||
}
|
||||
// printf("X %d y %d explored node num is : %d \n", cells_x, cells_y, counter);
|
||||
return dp_map_;
|
||||
}
|
||||
|
||||
std::vector<std::shared_ptr<Node2D>> GridSearch::getAdjacentPoints(int cells_x,
|
||||
int cells_y, const unsigned char* charMap, std::shared_ptr<Node2D> point) {
|
||||
std::vector<std::shared_ptr<Node2D>> adjacentNodes;
|
||||
float g = 0;
|
||||
// std::cout << "the cost-so-far of this node" << point->getG() << std::endl;
|
||||
for (int x = point->getX() - 1; x <= point->getX() + 1; ++x) {
|
||||
for (int y = point->getY() - 1; y <= point->getY() + 1; ++y) {
|
||||
if (charMap[x + y* cells_x] < 250 &&x < cells_x && y < cells_y
|
||||
&& x > 0 && y > 0) {
|
||||
std::shared_ptr<Node2D> node = std::make_shared<Node2D>(x,y);
|
||||
if((abs(x - point->getX()) + abs(y - point->getY())) == 2) {
|
||||
g = 1.4142;
|
||||
} else {
|
||||
g = 1;
|
||||
}
|
||||
node->SetPathCost(point->getG() + 0.1*charMap[x + y* cells_x] + g);
|
||||
node->setX(x);
|
||||
node->setY(y);
|
||||
adjacentNodes.emplace_back(node);
|
||||
}
|
||||
}
|
||||
}//end of for
|
||||
return adjacentNodes;
|
||||
}
|
||||
|
||||
}//namespace hybrid_astar_planner
|
||||
@ -0,0 +1,139 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* BSD 3-Clause License
|
||||
*
|
||||
* Copyright (c) 2021, dengpw
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1 Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2 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.
|
||||
* 3 Neither the name of the copyright holder 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: dengpw
|
||||
*********************************************************************/
|
||||
#include "planner_core.h"
|
||||
#include "node3d.h"
|
||||
#include <tf/transform_datatypes.h>
|
||||
namespace hybrid_astar_planner {
|
||||
|
||||
void HybridAStarPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
|
||||
if (!initialized_) {
|
||||
ROS_ERROR(
|
||||
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
|
||||
return;
|
||||
}
|
||||
//create a message for the plan
|
||||
geometry_msgs::PoseStamped transform_path;
|
||||
nav_msgs::Path gui_path;
|
||||
int size = path.size();
|
||||
gui_path.poses.resize(size);
|
||||
|
||||
gui_path.header.frame_id = frame_id_;
|
||||
gui_path.header.stamp = ros::Time::now();
|
||||
|
||||
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
|
||||
for (unsigned int i = 0; i < size; i++) {
|
||||
transform_path.pose.position = path[i].pose.position;
|
||||
gui_path.poses[i] = transform_path;//
|
||||
}
|
||||
|
||||
plan_pub_.publish(gui_path);
|
||||
// ROS_INFO("Publish the path to Rviz");
|
||||
|
||||
}//end of publishPlan
|
||||
|
||||
|
||||
void HybridAStarPlanner::publishPathNodes(const std::vector<geometry_msgs::PoseStamped>& path) {
|
||||
if (!initialized_) {
|
||||
ROS_ERROR(
|
||||
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
|
||||
return;
|
||||
}
|
||||
visualization_msgs::Marker pathVehicle;
|
||||
int nodeSize = path.size();
|
||||
pathVehicle.header.stamp = ros::Time(0);
|
||||
pathVehicle.color.r = 52.f / 255.f;
|
||||
pathVehicle.color.g = 250.f / 255.f;
|
||||
pathVehicle.color.b = 52.f / 255.f;
|
||||
pathVehicle.type = visualization_msgs::Marker::ARROW;
|
||||
pathVehicle.header.frame_id = frame_id_;
|
||||
pathVehicle.scale.x = 0.22;
|
||||
pathVehicle.scale.y = 0.18;
|
||||
pathVehicle.scale.z = 0.12;
|
||||
pathVehicle.color.a = 0.1;
|
||||
// 转化节点，并同时加上时间戳等信息
|
||||
for(int i = 0; i<nodeSize; i++) {
|
||||
pathVehicle.header.stamp = ros::Time(0);
|
||||
pathVehicle.pose = path[i].pose;
|
||||
pathVehicle.id = i;
|
||||
pathNodes.markers.push_back(pathVehicle);
|
||||
}
|
||||
// 发布这些车辆位置标记点
|
||||
path_vehicles_pub_.publish(pathNodes);
|
||||
|
||||
}//end of publishPathNodes
|
||||
|
||||
void HybridAStarPlanner::clearPathNodes() {
|
||||
// 初始化并配置节点为全清空模式
|
||||
visualization_msgs::Marker node;
|
||||
pathNodes.markers.clear();
|
||||
node.action = visualization_msgs::Marker::DELETEALL;
|
||||
node.header.frame_id = frame_id_;
|
||||
node.header.stamp = ros::Time(0);
|
||||
node.id = 0;
|
||||
node.action = 3;
|
||||
pathNodes.markers.push_back(node);
|
||||
path_vehicles_pub_.publish(pathNodes);
|
||||
// ROS_INFO("Clean the path nodes");
|
||||
}
|
||||
|
||||
void publishSearchNodes(Node3D node,ros::Publisher& pub,
|
||||
visualization_msgs::MarkerArray& pathNodes, int i) {
|
||||
visualization_msgs::Marker pathVehicle;
|
||||
pathVehicle.header.stamp = ros::Time(0);
|
||||
pathVehicle.color.r = 250.f / 255.f;
|
||||
pathVehicle.color.g = 250.f / 255.f;
|
||||
pathVehicle.color.b = 52.f / 255.f;
|
||||
pathVehicle.type = visualization_msgs::Marker::ARROW;
|
||||
pathVehicle.header.frame_id = "map";
|
||||
pathVehicle.scale.x = 0.22;
|
||||
pathVehicle.scale.y = 0.18;
|
||||
pathVehicle.scale.z = 0.12;
|
||||
pathVehicle.color.a = 0.1;
|
||||
// 转化节点，并同时加上时间戳等信息
|
||||
pathVehicle.header.stamp = ros::Time(0);
|
||||
pathVehicle.pose.position.x = node.getX();
|
||||
pathVehicle.pose.position.y = node.getY();
|
||||
pathVehicle.pose.position.z = 0;
|
||||
pathVehicle.pose.orientation = tf::createQuaternionMsgFromYaw(node.getT());
|
||||
pathVehicle.id = i;
|
||||
pathNodes.markers.push_back(pathVehicle);
|
||||
|
||||
// 发布这些车辆位置标记点
|
||||
pub.publish(pathNodes);
|
||||
|
||||
}//end of publishPathNodes
|
||||
}
|
||||
@ -0,0 +1,221 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Map2
|
||||
- /Path1
|
||||
- /MarkerArray3
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 300
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
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.699999988079071
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /map
|
||||
Unreliable: false
|
||||
Use Timestamp: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Arrow
|
||||
Topic: /move_base_simple/goal
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Pose
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Arrow
|
||||
Topic: /move_base_simple/start
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/MarkerArray
|
||||
Enabled: true
|
||||
Marker Topic: /sPathNodes
|
||||
Name: MarkerArray
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
- Class: rviz/MarkerArray
|
||||
Enabled: true
|
||||
Marker Topic: /sPathVehicle
|
||||
Name: MarkerArray
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
- Class: rviz/Axes
|
||||
Enabled: true
|
||||
Length: 1
|
||||
Name: Axes
|
||||
Radius: 0.10000000149011612
|
||||
Reference Frame: base_link
|
||||
Value: true
|
||||
- Alpha: 0.699999988079071
|
||||
Class: rviz/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /move_base/global_costmap/costmap
|
||||
Unreliable: false
|
||||
Use Timestamp: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /move_base/HybridAStarPlanner/plan
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/MarkerArray
|
||||
Enabled: true
|
||||
Marker Topic: /move_base/HybridAStarPlanner/pathVehicle
|
||||
Name: MarkerArray
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
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
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Angle: 0.019999396055936813
|
||||
Class: rviz/TopDownOrtho
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Scale: 78.33514404296875
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: TopDownOrtho (rviz)
|
||||
X: 2.5319337844848633
|
||||
Y: 1.5114061832427979
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 597
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000001b7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000001b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e40000003efc0100000002fb0000000800540069006d00650100000000000004e4000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000273000001b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1252
|
||||
X: 317
|
||||
Y: 27
|
||||
@ -0,0 +1,18 @@
|
||||
<launch>
|
||||
<node name="move_base" pkg="hybrid_astar_planner" type="test_planner" output="screen">
|
||||
<!-- <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> -->
|
||||
<rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
|
||||
<rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
|
||||
<rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/local_costmap_params.yaml" command="load" />
|
||||
<rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/global_costmap_params.yaml" command="load" />
|
||||
<rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/base_local_planner_params.yaml" command="load" />
|
||||
<rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/move_base_params.yaml" command="load" />
|
||||
<rosparam file="$(find hybrid_astar_planner)/test_the_plugin/param/plugins.yaml" command="load" ns="/costmap_node/costmap"/>
|
||||
<param name="base_global_planner" value="global_planner/GlobalPlanner" /><!--这个标志是选择使用特定的全局规划器-->
|
||||
<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
|
||||
<param name="use_hybrid_astar" value="true" />
|
||||
</node>
|
||||
<node name="tf_broadcaster" pkg="hybrid_astar_planner" type="tf_test_broadcaster" output="screen" />
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find hybrid_astar_planner)/test_the_plugin/maps/mymap2077.yaml" />
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find hybrid_astar_planner)/test_the_plugin/launch/config.rviz" />
|
||||
</launch>
|
||||
@ -0,0 +1,8 @@
|
||||
image: map_demo.png
|
||||
resolution: 1
|
||||
origin: [0.0, 0.0, 0.0]
|
||||
occupied_thresh: 0.1
|
||||
free_thresh: 0.05
|
||||
negate: 0
|
||||
|
||||
|
||||
|
After Width: | Height: | Size: 584 B |
File diff suppressed because one or more lines are too long
@ -0,0 +1,7 @@
|
||||
image: mm.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-1.000000, -12.200000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -0,0 +1,7 @@
|
||||
image: mymap.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-50.000000, -50.000000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -0,0 +1,7 @@
|
||||
image: mymap2077.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-7.000000, -7.000000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -0,0 +1,7 @@
|
||||
image: mymap2088.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-7.000000, -7.000000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -0,0 +1,7 @@
|
||||
image: mymap4.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-50.000000, -50.000000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -0,0 +1,7 @@
|
||||
image: mymap5.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-12.200000, -1.000000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
||||
@ -0,0 +1,22 @@
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.5
|
||||
# footprint: [[-0.08, -0.15], [-0.08, 0.15],[0.42, 0.15], [0.42, -0.15]]
|
||||
robot_radius: 0.1
|
||||
inflation_radius: 0.5
|
||||
transform_tolerance: 0.6
|
||||
observation_sources: scan
|
||||
# obstacles:
|
||||
# observation_sources: scan
|
||||
# scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
|
||||
map:
|
||||
data_type: LaserScan
|
||||
topic: /map
|
||||
marking: true
|
||||
clearing: true
|
||||
scan:
|
||||
data_type: LaserScan
|
||||
topic: scan
|
||||
marking: true
|
||||
clearing: true
|
||||
|
||||
map_type: costmap
|
||||
@ -0,0 +1,23 @@
|
||||
这些参数设置了代价图中的障碍物信息的阈值。
|
||||
obstacle_range: 2.5 #此参数确定了最大范围传感器读数，在此范围内的障碍物将会被标记并放入代价图中
|
||||
raytrace_range: 3.5 #这个范围内不存在的障碍物都会被清除
|
||||
# footprint: [[-0.08, -0.15], [-0.08, 0.15],[0.42, 0.15], [0.42, -0.15]]
|
||||
robot_radius: 0.15
|
||||
inflation_radius: 0.8
|
||||
transform_tolerance: 0.6
|
||||
observation_sources: scan
|
||||
# obstacles:
|
||||
# observation_sources: scan
|
||||
# scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
|
||||
map:
|
||||
data_type: LaserScan
|
||||
topic: /map
|
||||
marking: true
|
||||
clearing: true
|
||||
scan:
|
||||
data_type: LaserScan
|
||||
topic: scan
|
||||
marking: true
|
||||
clearing: true
|
||||
|
||||
map_type: costmap
|
||||
@ -0,0 +1,16 @@
|
||||
global_costmap:
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 0.5
|
||||
static_map: true
|
||||
transform_tolerance: 0.5
|
||||
cost_scaling_factor: 10.0
|
||||
# inflation_radius: 0.4
|
||||
resolution: 1.0
|
||||
plugins:
|
||||
- {name: static_map, type: "costmap_2d::StaticLayer"}
|
||||
- {name: obstacles, type: "costmap_2d::VoxelLayer"}
|
||||
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
|
||||
|
||||
|
||||
@ -0,0 +1,13 @@
|
||||
local_costmap:
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
static_map: false
|
||||
rolling_window: true
|
||||
width: 5
|
||||
height: 5
|
||||
resolution: 0.05
|
||||
transform_tolerance: 0.5
|
||||
cost_scaling_factor: 5
|
||||
# inflation_radius: 0.3
|
||||
@ -0,0 +1,13 @@
|
||||
shutdown_costmaps: false
|
||||
|
||||
controller_frequency: 5.0
|
||||
controller_patience: 3.0
|
||||
|
||||
planner_frequency: 0.5
|
||||
planner_patience: 5.0
|
||||
|
||||
oscillation_timeout: 10.0
|
||||
oscillation_distance: 0.2
|
||||
|
||||
conservative_reset_dist: 0.1
|
||||
|
||||
@ -0,0 +1,5 @@
|
||||
plugins: []
|
||||
plugins:
|
||||
- {name: static_map, type: "costmap_2d::StaticLayer"}
|
||||
- {name: obstacles, type: "costmap_2d::VoxelLayer"}
|
||||
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
|
||||
Loading…
Reference in new issue