混合A*源码解读（c++）
基于ros中通过slam建立的栅格地图，使用混合A*进行路径规划。首先是run_hybrid_astar.cpp:#include "hybrid_a_star/hybrid_a_star_flow.h"#include "3rd/backward.hpp"#include <ros/ros.h>namespace backward {backward::SignalHandling
·
源码reference
基于ros中通过slam建立的栅格地图，使用混合A*进行路径规划。
首先是run_hybrid_astar.cpp:
#include "hybrid_a_star/hybrid_a_star_flow.h"
#include "3rd/backward.hpp"
#include <ros/ros.h>
namespace backward {
backward::SignalHandling sh;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "run_hybrid_astar");
ros::NodeHandle node_handle("~");
HybridAStarFlow kinodynamic_astar_flow(node_handle);
ros::Rate rate(10);
while (ros::ok()) {
kinodynamic_astar_flow.Run();
ros::spinOnce();
rate.sleep();
}
ros::shutdown();
return 0;
}
创建了一个实例kinodynamic_astar_flow，以固定频率运行算法并处理ROS事件。
hybrid_a_star.cpp:
#include "hybrid_a_star/hybrid_a_star.h"
#include "hybrid_a_star/display_tools.h"
#include "hybrid_a_star/timer.h"
#include "hybrid_a_star/trajectory_optimizer.h"
#include <iostream>
// 参数分别为：steering_angle: 最大转向角度，转化为弧度使用。
// steering_angle_discrete_num: 转向角度的离散数量，控制转向角度的分辨率。
// segment_length: 从一个节点到另一个节点的路径段长度。
// segment_length_discrete_num: 路径段长度的离散数量，影响路径的平滑度和搜索的精度。
// wheel_base: 车辆的轴距，用于计算车辆转弯时的动态约束。
// steering_penalty, reversing_penalty, steering_change_penalty: 分别对应转向、倒车和转向变化的惩罚因子，用于在成本计算中对这些行为进行惩罚，以生成更符合实际驾驶情况的路径。
// shot_distance: 射线距离，用于在距离目标状态一定范围内时，采用解析解法（Reeds-Shepp路径）直接连接起点和终点。
HybridAStar::HybridAStar(double steering_angle, int steering_angle_discrete_num, double segment_length,
int segment_length_discrete_num, double wheel_base, double steering_penalty,
double reversing_penalty, double steering_change_penalty, double shot_distance,
int grid_size_phi) {
wheel_base_ = wheel_base;
segment_length_ = segment_length;
steering_radian_ = steering_angle * M_PI / 180.0; // angle to radian
steering_discrete_num_ = steering_angle_discrete_num;
steering_radian_step_size_ = steering_radian_ / steering_discrete_num_;
move_step_size_ = segment_length / segment_length_discrete_num;
segment_length_discrete_num_ = static_cast<int>(segment_length_discrete_num);
steering_penalty_ = steering_penalty;
steering_change_penalty_ = steering_change_penalty;
reversing_penalty_ = reversing_penalty;
shot_distance_ = shot_distance;
CHECK_EQ(static_cast<float>(segment_length_discrete_num_ * move_step_size_), static_cast<float>(segment_length_))
<< "The segment length must be divisible by the step size. segment_length: "
<< segment_length_ << " | step_size: " << move_step_size_;
// rs曲线的计算
rs_path_ptr_ = std::make_shared<RSPath>(wheel_base_ / std::tan(steering_radian_));
tie_breaker_ = 1.0 + 1e-3;
STATE_GRID_SIZE_PHI_ = grid_size_phi;
ANGULAR_RESOLUTION_ = 360.0 / STATE_GRID_SIZE_PHI_ * M_PI / 180.0;
}
HybridAStar::~HybridAStar() {
ReleaseMemory();
}
// Init 函数:
// 初始化地图参数，包括地图的边界和分辨率。
// 初始化状态节点地图，用于记录搜索过程中的每个状态。
// 状态节点图和栅格图，需要根据接收到的地图信息进行重新处理
void HybridAStar::Init(double x_lower, double x_upper, double y_lower, double y_upper,
double state_grid_resolution, double map_grid_resolution) {
SetVehicleShape(4.7, 2.0, 1.3); // 车辆参数，给定车辆长度宽度和后轴到车位的距离来定义。
map_x_lower_ = x_lower;
map_x_upper_ = x_upper;
map_y_lower_ = y_lower;
map_y_upper_ = y_upper;
STATE_GRID_RESOLUTION_ = state_grid_resolution;
MAP_GRID_RESOLUTION_ = map_grid_resolution;
STATE_GRID_SIZE_X_ = std::floor((map_x_upper_ - map_x_lower_) / STATE_GRID_RESOLUTION_);
STATE_GRID_SIZE_Y_ = std::floor((map_y_upper_ - map_y_lower_) / STATE_GRID_RESOLUTION_);
MAP_GRID_SIZE_X_ = std::floor((map_x_upper_ - map_x_lower_) / MAP_GRID_RESOLUTION_);
MAP_GRID_SIZE_Y_ = std::floor((map_y_upper_ - map_y_lower_) / MAP_GRID_RESOLUTION_);
if (map_data_) {
delete[] map_data_;
map_data_ = nullptr;
}
map_data_ = new uint8_t[MAP_GRID_SIZE_X_ * MAP_GRID_SIZE_Y_];
if (state_node_map_) {
for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {
if (state_node_map_[i] == nullptr)
continue;
for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {
if (state_node_map_[i][j] == nullptr)
continue;
for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {
if (state_node_map_[i][j][k] != nullptr) {
delete state_node_map_[i][j][k];
state_node_map_[i][j][k] = nullptr;
}
}
delete[] state_node_map_[i][j];
state_node_map_[i][j] = nullptr;
}
delete[] state_node_map_[i];
state_node_map_[i] = nullptr;
}
delete[] state_node_map_;
state_node_map_ = nullptr;
}
state_node_map_ = new StateNode::Ptr **[STATE_GRID_SIZE_X_];
for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {
state_node_map_[i] = new StateNode::Ptr *[STATE_GRID_SIZE_Y_];
for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {
state_node_map_[i][j] = new StateNode::Ptr[STATE_GRID_SIZE_PHI_];
for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {
state_node_map_[i][j][k] = nullptr;
}
}
}
}
// LineCheck 是一个低级函数，用于执行直线上的障碍物检测，通常被CheckCollision调用。
inline bool HybridAStar::LineCheck(double x0, double y0, double x1, double y1) {
bool steep = (std::abs(y1 - y0) > std::abs(x1 - x0));
if (steep) {
std::swap(x0, y0);
std::swap(y1, x1);
}
if (x0 > x1) {
std::swap(x0, x1);
std::swap(y0, y1);
}
auto delta_x = x1 - x0;
auto delta_y = std::abs(y1 - y0);
auto delta_error = delta_y / delta_x;
decltype(delta_x) error = 0;
decltype(delta_x) y_step;
auto yk = y0;
if (y0 < y1) {
y_step = 1;
} else {
y_step = -1;
}
auto N = static_cast<unsigned int>(x1 - x0);
for (unsigned int i = 0; i < N; ++i) {
if (steep) {
if (HasObstacle(Vec2i(yk, x0 + i * 1.0))
|| BeyondBoundary(Vec2d(yk * MAP_GRID_RESOLUTION_,
(x0 + i) * MAP_GRID_RESOLUTION_))
) {
return false;
}
} else {
if (HasObstacle(Vec2i(x0 + i * 1.0, yk))
|| BeyondBoundary(Vec2d((x0 + i) * MAP_GRID_RESOLUTION_,
yk * MAP_GRID_RESOLUTION_))
) {
return false;
}
}
error += delta_error;
if (error >= 0.5) {
yk += y_step;
error = error - 1.0;
}
}
return true;
}
// CheckCollision 用于检测车辆沿着给定路径是否与障碍物发生碰撞。
bool HybridAStar::CheckCollision(const double &x, const double &y, const double &theta) {
Timer timer;
Mat2d R;
R << std::cos(theta), -std::sin(theta),
std::sin(theta), std::cos(theta);
MatXd transformed_vehicle_shape;
transformed_vehicle_shape.resize(8, 1);
for (unsigned int i = 0; i < 4u; ++i) {
transformed_vehicle_shape.block<2, 1>(i * 2, 0)
= R * vehicle_shape_.block<2, 1>(i * 2, 0) + Vec2d(x, y);
}
Vec2i transformed_pt_index_0 = Coordinate2MapGridIndex(
transformed_vehicle_shape.block<2, 1>(0, 0)
);
Vec2i transformed_pt_index_1 = Coordinate2MapGridIndex(
transformed_vehicle_shape.block<2, 1>(2, 0)
);
Vec2i transformed_pt_index_2 = Coordinate2MapGridIndex(
transformed_vehicle_shape.block<2, 1>(4, 0)
);
Vec2i transformed_pt_index_3 = Coordinate2MapGridIndex(
transformed_vehicle_shape.block<2, 1>(6, 0)
);
double y1, y0, x1, x0;
// pt1 -> pt0
x0 = static_cast<double>(transformed_pt_index_0.x());
y0 = static_cast<double>(transformed_pt_index_0.y());
x1 = static_cast<double>(transformed_pt_index_1.x());
y1 = static_cast<double>(transformed_pt_index_1.y());
if (!LineCheck(x1, y1, x0, y0)) {
return false;
}
// pt2 -> pt1
x0 = static_cast<double>(transformed_pt_index_1.x());
y0 = static_cast<double>(transformed_pt_index_1.y());
x1 = static_cast<double>(transformed_pt_index_2.x());
y1 = static_cast<double>(transformed_pt_index_2.y());
if (!LineCheck(x1, y1, x0, y0)) {
return false;
}
// pt3 -> pt2
x0 = static_cast<double>(transformed_pt_index_2.x());
y0 = static_cast<double>(transformed_pt_index_2.y());
x1 = static_cast<double>(transformed_pt_index_3.x());
y1 = static_cast<double>(transformed_pt_index_3.y());
if (!LineCheck(x1, y1, x0, y0)) {
return false;
}
// pt0 -> pt3
x0 = static_cast<double>(transformed_pt_index_0.x());
y0 = static_cast<double>(transformed_pt_index_0.y());
x1 = static_cast<double>(transformed_pt_index_3.x());
y1 = static_cast<double>(transformed_pt_index_3.y());
if (!LineCheck(x0, y0, x1, y1)) {
return false;
}
check_collision_use_time += timer.End();
num_check_collision++;
return true;
}
bool HybridAStar::HasObstacle(const int grid_index_x, const int grid_index_y) const {
return (grid_index_x >= 0 && grid_index_x < MAP_GRID_SIZE_X_
&& grid_index_y >= 0 && grid_index_y < MAP_GRID_SIZE_Y_
&& (map_data_[grid_index_y * MAP_GRID_SIZE_X_ + grid_index_x] == 1));
}
bool HybridAStar::HasObstacle(const Vec2i &grid_index) const {
int grid_index_x = grid_index[0];
int grid_index_y = grid_index[1];
return (grid_index_x >= 0 && grid_index_x < MAP_GRID_SIZE_X_
&& grid_index_y >= 0 && grid_index_y < MAP_GRID_SIZE_Y_
&& (map_data_[grid_index_y * MAP_GRID_SIZE_X_ + grid_index_x] == 1));
}
void HybridAStar::SetObstacle(unsigned int x, unsigned int y) {
if (x < 0u || x > static_cast<unsigned int>(MAP_GRID_SIZE_X_)
|| y < 0u || y > static_cast<unsigned int>(MAP_GRID_SIZE_Y_)) {
return;
}
map_data_[x + y * MAP_GRID_SIZE_X_] = 1;
}
void HybridAStar::SetObstacle(const double pt_x, const double pt_y) {
if (pt_x < map_x_lower_ || pt_x > map_x_upper_ ||
pt_y < map_y_lower_ || pt_y > map_y_upper_) {
return;
}
int grid_index_x = static_cast<int>((pt_x - map_x_lower_) / MAP_GRID_RESOLUTION_);
int grid_index_y = static_cast<int>((pt_y - map_y_lower_) / MAP_GRID_RESOLUTION_);
map_data_[grid_index_x + grid_index_y * MAP_GRID_SIZE_X_] = 1;
}
// 定义车辆形状，用于碰撞检测。车辆被假设为矩形，通过给定车辆长度、宽度和后轴到车尾的距离来定义。
void HybridAStar::SetVehicleShape(double length, double width, double rear_axle_dist) {
vehicle_shape_.resize(8);
vehicle_shape_.block<2, 1>(0, 0) = Vec2d(-rear_axle_dist, width / 2);
vehicle_shape_.block<2, 1>(2, 0) = Vec2d(length - rear_axle_dist, width / 2);
vehicle_shape_.block<2, 1>(4, 0) = Vec2d(length - rear_axle_dist, -width / 2);
vehicle_shape_.block<2, 1>(6, 0) = Vec2d(-rear_axle_dist, -width / 2);
const double step_size = move_step_size_;
const auto N_length = static_cast<unsigned int>(length / step_size);
const auto N_width = static_cast<unsigned int> (width / step_size);
vehicle_shape_discrete_.resize(2, (N_length + N_width) * 2u);
const Vec2d edge_0_normalized = (vehicle_shape_.block<2, 1>(2, 0)
- vehicle_shape_.block<2, 1>(0, 0)).normalized();
for (unsigned int i = 0; i < N_length; ++i) {
vehicle_shape_discrete_.block<2, 1>(0, i + N_length)
= vehicle_shape_.block<2, 1>(4, 0) - edge_0_normalized * i * step_size;
vehicle_shape_discrete_.block<2, 1>(0, i)
= vehicle_shape_.block<2, 1>(0, 0) + edge_0_normalized * i * step_size;
}
const Vec2d edge_1_normalized = (vehicle_shape_.block<2, 1>(4, 0)
- vehicle_shape_.block<2, 1>(2, 0)).normalized();
for (unsigned int i = 0; i < N_width; ++i) {
vehicle_shape_discrete_.block<2, 1>(0, (2 * N_length) + i)
= vehicle_shape_.block<2, 1>(2, 0) + edge_1_normalized * i * step_size;
vehicle_shape_discrete_.block<2, 1>(0, (2 * N_length) + i + N_width)
= vehicle_shape_.block<2, 1>(6, 0) - edge_1_normalized * i * step_size;
}
}
__attribute__((unused)) Vec2d HybridAStar::CoordinateRounding(const Vec2d &pt) const {
return MapGridIndex2Coordinate(Coordinate2MapGridIndex(pt));
}
Vec2d HybridAStar::MapGridIndex2Coordinate(const Vec2i &grid_index) const {
Vec2d pt;
pt.x() = ((double) grid_index[0] + 0.5) * MAP_GRID_RESOLUTION_ + map_x_lower_;
pt.y() = ((double) grid_index[1] + 0.5) * MAP_GRID_RESOLUTION_ + map_y_lower_;
return pt;
}
Vec3i HybridAStar::State2Index(const Vec3d &state) const {
Vec3i index;
index[0] = std::min(std::max(int((state[0] - map_x_lower_) / STATE_GRID_RESOLUTION_), 0), STATE_GRID_SIZE_X_ - 1);
index[1] = std::min(std::max(int((state[1] - map_y_lower_) / STATE_GRID_RESOLUTION_), 0), STATE_GRID_SIZE_Y_ - 1);
index[2] = std::min(std::max(int((state[2] - (-M_PI)) / ANGULAR_RESOLUTION_), 0), STATE_GRID_SIZE_PHI_ - 1);
return index;
}
Vec2i HybridAStar::Coordinate2MapGridIndex(const Vec2d &pt) const {
Vec2i grid_index;
grid_index[0] = int((pt[0] - map_x_lower_) / MAP_GRID_RESOLUTION_);
grid_index[1] = int((pt[1] - map_y_lower_) / MAP_GRID_RESOLUTION_);
return grid_index;
}
// 生成当前节点的所有可能邻居节点，并检查它们是否可行（没有碰撞，没有越界）。
void HybridAStar::GetNeighborNodes(const StateNode::Ptr &curr_node_ptr,
std::vector<StateNode::Ptr> &neighbor_nodes) {
neighbor_nodes.clear();
for (int i = -steering_discrete_num_; i <= steering_discrete_num_; ++i) {
VectorVec3d intermediate_state;
bool has_obstacle = false;
double x = curr_node_ptr->state_.x();
double y = curr_node_ptr->state_.y();
double theta = curr_node_ptr->state_.z();
const double phi = i * steering_radian_step_size_;
// forward
for (int j = 1; j <= segment_length_discrete_num_; j++) {
DynamicModel(move_step_size_, phi, x, y, theta);
intermediate_state.emplace_back(Vec3d(x, y, theta));
if (!CheckCollision(x, y, theta)) {
has_obstacle = true;
break;
}
}
Vec3i grid_index = State2Index(intermediate_state.back());
if (!BeyondBoundary(intermediate_state.back().head(2)) && !has_obstacle) {
auto neighbor_forward_node_ptr = new StateNode(grid_index);
neighbor_forward_node_ptr->intermediate_states_ = intermediate_state;
neighbor_forward_node_ptr->state_ = intermediate_state.back();
neighbor_forward_node_ptr->steering_grade_ = i;
neighbor_forward_node_ptr->direction_ = StateNode::FORWARD;
neighbor_nodes.push_back(neighbor_forward_node_ptr);
}
// backward
has_obstacle = false;
intermediate_state.clear();
x = curr_node_ptr->state_.x();
y = curr_node_ptr->state_.y();
theta = curr_node_ptr->state_.z();
for (int j = 1; j <= segment_length_discrete_num_; j++) {
DynamicModel(-move_step_size_, phi, x, y, theta);
intermediate_state.emplace_back(Vec3d(x, y, theta));
if (!CheckCollision(x, y, theta)) {
has_obstacle = true;
break;
}
}
if (!BeyondBoundary(intermediate_state.back().head(2)) && !has_obstacle) {
grid_index = State2Index(intermediate_state.back());
auto neighbor_backward_node_ptr = new StateNode(grid_index);
neighbor_backward_node_ptr->intermediate_states_ = intermediate_state;
neighbor_backward_node_ptr->state_ = intermediate_state.back();
neighbor_backward_node_ptr->steering_grade_ = i;
neighbor_backward_node_ptr->direction_ = StateNode::BACKWARD;
neighbor_nodes.push_back(neighbor_backward_node_ptr);
}
}
}
// 用于根据车辆动力学模型（如简化的自行车模型）更新车辆状态。
void HybridAStar::DynamicModel(const double &step_size, const double &phi,
double &x, double &y, double &theta) const {
x = x + step_size * std::cos(theta);
y = y + step_size * std::sin(theta);
theta = Mod2Pi(theta + step_size / wheel_base_ * std::tan(phi));
}
double HybridAStar::Mod2Pi(const double &x) {
double v = fmod(x, 2 * M_PI);
if (v < -M_PI) {
v += 2.0 * M_PI;
} else if (v > M_PI) {
v -= 2.0 * M_PI;
}
return v;
}
bool HybridAStar::BeyondBoundary(const Vec2d &pt) const {
return pt.x() < map_x_lower_ || pt.x() > map_x_upper_ || pt.y() < map_y_lower_ || pt.y() > map_y_upper_;
}
// ComputeG 和 ComputeH 函数:
// 分别计算从起始节点到当前节点的实际成本（g成本）和从当前节点到目标节点的启发式成本（h成本）。
// 这两个成本用于评估节点的总成本（f成本 = g成本 + h成本）。
double HybridAStar::ComputeH(const StateNode::Ptr ¤t_node_ptr,
const StateNode::Ptr &terminal_node_ptr) {
double h;
// L2
// h = (current_node_ptr->state_.head(2) - terminal_node_ptr->state_.head(2)).norm();
// L1
h = (current_node_ptr->state_.head(2) - terminal_node_ptr->state_.head(2)).lpNorm<1>();
if (h < 3.0 * shot_distance_) {
h = rs_path_ptr_->Distance(current_node_ptr->state_.x(), current_node_ptr->state_.y(),
current_node_ptr->state_.z(),
terminal_node_ptr->state_.x(), terminal_node_ptr->state_.y(),
terminal_node_ptr->state_.z());
}
return h;
}
double HybridAStar::ComputeG(const StateNode::Ptr ¤t_node_ptr,
const StateNode::Ptr &neighbor_node_ptr) const {
double g;
if (neighbor_node_ptr->direction_ == StateNode::FORWARD) {
if (neighbor_node_ptr->steering_grade_ != current_node_ptr->steering_grade_) {
if (neighbor_node_ptr->steering_grade_ == 0) {
g = segment_length_ * steering_change_penalty_;
} else {
g = segment_length_ * steering_change_penalty_ * steering_penalty_;
}
} else {
if (neighbor_node_ptr->steering_grade_ == 0) {
g = segment_length_;
} else {
g = segment_length_ * steering_penalty_;
}
}
} else {
if (neighbor_node_ptr->steering_grade_ != current_node_ptr->steering_grade_) {
if (neighbor_node_ptr->steering_grade_ == 0) {
g = segment_length_ * steering_change_penalty_ * reversing_penalty_;
} else {
g = segment_length_ * steering_change_penalty_ * steering_penalty_ * reversing_penalty_;
}
} else {
if (neighbor_node_ptr->steering_grade_ == 0) {
g = segment_length_ * reversing_penalty_;
} else {
g = segment_length_ * steering_penalty_ * reversing_penalty_;
}
}
}
return g;
}
// Search 函数:
// 实现混合A*算法的主要搜索逻辑。
// 输入起始状态和目标状态，输出是否找到路径。
// 使用优先队列（基于f成本的多重映射）管理开放集，以保证总是首先扩展最有希望的节点。
bool HybridAStar::Search(const Vec3d &start_state, const Vec3d &goal_state) {
Timer search_used_time;
double neighbor_time = 0.0, compute_h_time = 0.0, compute_g_time = 0.0;
const Vec3i start_grid_index = State2Index(start_state);
const Vec3i goal_grid_index = State2Index(goal_state);
auto goal_node_ptr = new StateNode(goal_grid_index);
goal_node_ptr->state_ = goal_state;
goal_node_ptr->direction_ = StateNode::NO;
goal_node_ptr->steering_grade_ = 0;
auto start_node_ptr = new StateNode(start_grid_index);
start_node_ptr->state_ = start_state;
start_node_ptr->steering_grade_ = 0;
start_node_ptr->direction_ = StateNode::NO;
start_node_ptr->node_status_ = StateNode::IN_OPENSET;
start_node_ptr->intermediate_states_.emplace_back(start_state);
start_node_ptr->g_cost_ = 0.0;
start_node_ptr->f_cost_ = ComputeH(start_node_ptr, goal_node_ptr);
state_node_map_[start_grid_index.x()][start_grid_index.y()][start_grid_index.z()] = start_node_ptr;
state_node_map_[goal_grid_index.x()][goal_grid_index.y()][goal_grid_index.z()] = goal_node_ptr;
openset_.clear();
openset_.insert(std::make_pair(0, start_node_ptr));
std::vector<StateNode::Ptr> neighbor_nodes_ptr;
StateNode::Ptr current_node_ptr;
StateNode::Ptr neighbor_node_ptr;
int count = 0;
while (!openset_.empty()) {
current_node_ptr = openset_.begin()->second;
current_node_ptr->node_status_ = StateNode::IN_CLOSESET;
openset_.erase(openset_.begin());
if ((current_node_ptr->state_.head(2) - goal_node_ptr->state_.head(2)).norm() <= shot_distance_) {
double rs_length = 0.0;
if (AnalyticExpansions(current_node_ptr, goal_node_ptr, rs_length)) {
terminal_node_ptr_ = goal_node_ptr;
StateNode::Ptr grid_node_ptr = terminal_node_ptr_->parent_node_;
while (grid_node_ptr != nullptr) {
grid_node_ptr = grid_node_ptr->parent_node_;
path_length_ = path_length_ + segment_length_;
}
path_length_ = path_length_ - segment_length_ + rs_length;
std::cout << "ComputeH use time(ms): " << compute_h_time << std::endl;
std::cout << "check collision use time(ms): " << check_collision_use_time << std::endl;
std::cout << "GetNeighborNodes use time(ms): " << neighbor_time << std::endl;
std::cout << "average time of check collision(ms): "
<< check_collision_use_time / num_check_collision
<< std::endl;
ROS_INFO("\033[1;32m --> Time in Hybrid A star is %f ms, path length: %f \033[0m\n",
search_used_time.End(), path_length_);
check_collision_use_time = 0.0;
num_check_collision = 0.0;
return true;
}
}
Timer timer_get_neighbor;
GetNeighborNodes(current_node_ptr, neighbor_nodes_ptr);
neighbor_time = neighbor_time + timer_get_neighbor.End();
for (unsigned int i = 0; i < neighbor_nodes_ptr.size(); ++i) {
neighbor_node_ptr = neighbor_nodes_ptr[i];
Timer timer_compute_g;
const double neighbor_edge_cost = ComputeG(current_node_ptr, neighbor_node_ptr);
compute_g_time = compute_g_time + timer_get_neighbor.End();
Timer timer_compute_h;
const double current_h = ComputeH(current_node_ptr, goal_node_ptr) * tie_breaker_;
compute_h_time = compute_h_time + timer_compute_h.End();
const Vec3i &index = neighbor_node_ptr->grid_index_;
if (state_node_map_[index.x()][index.y()][index.z()] == nullptr) {
neighbor_node_ptr->g_cost_ = current_node_ptr->g_cost_ + neighbor_edge_cost;
neighbor_node_ptr->parent_node_ = current_node_ptr;
neighbor_node_ptr->node_status_ = StateNode::IN_OPENSET;
neighbor_node_ptr->f_cost_ = neighbor_node_ptr->g_cost_ + current_h;
openset_.insert(std::make_pair(neighbor_node_ptr->f_cost_, neighbor_node_ptr));
state_node_map_[index.x()][index.y()][index.z()] = neighbor_node_ptr;
continue;
} else if (state_node_map_[index.x()][index.y()][index.z()]->node_status_ == StateNode::IN_OPENSET) {
double g_cost_temp = current_node_ptr->g_cost_ + neighbor_edge_cost;
if (state_node_map_[index.x()][index.y()][index.z()]->g_cost_ > g_cost_temp) {
neighbor_node_ptr->g_cost_ = g_cost_temp;
neighbor_node_ptr->f_cost_ = g_cost_temp + current_h;
neighbor_node_ptr->parent_node_ = current_node_ptr;
neighbor_node_ptr->node_status_ = StateNode::IN_OPENSET;
/// TODO: This will cause a memory leak
//delete state_node_map_[index.x()][index.y()][index.z()];
state_node_map_[index.x()][index.y()][index.z()] = neighbor_node_ptr;
} else {
delete neighbor_node_ptr;
}
continue;
} else if (state_node_map_[index.x()][index.y()][index.z()]->node_status_ == StateNode::IN_CLOSESET) {
delete neighbor_node_ptr;
continue;
}
}
count++;
if (count > 50000000) {
ROS_WARN("Exceeded the number of iterations, the search failed");
return false;
}
}
return false;
}
VectorVec4d HybridAStar::GetSearchedTree() {
VectorVec4d tree;
Vec4d point_pair;
visited_node_number_ = 0;
for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {
for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {
for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {
if (state_node_map_[i][j][k] == nullptr || state_node_map_[i][j][k]->parent_node_ == nullptr) {
continue;
}
const unsigned int number_states = state_node_map_[i][j][k]->intermediate_states_.size() - 1;
for (unsigned int l = 0; l < number_states; ++l) {
point_pair.head(2) = state_node_map_[i][j][k]->intermediate_states_[l].head(2);
point_pair.tail(2) = state_node_map_[i][j][k]->intermediate_states_[l + 1].head(2);
tree.emplace_back(point_pair);
}
point_pair.head(2) = state_node_map_[i][j][k]->intermediate_states_[0].head(2);
point_pair.tail(2) = state_node_map_[i][j][k]->parent_node_->state_.head(2);
tree.emplace_back(point_pair);
visited_node_number_++;
}
}
}
return tree;
}
// 清理动态分配的内存，避免内存泄漏。
void HybridAStar::ReleaseMemory() {
if (map_data_ != nullptr) {
delete[] map_data_;
map_data_ = nullptr;
}
if (state_node_map_) {
for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {
if (state_node_map_[i] == nullptr)
continue;
for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {
if (state_node_map_[i][j] == nullptr)
continue;
for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {
if (state_node_map_[i][j][k] != nullptr) {
delete state_node_map_[i][j][k];
state_node_map_[i][j][k] = nullptr;
}
}
delete[] state_node_map_[i][j];
state_node_map_[i][j] = nullptr;
}
delete[] state_node_map_[i];
state_node_map_[i] = nullptr;
}
delete[] state_node_map_;
state_node_map_ = nullptr;
}
terminal_node_ptr_ = nullptr;
}
__attribute__((unused)) double HybridAStar::GetPathLength() const {
return path_length_;
}
VectorVec3d HybridAStar::GetPath() const {
VectorVec3d path;
std::vector<StateNode::Ptr> temp_nodes;
StateNode::Ptr state_grid_node_ptr = terminal_node_ptr_;
while (state_grid_node_ptr != nullptr) {
temp_nodes.emplace_back(state_grid_node_ptr);
state_grid_node_ptr = state_grid_node_ptr->parent_node_;
}
std::reverse(temp_nodes.begin(), temp_nodes.end());
for (const auto &node: temp_nodes) {
path.insert(path.end(), node->intermediate_states_.begin(),
node->intermediate_states_.end());
}
return path;
}
void HybridAStar::Reset() {
if (state_node_map_) {
for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {
if (state_node_map_[i] == nullptr)
continue;
for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {
if (state_node_map_[i][j] == nullptr)
continue;
for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {
if (state_node_map_[i][j][k] != nullptr) {
delete state_node_map_[i][j][k];
state_node_map_[i][j][k] = nullptr;
}
}
}
}
}
path_length_ = 0.0;
terminal_node_ptr_ = nullptr;
}
bool HybridAStar::AnalyticExpansions(const StateNode::Ptr ¤t_node_ptr,
const StateNode::Ptr &goal_node_ptr, double &length) {
VectorVec3d rs_path_poses = rs_path_ptr_->GetRSPath(current_node_ptr->state_,
goal_node_ptr->state_,
move_step_size_, length);
for (const auto &pose: rs_path_poses)
if (BeyondBoundary(pose.head(2)) || !CheckCollision(pose.x(), pose.y(), pose.z())) {
return false;
};
goal_node_ptr->intermediate_states_ = rs_path_poses;
goal_node_ptr->parent_node_ = current_node_ptr;
auto begin = goal_node_ptr->intermediate_states_.begin();
goal_node_ptr->intermediate_states_.erase(begin);
return true;
}
这里面首先定义了构造函数HybridAStar：
steering_angle：最大的转向角度
steering_angle_distance_num:转向角度的离散数量，控制转向角度的分辨率的。
segment_length：一个节点到另一个节点的路径段长度。
segment_length_discrete_num：路径离散长度的离散数量，影响路径平滑度和搜索的精度。
wheel_base：轴距，用于计算转弯的动态约束。
steering_penalty，reversing_penalty，steering_change_panalty，转向，倒车和转向变化的惩罚因子
shot_distance：射线距离，用于在距离目标状态一定范围内时，采用解析法（RS路径）直接连接起点和终点
下一个重要的就是hybrid_a_star_flow.cpp:
#include "hybrid_a_star/hybrid_a_star_flow.h"
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
double Mod2Pi(const double &x) {
double v = fmod(x, 2 * M_PI);
if (v < -M_PI) {
v += 2.0 * M_PI;
} else if (v > M_PI) {
v -= 2.0 * M_PI;
}
return v;
}
HybridAStarFlow::HybridAStarFlow(ros::NodeHandle &nh) {
double steering_angle = nh.param("planner/steering_angle", 10);
int steering_angle_discrete_num = nh.param("planner/steering_angle_discrete_num", 1);
double wheel_base = nh.param("planner/wheel_base", 1.0);
double segment_length = nh.param("planner/segment_length", 1.6);
int segment_length_discrete_num = nh.param("planner/segment_length_discrete_num", 8);
double steering_penalty = nh.param("planner/steering_penalty", 1.05);
double steering_change_penalty = nh.param("planner/steering_change_penalty", 1.5);
double reversing_penalty = nh.param("planner/reversing_penalty", 2.0);
double shot_distance = nh.param("planner/shot_distance", 5.0);
// std::make_shared是什么？
kinodynamic_astar_searcher_ptr_ = std::make_shared<HybridAStar>(
steering_angle, steering_angle_discrete_num, segment_length, segment_length_discrete_num, wheel_base,
steering_penalty, reversing_penalty, steering_change_penalty, shot_distance
);
costmap_sub_ptr_ = std::make_shared<CostMapSubscriber>(nh, "/map", 1);
init_pose_sub_ptr_ = std::make_shared<InitPoseSubscriber2D>(nh, "/initialpose", 1);
goal_pose_sub_ptr_ = std::make_shared<GoalPoseSubscriber2D>(nh, "/move_base_simple/goal", 1);
path_pub_ = nh.advertise<nav_msgs::Path>("searched_path", 1);
searched_tree_pub_ = nh.advertise<visualization_msgs::Marker>("searched_tree", 1);
vehicle_path_pub_ = nh.advertise<visualization_msgs::MarkerArray>("vehicle_path", 1);
has_map_ = false;
}
void HybridAStarFlow::Run() {
ReadData(); // 从订阅的话题中读取成本地图、初始位置、目标位置，并存储到对应的队列中。
// 如果没有地图数据，则直接返回。如果有地图数据，但地图还未初始化，则使用最新的成本地图初始化混合A*搜索器，并设置障碍物。
// if (!has_map_) {
// if (costmap_deque_.empty()) {
// return;
// }
// current_costmap_ptr_ = costmap_deque_.front();
// costmap_deque_.pop_front();
// const double map_resolution = 0.2;
// kinodynamic_astar_searcher_ptr_->Init(
// current_costmap_ptr_->info.origin.position.x,
// 1.0 * current_costmap_ptr_->info.width * current_costmap_ptr_->info.resolution,
// current_costmap_ptr_->info.origin.position.y,
// 1.0 * current_costmap_ptr_->info.height * current_costmap_ptr_->info.resolution,
// current_costmap_ptr_->info.resolution,
// map_resolution
// );
// unsigned int map_w = std::floor(current_costmap_ptr_->info.width / map_resolution);
// unsigned int map_h = std::floor(current_costmap_ptr_->info.height / map_resolution);
// for (unsigned int w = 0; w < map_w; ++w) {
// for (unsigned int h = 0; h < map_h; ++h) {
// auto x = static_cast<unsigned int> ((w + 0.5) * map_resolution
// / current_costmap_ptr_->info.resolution);
// auto y = static_cast<unsigned int> ((h + 0.5) * map_resolution
// / current_costmap_ptr_->info.resolution);
// if (current_costmap_ptr_->data[y * current_costmap_ptr_->info.width + x]) {
// kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);
// }
// }
// }
// has_map_ = true;
// }
// costmap_deque_.clear();
// test 3.8: not finish
// if (!has_map_) {
// if (costmap_deque_.empty()) {
// return;
// }
// current_costmap_ptr_ = costmap_deque_.front();
// costmap_deque_.pop_front();
// // 假定新的分辨率是原始分辨率的一半，例如0.05米/格
// const double new_resolution = 0.02; // 新的更细分辨率
// const double original_resolution = current_costmap_ptr_->info.resolution;
// const double scale_factor = original_resolution / new_resolution; // 计算缩放因子
// // 使用新的分辨率初始化kinodynamic_astar_searcher_ptr_
// kinodynamic_astar_searcher_ptr_->Init(
// current_costmap_ptr_->info.origin.position.x,
// 1.0 * current_costmap_ptr_->info.width * original_resolution, // 保持物理尺寸不变
// current_costmap_ptr_->info.origin.position.y,
// 1.0 * current_costmap_ptr_->info.height * original_resolution, // 保持物理尺寸不变
// new_resolution, // 使用新的分辨率
// new_resolution // 新的地图分辨率也应该是这个值
// );
// unsigned int map_w = std::round(current_costmap_ptr_->info.width * scale_factor);
// unsigned int map_h = std::round(current_costmap_ptr_->info.height * scale_factor);
// for (unsigned int w = 0; w < map_w; ++w) {
// for (unsigned int h = 0; h < map_h; ++h) {
// auto x = static_cast<unsigned int> (w / scale_factor);
// auto y = static_cast<unsigned int> (h / scale_factor);
// if (current_costmap_ptr_->data[y * current_costmap_ptr_->info.width + x]) {
// kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);
// }
// }
// }
// has_map_ = true;
// }
// costmap_deque_.clear();
// test 3.7_1:
if (!has_map_) {
if (costmap_deque_.empty()) {
return;
}
current_costmap_ptr_ = costmap_deque_.front();
costmap_deque_.pop_front();
double map_resolution = current_costmap_ptr_->info.resolution;
kinodynamic_astar_searcher_ptr_->Init(
current_costmap_ptr_->info.origin.position.x,
1.0 * current_costmap_ptr_->info.width * map_resolution,
current_costmap_ptr_->info.origin.position.y,
1.0 * current_costmap_ptr_->info.height * map_resolution,
map_resolution,
map_resolution // 这里同样使用地图的实际分辨率
);
// 使用实际的分辨率计算映射的宽度和高度
unsigned int map_w = current_costmap_ptr_->info.width;
unsigned int map_h = current_costmap_ptr_->info.height;
for (unsigned int w = 0; w < map_w; ++w) {
for (unsigned int h = 0; h < map_h; ++h) {
// 在这里，我们不再需要根据重新计算的分辨率调整索引
// 直接使用原始索引
if (current_costmap_ptr_->data[h * map_w + w]) {
kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);
}
}
}
has_map_ = true;
}
costmap_deque_.clear();
// // test 3.7_2:
// // remap search grid
// if (!has_map_) {
// if (costmap_deque_.empty()) {
// return;
// }
// current_costmap_ptr_ = costmap_deque_.front();
// costmap_deque_.pop_front();
// // const double map_resolution = 0.1; // why not from /map
// double map_resolution = current_costmap_ptr_->info.resolution;
// kinodynamic_astar_searcher_ptr_->Init(
// current_costmap_ptr_->info.origin.position.x,
// 1.0 * current_costmap_ptr_->info.width * current_costmap_ptr_->info.resolution,
// current_costmap_ptr_->info.origin.position.y,
// 1.0 * current_costmap_ptr_->info.height * current_costmap_ptr_->info.resolution,
// current_costmap_ptr_->info.resolution,
// map_resolution
// );
// unsigned int map_w = std::floor(current_costmap_ptr_->info.width / map_resolution);
// unsigned int map_h = std::floor(current_costmap_ptr_->info.height / map_resolution);
// for (unsigned int w = 0; w < map_w; ++w) {
// for (unsigned int h = 0; h < map_h; ++h) {
// auto x = static_cast<unsigned int> ((w + 0.5) * map_resolution
// / current_costmap_ptr_->info.resolution);
// auto y = static_cast<unsigned int> ((h + 0.5) * map_resolution
// / current_costmap_ptr_->info.resolution);
// if (current_costmap_ptr_->data[y * current_costmap_ptr_->info.width + x]) {
// kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);
// }
// }
// }
// has_map_ = true;
// }
// costmap_deque_.clear();
while (HasStartPose() && HasGoalPose()) {
InitPoseData();
double start_yaw = tf::getYaw(current_init_pose_ptr_->pose.pose.orientation);
double goal_yaw = tf::getYaw(current_goal_pose_ptr_->pose.orientation);
Vec3d start_state = Vec3d(
current_init_pose_ptr_->pose.pose.position.x,
current_init_pose_ptr_->pose.pose.position.y,
start_yaw
);
Vec3d goal_state = Vec3d(
current_goal_pose_ptr_->pose.position.x,
current_goal_pose_ptr_->pose.position.y,
goal_yaw
);
if (kinodynamic_astar_searcher_ptr_->Search(start_state, goal_state)) {
auto path = kinodynamic_astar_searcher_ptr_->GetPath();
PublishPath(path);
PublishVehiclePath(path, 4.0, 2.0, 5u);
PublishSearchedTree(kinodynamic_astar_searcher_ptr_->GetSearchedTree());
nav_msgs::Path path_ros;
geometry_msgs::PoseStamped pose_stamped;
for (const auto &pose: path) {
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = pose.x();
pose_stamped.pose.position.y = pose.y();
pose_stamped.pose.position.z = 0.0;
pose_stamped.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, pose.z());
path_ros.poses.emplace_back(pose_stamped);
}
path_ros.header.frame_id = "world";
path_ros.header.stamp = ros::Time::now();
static tf::TransformBroadcaster transform_broadcaster;
for (const auto &pose: path_ros.poses) {
tf::Transform transform;
transform.setOrigin(tf::Vector3(pose.pose.position.x, pose.pose.position.y, 0.0));
tf::Quaternion q;
q.setX(pose.pose.orientation.x);
q.setY(pose.pose.orientation.y);
q.setZ(pose.pose.orientation.z);
q.setW(pose.pose.orientation.w);
transform.setRotation(q);
transform_broadcaster.sendTransform(tf::StampedTransform(transform,
ros::Time::now(), "world",
"ground_link")
);
ros::Duration(0.05).sleep();
}
}
// debug
// std::cout << "visited nodes: " << kinodynamic_astar_searcher_ptr_->GetVisitedNodesNumber() << std::endl;
kinodynamic_astar_searcher_ptr_->Reset();
}
}
void HybridAStarFlow::ReadData() {
costmap_sub_ptr_->ParseData(costmap_deque_);
init_pose_sub_ptr_->ParseData(init_pose_deque_);
goal_pose_sub_ptr_->ParseData(goal_pose_deque_);
}
void HybridAStarFlow::InitPoseData() {
current_init_pose_ptr_ = init_pose_deque_.front();
init_pose_deque_.pop_front();
current_goal_pose_ptr_ = goal_pose_deque_.front();
goal_pose_deque_.pop_front();
}
bool HybridAStarFlow::HasGoalPose() {
return !goal_pose_deque_.empty();
}
bool HybridAStarFlow::HasStartPose() {
return !init_pose_deque_.empty();
}
void HybridAStarFlow::PublishPath(const VectorVec3d &path) {
nav_msgs::Path nav_path;
geometry_msgs::PoseStamped pose_stamped;
for (const auto &pose: path) {
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = pose.x();
pose_stamped.pose.position.y = pose.y();
pose_stamped.pose.position.z = 0.0;
pose_stamped.pose.orientation = tf::createQuaternionMsgFromYaw(pose.z());
nav_path.poses.emplace_back(pose_stamped);
}
nav_path.header.frame_id = "world";
nav_path.header.stamp = timestamp_;
path_pub_.publish(nav_path);
}
void HybridAStarFlow::PublishVehiclePath(const VectorVec3d &path, double width,
double length, unsigned int vehicle_interval = 5u) {
visualization_msgs::MarkerArray vehicle_array;
for (unsigned int i = 0; i < path.size(); i += vehicle_interval) {
visualization_msgs::Marker vehicle;
if (i == 0) {
vehicle.action = 3;
}
vehicle.header.frame_id = "world";
vehicle.header.stamp = ros::Time::now();
vehicle.type = visualization_msgs::Marker::CUBE;
vehicle.id = static_cast<int>(i / vehicle_interval);
vehicle.scale.x = width;
vehicle.scale.y = length;
vehicle.scale.z = 0.01;
vehicle.color.a = 0.1;
vehicle.color.r = 1.0;
vehicle.color.b = 0.0;
vehicle.color.g = 0.0;
vehicle.pose.position.x = path[i].x();
vehicle.pose.position.y = path[i].y();
vehicle.pose.position.z = 0.0;
vehicle.pose.orientation = tf::createQuaternionMsgFromYaw(path[i].z());
vehicle_array.markers.emplace_back(vehicle);
}
vehicle_path_pub_.publish(vehicle_array);
}
void HybridAStarFlow::PublishSearchedTree(const VectorVec4d &searched_tree) {
visualization_msgs::Marker tree_list;
tree_list.header.frame_id = "world";
tree_list.header.stamp = ros::Time::now();
tree_list.type = visualization_msgs::Marker::LINE_LIST;
tree_list.action = visualization_msgs::Marker::ADD;
tree_list.ns = "searched_tree";
tree_list.scale.x = 0.02;
tree_list.color.a = 1.0;
tree_list.color.r = 0;
tree_list.color.g = 0;
tree_list.color.b = 0;
tree_list.pose.orientation.w = 1.0;
tree_list.pose.orientation.x = 0.0;
tree_list.pose.orientation.y = 0.0;
tree_list.pose.orientation.z = 0.0;
geometry_msgs::Point point;
for (const auto &i: searched_tree) {
point.x = i.x();
point.y = i.y();
point.z = 0.0;
tree_list.points.emplace_back(point);
point.x = i.z();
point.y = i.w();
point.z = 0.0;
tree_list.points.emplace_back(point);
}
searched_tree_pub_.publish(tree_list);
}
1.参数的含义如下：
steering_angle: 最大转向角度，转化为弧度使用。
steering_angle_discrete_num: 转向角度的离散数量，控制转向角度的分辨率。
segment_length: 从一个节点到另一个节点的路径段长度。
segment_length_discrete_num: 路径段长度的离散数量，影响路径的平滑度和搜索的精度。
wheel_base: 车辆的轴距，用于计算车辆转弯时的动态约束。
steering_penalty, reversing_penalty, steering_change_penalty: 分别对应转向、倒车和转向变化的惩罚因子，用于在成本计算中对这些行为进行惩罚，以生成更符合实际驾驶情况的路径。
shot_distance: 射线距离，用于在距离目标状态一定范围内时，采用解析解法（Reeds-Shepp路径）直接连接起点和终点。
添加：车辆参数（这是自己添加的，主要影响障碍物检测等）
vehicle_length
vehicle_width
rear_axle_to_back
2.源代码中的小bug：
unsigned int map_w = std::floor(current_costmap_ptr_->info.width / map_resolution);
unsigned int map_h = std::floor(current_costmap_ptr_->info.height / map_resolution);
------->
unsigned int map_w = std::floor(current_costmap_ptr_->info.width *current_costmap_ptr_->info.resolution / map_resolution);
unsigned int map_h = std::floor(current_costmap_ptr_->info.height *current_costmap_ptr_->info.resolution/ map_resolution);
std::make_shared被用于创建四个不同类型对象的共享指针：
kinodynamic_astar_searcher_ptr_ 是 HybridAStar 类型的共享指针，用于执行混合A*搜索算法。
hybrid的算法解读到这里基本就可以先使用了。
效果：
有人也对他进行了改进，一个是加到了ros导航里面，正好这几天准备好好学习一下。还有一个是优化了起点终点的设置逻辑，这个我应该要按照自己的来，调度系统要的应该就是一系列的途经点。
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
13
0- 0
已为社区贡献8条内容
所有评论(0)