add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,81 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_theta_star_planner)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(nav2_core REQUIRED)
|
||||
find_package(nav2_costmap_2d REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
|
||||
nav2_package() #Calls the nav2_package.cmake file
|
||||
add_compile_options(-O3)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
set(library_name ${PROJECT_NAME})
|
||||
|
||||
set(dependencies ament_cmake
|
||||
builtin_interfaces
|
||||
nav2_common
|
||||
nav2_core
|
||||
nav2_costmap_2d
|
||||
nav2_msgs
|
||||
nav2_util
|
||||
pluginlib
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
rclcpp_lifecycle
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
|
||||
add_library(${library_name} SHARED
|
||||
src/theta_star.cpp
|
||||
src/theta_star_planner.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${library_name} ${dependencies})
|
||||
|
||||
target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS")
|
||||
|
||||
pluginlib_export_plugin_description_file(nav2_core theta_star_planner.xml)
|
||||
|
||||
install(TARGETS ${library_name}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
install(FILES theta_star_planner.xml
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
set(gtest_disable_pthreads OFF)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
ament_add_gtest(test_theta_star test/test_theta_star.cpp)
|
||||
ament_target_dependencies(test_theta_star ${dependencies})
|
||||
target_link_libraries(test_theta_star ${library_name})
|
||||
endif()
|
||||
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(${library_name})
|
||||
ament_export_dependencies(${dependencies})
|
||||
ament_package()
|
||||
@@ -0,0 +1,94 @@
|
||||
# Nav2 Theta Star Planner
|
||||
The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta\* Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots.
|
||||
|
||||
See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-thetastar.html) for additional parameter descriptions.
|
||||
|
||||
## Features
|
||||
- The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\*
|
||||
- As it also considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases)
|
||||
- Uses the costs from the costmap to penalise high cost regions
|
||||
- Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces
|
||||
- Is well suited for smaller robots of the omni-directional and differential drive kind
|
||||
- The algorithmic part of the planner has been segregated from the plugin part to allow for reusability
|
||||
|
||||
## Metrics
|
||||
For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m -
|
||||
|
||||
|
||||
The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5`
|
||||
|
||||
## Cost Function Details
|
||||
### Symbols and their meanings
|
||||
**g(a)** - cost function cost for the node 'a'
|
||||
|
||||
**h(a)** - heuristic function cost for the node 'a'
|
||||
|
||||
**f(a)** - total cost (g(a) + h(a)) for the node 'a'
|
||||
|
||||
**LETHAL_COST** - a value of the costmap traversal cost that inscribes an obstacle with
|
||||
respect to a function, value = 253
|
||||
|
||||
**curr** - represents the node whose neighbours are being added to the list
|
||||
|
||||
**neigh** - one of the neighboring nodes of curr
|
||||
|
||||
**par** - parent node of curr
|
||||
|
||||
**euc_cost(a,b)** - euclidean distance between the node type 'a' and type 'b'
|
||||
|
||||
**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the node type 'a' and type 'b'
|
||||
|
||||
### Cost function
|
||||
```
|
||||
g(neigh) = g(curr) + w_euc_cost*euc_cost(curr, neigh) + w_traversal_cost*(costmap_cost(curr,neigh)/LETHAL_COST)^2
|
||||
h(neigh) = w_heuristic_cost * euc_cost(neigh, goal)
|
||||
f(neigh) = g(neigh) + h(neigh)
|
||||
```
|
||||
Because of how the program works when the 'neigh' init_rclcpp is to be expanded, depending
|
||||
on the result of the LOS check, (if the LOS check returns true) the value of g(neigh) might change to `g(par) +
|
||||
w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2`
|
||||
|
||||
## Parameters
|
||||
The parameters of the planner are :
|
||||
- ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8
|
||||
- ` .w_euc_cost ` : weight applied on the length of the path.
|
||||
- ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs.
|
||||
Below are the default values of the parameters :
|
||||
```
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
planner_plugin_types: ["nav2_theta_star_planner/ThetaStarPlanner"]
|
||||
use_sim_time: True
|
||||
planner_plugin_ids: ["GridBased"]
|
||||
GridBased:
|
||||
how_many_corners: 8
|
||||
w_euc_cost: 1.0
|
||||
w_traversal_cost: 2.0
|
||||
```
|
||||
|
||||
## Usage Notes
|
||||
|
||||
### Tuning the Parameters
|
||||
Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (ie before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1.
|
||||
|
||||
This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (thus leading to only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor: 10.0`, `inflation_radius: 5.5` and then decrease the value of `cost_scaling_factor` to achieve the said potential field.
|
||||
|
||||
Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother.
|
||||
|
||||
`w_heuristic_cost` is an internal setting that is not user changeable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent.
|
||||
|
||||
To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversely make the paths less taut. So it is recommended that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time.
|
||||
|
||||
Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to.
|
||||
|
||||
While tuning the planner's parameters you can also change the `inflation_layer`'s parameters (of the global costmap) to tune the behavior of the paths.
|
||||
|
||||
### Path Smoothing
|
||||
Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is, and the number of cells in that turn.
|
||||
|
||||
This planner is recommended to be used with local planners like DWB or TEB (or other any local planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons.
|
||||
|
||||
While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so because it might increase the query times from the planner. Test the planners performance on the higher cm/px costmaps before making a switch to finer costmaps.
|
||||
|
||||
### When to use this planner?
|
||||
This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). Another use case would be when you have corridors that are misaligned in the map image, in such a case this planner would be able to give straight-line like paths as it considers line of sight and thus avoid giving jagged paths which arises with other planners because of the finite directions of motion they consider.
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 112 KiB |
@@ -0,0 +1,315 @@
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_
|
||||
#define NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <chrono>
|
||||
#include <vector>
|
||||
#include <queue>
|
||||
#include <algorithm>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
|
||||
const double INF_COST = DBL_MAX;
|
||||
const int UNKNOWN_COST = 255;
|
||||
const int OBS_COST = 254;
|
||||
const int LETHAL_COST = 252;
|
||||
|
||||
struct coordsM
|
||||
{
|
||||
int x, y;
|
||||
};
|
||||
|
||||
struct coordsW
|
||||
{
|
||||
double x, y;
|
||||
};
|
||||
|
||||
struct tree_node
|
||||
{
|
||||
int x, y;
|
||||
double g = INF_COST;
|
||||
double h = INF_COST;
|
||||
const tree_node * parent_id = nullptr;
|
||||
bool is_in_queue = false;
|
||||
double f = INF_COST;
|
||||
};
|
||||
|
||||
struct comp
|
||||
{
|
||||
bool operator()(const tree_node * p1, const tree_node * p2)
|
||||
{
|
||||
return (p1->f) > (p2->f);
|
||||
}
|
||||
};
|
||||
|
||||
namespace theta_star
|
||||
{
|
||||
class ThetaStar
|
||||
{
|
||||
public:
|
||||
coordsM src_{}, dst_{};
|
||||
nav2_costmap_2d::Costmap2D * costmap_{};
|
||||
|
||||
/// weight on the costmap traversal cost
|
||||
double w_traversal_cost_;
|
||||
/// weight on the euclidean distance cost (used for calculations of g_cost)
|
||||
double w_euc_cost_;
|
||||
/// weight on the heuristic cost (used for h_cost calculations)
|
||||
double w_heuristic_cost_;
|
||||
/// parameter to set the number of adjacent nodes to be searched on
|
||||
int how_many_corners_;
|
||||
/// parameter to set weather the planner can plan through unknown space
|
||||
bool allow_unknown_;
|
||||
/// the x-directional and y-directional lengths of the map respectively
|
||||
int size_x_, size_y_;
|
||||
|
||||
ThetaStar();
|
||||
|
||||
~ThetaStar() = default;
|
||||
|
||||
/**
|
||||
* @brief it iteratively searches upon the nodes in the queue (open list) until the
|
||||
* current node is the goal pose or the size of queue becomes 0
|
||||
* @param raw_path is used to return the path obtained by executing the algorithm
|
||||
* @return true if a path is found, false if no path is found in between the start and goal pose
|
||||
*/
|
||||
bool generatePath(std::vector<coordsW> & raw_path);
|
||||
|
||||
/**
|
||||
* @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST
|
||||
* @return the result of the check
|
||||
*/
|
||||
inline bool isSafe(const int & cx, const int & cy) const
|
||||
{
|
||||
return (costmap_->getCost(
|
||||
cx,
|
||||
cy) == UNKNOWN_COST && allow_unknown_) || costmap_->getCost(cx, cy) < LETHAL_COST;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief initialises the values of the start and goal points
|
||||
*/
|
||||
void setStartAndGoal(
|
||||
const geometry_msgs::msg::PoseStamped & start,
|
||||
const geometry_msgs::msg::PoseStamped & goal);
|
||||
|
||||
/**
|
||||
* @brief checks whether the start and goal points have costmap costs greater than LETHAL_COST
|
||||
* @return true if the cost of any one of the points is greater than LETHAL_COST
|
||||
*/
|
||||
bool isUnsafeToPlan() const
|
||||
{
|
||||
return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y));
|
||||
}
|
||||
|
||||
int nodes_opened = 0;
|
||||
|
||||
protected:
|
||||
/// for the coordinates (x,y), it stores at node_position_[size_x_ * y + x],
|
||||
/// the pointer to the location at which the data of the node is present in nodes_data_
|
||||
/// it is initialised with size_x_ * size_y_ elements
|
||||
/// and its number of elements increases to account for a change in map size
|
||||
std::vector<tree_node *> node_position_;
|
||||
|
||||
/// the vector nodes_data_ stores the coordinates, costs and index of the parent node,
|
||||
/// and whether or not the node is present in queue_, for all the nodes searched
|
||||
/// it is initialised with no elements
|
||||
/// and its size increases depending on the number of nodes searched
|
||||
std::vector<tree_node> nodes_data_;
|
||||
|
||||
/// this is the priority queue (open_list) to select the next node to be expanded
|
||||
std::priority_queue<tree_node *, std::vector<tree_node *>, comp> queue_;
|
||||
|
||||
/// it is a counter like variable used to generate consecutive indices
|
||||
/// such that the data for all the nodes (in open and closed lists) could be stored
|
||||
/// consecutively in nodes_data_
|
||||
int index_generated_;
|
||||
|
||||
const coordsM moves[8] = {{0, 1},
|
||||
{0, -1},
|
||||
{1, 0},
|
||||
{-1, 0},
|
||||
{1, -1},
|
||||
{-1, 1},
|
||||
{1, 1},
|
||||
{-1, -1}};
|
||||
|
||||
tree_node * exp_node;
|
||||
|
||||
|
||||
/** @brief it performs a line of sight (los) check between the current node and the parent node of its parent node;
|
||||
* if an los is found and the new costs calculated are lesser, then the cost and parent node
|
||||
* of the current node is updated
|
||||
* @param data of the current node
|
||||
*/
|
||||
void resetParent(tree_node * curr_data);
|
||||
|
||||
/**
|
||||
* @brief this function expands the current node
|
||||
* @param curr_data used to send the data of the current node
|
||||
* @param curr_id used to send the index of the current node as stored in nodes_position_
|
||||
*/
|
||||
void setNeighbors(const tree_node * curr_data);
|
||||
|
||||
/**
|
||||
* @brief performs the line of sight check using Bresenham's Algorithm,
|
||||
* and has been modified to calculate the traversal cost incurred in a straight line path between
|
||||
* the two points whose coordinates are (x0, y0) and (x1, y1)
|
||||
* @param sl_cost is used to return the cost thus incurred
|
||||
* @return true if a line of sight exists between the points
|
||||
*/
|
||||
bool losCheck(
|
||||
const int & x0, const int & y0, const int & x1, const int & y1,
|
||||
double & sl_cost) const;
|
||||
|
||||
/**
|
||||
* @brief it returns the path by backtracking from the goal to the start, by using their parent nodes
|
||||
* @param raw_points used to return the path thus found
|
||||
* @param curr_id sends in the index of the goal coordinate, as stored in nodes_position
|
||||
*/
|
||||
void backtrace(std::vector<coordsW> & raw_points, const tree_node * curr_n) const;
|
||||
|
||||
/**
|
||||
* @brief it is an overloaded function to ease the cost calculations while performing the LOS check
|
||||
* @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node (cx, cy) at every instance; it is also being returned
|
||||
* @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise
|
||||
*/
|
||||
bool isSafe(const int & cx, const int & cy, double & cost) const
|
||||
{
|
||||
double curr_cost = getCost(cx, cy);
|
||||
if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || curr_cost < LETHAL_COST) {
|
||||
if (costmap_->getCost(cx, cy) == UNKNOWN_COST) {
|
||||
curr_cost = OBS_COST - 1;
|
||||
}
|
||||
cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST;
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief this function scales the costmap cost by shifting the origin to 25 and then multiply
|
||||
* the actual costmap cost by 0.9 to keep the output in the range of [25, 255)
|
||||
*/
|
||||
inline double getCost(const int & cx, const int & cy) const
|
||||
{
|
||||
return 26 + 0.9 * costmap_->getCost(cx, cy);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief for the point(cx, cy), its traversal cost is calculated by
|
||||
* <parameter>*(<actual_traversal_cost_from_costmap>)^2/(<max_cost>)^2
|
||||
* @return the traversal cost thus calculated
|
||||
*/
|
||||
inline double getTraversalCost(const int & cx, const int & cy)
|
||||
{
|
||||
double curr_cost = getCost(cx, cy);
|
||||
return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief calculates the piecewise straight line euclidean distances by
|
||||
* <euc_cost_parameter>*<euclidean distance between the points (ax, ay) and (bx, by)>
|
||||
* @return the distance thus calculated
|
||||
*/
|
||||
inline double getEuclideanCost(const int & ax, const int & ay, const int & bx, const int & by)
|
||||
{
|
||||
return w_euc_cost_ * std::hypot(ax - bx, ay - by);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief for the point(cx, cy), its heuristic cost is calculated by
|
||||
* <heuristic_cost_parameter>*<euclidean distance between the point and goal>
|
||||
* @return the heuristic cost
|
||||
*/
|
||||
inline double getHCost(const int & cx, const int & cy)
|
||||
{
|
||||
return w_heuristic_cost_ * std::hypot(cx - dst_.x, cy - dst_.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief checks if the given coordinates(cx, cy) lies within the map
|
||||
* @return the result of the check
|
||||
*/
|
||||
inline bool withinLimits(const int & cx, const int & cy) const
|
||||
{
|
||||
return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief checks if the coordinates of a node is the goal or not
|
||||
* @return the result of the check
|
||||
*/
|
||||
inline bool isGoal(const tree_node & this_node) const
|
||||
{
|
||||
return this_node.x == dst_.x && this_node.y == dst_.y;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief initialises the node_position_ vector by storing -1 as index for all points(x, y) within the limits of the map
|
||||
* @param size_inc is used to increase the number of elements in node_position_ in case the size of the map increases
|
||||
*/
|
||||
void initializePosn(int size_inc = 0);
|
||||
|
||||
/**
|
||||
* @brief it stores id_this in node_position_ at the index [ size_x_*cy + cx ]
|
||||
* @param id_this a pointer to the location at which the data of the point(cx, cy) is stored in nodes_data_
|
||||
*/
|
||||
inline void addIndex(const int & cx, const int & cy, tree_node * node_this)
|
||||
{
|
||||
node_position_[size_x_ * cy + cx] = node_this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief retrieves the pointer of the location at which the data of the point(cx, cy) is stored in nodes_data
|
||||
* @return id_this is the pointer to that location
|
||||
*/
|
||||
inline tree_node * getIndex(const int & cx, const int & cy)
|
||||
{
|
||||
return node_position_[size_x_ * cy + cx];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief this function depending on the size of the nodes_data_ vector allots space to store the data for a node(x, y)
|
||||
* @param id_this is the index at which the data is stored/has to be stored for that node
|
||||
*/
|
||||
void addToNodesData(const int & id_this)
|
||||
{
|
||||
if (static_cast<int>(nodes_data_.size()) <= id_this) {
|
||||
nodes_data_.push_back({});
|
||||
} else {
|
||||
nodes_data_[id_this] = {};
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief initialises the values of global variables at beginning of the execution of the generatePath function
|
||||
*/
|
||||
void resetContainers();
|
||||
|
||||
/**
|
||||
* @brief clears the priority queue after each execution of the generatePath function
|
||||
*/
|
||||
void clearQueue()
|
||||
{
|
||||
queue_ = std::priority_queue<tree_node *, std::vector<tree_node *>, comp>();
|
||||
}
|
||||
};
|
||||
} // namespace theta_star
|
||||
|
||||
#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_
|
||||
+101
@@ -0,0 +1,101 @@
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_
|
||||
#define NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
#include <queue>
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_core/global_planner.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "nav2_theta_star_planner/theta_star.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
|
||||
namespace nav2_theta_star_planner
|
||||
{
|
||||
|
||||
class ThetaStarPlanner : public nav2_core::GlobalPlanner
|
||||
{
|
||||
public:
|
||||
void 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) override;
|
||||
|
||||
void cleanup() override;
|
||||
|
||||
void activate() override;
|
||||
|
||||
void deactivate() override;
|
||||
|
||||
nav_msgs::msg::Path createPlan(
|
||||
const geometry_msgs::msg::PoseStamped & start,
|
||||
const geometry_msgs::msg::PoseStamped & goal) override;
|
||||
|
||||
protected:
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("ThetaStarPlanner")};
|
||||
std::string global_frame_, name_;
|
||||
bool use_final_approach_orientation_;
|
||||
|
||||
// parent node weak ptr
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node_;
|
||||
|
||||
std::unique_ptr<theta_star::ThetaStar> planner_;
|
||||
|
||||
// Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
|
||||
|
||||
/**
|
||||
* @brief the function responsible for calling the algorithm and retrieving a path from it
|
||||
* @return global_path is the planned path to be taken
|
||||
*/
|
||||
void getPlan(nav_msgs::msg::Path & global_path);
|
||||
|
||||
/**
|
||||
* @brief interpolates points between the consecutive waypoints of the path
|
||||
* @param raw_path is used to send in the path received from the planner
|
||||
* @param dist_bw_points is used to send in the interpolation_resolution (which has been set as the costmap resolution)
|
||||
* @return the final path with waypoints at a distance of the value of interpolation_resolution of each other
|
||||
*/
|
||||
static nav_msgs::msg::Path linearInterpolation(
|
||||
const std::vector<coordsW> & raw_path,
|
||||
const double & dist_bw_points);
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a paramter change is detected
|
||||
* @param parameters list of changed parameters
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
};
|
||||
} // namespace nav2_theta_star_planner
|
||||
|
||||
#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_
|
||||
@@ -0,0 +1,34 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>nav2_theta_star_planner</name>
|
||||
<version>1.1.18</version>
|
||||
<description>Theta* Global Planning Plugin</description>
|
||||
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
|
||||
<maintainer email="anshumaan567@gmail.com">Anshumaan Singh</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>builtin_interfaces</depend>
|
||||
<depend>nav2_common</depend>
|
||||
<depend>nav2_core</depend>
|
||||
<depend>nav2_costmap_2d</depend>
|
||||
<depend>nav2_msgs</depend>
|
||||
<depend>nav2_util</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
<nav2_core plugin="${prefix}/global_planner_plugin.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@@ -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)
|
||||
@@ -0,0 +1,232 @@
|
||||
// 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 <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_theta_star_planner/theta_star.hpp"
|
||||
#include "nav2_theta_star_planner/theta_star_planner.hpp"
|
||||
|
||||
class init_rclcpp
|
||||
{
|
||||
public:
|
||||
init_rclcpp() {rclcpp::init(0, nullptr);}
|
||||
~init_rclcpp() {rclcpp::shutdown();}
|
||||
};
|
||||
|
||||
/// class created to access the protected members of the ThetaStar class
|
||||
/// u is used as shorthand for use
|
||||
class test_theta_star : public theta_star::ThetaStar
|
||||
{
|
||||
public:
|
||||
int getSizeOfNodePosition()
|
||||
{
|
||||
return static_cast<int>(node_position_.size());
|
||||
}
|
||||
|
||||
bool ulosCheck(const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost)
|
||||
{
|
||||
return losCheck(x0, y0, x1, y1, sl_cost);
|
||||
}
|
||||
|
||||
bool uwithinLimits(const int & cx, const int & cy) {return withinLimits(cx, cy);}
|
||||
|
||||
bool uisGoal(const tree_node & this_node) {return isGoal(this_node);}
|
||||
|
||||
void uinitializePosn(int size_inc = 0)
|
||||
{
|
||||
node_position_.reserve(size_x_ * size_y_); initializePosn(size_inc);
|
||||
}
|
||||
|
||||
void uaddIndex(const int & cx, const int & cy) {addIndex(cx, cy, &nodes_data_[0]);}
|
||||
|
||||
tree_node * ugetIndex(const int & cx, const int & cy) {return getIndex(cx, cy);}
|
||||
|
||||
tree_node * test_getIndex() {return &nodes_data_[0];}
|
||||
|
||||
void uaddToNodesData(const int & id) {addToNodesData(id);}
|
||||
|
||||
void uresetContainers() {nodes_data_.clear(); resetContainers();}
|
||||
|
||||
bool runAlgo(std::vector<coordsW> & path)
|
||||
{
|
||||
if (!isUnsafeToPlan()) {
|
||||
return generatePath(path);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
init_rclcpp node;
|
||||
|
||||
// Tests meant to test the algorithm itself and its helper functions
|
||||
TEST(ThetaStarTest, test_theta_star) {
|
||||
auto planner_ = std::make_unique<test_theta_star>();
|
||||
planner_->costmap_ = new nav2_costmap_2d::Costmap2D(50, 50, 1.0, 0.0, 0.0, 0);
|
||||
for (int i = 7; i <= 14; i++) {
|
||||
for (int j = 7; j <= 14; j++) {
|
||||
planner_->costmap_->setCost(i, j, 253);
|
||||
}
|
||||
}
|
||||
coordsM s = {5, 5}, g = {18, 18};
|
||||
int size_x = 20, size_y = 20;
|
||||
planner_->size_x_ = size_x;
|
||||
planner_->size_y_ = size_y;
|
||||
geometry_msgs::msg::PoseStamped start, goal;
|
||||
start.pose.position.x = s.x;
|
||||
start.pose.position.y = s.y;
|
||||
start.pose.orientation.w = 1.0;
|
||||
goal.pose.position.x = g.x;
|
||||
goal.pose.position.y = g.y;
|
||||
goal.pose.orientation.w = 1.0;
|
||||
/// Check if the setStartAndGoal function works properly
|
||||
planner_->setStartAndGoal(start, goal);
|
||||
EXPECT_TRUE(planner_->src_.x == s.x && planner_->src_.y == s.y);
|
||||
EXPECT_TRUE(planner_->dst_.x == g.x && planner_->dst_.y == g.y);
|
||||
/// Check if the initializePosn function works properly
|
||||
planner_->uinitializePosn(size_x * size_y);
|
||||
EXPECT_EQ(planner_->getSizeOfNodePosition(), (size_x * size_y));
|
||||
|
||||
/// Check if the withinLimits function works properly
|
||||
EXPECT_TRUE(planner_->uwithinLimits(18, 18));
|
||||
EXPECT_FALSE(planner_->uwithinLimits(120, 140));
|
||||
|
||||
tree_node n = {g.x, g.y, 120, 0, NULL, false, 20};
|
||||
n.parent_id = &n;
|
||||
/// Check if the isGoal function works properly
|
||||
EXPECT_TRUE(planner_->uisGoal(n)); // both (x,y) are the goal coordinates
|
||||
n.x = 25;
|
||||
EXPECT_FALSE(planner_->uisGoal(n)); // only y coordinate matches with that of goal
|
||||
n.x = g.x;
|
||||
n.y = 20;
|
||||
EXPECT_FALSE(planner_->uisGoal(n)); // only x coordinate matches with that of goal
|
||||
n.x = 30;
|
||||
EXPECT_FALSE(planner_->uisGoal(n)); // both (x, y) are different from the goal coordinate
|
||||
|
||||
/// Check if the isSafe functions work properly
|
||||
EXPECT_TRUE(planner_->isSafe(5, 5)); // cost at this point is 0
|
||||
EXPECT_FALSE(planner_->isSafe(10, 10)); // cost at this point is 253 (>LETHAL_COST)
|
||||
|
||||
/// Check if the functions addIndex & getIndex work properly
|
||||
coordsM c = {18, 18};
|
||||
planner_->uaddToNodesData(0);
|
||||
planner_->uaddIndex(c.x, c.y);
|
||||
tree_node * c_node = planner_->ugetIndex(c.x, c.y);
|
||||
EXPECT_EQ(c_node, planner_->test_getIndex());
|
||||
|
||||
double sl_cost = 0.0;
|
||||
/// Checking for the case where the losCheck should return the value as true
|
||||
EXPECT_TRUE(planner_->ulosCheck(2, 2, 7, 20, sl_cost));
|
||||
/// and as false
|
||||
EXPECT_FALSE(planner_->ulosCheck(2, 2, 18, 18, sl_cost));
|
||||
|
||||
planner_->uresetContainers();
|
||||
std::vector<coordsW> path;
|
||||
/// Check if the planner returns a path for the case where a path exists
|
||||
EXPECT_TRUE(planner_->runAlgo(path));
|
||||
EXPECT_GT(static_cast<int>(path.size()), 0);
|
||||
/// and where it doesn't exist
|
||||
path.clear();
|
||||
planner_->src_ = {10, 10};
|
||||
EXPECT_FALSE(planner_->runAlgo(path));
|
||||
EXPECT_EQ(static_cast<int>(path.size()), 0);
|
||||
}
|
||||
|
||||
// Smoke tests meant to detect issues arising from the plugin part rather than the algorithm
|
||||
TEST(ThetaStarPlanner, test_theta_star_planner) {
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr life_node =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("ThetaStarPlannerTest");
|
||||
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
|
||||
std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
|
||||
costmap_ros->on_configure(rclcpp_lifecycle::State());
|
||||
|
||||
geometry_msgs::msg::PoseStamped start, goal;
|
||||
start.pose.position.x = 0.0;
|
||||
start.pose.position.y = 0.0;
|
||||
start.pose.orientation.w = 1.0;
|
||||
goal = start;
|
||||
auto planner_2d = std::make_unique<nav2_theta_star_planner::ThetaStarPlanner>();
|
||||
planner_2d->configure(life_node, "test", nullptr, costmap_ros);
|
||||
planner_2d->activate();
|
||||
|
||||
nav_msgs::msg::Path path = planner_2d->createPlan(start, goal);
|
||||
EXPECT_GT(static_cast<int>(path.poses.size()), 0);
|
||||
|
||||
// test if the goal is unsafe
|
||||
for (int i = 7; i <= 14; i++) {
|
||||
for (int j = 7; j <= 14; j++) {
|
||||
costmap_ros->getCostmap()->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
goal.pose.position.x = 1.0;
|
||||
goal.pose.position.y = 1.0;
|
||||
path = planner_2d->createPlan(start, goal);
|
||||
EXPECT_EQ(static_cast<int>(path.poses.size()), 0);
|
||||
|
||||
planner_2d->deactivate();
|
||||
planner_2d->cleanup();
|
||||
|
||||
planner_2d.reset();
|
||||
costmap_ros->on_cleanup(rclcpp_lifecycle::State());
|
||||
life_node.reset();
|
||||
costmap_ros.reset();
|
||||
}
|
||||
|
||||
TEST(ThetaStarPlanner, test_theta_star_reconfigure)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr life_node =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("ThetaStarPlannerTest");
|
||||
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
|
||||
std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
|
||||
costmap_ros->on_configure(rclcpp_lifecycle::State());
|
||||
|
||||
auto planner = std::make_unique<nav2_theta_star_planner::ThetaStarPlanner>();
|
||||
try {
|
||||
// Expect to throw due to invalid prims file in param
|
||||
planner->configure(life_node, "test", nullptr, costmap_ros);
|
||||
} catch (...) {
|
||||
}
|
||||
planner->activate();
|
||||
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
life_node->get_node_base_interface(), life_node->get_node_topics_interface(),
|
||||
life_node->get_node_graph_interface(),
|
||||
life_node->get_node_services_interface());
|
||||
|
||||
auto results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("test.how_many_corners", 8),
|
||||
rclcpp::Parameter("test.w_euc_cost", 1.0),
|
||||
rclcpp::Parameter("test.w_traversal_cost", 2.0),
|
||||
rclcpp::Parameter("test.use_final_approach_orientation", false),
|
||||
rclcpp::Parameter("test.allow_unknown", false)});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
life_node->get_node_base_interface(),
|
||||
results);
|
||||
|
||||
EXPECT_EQ(life_node->get_parameter("test.how_many_corners").as_int(), 8);
|
||||
EXPECT_EQ(
|
||||
life_node->get_parameter("test.w_euc_cost").as_double(),
|
||||
1.0);
|
||||
EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0);
|
||||
EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false);
|
||||
EXPECT_EQ(life_node->get_parameter("test.allow_unknown").as_bool(), false);
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
life_node->get_node_base_interface(),
|
||||
results);
|
||||
}
|
||||
@@ -0,0 +1,5 @@
|
||||
<library path="nav2_theta_star_planner">
|
||||
<class name="nav2_theta_star_planner/ThetaStarPlanner" type="nav2_theta_star_planner::ThetaStarPlanner" base_class_type="nav2_core::GlobalPlanner">
|
||||
<description>An implementation of the Theta* Algorithm</description>
|
||||
</class>
|
||||
</library>
|
||||
Reference in New Issue
Block a user