add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,261 @@
|
||||
// Copyright 2020 Anshumaan Singh
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <vector>
|
||||
#include "nav2_theta_star_planner/theta_star.hpp"
|
||||
|
||||
namespace theta_star
|
||||
{
|
||||
|
||||
ThetaStar::ThetaStar()
|
||||
: w_traversal_cost_(1.0),
|
||||
w_euc_cost_(2.0),
|
||||
w_heuristic_cost_(1.0),
|
||||
how_many_corners_(8),
|
||||
allow_unknown_(true),
|
||||
size_x_(0),
|
||||
size_y_(0),
|
||||
index_generated_(0)
|
||||
{
|
||||
exp_node = new tree_node;
|
||||
}
|
||||
|
||||
void ThetaStar::setStartAndGoal(
|
||||
const geometry_msgs::msg::PoseStamped & start,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
unsigned int s[2], d[2];
|
||||
costmap_->worldToMap(start.pose.position.x, start.pose.position.y, s[0], s[1]);
|
||||
costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, d[0], d[1]);
|
||||
|
||||
src_ = {static_cast<int>(s[0]), static_cast<int>(s[1])};
|
||||
dst_ = {static_cast<int>(d[0]), static_cast<int>(d[1])};
|
||||
}
|
||||
|
||||
bool ThetaStar::generatePath(std::vector<coordsW> & raw_path)
|
||||
{
|
||||
resetContainers();
|
||||
addToNodesData(index_generated_);
|
||||
double src_g_cost = getTraversalCost(src_.x, src_.y), src_h_cost = getHCost(src_.x, src_.y);
|
||||
nodes_data_[index_generated_] =
|
||||
{src_.x, src_.y, src_g_cost, src_h_cost, &nodes_data_[index_generated_], true,
|
||||
src_g_cost + src_h_cost};
|
||||
queue_.push({&nodes_data_[index_generated_]});
|
||||
addIndex(src_.x, src_.y, &nodes_data_[index_generated_]);
|
||||
tree_node * curr_data = &nodes_data_[index_generated_];
|
||||
index_generated_++;
|
||||
nodes_opened = 0;
|
||||
|
||||
while (!queue_.empty()) {
|
||||
nodes_opened++;
|
||||
|
||||
if (isGoal(*curr_data)) {
|
||||
break;
|
||||
}
|
||||
|
||||
resetParent(curr_data);
|
||||
setNeighbors(curr_data);
|
||||
|
||||
curr_data = queue_.top();
|
||||
queue_.pop();
|
||||
}
|
||||
|
||||
if (queue_.empty()) {
|
||||
raw_path.clear();
|
||||
return false;
|
||||
}
|
||||
|
||||
backtrace(raw_path, curr_data);
|
||||
clearQueue();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void ThetaStar::resetParent(tree_node * curr_data)
|
||||
{
|
||||
double g_cost, los_cost = 0;
|
||||
curr_data->is_in_queue = false;
|
||||
const tree_node * curr_par = curr_data->parent_id;
|
||||
const tree_node * maybe_par = curr_par->parent_id;
|
||||
|
||||
if (losCheck(curr_data->x, curr_data->y, maybe_par->x, maybe_par->y, los_cost)) {
|
||||
g_cost = maybe_par->g +
|
||||
getEuclideanCost(curr_data->x, curr_data->y, maybe_par->x, maybe_par->y) + los_cost;
|
||||
|
||||
if (g_cost < curr_data->g) {
|
||||
curr_data->parent_id = maybe_par;
|
||||
curr_data->g = g_cost;
|
||||
curr_data->f = g_cost + curr_data->h;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ThetaStar::setNeighbors(const tree_node * curr_data)
|
||||
{
|
||||
int mx, my;
|
||||
tree_node * m_id = nullptr;
|
||||
double g_cost, h_cost, cal_cost;
|
||||
|
||||
for (int i = 0; i < how_many_corners_; i++) {
|
||||
mx = curr_data->x + moves[i].x;
|
||||
my = curr_data->y + moves[i].y;
|
||||
|
||||
if (withinLimits(mx, my)) {
|
||||
if (!isSafe(mx, my)) {
|
||||
continue;
|
||||
}
|
||||
} else {
|
||||
continue;
|
||||
}
|
||||
|
||||
g_cost = curr_data->g + getEuclideanCost(curr_data->x, curr_data->y, mx, my) +
|
||||
getTraversalCost(mx, my);
|
||||
|
||||
m_id = getIndex(mx, my);
|
||||
|
||||
if (m_id == nullptr) {
|
||||
addToNodesData(index_generated_);
|
||||
m_id = &nodes_data_[index_generated_];
|
||||
addIndex(mx, my, m_id);
|
||||
index_generated_++;
|
||||
}
|
||||
|
||||
exp_node = m_id;
|
||||
|
||||
h_cost = getHCost(mx, my);
|
||||
cal_cost = g_cost + h_cost;
|
||||
if (exp_node->f > cal_cost) {
|
||||
exp_node->g = g_cost;
|
||||
exp_node->h = h_cost;
|
||||
exp_node->f = cal_cost;
|
||||
exp_node->parent_id = curr_data;
|
||||
if (!exp_node->is_in_queue) {
|
||||
exp_node->x = mx;
|
||||
exp_node->y = my;
|
||||
exp_node->is_in_queue = true;
|
||||
queue_.push({m_id});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ThetaStar::backtrace(std::vector<coordsW> & raw_points, const tree_node * curr_n) const
|
||||
{
|
||||
std::vector<coordsW> path_rev;
|
||||
coordsW world{};
|
||||
do {
|
||||
costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y);
|
||||
path_rev.push_back(world);
|
||||
if (path_rev.size() > 1) {
|
||||
curr_n = curr_n->parent_id;
|
||||
}
|
||||
} while (curr_n->parent_id != curr_n);
|
||||
costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y);
|
||||
path_rev.push_back(world);
|
||||
|
||||
raw_points.reserve(path_rev.size());
|
||||
for (int i = static_cast<int>(path_rev.size()) - 1; i >= 0; i--) {
|
||||
raw_points.push_back(path_rev[i]);
|
||||
}
|
||||
}
|
||||
|
||||
bool ThetaStar::losCheck(
|
||||
const int & x0, const int & y0, const int & x1, const int & y1,
|
||||
double & sl_cost) const
|
||||
{
|
||||
sl_cost = 0;
|
||||
|
||||
int cx, cy;
|
||||
int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0;
|
||||
int sx, sy;
|
||||
sx = x1 > x0 ? 1 : -1;
|
||||
sy = y1 > y0 ? 1 : -1;
|
||||
|
||||
int u_x = (sx - 1) / 2;
|
||||
int u_y = (sy - 1) / 2;
|
||||
cx = x0;
|
||||
cy = y0;
|
||||
|
||||
if (dx >= dy) {
|
||||
while (cx != x1) {
|
||||
f += dy;
|
||||
if (f >= dx) {
|
||||
if (!isSafe(cx + u_x, cy + u_y, sl_cost)) {
|
||||
return false;
|
||||
}
|
||||
cy += sy;
|
||||
f -= dx;
|
||||
}
|
||||
if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) {
|
||||
return false;
|
||||
}
|
||||
if (dy == 0 && !isSafe(cx + u_x, cy, sl_cost) && !isSafe(cx + u_x, cy - 1, sl_cost)) {
|
||||
return false;
|
||||
}
|
||||
cx += sx;
|
||||
}
|
||||
} else {
|
||||
while (cy != y1) {
|
||||
f = f + dx;
|
||||
if (f >= dy) {
|
||||
if (!isSafe(cx + u_x, cy + u_y, sl_cost)) {
|
||||
return false;
|
||||
}
|
||||
cx += sx;
|
||||
f -= dy;
|
||||
}
|
||||
if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) {
|
||||
return false;
|
||||
}
|
||||
if (dx == 0 && !isSafe(cx, cy + u_y, sl_cost) && !isSafe(cx - 1, cy + u_y, sl_cost)) {
|
||||
return false;
|
||||
}
|
||||
cy += sy;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void ThetaStar::resetContainers()
|
||||
{
|
||||
index_generated_ = 0;
|
||||
int last_size_x = size_x_;
|
||||
int last_size_y = size_y_;
|
||||
int curr_size_x = static_cast<int>(costmap_->getSizeInCellsX());
|
||||
int curr_size_y = static_cast<int>(costmap_->getSizeInCellsY());
|
||||
if (((last_size_x != curr_size_x) || (last_size_y != curr_size_y)) &&
|
||||
static_cast<int>(node_position_.size()) < (curr_size_x * curr_size_y))
|
||||
{
|
||||
initializePosn(curr_size_y * curr_size_x - last_size_y * last_size_x);
|
||||
nodes_data_.reserve(curr_size_x * curr_size_y);
|
||||
} else {
|
||||
initializePosn();
|
||||
}
|
||||
size_x_ = curr_size_x;
|
||||
size_y_ = curr_size_y;
|
||||
}
|
||||
|
||||
void ThetaStar::initializePosn(int size_inc)
|
||||
{
|
||||
if (!node_position_.empty()) {
|
||||
for (int i = 0; i < size_x_ * size_y_; i++) {
|
||||
node_position_[i] = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < size_inc; i++) {
|
||||
node_position_.push_back(nullptr);
|
||||
}
|
||||
}
|
||||
} // namespace theta_star
|
||||
@@ -0,0 +1,249 @@
|
||||
// Copyright 2020 Anshumaan Singh
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include "nav2_theta_star_planner/theta_star_planner.hpp"
|
||||
#include "nav2_theta_star_planner/theta_star.hpp"
|
||||
|
||||
namespace nav2_theta_star_planner
|
||||
{
|
||||
void ThetaStarPlanner::configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
planner_ = std::make_unique<theta_star::ThetaStar>();
|
||||
parent_node_ = parent;
|
||||
auto node = parent_node_.lock();
|
||||
logger_ = node->get_logger();
|
||||
clock_ = node->get_clock();
|
||||
name_ = name;
|
||||
tf_ = tf;
|
||||
planner_->costmap_ = costmap_ros->getCostmap();
|
||||
global_frame_ = costmap_ros->getGlobalFrameID();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name_ + ".how_many_corners", rclcpp::ParameterValue(8));
|
||||
|
||||
node->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_);
|
||||
|
||||
if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) {
|
||||
planner_->how_many_corners_ = 8;
|
||||
RCLCPP_WARN(logger_, "Your value for - .how_many_corners was overridden, and is now set to 8");
|
||||
}
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name_ + ".allow_unknown", rclcpp::ParameterValue(true));
|
||||
node->get_parameter(name_ + ".allow_unknown", planner_->allow_unknown_);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name_ + ".w_euc_cost", rclcpp::ParameterValue(1.0));
|
||||
node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(2.0));
|
||||
node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_);
|
||||
|
||||
planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0;
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_);
|
||||
}
|
||||
|
||||
void ThetaStarPlanner::cleanup()
|
||||
{
|
||||
RCLCPP_INFO(logger_, "CleaningUp plugin %s of type nav2_theta_star_planner", name_.c_str());
|
||||
planner_.reset();
|
||||
}
|
||||
|
||||
void ThetaStarPlanner::activate()
|
||||
{
|
||||
RCLCPP_INFO(logger_, "Activating plugin %s of type nav2_theta_star_planner", name_.c_str());
|
||||
// Add callback for dynamic parameters
|
||||
auto node = parent_node_.lock();
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&ThetaStarPlanner::dynamicParametersCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void ThetaStarPlanner::deactivate()
|
||||
{
|
||||
RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str());
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path ThetaStarPlanner::createPlan(
|
||||
const geometry_msgs::msg::PoseStamped & start,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
nav_msgs::msg::Path global_path;
|
||||
auto start_time = std::chrono::steady_clock::now();
|
||||
|
||||
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(planner_->costmap_->getMutex()));
|
||||
|
||||
// Corner case of start and goal beeing on the same cell
|
||||
unsigned int mx_start, my_start, mx_goal, my_goal;
|
||||
if (!planner_->costmap_->worldToMap(
|
||||
start.pose.position.x, start.pose.position.y, mx_start, my_start))
|
||||
{
|
||||
RCLCPP_WARN(logger_, "Start Coordinates were outside map bounds");
|
||||
return global_path;
|
||||
}
|
||||
|
||||
if (!planner_->costmap_->worldToMap(
|
||||
goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal))
|
||||
{
|
||||
RCLCPP_WARN(logger_, "Goal Coordinates were outside map bounds");
|
||||
return global_path;
|
||||
}
|
||||
|
||||
if (mx_start == mx_goal && my_start == my_goal) {
|
||||
if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) {
|
||||
RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles");
|
||||
return global_path;
|
||||
}
|
||||
global_path.header.stamp = clock_->now();
|
||||
global_path.header.frame_id = global_frame_;
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = global_path.header;
|
||||
pose.pose.position.z = 0.0;
|
||||
|
||||
pose.pose = start.pose;
|
||||
// if we have a different start and goal orientation, set the unique path pose to the goal
|
||||
// orientation, unless use_final_approach_orientation=true where we need it to be the start
|
||||
// orientation to avoid movement from the local planner
|
||||
if (start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_) {
|
||||
pose.pose.orientation = goal.pose.orientation;
|
||||
}
|
||||
global_path.poses.push_back(pose);
|
||||
return global_path;
|
||||
}
|
||||
|
||||
planner_->setStartAndGoal(start, goal);
|
||||
RCLCPP_DEBUG(
|
||||
logger_, "Got the src and dst... (%i, %i) && (%i, %i)",
|
||||
planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y);
|
||||
getPlan(global_path);
|
||||
// check if a plan is generated
|
||||
size_t plan_size = global_path.poses.size();
|
||||
if (plan_size > 0) {
|
||||
global_path.poses.back().pose.orientation = goal.pose.orientation;
|
||||
}
|
||||
|
||||
// If use_final_approach_orientation=true, interpolate the last pose orientation from the
|
||||
// previous pose to set the orientation to the 'final approach' orientation of the robot so
|
||||
// it does not rotate.
|
||||
// And deal with corner case of plan of length 1
|
||||
if (use_final_approach_orientation_) {
|
||||
if (plan_size == 1) {
|
||||
global_path.poses.back().pose.orientation = start.pose.orientation;
|
||||
} else if (plan_size > 1) {
|
||||
double dx, dy, theta;
|
||||
auto last_pose = global_path.poses.back().pose.position;
|
||||
auto approach_pose = global_path.poses[plan_size - 2].pose.position;
|
||||
dx = last_pose.x - approach_pose.x;
|
||||
dy = last_pose.y - approach_pose.y;
|
||||
theta = atan2(dy, dx);
|
||||
global_path.poses.back().pose.orientation =
|
||||
nav2_util::geometry_utils::orientationAroundZAxis(theta);
|
||||
}
|
||||
}
|
||||
|
||||
auto stop_time = std::chrono::steady_clock::now();
|
||||
auto dur = std::chrono::duration_cast<std::chrono::microseconds>(stop_time - start_time);
|
||||
RCLCPP_DEBUG(logger_, "the time taken is : %i", static_cast<int>(dur.count()));
|
||||
RCLCPP_DEBUG(logger_, "the nodes_opened are: %i", planner_->nodes_opened);
|
||||
return global_path;
|
||||
}
|
||||
|
||||
void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path)
|
||||
{
|
||||
std::vector<coordsW> path;
|
||||
if (planner_->isUnsafeToPlan()) {
|
||||
RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! ");
|
||||
global_path.poses.clear();
|
||||
} else if (planner_->generatePath(path)) {
|
||||
global_path = linearInterpolation(path, planner_->costmap_->getResolution());
|
||||
} else {
|
||||
RCLCPP_ERROR(logger_, "Could not generate path between the given poses");
|
||||
global_path.poses.clear();
|
||||
}
|
||||
global_path.header.stamp = clock_->now();
|
||||
global_path.header.frame_id = global_frame_;
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation(
|
||||
const std::vector<coordsW> & raw_path,
|
||||
const double & dist_bw_points)
|
||||
{
|
||||
nav_msgs::msg::Path pa;
|
||||
|
||||
geometry_msgs::msg::PoseStamped p1;
|
||||
for (unsigned int j = 0; j < raw_path.size() - 1; j++) {
|
||||
coordsW pt1 = raw_path[j];
|
||||
p1.pose.position.x = pt1.x;
|
||||
p1.pose.position.y = pt1.y;
|
||||
pa.poses.push_back(p1);
|
||||
|
||||
coordsW pt2 = raw_path[j + 1];
|
||||
double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y);
|
||||
int loops = static_cast<int>(distance / dist_bw_points);
|
||||
double sin_alpha = (pt2.y - pt1.y) / distance;
|
||||
double cos_alpha = (pt2.x - pt1.x) / distance;
|
||||
for (int k = 1; k < loops; k++) {
|
||||
p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha;
|
||||
p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha;
|
||||
pa.poses.push_back(p1);
|
||||
}
|
||||
}
|
||||
|
||||
return pa;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
ThetaStarPlanner::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_INTEGER) {
|
||||
if (name == name_ + ".how_many_corners") {
|
||||
planner_->how_many_corners_ = parameter.as_int();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == name_ + ".w_euc_cost") {
|
||||
planner_->w_euc_cost_ = parameter.as_double();
|
||||
} else if (name == name_ + ".w_traversal_cost") {
|
||||
planner_->w_traversal_cost_ = parameter.as_double();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_BOOL) {
|
||||
if (name == name_ + ".use_final_approach_orientation") {
|
||||
use_final_approach_orientation_ = parameter.as_bool();
|
||||
} else if (name == name_ + ".allow_unknown") {
|
||||
planner_->allow_unknown_ = parameter.as_bool();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_theta_star_planner
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_theta_star_planner::ThetaStarPlanner, nav2_core::GlobalPlanner)
|
||||
Reference in New Issue
Block a user