AstarSearch::AstarSearch()
{
ros::NodeHandle private_nh_("~");
// base configs
private_nh_.param<bool>("use_back", use_back_, true); //车辆是否可以倒车
private_nh_.param<bool>("use_potential_heuristic", use_potential_heuristic_, true);//障碍物概率值比较大的,给一个大的启发值,即避开疑似障碍物的点
private_nh_.param<bool>("use_wavefront_heuristic", use_wavefront_heuristic_, false);//是否使用bfs的方法检查起点到终点是否可通行
private_nh_.param<double>("time_limit", time_limit_, 5000.0); //搜索时限
// robot configs
private_nh_.param<double>("robot_length", robot_length_, 4.5);//车身长度
private_nh_.param<double>("robot_width", robot_width_, 1.75);//车身宽度
private_nh_.param<double>("robot_base2back", robot_base2back_, 1.0);//底盘中心即后轴的位置
private_nh_.param<double>("minimum_turning_radius", minimum_turning_radius_, 6.0);//最小转弯半径
// search configs
private_nh_.param<int>("theta_size", theta_size_, 48);//将角度均分为多少份
private_nh_.param<double>("angle_goal_range", angle_goal_range_, 6.0);//到达终点的角度阈值
private_nh_.param<double>("curve_weight", curve_weight_, 1.2);//启发函数的曲率权重
private_nh_.param<double>("reverse_weight", reverse_weight_, 2.00);//启发函数的倒车权重
private_nh_.param<double>("lateral_goal_range", lateral_goal_range_, 0.5);//a距离终点的横向距离阈值
private_nh_.param<double>("longitudinal_goal_range", longitudinal_goal_range_, 2.0);//距离终点的纵向距离阈值
// costmap configs
private_nh_.param<int>("obstacle_threshold", obstacle_threshold_, 100);//costmap判断是否是障碍物的阈值
private_nh_.param<double>("potential_weight", potential_weight_, 10.0);//启发函数潜在障碍物权重
private_nh_.param<double>("distance_heuristic_weight", distance_heuristic_weight_, 1.0);//启发函数距离权重
createStateUpdateTable();
}
AstarSearch::~AstarSearch()
{
}
// state update table for hybrid astar以运动的最小距离做搜索,每步搜索六个方向,向前、前左、前右、向后、后左、后右,确定车在不同角度下,在不同的搜索方向搜索移动的距离和角度
void AstarSearch::createStateUpdateTable()
{
// Vehicle moving for each angle
state_update_table_.resize(theta_size_);
double dtheta = 2.0 * M_PI / theta_size_;
// Minimum moving distance with one state update
// arc = r * theta
double step = minimum_turning_radius_ * dtheta;
for (int i = 0; i < theta_size_; i++)
{
double theta = dtheta * i;
// Calculate right and left circle
// Robot moves along these circles
double right_circle_center_x = minimum_turning_radius_ * std::sin(theta);
double right_circle_center_y = minimum_turning_radius_ * -std::cos(theta);
double left_circle_center_x = -right_circle_center_x;
double left_circle_center_y = -right_circle_center_y;
// Calculate x and y shift to next state
NodeUpdate nu;
// forward
nu.shift_x = step * std::cos(theta);
nu.shift_y = step * std::sin(theta);
nu.rotation = 0.0;
nu.index_theta = 0;
nu.step = step;
nu.curve = false;
nu.back = false;
state_update_table_[i].emplace_back(nu);
// forward right
nu.shift_x = right_circle_center_x + minimum_turning_radius_ * std::cos(M_PI_2 + theta - dtheta);
nu.shift_y = right_circle_center_y + minimum_turning_radius_ * std::sin(M_PI_2 + theta - dtheta);
nu.rotation = -dtheta;
nu.index_theta = -1;
nu.step = step;
nu.curve = true;
nu.back = false;
state_update_table_[i].emplace_back(nu);
// forward left
nu.shift_x = left_circle_center_x + minimum_turning_radius_ * std::cos(-M_PI_2 + theta + dtheta);
nu.shift_y = left_circle_center_y + minimum_turning_radius_ * std::sin(-M_PI_2 + theta + dtheta);
nu.rotation = dtheta;
nu.index_theta = 1;
nu.step = step;
nu.curve = true;
nu.back = false;
state_update_table_[i].emplace_back(nu);
if (use_back_)
{
// backward
nu.shift_x = step * std::cos(theta) * -1.0;
nu.shift_y = step * std::sin(theta) * -1.0;
nu.rotation = 0;
nu.index_theta = 0;
nu.step = step;
nu.curve = false;
nu.back = true;
state_update_table_[i].emplace_back(nu);
// backward right
nu.shift_x = right_circle_center_x + minimum_turning_radius_ * std::cos(M_PI_2 + theta + dtheta);
nu.shift_y = right_circle_center_y + minimum_turning_radius_ * std::sin(M_PI_2 + theta + dtheta);
nu.rotation = dtheta;
nu.index_theta = 1;
nu.step = step;
nu.curve = true;
nu.back = true;
state_update_table_[i].emplace_back(nu);
// backward left
nu.shift_x = left_circle_center_x + minimum_turning_radius_ * std::cos(-1.0 * M_PI_2 + theta - dtheta);
nu.shift_y = left_circle_center_y + minimum_turning_radius_ * std::sin(-1.0 * M_PI_2 + theta - dtheta);
nu.rotation = dtheta * -1.0;
nu.index_theta = -1;
nu.step = step;
nu.curve = true;
nu.back = true;
state_update_table_[i].emplace_back(nu);
}
}
}
//根据地图信息初始化node
void AstarSearch::initialize(const nav_msgs::OccupancyGrid& costmap)
{
costmap_ = costmap;
int height = costmap_.info.height;
int width = costmap_.info.width;
// size initialization
nodes_.resize(height);
for (int i = 0; i < height; i++)
{
nodes_[i].resize(width);
}
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
nodes_[i][j].resize(theta_size_);
}
}
// cost initialization
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
// Index of subscribing OccupancyGrid message
int og_index = i * width + j;
int cost = costmap_.data[og_index];
// hc is set to be 0 when reset()
if (cost == 0)
{
continue;
}
// obstacle or unknown area障碍物和未知区域直接认为障碍物
if (cost < 0 || obstacle_threshold_ <= cost)
{
nodes_[i][j][0].status = STATUS::OBS;
}
// the cost more than threshold is regarded almost same as an obstacle
// because of its very high cost疑似障碍物即占用概率较大的,直接赋值给一个大的启发值
if (use_potential_heuristic_)
{
nodes_[i][j][0].hc = cost * potential_weight_;
}
}
}
}
//设置起点和终点后进行搜索
bool AstarSearch::makePlan(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& goal_pose)
{
if (!setStartNode(start_pose))
{
ROS_DEBUG("Invalid start pose");
return false;
}
if (!setGoalNode(goal_pose))
{
ROS_DEBUG("Invalid goal pose");
return false;
}
return search();
}
//设置起点,将实际坐标转到地图坐标,同时做越界检查和碰撞检查,赋值实际距离和启发距离
bool AstarSearch::setStartNode(const geometry_msgs::Pose& start_pose)
{
// Get index of start pose
int index_x, index_y, index_theta;
start_pose_local_.pose = start_pose;
poseToIndex(start_pose_local_.pose, &index_x, &index_y, &index_theta);
SimpleNode start_sn(index_x, index_y, index_theta, 0, 0);
// Check if start is valid
if (isOutOfRange(index_x, index_y) || detectCollision(start_sn))
{
return false;
}
// Set start node
AstarNode& start_node = nodes_[index_y][index_x][index_theta];
start_node.x = start_pose_local_.pose.position.x;
start_node.y = start_pose_local_.pose.position.y;
start_node.theta = 2.0 * M_PI / theta_size_ * index_theta;
start_node.gc = 0;
start_node.move_distance = 0;
start_node.back = false;
start_node.status = STATUS::OPEN;
start_node.parent = NULL;
// set euclidean distance heuristic cost
if (!use_wavefront_heuristic_ && !use_potential_heuristic_)
{
start_node.hc = calcDistance(start_pose_local_.pose.position.x, start_pose_local_.pose.position.y,
goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) *
distance_heuristic_weight_;
}
else if (use_potential_heuristic_)
{
start_node.gc += start_node.hc;
start_node.hc += calcDistance(start_pose_local_.pose.position.x, start_pose_local_.pose.position.y,
goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) +
distance_heuristic_weight_;
}
// Push start node to openlist
start_sn.cost = start_node.gc + start_node.hc;
openlist_.push(start_sn);
return true;
}
//设置终点,将实际坐标转到地图坐标,同时做越界检查和碰撞检查,赋值实际距离和启发距离
bool AstarSearch::setGoalNode(const geometry_msgs::Pose& goal_pose)
{
goal_pose_local_.pose = goal_pose;
goal_yaw_ = modifyTheta(tf::getYaw(goal_pose_local_.pose.orientation));
// Get index of goal pose
int index_x, index_y, index_theta;
poseToIndex(goal_pose_local_.pose, &index_x, &index_y, &index_theta);
SimpleNode goal_sn(index_x, index_y, index_theta, 0, 0);
// Check if goal is valid
if (isOutOfRange(index_x, index_y) || detectCollision(goal_sn))
{
return false;
}
// Calculate wavefront heuristic cost
if (use_wavefront_heuristic_)
{
// auto start = std::chrono::system_clock::now();
bool wavefront_result = calcWaveFrontHeuristic(goal_sn);
// auto end = std::chrono::system_clock::now();
// auto usec = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
// std::cout << "wavefront : " << usec / 1000.0 << "[msec]" << std::endl;
if (!wavefront_result)
{
ROS_DEBUG("Reachable is false...");
return false;
}
}
return true;
}
// 实际坐标转地图坐标
void AstarSearch::poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y, int* index_theta)
{
tf::Transform orig_tf;
tf::poseMsgToTF(costmap_.info.origin, orig_tf);
geometry_msgs::Pose pose2d = transformPose(pose, orig_tf.inverse());
*index_x = pose2d.position.x / costmap_.info.resolution;
*index_y = pose2d.position.y / costmap_.info.resolution;
tf::Quaternion quat;
tf::quaternionMsgToTF(pose2d.orientation, quat);
double yaw = tf::getYaw(quat);
if (yaw < 0)
yaw += 2.0 * M_PI;
// Descretize angle
static double one_angle_range = 2.0 * M_PI / theta_size_;
*index_theta = yaw / one_angle_range;
*index_theta %= theta_size_;
}
//实际坐标转地图坐标
void AstarSearch::pointToIndex(const geometry_msgs::Point& point, int* index_x, int* index_y)
{
geometry_msgs::Pose pose;
pose.position = point;
int index_theta;
poseToIndex(pose, index_x, index_y, &index_theta);
}
//越界检查:超出当前地图边界
bool AstarSearch::isOutOfRange(int index_x, int index_y)
{
if (index_x < 0 || index_x >= static_cast<int>(costmap_.info.width) || index_y < 0 ||
index_y >= static_cast<int>(costmap_.info.height))
{
return true;
}
return false;
}
//路径搜索
bool AstarSearch::search()
{
ros::WallTime begin = ros::WallTime::now();
// Start A* search
// If the openlist is empty, search failed
while (!openlist_.empty())
{
// Check time and terminate if the search reaches the time limit
ros::WallTime now = ros::WallTime::now();
double msec = (now - begin).toSec() * 1000.0;
if (msec > time_limit_)
{
ROS_DEBUG("Exceed time limit of %lf [ms]", time_limit_);
return false;
}
// Pop minimum cost node from openlist
SimpleNode top_sn = openlist_.top();
openlist_.pop();
// Expand nodes from this node
AstarNode* current_an = &nodes_[top_sn.index_y][top_sn.index_x][top_sn.index_theta];
current_an->status = STATUS::CLOSED;
// Goal check
if (isGoal(current_an->x, current_an->y, current_an->theta))
{
ROS_DEBUG("Search time: %lf [msec]", (now - begin).toSec() * 1000.0);
setPath(top_sn);
return true;
}
// Expand nodes搜索6个方向
for (const auto& state : state_update_table_[top_sn.index_theta])
{
// Next state
double next_x = current_an->x + state.shift_x;
double next_y = current_an->y + state.shift_y;
double next_theta = modifyTheta(current_an->theta + state.rotation);
double move_cost = state.step;
double move_distance = current_an->move_distance + state.step;
// Increase reverse cost 一个倒车,一个不倒车,不期望频繁改变运动方向
if (state.back != current_an->back)
move_cost *= reverse_weight_;
// Calculate index of the next state
SimpleNode next_sn;
geometry_msgs::Point next_pos;
next_pos.x = next_x;
next_pos.y = next_y;
pointToIndex(next_pos, &next_sn.index_x, &next_sn.index_y);
next_sn.index_theta = top_sn.index_theta + state.index_theta;
// Avoid invalid index
next_sn.index_theta = (next_sn.index_theta + theta_size_) % theta_size_;
// Check if the index is valid
if (isOutOfRange(next_sn.index_x, next_sn.index_y) || detectCollision(next_sn))
{
continue;
}
AstarNode* next_an = &nodes_[next_sn.index_y][next_sn.index_x][next_sn.index_theta];
double next_gc = current_an->gc + move_cost;
double next_hc = nodes_[next_sn.index_y][next_sn.index_x][0].hc; // wavefront or distance transform heuristic
// increase the cost with euclidean distance
if (use_potential_heuristic_)
{
next_gc += nodes_[next_sn.index_y][next_sn.index_x][0].hc;
next_hc += calcDistance(next_x, next_y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) *
distance_heuristic_weight_;
}
// increase the cost with euclidean distance
if (!use_wavefront_heuristic_ && !use_potential_heuristic_)
{
next_hc = calcDistance(next_x, next_y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) *
distance_heuristic_weight_;
}
// NONE
if (next_an->status == STATUS::NONE)
{
next_an->status = STATUS::OPEN;
next_an->x = next_x;
next_an->y = next_y;
next_an->theta = next_theta;
next_an->gc = next_gc;
next_an->hc = next_hc;
next_an->move_distance = move_distance;
next_an->back = state.back;
next_an->parent = current_an;
next_sn.cost = next_an->gc + next_an->hc;
openlist_.push(next_sn);
continue;
}
// OPEN or CLOSED
if (next_an->status == STATUS::OPEN || next_an->status == STATUS::CLOSED)
{
if (next_gc < next_an->gc)
{
next_an->status = STATUS::OPEN;
next_an->x = next_x;
next_an->y = next_y;
next_an->theta = next_theta;
next_an->gc = next_gc;
next_an->hc = next_hc; // already calculated ?
next_an->move_distance = move_distance;
next_an->back = state.back;
next_an->parent = current_an;
next_sn.cost = next_an->gc + next_an->hc;
openlist_.push(next_sn);
continue;
}
}
} // state update
}
// Failed to find path
ROS_DEBUG("Open list is empty...");
return false;
}
//获取路径,从终点回溯路径,再反转
void AstarSearch::setPath(const SimpleNode& goal)
{
std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = costmap_.header.frame_id;
path_.header = header;
// From the goal node to the start node
AstarNode* node = &nodes_[goal.index_y][goal.index_x][goal.index_theta];
while (node != NULL)
{
// Set tf pose
tf::Vector3 origin(node->x, node->y, 0);
tf::Pose tf_pose;
tf_pose.setOrigin(origin);
tf_pose.setRotation(tf::createQuaternionFromYaw(node->theta));
// Set path as ros message
geometry_msgs::PoseStamped ros_pose;
tf::poseTFToMsg(tf_pose, ros_pose.pose);
ros_pose.header = header;
path_.poses.push_back(ros_pose);
// To the next node
node = node->parent;
}
// Reverse the vector to be start to goal order
std::reverse(path_.poses.begin(), path_.poses.end());
}
// Check if the next state is the goal
// Check lateral offset, longitudinal offset and angle
//检查是否到达了目标点,有一定阈值,其实是在终点附近
bool AstarSearch::isGoal(double x, double y, double theta)
{
// To reduce computation time, we use square value for distance
static const double lateral_goal_range =
lateral_goal_range_ / 2.0; // [meter], divide by 2 means we check left and right
static const double longitudinal_goal_range =
longitudinal_goal_range_ / 2.0; // [meter], check only behind of the goal
static const double goal_angle = M_PI * (angle_goal_range_ / 2.0) / 180.0; // degrees -> radian
// Calculate the node coordinate seen from the goal point
tf::Point p(x, y, 0);
geometry_msgs::Point relative_node_point = calcRelativeCoordinate(goal_pose_local_.pose, p);
// Check Pose of goal
if (relative_node_point.x < 0 && // shoud be behind of goal
std::fabs(relative_node_point.x) < longitudinal_goal_range &&
std::fabs(relative_node_point.y) < lateral_goal_range)
{
// Check the orientation of goal
if (calcDiffOfRadian(goal_yaw_, theta) < goal_angle)
{
return true;
}
}
return false;
}
//检查是否是障碍物
bool AstarSearch::isObs(int index_x, int index_y)
{
if (nodes_[index_y][index_x][0].status == STATUS::OBS)
{
return true;
}
return false;
}
//障碍物检查 :以机器人后轴为中心,同时视车身为一个矩形,检查该矩形内是否有障碍物
bool AstarSearch::detectCollision(const SimpleNode& sn)
{
// Define the robot as rectangle
static double left = -1.0 * robot_base2back_;
static double right = robot_length_ - robot_base2back_;
static double top = robot_width_ / 2.0;
static double bottom = -1.0 * robot_width_ / 2.0;
static double resolution = costmap_.info.resolution;
// Coordinate of base_link in OccupancyGrid frame
static double one_angle_range = 2.0 * M_PI / theta_size_;
double base_x = sn.index_x * resolution;
double base_y = sn.index_y * resolution;
double base_theta = sn.index_theta * one_angle_range;//车身朝向
// Calculate cos and sin in advance
double cos_theta = std::cos(base_theta);
double sin_theta = std::sin(base_theta);
// Convert each point to index and check if the node is Obstacle
for (double x = left; x < right; x += resolution)
{
for (double y = top; y > bottom; y -= resolution)
{
// 2D point rotation
int index_x = (x * cos_theta - y * sin_theta + base_x) / resolution;
int index_y = (x * sin_theta + y * cos_theta + base_y) / resolution;
if (isOutOfRange(index_x, index_y))
{
return true;
}
else if (nodes_[index_y][index_x][0].status == STATUS::OBS)
{
return true;
}
}
}
return false;
}
// bfs的方法不考虑车的运动学约束,检查终点能否到起点,如果到不了,那就说明没有路可以到达,没有继续下去的必要了
bool AstarSearch::calcWaveFrontHeuristic(const SimpleNode& sn)
{
// Set start point for wavefront search
// This is goal for Astar search
nodes_[sn.index_y][sn.index_x][0].hc = 0;
WaveFrontNode wf_node(sn.index_x, sn.index_y, 1e-10);
std::queue<WaveFrontNode> qu;
qu.push(wf_node);
// State update table for wavefront search
// Nodes are expanded for each neighborhood cells (moore neighborhood)
double resolution = costmap_.info.resolution;
static std::vector<WaveFrontNode> updates = {
getWaveFrontNode(0, 1, resolution),
getWaveFrontNode(-1, 0, resolution),
getWaveFrontNode(1, 0, resolution),
getWaveFrontNode(0, -1, resolution),
getWaveFrontNode(-1, 1, std::hypot(resolution, resolution)),
getWaveFrontNode(1, 1, std::hypot(resolution, resolution)),
getWaveFrontNode(-1, -1, std::hypot(resolution, resolution)),
getWaveFrontNode(1, -1, std::hypot(resolution, resolution)),
};
// Get start index
int start_index_x;
int start_index_y;
int start_index_theta;
poseToIndex(start_pose_local_.pose, &start_index_x, &start_index_y, &start_index_theta);
// Whether the robot can reach goal
bool reachable = false;
// Start wavefront search
while (!qu.empty())
{
WaveFrontNode ref = qu.front();
qu.pop();
WaveFrontNode next;
for (const auto& u : updates)
{
next.index_x = ref.index_x + u.index_x;
next.index_y = ref.index_y + u.index_y;
// out of range OR already visited OR obstacle node
if (isOutOfRange(next.index_x, next.index_y) || nodes_[next.index_y][next.index_x][0].hc > 0 ||
nodes_[next.index_y][next.index_x][0].status == STATUS::OBS)
{
continue;
}
// Take the size of robot into account
if (detectCollisionWaveFront(next))
{
continue;
}
// Check if we can reach from start to goal
if (next.index_x == start_index_x && next.index_y == start_index_y)
{
reachable = true;
}
// Set wavefront heuristic cost
next.hc = ref.hc + u.hc;
nodes_[next.index_y][next.index_x][0].hc = next.hc;
qu.push(next);
}
}
// End of search
return reachable;
}
// Simple collidion detection for wavefront search以矩形做车身的碰撞检查
bool AstarSearch::detectCollisionWaveFront(const WaveFrontNode& ref)
{
// Define the robot as square
static double half = robot_width_ / 2;
double robot_x = ref.index_x * costmap_.info.resolution;
double robot_y = ref.index_y * costmap_.info.resolution;
for (double y = half; y > -1.0 * half; y -= costmap_.info.resolution)
{
for (double x = -1.0 * half; x < half; x += costmap_.info.resolution)
{
int index_x = (robot_x + x) / costmap_.info.resolution;
int index_y = (robot_y + y) / costmap_.info.resolution;
if (isOutOfRange(index_x, index_y))
{
return true;
}
if (nodes_[index_y][index_x][0].status == STATUS::OBS)
{
return true;
}
}
}
return false;
}
void AstarSearch::reset()
{
path_.poses.clear();
// Clear queue
std::priority_queue<SimpleNode, std::vector<SimpleNode>, std::greater<SimpleNode>> empty;
std::swap(openlist_, empty);
// ros::WallTime begin = ros::WallTime::now();
// Reset node info here ...?
for (size_t i = 0; i < costmap_.info.height; i++)
{
for (size_t j = 0; j < costmap_.info.width; j++)
{
for (int k = 0; k < theta_size_; k++)
{
// other values will be updated during the search
nodes_[i][j][k].status = STATUS::NONE;
nodes_[i][j][k].hc = 0;
}
}
}
// ros::WallTime end = ros::WallTime::now();
// ROS_INFO("Reset time: %lf [ms]", (end - begin).toSec() * 1000);
}
autoware astar_search hybrid A*
最新推荐文章于 2024-08-22 16:03:18 发布