Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Foxy sync smac planner #2071

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 43 additions & 3 deletions doc/parameters/param_list.md
Original file line number Diff line number Diff line change
Expand Up @@ -166,7 +166,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
| `<data source>`.marking | true | Whether source should mark in costmap |
| `<data source>`.clearing | false | Whether source should raytrace clear in costmap |
| `<data source>`.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap |
| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |

# controller_server

Expand Down Expand Up @@ -276,7 +276,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
| `<dwb plugin>`.`<name>`.x_only_threshold | 0.05 | Threshold to check in the X velocity direction |
| `<dwb plugin>`.`<name>`.scale | 1.0 | Weighed scale for critic |

## kinematic_parameters
## kinematic_parameters

| Parameter | Default | Description |
| ----------| --------| ------------|
Expand All @@ -295,7 +295,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
| `<dwb plugin>`.decel_lim_y | 0.0 | Maximum deceleration Y (m/s^2) |
| `<dwb plugin>`.decel_lim_theta | 0.0 | Maximum deceleration rotation (rad/s^2) |

## xy_theta_iterator
## xy_theta_iterator

| Parameter | Default | Description |
| ----------| --------| ------------|
Expand Down Expand Up @@ -473,6 +473,46 @@ When `planner_plugins` parameter is not overridden, the following default plugin
| `<name>`.use_astar | false | Whether to use A*, if false, uses Dijstra's expansion |
| `<name>`.allow_unknown | true | Whether to allow planning in unknown space |

# smac_planner

* `<name>`: Corresponding planner plugin ID for this type

| Parameter | Default | Description |
| ----------| --------| ------------|
| `<name>`.tolerance | 0.5 | Tolerance in meters between requested goal pose and end of path |
| `<name>`.downsample_costmap | false | Whether to downsample costmap |
| `<name>`.downsampling_factor | 1 | Factor to downsample costmap by |
| `<name>`.allow_unknown | true | whether to allow traversing in unknown space |
| `<name>`.max_iterations | -1 | Number of iterations before failing, disabled by -1 |
| `<name>`.max_on_approach_iterations | 1000 | Iterations after within threshold before returning approximate path with best heuristic |
| `<name>`.max_planning_time_ms | 5000 | Maximum planning time in ms |
| `<name>`.smooth_path | false | Whether to smooth path with CG smoother |
| `<name>`.motion_model_for_search | DUBIN | Motion model to search with. Options for SE2: DUBIN, REEDS_SHEPP. 2D: MOORE, VON_NEUMANN |
| `<name>`.angle_quantization_bins | 1 | Number of angle quantization bins for SE2 node |
| `<name>`.minimum_turning_radius | 0.20 | Minimum turning radius in m of vehicle or desired path |
| `<name>`.reverse_penalty | 2.0 | Penalty to apply to SE2 node if in reverse direction |
| `<name>`.change_penalty | 0.5 | Penalty to apply to SE2 node if changing direction |
| `<name>`.non_straight_penalty | 1.1 | Penalty to apply to SE2 node if non-straight direction |
| `<name>`.cost_penalty | 1.2 | Penalty to apply to SE2 node for cost at pose |
| `<name>`.analytic_expansion_ratio | 2.0 | For SE2 nodes the planner will attempt an analytic path expansion every N iterations, where N = closest_distance_to_goal / analytic_expansion_ratio. Higher ratios result in more frequent expansions |
| `<name>`.smoother.smoother.w_curve | 1.5 | Smoother weight on curvature of path |
| `<name>`.smoother.smoother.w_dist | 0.0 | Smoother weight on distance from original path |
| `<name>`.smoother.smoother.w_smooth | 15000 | Smoother weight on distance between nodes |
| `<name>`.smoother.smoother.w_cost | 1.5 | Smoother weight on costmap cost |
| `<name>`.smoother.smoother.cost_scaling_factor | 10.0 | Inflation layer's scale factor |
| `<name>`.smoother.optimizer.max_time | 0.10 | Maximum time to spend smoothing, in seconds |
| `<name>`.smoother.optimizer.max_iterations | 500 | Maximum number of iterations to spend smoothing |
| `<name>`.smoother.optimizer.debug_optimizer | false | Whether to print debug info from Ceres |
| `<name>`.smoother.optimizer.gradient_tol | 1e-10 | Minimum change in gradient to terminate smoothing |
| `<name>`.smoother.optimizer.fn_tol | 1e-7 | Minimum change in function to terminate smoothing |
| `<name>`.smoother.optimizer.param_tol | 1e-15 | Minimum change in parameter block to terminate smoothing |

| `<name>`.smoother.optimizer.advanced.min_line_search_step_size | 1e-20 | Terminate smoothing iteration if change in parameter block less than this |
| `<name>`.smoother.optimizer.advanced.max_num_line_search_step_size_iterations | 50 | Maximum iterations for line search |
| `<name>`.smoother.optimizer.advanced.line_search_sufficient_function_decrease | 1e-20 | Function decrease amount to terminate current line search iteration |
| `<name>`.smoother.optimizer.advanced.max_num_line_search_direction_restarts | 10 | Maximum umber of restarts of line search when over-estimating |
| `<name>`.smoother.optimizer.advanced.max_line_search_step_expansion | 50 | Step size multiplier at each iteration of line search |

# waypoint_follower

| Parameter | Default | Description |
Expand Down
1 change: 1 addition & 0 deletions nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -159,6 +159,7 @@ local_costmap:
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
Expand Down
1 change: 1 addition & 0 deletions nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -159,6 +159,7 @@ local_costmap:
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
Expand Down
4 changes: 4 additions & 0 deletions nav2_bringup/bringup/params/nav2_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -168,6 +168,7 @@ local_costmap:
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
Expand Down Expand Up @@ -204,6 +205,7 @@ global_costmap:
use_sim_time: True
robot_radius: 0.22
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
Expand All @@ -220,6 +222,8 @@ global_costmap:
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
Expand Down
14 changes: 14 additions & 0 deletions nav2_bringup/bringup/rviz/nav2_default_view.rviz
Original file line number Diff line number Diff line change
Expand Up @@ -249,6 +249,20 @@ Visualization Manager:
Value: /global_costmap/costmap
Use Timestamp: false
Value: true
- Alpha: 0.3
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Downsampled Costmap
Topic:
Depth: 1
Durability Policy: Transient Local
History Policy: Keep Last
Reliability Policy: Reliable
Value: /downsampled_costmap
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz_default_plugins/Path
Expand Down
8 changes: 8 additions & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,14 @@ class Costmap2D
*/
unsigned char getCost(unsigned int mx, unsigned int my) const;

/**
* @brief Get the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @return The cost of the cell
*/
unsigned char getCost(unsigned int index) const;

/**
* @brief Set the cost of a cell in the costmap
* @param mx The x coordinate of the cell
Expand Down
8 changes: 8 additions & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -248,6 +248,14 @@ class Costmap2DROS : public nav2_util::LifecycleNode

std::shared_ptr<tf2_ros::Buffer> getTfBuffer() {return tf_buffer_;}

/**
* @brief Get the costmap's use_radius_ parameter, corresponding to
* whether the footprint for the robot is a circle with radius robot_radius_
* or an arbitrarily defined footprint in footprint_.
* @return use_radius_
*/
bool getUseRadius() {return use_radius_;}

protected:
rclcpp::Node::SharedPtr client_node_;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class CostmapTopicCollisionChecker
CostmapSubscriber & costmap_sub_;
FootprintSubscriber & footprint_sub_;
double transform_tolerance_;
FootprintCollisionChecker collision_checker_;
FootprintCollisionChecker<std::shared_ptr<Costmap2D>> collision_checker_;
};

} // namespace nav2_costmap_2d
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,20 +32,21 @@ namespace nav2_costmap_2d
{
typedef std::vector<geometry_msgs::msg::Point> Footprint;

template<typename CostmapT>
class FootprintCollisionChecker
{
public:
FootprintCollisionChecker();
explicit FootprintCollisionChecker(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap);
explicit FootprintCollisionChecker(CostmapT costmap);
double footprintCost(const Footprint footprint);
double footprintCostAtPose(double x, double y, double theta, const Footprint footprint);
double lineCost(int x0, int x1, int y0, int y1) const;
bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
double pointCost(int x, int y) const;
void setCostmap(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap);
void setCostmap(CostmapT costmap);

private:
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
protected:
CostmapT costmap_;
};

} // namespace nav2_costmap_2d
Expand Down
5 changes: 5 additions & 0 deletions nav2_costmap_2d/src/costmap_2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -212,6 +212,11 @@ unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
return costmap_[getIndex(mx, my)];
}

unsigned char Costmap2D::getCost(unsigned int undex) const
{
return costmap_[undex];
}

void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{
costmap_[getIndex(mx, my)] = cost;
Expand Down
30 changes: 21 additions & 9 deletions nav2_costmap_2d/src/footprint_collision_checker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,18 +31,21 @@ using namespace std::chrono_literals;
namespace nav2_costmap_2d
{

FootprintCollisionChecker::FootprintCollisionChecker()
template<typename CostmapT>
FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker()
: costmap_(nullptr)
{
}

FootprintCollisionChecker::FootprintCollisionChecker(
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap)
template<typename CostmapT>
FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker(
CostmapT costmap)
: costmap_(costmap)
{
}

double FootprintCollisionChecker::footprintCost(const Footprint footprint)
template<typename CostmapT>
double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint footprint)
{
// now we really have to lay down the footprint in the costmap_ grid
unsigned int x0, x1, y0, y1;
Expand Down Expand Up @@ -80,7 +83,8 @@ double FootprintCollisionChecker::footprintCost(const Footprint footprint)
return footprint_cost;
}

double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const
template<typename CostmapT>
double FootprintCollisionChecker<CostmapT>::lineCost(int x0, int x1, int y0, int y1) const
{
double line_cost = 0.0;
double point_cost = -1.0;
Expand All @@ -96,23 +100,27 @@ double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const
return line_cost;
}

bool FootprintCollisionChecker::worldToMap(
template<typename CostmapT>
bool FootprintCollisionChecker<CostmapT>::worldToMap(
double wx, double wy, unsigned int & mx, unsigned int & my)
{
return costmap_->worldToMap(wx, wy, mx, my);
}

double FootprintCollisionChecker::pointCost(int x, int y) const
template<typename CostmapT>
double FootprintCollisionChecker<CostmapT>::pointCost(int x, int y) const
{
return costmap_->getCost(x, y);
}

void FootprintCollisionChecker::setCostmap(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap)
template<typename CostmapT>
void FootprintCollisionChecker<CostmapT>::setCostmap(CostmapT costmap)
{
costmap_ = costmap;
}

double FootprintCollisionChecker::footprintCostAtPose(
template<typename CostmapT>
double FootprintCollisionChecker<CostmapT>::footprintCostAtPose(
double x, double y, double theta, const Footprint footprint)
{
double cos_th = cos(theta);
Expand All @@ -128,4 +136,8 @@ double FootprintCollisionChecker::footprintCostAtPose(
return footprintCost(oriented_footprint);
}

// declare our valid template parameters
template class FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>;
template class FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>;

} // namespace nav2_costmap_2d
15 changes: 10 additions & 5 deletions nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,8 @@ TEST(collision_footprint, test_basic)

nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};

nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker(costmap_);

auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);

Expand All @@ -51,7 +52,8 @@ TEST(collision_footprint, test_point_cost)
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);

nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker(costmap_);

auto value = collision_checker.pointCost(50, 50);

Expand All @@ -63,7 +65,8 @@ TEST(collision_footprint, test_world_to_map)
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.1, 0, 0, 0);

nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker(costmap_);

unsigned int x, y;

Expand Down Expand Up @@ -105,7 +108,8 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement)

nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};

nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker(costmap_);

auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
EXPECT_NEAR(value, 0.0, 0.001);
Expand Down Expand Up @@ -140,7 +144,8 @@ TEST(collision_footprint, test_point_and_line_cost)

nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};

nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_);
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker(costmap_);

auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint);
EXPECT_NEAR(value, 0.0, 0.001);
Expand Down
14 changes: 14 additions & 0 deletions nav2_navfn_planner/src/navfn_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
// the Global Dynamic Window Approach. IEEE.
// https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf

// #define BENCHMARK_TESTING

#include "nav2_navfn_planner/navfn_planner.hpp"

#include <chrono>
Expand Down Expand Up @@ -114,6 +116,10 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal)
{
#ifdef BENCHMARK_TESTING
steady_clock::time_point a = steady_clock::now();
#endif

// Update planner based on the new costmap size
if (isPlannerOutOfDate()) {
planner_->setNavArr(
Expand All @@ -128,6 +134,14 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
node_->get_logger(), "%s: failed to create plan with "
"tolerance %.2f.", name_.c_str(), tolerance_);
}

#ifdef BENCHMARK_TESTING
steady_clock::time_point b = steady_clock::now();
duration<double> time_span = duration_cast<duration<double>>(b - a);
std::cout << "It took " << time_span.count() * 1000 <<
" milliseconds with " << num_iterations << " iterations." << std::endl;
#endif

return path;
}

Expand Down
2 changes: 1 addition & 1 deletion nav2_planner/src/planner_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ PlannerServer::PlannerServer()

// Declare this node's parameters
declare_parameter("planner_plugins", default_ids_);
declare_parameter("expected_planner_frequency", 20.0);
declare_parameter("expected_planner_frequency", 1.0);

// Setup the global costmap
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
Expand Down