add humble-navigation2
This commit is contained in:
X-lanni
Binary file not shown.
|
After Width: | Height: | Size: 1.3 MiB |
@@ -0,0 +1,118 @@
|
||||
# Test costmap downsampler
|
||||
ament_add_gtest(test_costmap_downsampler
|
||||
test_costmap_downsampler.cpp
|
||||
)
|
||||
ament_target_dependencies(test_costmap_downsampler
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_costmap_downsampler
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Test Node2D
|
||||
ament_add_gtest(test_node2d
|
||||
test_node2d.cpp
|
||||
)
|
||||
ament_target_dependencies(test_node2d
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_node2d
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Test NodeHybrid
|
||||
ament_add_gtest(test_nodehybrid
|
||||
test_nodehybrid.cpp
|
||||
)
|
||||
ament_target_dependencies(test_nodehybrid
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_nodehybrid
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Test NodeBasic
|
||||
ament_add_gtest(test_nodebasic
|
||||
test_nodebasic.cpp
|
||||
)
|
||||
ament_target_dependencies(test_nodebasic
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_nodebasic
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Test collision checker
|
||||
ament_add_gtest(test_collision_checker
|
||||
test_collision_checker.cpp
|
||||
)
|
||||
ament_target_dependencies(test_collision_checker
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_collision_checker
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Test A*
|
||||
ament_add_gtest(test_a_star
|
||||
test_a_star.cpp
|
||||
)
|
||||
ament_target_dependencies(test_a_star
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_a_star
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Test SMAC Hybrid
|
||||
ament_add_gtest(test_smac_hybrid
|
||||
test_smac_hybrid.cpp
|
||||
)
|
||||
ament_target_dependencies(test_smac_hybrid
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_smac_hybrid
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Test SMAC 2D
|
||||
ament_add_gtest(test_smac_2d
|
||||
test_smac_2d.cpp
|
||||
)
|
||||
ament_target_dependencies(test_smac_2d
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_smac_2d
|
||||
${library_name}_2d
|
||||
)
|
||||
|
||||
# Test SMAC lattice
|
||||
ament_add_gtest(test_smac_lattice
|
||||
test_smac_lattice.cpp
|
||||
)
|
||||
ament_target_dependencies(test_smac_lattice
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_smac_lattice
|
||||
${library_name}_lattice
|
||||
)
|
||||
|
||||
# Test SMAC Smoother
|
||||
ament_add_gtest(test_smoother
|
||||
test_smoother.cpp
|
||||
)
|
||||
ament_target_dependencies(test_smoother
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_smoother
|
||||
${library_name}_lattice
|
||||
${library_name}
|
||||
${library_name}_2d
|
||||
)
|
||||
|
||||
#Test Lattice node
|
||||
ament_add_gtest(test_lattice_node test_nodelattice.cpp)
|
||||
|
||||
ament_target_dependencies(test_lattice_node ${dependencies})
|
||||
|
||||
target_link_libraries(test_lattice_node ${library_name})
|
||||
@@ -0,0 +1,207 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__OPTIONS_HPP_
|
||||
#define DEPRECATED__OPTIONS_HPP_
|
||||
|
||||
#include <string>
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct nav2_smac_planner::SmootherParams
|
||||
* @brief Parameters for the smoother cost function
|
||||
*/
|
||||
struct SmootherParams
|
||||
{
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::SmootherParams
|
||||
*/
|
||||
SmootherParams()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get params from ROS parameter
|
||||
* @param node_ Ptr to node
|
||||
* @param name Name of plugin
|
||||
*/
|
||||
void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
|
||||
{
|
||||
std::string local_name = name + std::string(".smoother.smoother.");
|
||||
|
||||
// Smoother params
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "w_curve", rclcpp::ParameterValue(1.5));
|
||||
node->get_parameter(local_name + "w_curve", curvature_weight);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "w_cost", rclcpp::ParameterValue(0.0));
|
||||
node->get_parameter(local_name + "w_cost", costmap_weight);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "w_dist", rclcpp::ParameterValue(0.0));
|
||||
node->get_parameter(local_name + "w_dist", distance_weight);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0));
|
||||
node->get_parameter(local_name + "w_smooth", smooth_weight);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "cost_scaling_factor", rclcpp::ParameterValue(10.0));
|
||||
node->get_parameter(local_name + "cost_scaling_factor", costmap_factor);
|
||||
}
|
||||
|
||||
double smooth_weight{0.0};
|
||||
double costmap_weight{0.0};
|
||||
double distance_weight{0.0};
|
||||
double curvature_weight{0.0};
|
||||
double max_curvature{0.0};
|
||||
double costmap_factor{0.0};
|
||||
double max_time;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct nav2_smac_planner::OptimizerParams
|
||||
* @brief Parameters for the ceres optimizer
|
||||
*/
|
||||
struct OptimizerParams
|
||||
{
|
||||
OptimizerParams()
|
||||
: debug(false),
|
||||
max_iterations(50),
|
||||
max_time(1e4),
|
||||
param_tol(1e-8),
|
||||
fn_tol(1e-6),
|
||||
gradient_tol(1e-10)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @struct AdvancedParams
|
||||
* @brief Advanced parameters for the ceres optimizer
|
||||
*/
|
||||
struct AdvancedParams
|
||||
{
|
||||
AdvancedParams()
|
||||
: min_line_search_step_size(1e-9),
|
||||
max_num_line_search_step_size_iterations(20),
|
||||
line_search_sufficient_function_decrease(1e-4),
|
||||
max_num_line_search_direction_restarts(20),
|
||||
max_line_search_step_contraction(1e-3),
|
||||
min_line_search_step_contraction(0.6),
|
||||
line_search_sufficient_curvature_decrease(0.9),
|
||||
max_line_search_step_expansion(10)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get advanced params from ROS parameter
|
||||
* @param node_ Ptr to node
|
||||
* @param name Name of plugin
|
||||
*/
|
||||
void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
|
||||
{
|
||||
std::string local_name = name + std::string(".smoother.optimizer.advanced.");
|
||||
|
||||
// Optimizer advanced params
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "min_line_search_step_size",
|
||||
rclcpp::ParameterValue(1e-20));
|
||||
node->get_parameter(
|
||||
local_name + "min_line_search_step_size",
|
||||
min_line_search_step_size);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_num_line_search_step_size_iterations",
|
||||
rclcpp::ParameterValue(50));
|
||||
node->get_parameter(
|
||||
local_name + "max_num_line_search_step_size_iterations",
|
||||
max_num_line_search_step_size_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "line_search_sufficient_function_decrease",
|
||||
rclcpp::ParameterValue(1e-20));
|
||||
node->get_parameter(
|
||||
local_name + "line_search_sufficient_function_decrease",
|
||||
line_search_sufficient_function_decrease);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_num_line_search_direction_restarts",
|
||||
rclcpp::ParameterValue(10));
|
||||
node->get_parameter(
|
||||
local_name + "max_num_line_search_direction_restarts",
|
||||
max_num_line_search_direction_restarts);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_line_search_step_expansion",
|
||||
rclcpp::ParameterValue(50));
|
||||
node->get_parameter(
|
||||
local_name + "max_line_search_step_expansion",
|
||||
max_line_search_step_expansion);
|
||||
}
|
||||
|
||||
|
||||
double min_line_search_step_size; // Ceres default: 1e-9
|
||||
int max_num_line_search_step_size_iterations; // Ceres default: 20
|
||||
double line_search_sufficient_function_decrease; // Ceres default: 1e-4
|
||||
int max_num_line_search_direction_restarts; // Ceres default: 5
|
||||
|
||||
double max_line_search_step_contraction; // Ceres default: 1e-3
|
||||
double min_line_search_step_contraction; // Ceres default: 0.6
|
||||
double line_search_sufficient_curvature_decrease; // Ceres default: 0.9
|
||||
int max_line_search_step_expansion; // Ceres default: 10
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Get params from ROS parameter
|
||||
* @param node_ Ptr to node
|
||||
* @param name Name of plugin
|
||||
*/
|
||||
void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
|
||||
{
|
||||
std::string local_name = name + std::string(".smoother.optimizer.");
|
||||
|
||||
// Optimizer params
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "param_tol", rclcpp::ParameterValue(1e-15));
|
||||
node->get_parameter(local_name + "param_tol", param_tol);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7));
|
||||
node->get_parameter(local_name + "fn_tol", fn_tol);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10));
|
||||
node->get_parameter(local_name + "gradient_tol", gradient_tol);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_iterations", rclcpp::ParameterValue(500));
|
||||
node->get_parameter(local_name + "max_iterations", max_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "max_time", rclcpp::ParameterValue(0.100));
|
||||
node->get_parameter(local_name + "max_time", max_time);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, local_name + "debug_optimizer", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(local_name + "debug_optimizer", debug);
|
||||
|
||||
advanced.get(node, name);
|
||||
}
|
||||
|
||||
bool debug;
|
||||
int max_iterations; // Ceres default: 50
|
||||
double max_time; // Ceres default: 1e4
|
||||
|
||||
double param_tol; // Ceres default: 1e-8
|
||||
double fn_tol; // Ceres default: 1e-6
|
||||
double gradient_tol; // Ceres default: 1e-10
|
||||
|
||||
AdvancedParams advanced;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__OPTIONS_HPP_
|
||||
@@ -0,0 +1,146 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__SMOOTHER_HPP_
|
||||
#define DEPRECATED__SMOOTHER_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_smac_planner/smoother_cost_function.hpp"
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @class nav2_smac_planner::Smoother
|
||||
* @brief A Conjugate Gradient 2D path smoother implementation
|
||||
*/
|
||||
class Smoother
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::Smoother
|
||||
*/
|
||||
Smoother() {}
|
||||
|
||||
/**
|
||||
* @brief A destructor for nav2_smac_planner::Smoother
|
||||
*/
|
||||
~Smoother() {}
|
||||
|
||||
/**
|
||||
* @brief Initialization of the smoother
|
||||
* @param params OptimizerParam struct
|
||||
*/
|
||||
void initialize(const OptimizerParams params)
|
||||
{
|
||||
_debug = params.debug;
|
||||
|
||||
// General Params
|
||||
|
||||
// 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT
|
||||
_options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
|
||||
_options.line_search_type = ceres::WOLFE;
|
||||
_options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
|
||||
_options.line_search_interpolation_type = ceres::CUBIC;
|
||||
|
||||
_options.max_num_iterations = params.max_iterations;
|
||||
_options.max_solver_time_in_seconds = params.max_time;
|
||||
|
||||
_options.function_tolerance = params.fn_tol;
|
||||
_options.gradient_tolerance = params.gradient_tol;
|
||||
_options.parameter_tolerance = params.param_tol;
|
||||
|
||||
_options.min_line_search_step_size = params.advanced.min_line_search_step_size;
|
||||
_options.max_num_line_search_step_size_iterations =
|
||||
params.advanced.max_num_line_search_step_size_iterations;
|
||||
_options.line_search_sufficient_function_decrease =
|
||||
params.advanced.line_search_sufficient_function_decrease;
|
||||
_options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
|
||||
_options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
|
||||
_options.max_num_line_search_direction_restarts =
|
||||
params.advanced.max_num_line_search_direction_restarts;
|
||||
_options.line_search_sufficient_curvature_decrease =
|
||||
params.advanced.line_search_sufficient_curvature_decrease;
|
||||
_options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
|
||||
|
||||
if (_debug) {
|
||||
_options.minimizer_progress_to_stdout = true;
|
||||
} else {
|
||||
_options.logging_type = ceres::SILENT;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Smoother method
|
||||
* @param path Reference to path
|
||||
* @param costmap Pointer to minimal costmap
|
||||
* @param smoother parameters weights
|
||||
* @return If smoothing was successful
|
||||
*/
|
||||
bool smooth(
|
||||
std::vector<Eigen::Vector2d> & path,
|
||||
nav2_costmap_2d::Costmap2D * costmap,
|
||||
const SmootherParams & params)
|
||||
{
|
||||
_options.max_solver_time_in_seconds = params.max_time;
|
||||
|
||||
#ifdef _MSC_VER
|
||||
std::vector<double> parameters_vec(path.size() * 2);
|
||||
double * parameters = parameters_vec.data();
|
||||
#else
|
||||
double parameters[path.size() * 2]; // NOLINT
|
||||
#endif
|
||||
for (unsigned int i = 0; i != path.size(); i++) {
|
||||
parameters[2 * i] = path[i][0];
|
||||
parameters[2 * i + 1] = path[i][1];
|
||||
}
|
||||
|
||||
ceres::GradientProblemSolver::Summary summary;
|
||||
ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params));
|
||||
ceres::Solve(_options, problem, parameters, &summary);
|
||||
|
||||
if (_debug) {
|
||||
std::cout << summary.FullReport() << '\n';
|
||||
}
|
||||
|
||||
if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i != path.size(); i++) {
|
||||
path[i][0] = parameters[2 * i];
|
||||
path[i][1] = parameters[2 * i + 1];
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
bool _debug;
|
||||
ceres::GradientProblemSolver::Options _options;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__SMOOTHER_HPP_
|
||||
@@ -0,0 +1,542 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
|
||||
#define DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_smac_planner/options.hpp"
|
||||
|
||||
#define EPSILON 0.0001
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct nav2_smac_planner::UnconstrainedSmootherCostFunction
|
||||
* @brief Cost function for path smoothing with multiple terms
|
||||
* including curvature, smoothness, collision, and avoid obstacles.
|
||||
*/
|
||||
class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::UnconstrainedSmootherCostFunction
|
||||
* @param original_path Original unsmoothed path to smooth
|
||||
* @param costmap A costmap to get values for collision and obstacle avoidance
|
||||
*/
|
||||
UnconstrainedSmootherCostFunction(
|
||||
std::vector<Eigen::Vector2d> * original_path,
|
||||
nav2_costmap_2d::Costmap2D * costmap,
|
||||
const SmootherParams & params)
|
||||
: _original_path(original_path),
|
||||
_num_params(2 * original_path->size()),
|
||||
_costmap(costmap),
|
||||
_params(params)
|
||||
{
|
||||
// int height = costmap->getSizeInCellsX();
|
||||
// int width = costmap->getSizeInCellsY();
|
||||
// bool** binMap;
|
||||
// binMap = new bool*[width];
|
||||
|
||||
// for (int x = 0; x < width; x++) { binMap[x] = new bool[height]; }
|
||||
|
||||
// for (int x = 0; x < width; ++x) {
|
||||
// for (int y = 0; y < height; ++y) {
|
||||
// binMap[x][y] = costmap->getCost(x,y) >= 253 ? true : false;
|
||||
// }
|
||||
// }
|
||||
|
||||
// voronoiDiagram.initializeMap(width, height, binMap);
|
||||
// voronoiDiagram.update();
|
||||
// voronoiDiagram.visualize();
|
||||
}
|
||||
|
||||
/**
|
||||
* @struct CurvatureComputations
|
||||
* @brief Cache common computations between the curvature terms to minimize recomputations
|
||||
*/
|
||||
struct CurvatureComputations
|
||||
{
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::CurvatureComputations
|
||||
*/
|
||||
CurvatureComputations()
|
||||
{
|
||||
valid = true;
|
||||
}
|
||||
|
||||
bool valid;
|
||||
/**
|
||||
* @brief Check if result is valid for penalty
|
||||
* @return is valid (non-nan, non-inf, and turning angle > max)
|
||||
*/
|
||||
bool isValid()
|
||||
{
|
||||
return valid;
|
||||
}
|
||||
|
||||
Eigen::Vector2d delta_xi{0.0, 0.0};
|
||||
Eigen::Vector2d delta_xi_p{0.0, 0.0};
|
||||
double delta_xi_norm{0};
|
||||
double delta_xi_p_norm{0};
|
||||
double delta_phi_i{0};
|
||||
double turning_rad{0};
|
||||
double ki_minus_kmax{0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Smoother cost function evaluation
|
||||
* @param parameters X,Y pairs of points
|
||||
* @param cost total cost of path
|
||||
* @param gradient of path at each X,Y pair from cost function derived analytically
|
||||
* @return if successful in computing values
|
||||
*/
|
||||
virtual bool Evaluate(
|
||||
const double * parameters,
|
||||
double * cost,
|
||||
double * gradient) const
|
||||
{
|
||||
Eigen::Vector2d xi;
|
||||
Eigen::Vector2d xi_p1;
|
||||
Eigen::Vector2d xi_m1;
|
||||
unsigned int x_index, y_index;
|
||||
cost[0] = 0.0;
|
||||
double cost_raw = 0.0;
|
||||
double grad_x_raw = 0.0;
|
||||
double grad_y_raw = 0.0;
|
||||
unsigned int mx, my;
|
||||
bool valid_coords = true;
|
||||
double costmap_cost = 0.0;
|
||||
|
||||
// cache some computations between the residual and jacobian
|
||||
CurvatureComputations curvature_params;
|
||||
|
||||
for (int i = 0; i != NumParameters() / 2; i++) {
|
||||
x_index = 2 * i;
|
||||
y_index = 2 * i + 1;
|
||||
gradient[x_index] = 0.0;
|
||||
gradient[y_index] = 0.0;
|
||||
if (i < 1 || i >= NumParameters() / 2 - 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]);
|
||||
xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]);
|
||||
xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]);
|
||||
|
||||
// compute cost
|
||||
addSmoothingResidual(_params.smooth_weight, xi, xi_p1, xi_m1, cost_raw);
|
||||
addCurvatureResidual(_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, cost_raw);
|
||||
addDistanceResidual(_params.distance_weight, xi, _original_path->at(i), cost_raw);
|
||||
|
||||
if (valid_coords = _costmap->worldToMap(xi[0], xi[1], mx, my)) {
|
||||
costmap_cost = _costmap->getCost(mx, my);
|
||||
addCostResidual(_params.costmap_weight, costmap_cost, cost_raw, xi);
|
||||
}
|
||||
|
||||
if (gradient != NULL) {
|
||||
// compute gradient
|
||||
gradient[x_index] = 0.0;
|
||||
gradient[y_index] = 0.0;
|
||||
addSmoothingJacobian(_params.smooth_weight, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
|
||||
addCurvatureJacobian(
|
||||
_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params,
|
||||
grad_x_raw, grad_y_raw);
|
||||
addDistanceJacobian(
|
||||
_params.distance_weight, xi, _original_path->at(
|
||||
i), grad_x_raw, grad_y_raw);
|
||||
|
||||
if (valid_coords) {
|
||||
addCostJacobian(_params.costmap_weight, mx, my, costmap_cost, grad_x_raw, grad_y_raw);
|
||||
}
|
||||
|
||||
gradient[x_index] = grad_x_raw;
|
||||
gradient[y_index] = grad_y_raw;
|
||||
}
|
||||
}
|
||||
|
||||
cost[0] = cost_raw;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get number of parameter blocks
|
||||
* @return Number of parameters in cost function
|
||||
*/
|
||||
virtual int NumParameters() const {return _num_params;}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Cost function term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addSmoothingResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & r) const
|
||||
{
|
||||
r += weight * (
|
||||
pt_p.dot(pt_p) -
|
||||
4 * pt_p.dot(pt) +
|
||||
2 * pt_p.dot(pt_m) +
|
||||
4 * pt.dot(pt) -
|
||||
4 * pt.dot(pt_m) +
|
||||
pt_m.dot(pt_m)); // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addSmoothingJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
j0 += weight *
|
||||
(-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative
|
||||
j1 += weight *
|
||||
(-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function term for maximum curved paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addCurvatureResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & r) const
|
||||
{
|
||||
curvature_params.valid = true;
|
||||
curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
|
||||
curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]);
|
||||
curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
|
||||
curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
|
||||
|
||||
if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
|
||||
std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
|
||||
std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm))
|
||||
{
|
||||
// ensure we have non-nan values returned
|
||||
curvature_params.valid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
const double & delta_xi_by_xi_p =
|
||||
curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
|
||||
double projection =
|
||||
curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
|
||||
if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
|
||||
projection = 1.0;
|
||||
}
|
||||
|
||||
curvature_params.delta_phi_i = std::acos(projection);
|
||||
curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
|
||||
|
||||
curvature_params.ki_minus_kmax = curvature_params.turning_rad - _params.max_curvature;
|
||||
|
||||
if (curvature_params.ki_minus_kmax <= EPSILON) {
|
||||
// Quadratic penalty need not apply
|
||||
curvature_params.valid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
r += weight *
|
||||
curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for maximum curvature paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct with cached values to speed up Jacobian computation
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addCurvatureJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & /*pt_m*/,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
|
||||
-1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
|
||||
// const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
|
||||
auto neg_pt_plus = -1 * pt_p;
|
||||
Eigen::Vector2d p1 = normalizedOrthogonalComplement(
|
||||
pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
|
||||
Eigen::Vector2d p2 = normalizedOrthogonalComplement(
|
||||
neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
|
||||
|
||||
const double & u = 2 * curvature_params.ki_minus_kmax;
|
||||
const double & common_prefix =
|
||||
(1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
|
||||
const double & common_suffix = curvature_params.delta_phi_i /
|
||||
(curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
|
||||
|
||||
const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
|
||||
curvature_params.delta_xi_norm;
|
||||
|
||||
const Eigen::Vector2d jacobian = u *
|
||||
(common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_im1 = u *
|
||||
(common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
|
||||
|
||||
// Old formulation we may require again.
|
||||
// j0 += weight *
|
||||
// (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);
|
||||
// j1 += weight *
|
||||
// (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);
|
||||
|
||||
j0 += weight * jacobian[0]; // xi x component of partial-derivative
|
||||
j1 += weight * jacobian[1]; // xi x component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for steering away changes in pose
|
||||
* @param weight Weight to apply to function
|
||||
* @param xi Point Xi for evaluation
|
||||
* @param xi_original original point Xi for evaluation
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addDistanceResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & xi,
|
||||
const Eigen::Vector2d & xi_original,
|
||||
double & r) const
|
||||
{
|
||||
r += weight * (xi - xi_original).dot(xi - xi_original); // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for steering away changes in pose
|
||||
* @param weight Weight to apply to function
|
||||
* @param xi Point Xi for evaluation
|
||||
* @param xi_original original point Xi for evaluation
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addDistanceJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & xi,
|
||||
const Eigen::Vector2d & xi_original,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
j0 += weight * 2 * (xi[0] - xi_original[0]); // xi y component of partial-derivative
|
||||
j1 += weight * 2 * (xi[1] - xi_original[1]); // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Cost function term for steering away from costs
|
||||
* @param weight Weight to apply to function
|
||||
* @param value Point Xi's cost'
|
||||
* @param params computed values to reduce overhead
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addCostResidual(
|
||||
const double & weight,
|
||||
const double & value,
|
||||
double & r,
|
||||
Eigen::Vector2d & xi) const
|
||||
{
|
||||
if (value == FREE) {
|
||||
return;
|
||||
}
|
||||
|
||||
r += weight * value * value; // objective function value
|
||||
|
||||
|
||||
// float obsDst = voronoiDiagram.getDistance((int)xi[0], (int)xi[1]);
|
||||
|
||||
// if (abs(obsDst) > 0.3) {
|
||||
// return;
|
||||
// }
|
||||
|
||||
// r += weight * (abs(obsDst) - 0.3) * (abs(obsDst) - 0.3);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for steering away from costs
|
||||
* @param weight Weight to apply to function
|
||||
* @param mx Point Xi's x coordinate in map frame
|
||||
* @param mx Point Xi's y coordinate in map frame
|
||||
* @param value Point Xi's cost'
|
||||
* @param params computed values to reduce overhead
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addCostJacobian(
|
||||
const double & weight,
|
||||
const unsigned int & mx,
|
||||
const unsigned int & my,
|
||||
const double & value,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
if (value == FREE) {
|
||||
return;
|
||||
}
|
||||
|
||||
const Eigen::Vector2d grad = getCostmapGradient(mx, my);
|
||||
const double common_prefix = -2.0 * _params.costmap_factor * weight * value * value;
|
||||
|
||||
j0 += common_prefix * grad[0]; // xi x component of partial-derivative
|
||||
j1 += common_prefix * grad[1]; // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Computing the gradient of the costmap using
|
||||
* the 2 point numerical differentiation method
|
||||
* @param mx Point Xi's x coordinate in map frame
|
||||
* @param mx Point Xi's y coordinate in map frame
|
||||
* @param params Params reference to store gradients
|
||||
*/
|
||||
inline Eigen::Vector2d getCostmapGradient(
|
||||
const unsigned int mx,
|
||||
const unsigned int my) const
|
||||
{
|
||||
// find unit vector that describes that direction
|
||||
// via 7 point taylor series approximation for gradient at Xi
|
||||
Eigen::Vector2d gradient;
|
||||
|
||||
double l_1 = 0.0;
|
||||
double l_2 = 0.0;
|
||||
double l_3 = 0.0;
|
||||
double r_1 = 0.0;
|
||||
double r_2 = 0.0;
|
||||
double r_3 = 0.0;
|
||||
|
||||
if (mx < _costmap->getSizeInCellsX()) {
|
||||
r_1 = static_cast<double>(_costmap->getCost(mx + 1, my));
|
||||
}
|
||||
if (mx + 1 < _costmap->getSizeInCellsX()) {
|
||||
r_2 = static_cast<double>(_costmap->getCost(mx + 2, my));
|
||||
}
|
||||
if (mx + 2 < _costmap->getSizeInCellsX()) {
|
||||
r_3 = static_cast<double>(_costmap->getCost(mx + 3, my));
|
||||
}
|
||||
|
||||
if (mx > 0) {
|
||||
l_1 = static_cast<double>(_costmap->getCost(mx - 1, my));
|
||||
}
|
||||
if (mx - 1 > 0) {
|
||||
l_2 = static_cast<double>(_costmap->getCost(mx - 2, my));
|
||||
}
|
||||
if (mx - 2 > 0) {
|
||||
l_3 = static_cast<double>(_costmap->getCost(mx - 3, my));
|
||||
}
|
||||
|
||||
gradient[1] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60;
|
||||
|
||||
if (my < _costmap->getSizeInCellsY()) {
|
||||
r_1 = static_cast<double>(_costmap->getCost(mx, my + 1));
|
||||
}
|
||||
if (my + 1 < _costmap->getSizeInCellsY()) {
|
||||
r_2 = static_cast<double>(_costmap->getCost(mx, my + 2));
|
||||
}
|
||||
if (my + 2 < _costmap->getSizeInCellsY()) {
|
||||
r_3 = static_cast<double>(_costmap->getCost(mx, my + 3));
|
||||
}
|
||||
|
||||
if (my > 0) {
|
||||
l_1 = static_cast<double>(_costmap->getCost(mx, my - 1));
|
||||
}
|
||||
if (my - 1 > 0) {
|
||||
l_2 = static_cast<double>(_costmap->getCost(mx, my - 2));
|
||||
}
|
||||
if (my - 2 > 0) {
|
||||
l_3 = static_cast<double>(_costmap->getCost(mx, my - 3));
|
||||
}
|
||||
|
||||
gradient[0] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60;
|
||||
|
||||
gradient.normalize();
|
||||
return gradient;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Computing the normalized orthogonal component of 2 vectors
|
||||
* @param a Vector
|
||||
* @param b Vector
|
||||
* @param norm a Vector's norm
|
||||
* @param norm b Vector's norm
|
||||
* @return Normalized vector of orthogonal components
|
||||
*/
|
||||
inline Eigen::Vector2d normalizedOrthogonalComplement(
|
||||
const Eigen::Vector2d & a,
|
||||
const Eigen::Vector2d & b,
|
||||
const double & a_norm,
|
||||
const double & b_norm) const
|
||||
{
|
||||
return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
|
||||
}
|
||||
|
||||
std::vector<Eigen::Vector2d> * _original_path{nullptr};
|
||||
int _num_params;
|
||||
nav2_costmap_2d::Costmap2D * _costmap{nullptr};
|
||||
SmootherParams _params;
|
||||
// DynamicVoronoi voronoiDiagram;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
|
||||
@@ -0,0 +1,213 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__UPSAMPLER_HPP_
|
||||
#define DEPRECATED__UPSAMPLER_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <algorithm>
|
||||
#include <utility>
|
||||
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_smac_planner/upsampler_cost_function.hpp"
|
||||
#include "nav2_smac_planner/upsampler_cost_function_nlls.hpp"
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @class nav2_smac_planner::Upsampler
|
||||
* @brief A Conjugate Gradient 2D path upsampler implementation
|
||||
*/
|
||||
class Upsampler
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::Upsampler
|
||||
*/
|
||||
Upsampler() {}
|
||||
|
||||
/**
|
||||
* @brief A destructor for nav2_smac_planner::Upsampler
|
||||
*/
|
||||
~Upsampler() {}
|
||||
|
||||
/**
|
||||
* @brief Initialization of the Upsampler
|
||||
*/
|
||||
void initialize(const OptimizerParams params)
|
||||
{
|
||||
_debug = params.debug;
|
||||
|
||||
// General Params
|
||||
|
||||
// 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT
|
||||
_options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
|
||||
_options.line_search_type = ceres::WOLFE;
|
||||
_options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
|
||||
_options.line_search_interpolation_type = ceres::CUBIC;
|
||||
|
||||
_options.max_num_iterations = params.max_iterations; // 5000
|
||||
_options.max_solver_time_in_seconds = params.max_time; // 5.0; // TODO
|
||||
|
||||
_options.function_tolerance = params.fn_tol;
|
||||
_options.gradient_tolerance = params.gradient_tol;
|
||||
_options.parameter_tolerance = params.param_tol; // 1e-20;
|
||||
|
||||
_options.min_line_search_step_size = params.advanced.min_line_search_step_size; // 1e-30;
|
||||
_options.max_num_line_search_step_size_iterations =
|
||||
params.advanced.max_num_line_search_step_size_iterations;
|
||||
_options.line_search_sufficient_function_decrease =
|
||||
params.advanced.line_search_sufficient_function_decrease; // 1e-30;
|
||||
_options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
|
||||
_options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
|
||||
_options.max_num_line_search_direction_restarts =
|
||||
params.advanced.max_num_line_search_direction_restarts;
|
||||
_options.line_search_sufficient_curvature_decrease =
|
||||
params.advanced.line_search_sufficient_curvature_decrease;
|
||||
_options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
|
||||
|
||||
if (_debug) {
|
||||
_options.minimizer_progress_to_stdout = true;
|
||||
} else {
|
||||
_options.logging_type = ceres::SILENT;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Upsampling method
|
||||
* @param path Reference to path
|
||||
* @param upsample parameters weights
|
||||
* @param upsample_ratio upsample ratio
|
||||
* @return If Upsampler was successful
|
||||
*/
|
||||
bool upsample(
|
||||
std::vector<Eigen::Vector2d> & path,
|
||||
const SmootherParams & params,
|
||||
const int & upsample_ratio)
|
||||
{
|
||||
_options.max_solver_time_in_seconds = params.max_time;
|
||||
|
||||
if (upsample_ratio != 2 && upsample_ratio != 4) {
|
||||
// invalid inputs
|
||||
return false;
|
||||
}
|
||||
|
||||
const int param_ratio = upsample_ratio * 2.0;
|
||||
const int total_size = 2 * (path.size() * upsample_ratio - upsample_ratio + 1);
|
||||
double parameters[total_size]; // NOLINT
|
||||
|
||||
// 20-4hz regularly, but dosnt work in faster cases
|
||||
// Linearly distribute initial poses for optimization
|
||||
// TODO(stevemacenski) generalize for 2x and 4x
|
||||
unsigned int next_pt;
|
||||
Eigen::Vector2d interpolated;
|
||||
std::vector<Eigen::Vector2d> temp_path;
|
||||
for (unsigned int pt = 0; pt != path.size() - 1; pt++) {
|
||||
next_pt = pt + 1;
|
||||
interpolated = (path[next_pt] + path[pt]) / 2.0;
|
||||
|
||||
parameters[param_ratio * pt] = path[pt][0];
|
||||
parameters[param_ratio * pt + 1] = path[pt][1];
|
||||
temp_path.push_back(path[pt]);
|
||||
|
||||
parameters[param_ratio * pt + 2] = interpolated[0];
|
||||
parameters[param_ratio * pt + 3] = interpolated[1];
|
||||
temp_path.push_back(interpolated);
|
||||
}
|
||||
|
||||
parameters[total_size - 2] = path.back()[0];
|
||||
parameters[total_size - 1] = path.back()[1];
|
||||
temp_path.push_back(path.back());
|
||||
|
||||
// Solve the upsampling problem
|
||||
ceres::GradientProblemSolver::Summary summary;
|
||||
ceres::GradientProblem problem(new UpsamplerCostFunction(temp_path, params, upsample_ratio));
|
||||
ceres::Solve(_options, problem, parameters, &summary);
|
||||
|
||||
|
||||
path.resize(total_size / 2);
|
||||
for (int i = 0; i != total_size / 2; i++) {
|
||||
path[i][0] = parameters[2 * i];
|
||||
path[i][1] = parameters[2 * i + 1];
|
||||
}
|
||||
|
||||
// 10-15 hz, regularly
|
||||
// std::vector<Eigen::Vector2d> path_double_sampled;
|
||||
// for (int i = 0; i != path.size() - 1; i++) { // last term should not be upsampled
|
||||
// path_double_sampled.push_back(path[i]);
|
||||
// path_double_sampled.push_back((path[i+1] + path[i]) / 2);
|
||||
// }
|
||||
|
||||
// std::unique_ptr<ceres::Problem> problem = std::make_unique<ceres::Problem>();
|
||||
// for (uint i = 1; i != path_double_sampled.size() - 1; i++) {
|
||||
// ceres::CostFunction * cost_fn =
|
||||
// new UpsamplerConstrainedCostFunction(path_double_sampled, params, 2, i);
|
||||
// problem->AddResidualBlock(
|
||||
// cost_fn, nullptr, &path_double_sampled[i][0], &path_double_sampled[i][1]);
|
||||
// // locking initial coordinates unnecessary since there's no update between terms in NLLS
|
||||
// }
|
||||
|
||||
// ceres::Solver::Summary summary;
|
||||
// _options.minimizer_type = ceres::LINE_SEARCH;
|
||||
// ceres::Solve(_options, problem.get(), &summary);
|
||||
|
||||
// if (upsample_ratio == 4) {
|
||||
// std::vector<Eigen::Vector2d> path_quad_sampled;
|
||||
// for (int i = 0; i != path_double_sampled.size() - 1; i++) {
|
||||
// path_quad_sampled.push_back(path_double_sampled[i]);
|
||||
// path_quad_sampled.push_back((path_double_sampled[i+1] + path_double_sampled[i]) / 2.0);
|
||||
// }
|
||||
|
||||
// std::unique_ptr<ceres::Problem> problem2 = std::make_unique<ceres::Problem>();
|
||||
// for (uint i = 1; i != path_quad_sampled.size() - 1; i++) {
|
||||
// ceres::CostFunction * cost_fn =
|
||||
// new UpsamplerConstrainedCostFunction(path_quad_sampled, params, 4, i);
|
||||
// problem2->AddResidualBlock(
|
||||
// cost_fn, nullptr, &path_quad_sampled[i][0], &path_quad_sampled[i][1]);
|
||||
// }
|
||||
|
||||
// ceres::Solve(_options, problem2.get(), &summary);
|
||||
|
||||
// path = path_quad_sampled;
|
||||
// } else {
|
||||
// path = path_double_sampled;
|
||||
// }
|
||||
|
||||
if (_debug) {
|
||||
std::cout << summary.FullReport() << '\n';
|
||||
}
|
||||
|
||||
if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
bool _debug;
|
||||
ceres::GradientProblemSolver::Options _options;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__UPSAMPLER_HPP_
|
||||
@@ -0,0 +1,366 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
|
||||
#define DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_smac_planner/options.hpp"
|
||||
|
||||
#define EPSILON 0.0001
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
/**
|
||||
* @struct nav2_smac_planner::UpsamplerCostFunction
|
||||
* @brief Cost function for path upsampling with multiple terms using unconstrained
|
||||
* optimization including curvature, smoothness, collision, and avoid obstacles.
|
||||
*/
|
||||
class UpsamplerCostFunction : public ceres::FirstOrderFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::UpsamplerCostFunction
|
||||
* @param num_points Number of path points to consider
|
||||
*/
|
||||
UpsamplerCostFunction(
|
||||
const std::vector<Eigen::Vector2d> & path,
|
||||
const SmootherParams & params,
|
||||
const int & upsample_ratio)
|
||||
: _num_params(2 * path.size()),
|
||||
_params(params),
|
||||
_upsample_ratio(upsample_ratio),
|
||||
_path(path)
|
||||
{
|
||||
}
|
||||
// TODO(stevemacenski) removed upsample_ratio because temp upsampling on path size
|
||||
|
||||
/**
|
||||
* @struct CurvatureComputations
|
||||
* @brief Cache common computations between the curvature terms to minimize recomputations
|
||||
*/
|
||||
struct CurvatureComputations
|
||||
{
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::CurvatureComputations
|
||||
*/
|
||||
CurvatureComputations()
|
||||
{
|
||||
valid = false;
|
||||
}
|
||||
|
||||
bool valid;
|
||||
/**
|
||||
* @brief Check if result is valid for penalty
|
||||
* @return is valid (non-nan, non-inf, and turning angle > max)
|
||||
*/
|
||||
bool isValid()
|
||||
{
|
||||
return valid;
|
||||
}
|
||||
|
||||
Eigen::Vector2d delta_xi{0, 0};
|
||||
Eigen::Vector2d delta_xi_p{0, 0};
|
||||
double delta_xi_norm{0};
|
||||
double delta_xi_p_norm{0};
|
||||
double delta_phi_i{0};
|
||||
double turning_rad{0};
|
||||
double ki_minus_kmax{0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Smoother cost function evaluation
|
||||
* @param parameters X,Y pairs of points
|
||||
* @param cost total cost of path
|
||||
* @param gradient of path at each X,Y pair from cost function derived analytically
|
||||
* @return if successful in computing values
|
||||
*/
|
||||
virtual bool Evaluate(
|
||||
const double * parameters,
|
||||
double * cost,
|
||||
double * gradient) const
|
||||
{
|
||||
Eigen::Vector2d xi;
|
||||
Eigen::Vector2d xi_p1;
|
||||
Eigen::Vector2d xi_m1;
|
||||
uint x_index, y_index;
|
||||
cost[0] = 0.0;
|
||||
double cost_raw = 0.0;
|
||||
double grad_x_raw = 0.0;
|
||||
double grad_y_raw = 0.0;
|
||||
|
||||
// cache some computations between the residual and jacobian
|
||||
CurvatureComputations curvature_params;
|
||||
|
||||
for (int i = 0; i != NumParameters() / 2; i++) {
|
||||
x_index = 2 * i;
|
||||
y_index = 2 * i + 1;
|
||||
gradient[x_index] = 0.0;
|
||||
gradient[y_index] = 0.0;
|
||||
if (i < 1 || i >= NumParameters() / 2 - 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// if original point's neighbors TODO
|
||||
if (i % _upsample_ratio == 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]);
|
||||
|
||||
// TODO(stevemacenski): from deep copy to make sure no feedback _path
|
||||
xi_p1 = _path.at(i + 1);
|
||||
xi_m1 = _path.at(i - 1);
|
||||
// xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]);
|
||||
// xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]);
|
||||
|
||||
// compute cost
|
||||
addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw);
|
||||
addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw);
|
||||
|
||||
if (gradient != NULL) {
|
||||
// compute gradient
|
||||
addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
|
||||
addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw);
|
||||
|
||||
gradient[x_index] = grad_x_raw;
|
||||
gradient[y_index] = grad_y_raw;
|
||||
}
|
||||
}
|
||||
|
||||
cost[0] = cost_raw;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get number of parameter blocks
|
||||
* @return Number of parameters in cost function
|
||||
*/
|
||||
virtual int NumParameters() const {return _num_params;}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Cost function term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addSmoothingResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & r) const
|
||||
{
|
||||
r += weight * (
|
||||
pt_p.dot(pt_p) -
|
||||
4 * pt_p.dot(pt) +
|
||||
2 * pt_p.dot(pt_m) +
|
||||
4 * pt.dot(pt) -
|
||||
4 * pt.dot(pt_m) +
|
||||
pt_m.dot(pt_m)); // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addSmoothingJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
j0 += weight *
|
||||
(-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative
|
||||
j1 += weight *
|
||||
(-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get path curvature information
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
*/
|
||||
inline void getCurvatureParams(
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params) const
|
||||
{
|
||||
curvature_params.valid = true;
|
||||
curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
|
||||
curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]);
|
||||
curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
|
||||
curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
|
||||
|
||||
if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
|
||||
std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
|
||||
std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm))
|
||||
{
|
||||
// ensure we have non-nan values returned
|
||||
curvature_params.valid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
const double & delta_xi_by_xi_p =
|
||||
curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
|
||||
double projection =
|
||||
curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
|
||||
if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
|
||||
projection = 1.0;
|
||||
}
|
||||
|
||||
curvature_params.delta_phi_i = std::acos(projection);
|
||||
curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
|
||||
|
||||
curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
|
||||
_params.max_curvature;
|
||||
// TODO(stevemacenski) is use of upsample_ratio correct here? small number?
|
||||
// TODO(stevemacenski) can remove the subtraction with a
|
||||
// lower weight value, does have direction issue, maybe just tuning?
|
||||
|
||||
if (curvature_params.ki_minus_kmax <= EPSILON) {
|
||||
// Quadratic penalty need not apply
|
||||
curvature_params.valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function term for maximum curved paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addCurvatureResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & r) const
|
||||
{
|
||||
getCurvatureParams(pt, pt_p, pt_m, curvature_params);
|
||||
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
r += weight *
|
||||
curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for maximum curvature paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct with cached values to speed up Jacobian computation
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addCurvatureJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & /*pt_m*/,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
|
||||
-1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
|
||||
// const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
|
||||
auto neg_pt_plus = -1 * pt_p;
|
||||
Eigen::Vector2d p1 = normalizedOrthogonalComplement(
|
||||
pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
|
||||
Eigen::Vector2d p2 = normalizedOrthogonalComplement(
|
||||
neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
|
||||
|
||||
const double & u = 2 * curvature_params.ki_minus_kmax;
|
||||
const double & common_prefix =
|
||||
(1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
|
||||
const double & common_suffix = curvature_params.delta_phi_i /
|
||||
(curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
|
||||
const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
|
||||
curvature_params.delta_xi_norm;
|
||||
|
||||
const Eigen::Vector2d jacobian = u *
|
||||
(common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_im1 = u *
|
||||
(common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
|
||||
j0 += weight * jacobian[0]; // xi y component of partial-derivative
|
||||
j1 += weight * jacobian[1]; // xi x component of partial-derivative
|
||||
// j0 += weight *
|
||||
// (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);
|
||||
// j1 += weight *
|
||||
// (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Computing the normalized orthogonal component of 2 vectors
|
||||
* @param a Vector
|
||||
* @param b Vector
|
||||
* @param norm a Vector's norm
|
||||
* @param norm b Vector's norm
|
||||
* @return Normalized vector of orthogonal components
|
||||
*/
|
||||
inline Eigen::Vector2d normalizedOrthogonalComplement(
|
||||
const Eigen::Vector2d & a,
|
||||
const Eigen::Vector2d & b,
|
||||
const double & a_norm,
|
||||
const double & b_norm) const
|
||||
{
|
||||
return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
|
||||
}
|
||||
|
||||
int _num_params;
|
||||
SmootherParams _params;
|
||||
int _upsample_ratio;
|
||||
std::vector<Eigen::Vector2d> _path;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
|
||||
@@ -0,0 +1,334 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
|
||||
#define DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "ceres/ceres.h"
|
||||
#include "Eigen/Core"
|
||||
#include "nav2_smac_planner/types.hpp"
|
||||
#include "nav2_smac_planner/options.hpp"
|
||||
|
||||
#define EPSILON 0.0001
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
/**
|
||||
* @struct nav2_smac_planner::UpsamplerConstrainedCostFunction
|
||||
* @brief Cost function for path upsampling with multiple terms using NLLS
|
||||
* including curvature, smoothness, collision, and avoid obstacles.
|
||||
*/
|
||||
class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::UpsamplerConstrainedCostFunction
|
||||
* @param num_points Number of path points to consider
|
||||
*/
|
||||
UpsamplerConstrainedCostFunction(
|
||||
const std::vector<Eigen::Vector2d> & path,
|
||||
const SmootherParams & params,
|
||||
const int & upsample_ratio,
|
||||
const int & i)
|
||||
: _path(path),
|
||||
_params(params),
|
||||
_upsample_ratio(upsample_ratio),
|
||||
index(i)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @struct CurvatureComputations
|
||||
* @brief Cache common computations between the curvature terms to minimize recomputations
|
||||
*/
|
||||
struct CurvatureComputations
|
||||
{
|
||||
/**
|
||||
* @brief A constructor for nav2_smac_planner::CurvatureComputations
|
||||
*/
|
||||
CurvatureComputations()
|
||||
{
|
||||
valid = true;
|
||||
}
|
||||
|
||||
bool valid;
|
||||
/**
|
||||
* @brief Check if result is valid for penalty
|
||||
* @return is valid (non-nan, non-inf, and turning angle > max)
|
||||
*/
|
||||
bool isValid()
|
||||
{
|
||||
return valid;
|
||||
}
|
||||
|
||||
Eigen::Vector2d delta_xi{0, 0};
|
||||
Eigen::Vector2d delta_xi_p{0, 0};
|
||||
double delta_xi_norm{0};
|
||||
double delta_xi_p_norm{0};
|
||||
double delta_phi_i{0};
|
||||
double turning_rad{0};
|
||||
double ki_minus_kmax{0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Smoother cost function evaluation
|
||||
* @param parameters X,Y pairs of points
|
||||
* @param cost total cost of path
|
||||
* @param gradient of path at each X,Y pair from cost function derived analytically
|
||||
* @return if successful in computing values
|
||||
*/
|
||||
|
||||
bool Evaluate(
|
||||
double const * const * parameters,
|
||||
double * residuals,
|
||||
double ** jacobians) const override
|
||||
{
|
||||
Eigen::Vector2d xi = Eigen::Vector2d(parameters[0][0], parameters[1][0]);
|
||||
Eigen::Vector2d xi_p1 = _path.at(index + 1);
|
||||
Eigen::Vector2d xi_m1 = _path.at(index - 1);
|
||||
CurvatureComputations curvature_params;
|
||||
double grad_x_raw = 0, grad_y_raw = 0, cost_raw = 0;
|
||||
|
||||
// compute cost
|
||||
addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw);
|
||||
addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw);
|
||||
|
||||
residuals[0] = 0;
|
||||
residuals[0] = cost_raw; // objective function value x
|
||||
|
||||
if (jacobians != NULL && jacobians[0] != NULL) {
|
||||
addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
|
||||
addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw);
|
||||
|
||||
jacobians[0][0] = 0;
|
||||
jacobians[1][0] = 0;
|
||||
jacobians[0][0] = grad_x_raw; // x derivative
|
||||
jacobians[1][0] = grad_y_raw; // y derivative
|
||||
jacobians[0][1] = 0.0;
|
||||
jacobians[1][1] = 0.0;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Cost function term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addSmoothingResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & r) const
|
||||
{
|
||||
r += weight * (
|
||||
pt_p.dot(pt_p) -
|
||||
4 * pt_p.dot(pt) +
|
||||
2 * pt_p.dot(pt_m) +
|
||||
4 * pt.dot(pt) -
|
||||
4 * pt.dot(pt_m) +
|
||||
pt_m.dot(pt_m)); // objective function value
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for smooth paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addSmoothingJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
j0 += weight *
|
||||
(-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative
|
||||
j1 += weight *
|
||||
(-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get path curvature information
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
*/
|
||||
inline void getCurvatureParams(
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params) const
|
||||
{
|
||||
curvature_params.valid = true;
|
||||
curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
|
||||
curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]);
|
||||
curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
|
||||
curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
|
||||
|
||||
if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
|
||||
std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
|
||||
std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm))
|
||||
{
|
||||
// ensure we have non-nan values returned
|
||||
curvature_params.valid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
const double & delta_xi_by_xi_p =
|
||||
curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
|
||||
double projection =
|
||||
curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
|
||||
if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
|
||||
projection = 1.0;
|
||||
}
|
||||
|
||||
curvature_params.delta_phi_i = std::acos(projection);
|
||||
curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
|
||||
|
||||
curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
|
||||
_params.max_curvature;
|
||||
|
||||
if (curvature_params.ki_minus_kmax <= EPSILON) {
|
||||
// Quadratic penalty need not apply
|
||||
curvature_params.valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function term for maximum curved paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct to cache computations for the jacobian to use
|
||||
* @param r Residual (cost) of term
|
||||
*/
|
||||
inline void addCurvatureResidual(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & pt_m,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & r) const
|
||||
{
|
||||
getCurvatureParams(pt, pt_p, pt_m, curvature_params);
|
||||
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// objective function value
|
||||
r += weight *
|
||||
curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cost function derivative term for maximum curvature paths
|
||||
* @param weight Weight to apply to function
|
||||
* @param pt Point Xi for evaluation
|
||||
* @param pt Point Xi+1 for calculating Xi's cost
|
||||
* @param pt Point Xi-1 for calculating Xi's cost
|
||||
* @param curvature_params A struct with cached values to speed up Jacobian computation
|
||||
* @param j0 Gradient of X term
|
||||
* @param j1 Gradient of Y term
|
||||
*/
|
||||
inline void addCurvatureJacobian(
|
||||
const double & weight,
|
||||
const Eigen::Vector2d & pt,
|
||||
const Eigen::Vector2d & pt_p,
|
||||
const Eigen::Vector2d & /*pt_m*/,
|
||||
CurvatureComputations & curvature_params,
|
||||
double & j0,
|
||||
double & j1) const
|
||||
{
|
||||
if (!curvature_params.isValid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
|
||||
-1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
|
||||
// const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
|
||||
auto neg_pt_plus = -1 * pt_p;
|
||||
Eigen::Vector2d p1 = normalizedOrthogonalComplement(
|
||||
pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
|
||||
Eigen::Vector2d p2 = normalizedOrthogonalComplement(
|
||||
neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
|
||||
|
||||
const double & u = 2 * curvature_params.ki_minus_kmax;
|
||||
const double & common_prefix =
|
||||
(1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
|
||||
const double & common_suffix = curvature_params.delta_phi_i /
|
||||
(curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
|
||||
const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
|
||||
curvature_params.delta_xi_norm;
|
||||
|
||||
const Eigen::Vector2d jacobian = u *
|
||||
(common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_im1 = u *
|
||||
(common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
|
||||
const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
|
||||
j0 += weight * jacobian[0]; // xi x component of partial-derivative
|
||||
j1 += weight * jacobian[1]; // xi y component of partial-derivative
|
||||
// j0 += weight *
|
||||
// (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);
|
||||
// j1 += weight *
|
||||
// (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);
|
||||
}
|
||||
/**
|
||||
* @brief Computing the normalized orthogonal component of 2 vectors
|
||||
* @param a Vector
|
||||
* @param b Vector
|
||||
* @param norm a Vector's norm
|
||||
* @param norm b Vector's norm
|
||||
* @return Normalized vector of orthogonal components
|
||||
*/
|
||||
inline Eigen::Vector2d normalizedOrthogonalComplement(
|
||||
const Eigen::Vector2d & a,
|
||||
const Eigen::Vector2d & b,
|
||||
const double & a_norm,
|
||||
const double & b_norm) const
|
||||
{
|
||||
return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
|
||||
}
|
||||
|
||||
std::vector<Eigen::Vector2d> _path;
|
||||
SmootherParams _params;
|
||||
int _upsample_ratio;
|
||||
int index;
|
||||
};
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 120 KiB |
@@ -0,0 +1,308 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <limits>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_subscriber.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_smac_planner/node_hybrid.hpp"
|
||||
#include "nav2_smac_planner/node_lattice.hpp"
|
||||
#include "nav2_smac_planner/a_star.hpp"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
#include "ament_index_cpp/get_package_share_directory.hpp"
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
TEST(AStarTest, test_a_star_2d)
|
||||
{
|
||||
auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star(
|
||||
nav2_smac_planner::MotionModel::TWOD, info);
|
||||
int max_iterations = 10000;
|
||||
float tolerance = 0.0;
|
||||
float some_tolerance = 20.0;
|
||||
int it_on_approach = 10;
|
||||
double max_planning_time = 120.0;
|
||||
int num_it = 0;
|
||||
|
||||
a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 0.0, 1);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmapA =
|
||||
new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
|
||||
// island in the middle of lethal cost to cross
|
||||
for (unsigned int i = 40; i <= 60; ++i) {
|
||||
for (unsigned int j = 40; j <= 60; ++j) {
|
||||
costmapA->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
|
||||
// functional case testing
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 1, lnode);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
a_star.setCollisionChecker(checker.get());
|
||||
a_star.setStart(20u, 20u, 0);
|
||||
a_star.setGoal(80u, 80u, 0);
|
||||
nav2_smac_planner::Node2D::CoordinateVector path;
|
||||
EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
|
||||
EXPECT_EQ(num_it, 2414);
|
||||
|
||||
// check path is the right size and collision free
|
||||
EXPECT_EQ(path.size(), 82u);
|
||||
for (unsigned int i = 0; i != path.size(); i++) {
|
||||
EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
|
||||
}
|
||||
|
||||
// setting non-zero dim 3 for 2D search
|
||||
EXPECT_THROW(a_star.setGoal(0, 0, 10), std::runtime_error);
|
||||
EXPECT_THROW(a_star.setStart(0, 0, 10), std::runtime_error);
|
||||
|
||||
path.clear();
|
||||
// failure cases with invalid inputs
|
||||
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star_2(
|
||||
nav2_smac_planner::MotionModel::TWOD, info);
|
||||
a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1);
|
||||
num_it = 0;
|
||||
EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
|
||||
a_star_2.setCollisionChecker(checker.get());
|
||||
num_it = 0;
|
||||
EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
|
||||
a_star_2.setStart(50, 50, 0); // invalid
|
||||
a_star_2.setGoal(0, 0, 0); // valid
|
||||
num_it = 0;
|
||||
EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
|
||||
a_star_2.setStart(0, 0, 0); // valid
|
||||
a_star_2.setGoal(50, 50, 0); // invalid
|
||||
num_it = 0;
|
||||
EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
|
||||
num_it = 0;
|
||||
// invalid goal but liberal tolerance
|
||||
a_star_2.setStart(20, 20, 0); // valid
|
||||
a_star_2.setGoal(50, 50, 0); // invalid
|
||||
EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance));
|
||||
EXPECT_EQ(path.size(), 21u);
|
||||
for (unsigned int i = 0; i != path.size(); i++) {
|
||||
EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
|
||||
}
|
||||
|
||||
EXPECT_TRUE(a_star_2.getStart() != nullptr);
|
||||
EXPECT_TRUE(a_star_2.getGoal() != nullptr);
|
||||
EXPECT_EQ(a_star_2.getSizeX(), 100u);
|
||||
EXPECT_EQ(a_star_2.getSizeY(), 100u);
|
||||
EXPECT_EQ(a_star_2.getSizeDim3(), 1u);
|
||||
EXPECT_EQ(a_star_2.getToleranceHeuristic(), 20.0);
|
||||
EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10);
|
||||
|
||||
delete costmapA;
|
||||
}
|
||||
|
||||
TEST(AStarTest, test_a_star_se2)
|
||||
{
|
||||
auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.change_penalty = 0.1;
|
||||
info.non_straight_penalty = 1.1;
|
||||
info.reverse_penalty = 2.0;
|
||||
info.minimum_turning_radius = 8; // in grid coordinates
|
||||
info.retrospective_penalty = 0.015;
|
||||
info.analytic_expansion_max_length = 20.0; // in grid coordinates
|
||||
info.analytic_expansion_ratio = 3.5;
|
||||
unsigned int size_theta = 72;
|
||||
info.cost_penalty = 1.7;
|
||||
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
|
||||
nav2_smac_planner::MotionModel::DUBIN, info);
|
||||
int max_iterations = 10000;
|
||||
float tolerance = 10.0;
|
||||
int it_on_approach = 10;
|
||||
double max_planning_time = 120.0;
|
||||
int num_it = 0;
|
||||
|
||||
a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmapA =
|
||||
new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
|
||||
// island in the middle of lethal cost to cross
|
||||
for (unsigned int i = 40; i <= 60; ++i) {
|
||||
for (unsigned int j = 40; j <= 60; ++j) {
|
||||
costmapA->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
// functional case testing
|
||||
a_star.setCollisionChecker(checker.get());
|
||||
a_star.setStart(10u, 10u, 0u);
|
||||
a_star.setGoal(80u, 80u, 40u);
|
||||
nav2_smac_planner::NodeHybrid::CoordinateVector path;
|
||||
EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
|
||||
|
||||
// check path is the right size and collision free
|
||||
EXPECT_EQ(num_it, 3222);
|
||||
EXPECT_EQ(path.size(), 63u);
|
||||
for (unsigned int i = 0; i != path.size(); i++) {
|
||||
EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
|
||||
}
|
||||
// no skipped nodes
|
||||
for (unsigned int i = 1; i != path.size(); i++) {
|
||||
EXPECT_LT(hypotf(path[i].x - path[i - 1].x, path[i].y - path[i - 1].y), 2.1f);
|
||||
}
|
||||
|
||||
delete costmapA;
|
||||
}
|
||||
|
||||
TEST(AStarTest, test_a_star_lattice)
|
||||
{
|
||||
auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.change_penalty = 0.05;
|
||||
info.non_straight_penalty = 1.05;
|
||||
info.reverse_penalty = 2.0;
|
||||
info.retrospective_penalty = 0.1;
|
||||
info.analytic_expansion_ratio = 3.5;
|
||||
info.lattice_filepath =
|
||||
ament_index_cpp::get_package_share_directory("nav2_smac_planner") +
|
||||
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
|
||||
"/output.json";
|
||||
info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05
|
||||
info.analytic_expansion_max_length = 20.0; // in grid coordinates
|
||||
unsigned int size_theta = 16;
|
||||
info.cost_penalty = 2.0;
|
||||
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeLattice> a_star(
|
||||
nav2_smac_planner::MotionModel::STATE_LATTICE, info);
|
||||
int max_iterations = 10000;
|
||||
float tolerance = 10.0;
|
||||
int it_on_approach = 10;
|
||||
double max_planning_time = 120.0;
|
||||
int num_it = 0;
|
||||
|
||||
a_star.initialize(
|
||||
false, max_iterations, std::numeric_limits<int>::max(), max_planning_time, 401, size_theta);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmapA =
|
||||
new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0);
|
||||
// island in the middle of lethal cost to cross
|
||||
for (unsigned int i = 20; i <= 30; ++i) {
|
||||
for (unsigned int j = 20; j <= 30; ++j) {
|
||||
costmapA->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
// functional case testing
|
||||
a_star.setCollisionChecker(checker.get());
|
||||
a_star.setStart(5u, 5u, 0u);
|
||||
a_star.setGoal(40u, 40u, 1u);
|
||||
nav2_smac_planner::NodeLattice::CoordinateVector path;
|
||||
EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
|
||||
|
||||
// check path is the right size and collision free
|
||||
EXPECT_EQ(num_it, 21);
|
||||
EXPECT_GT(path.size(), 47u);
|
||||
for (unsigned int i = 0; i != path.size(); i++) {
|
||||
EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
|
||||
}
|
||||
// no skipped nodes
|
||||
for (unsigned int i = 1; i != path.size(); i++) {
|
||||
EXPECT_LT(hypotf(path[i].x - path[i - 1].x, path[i].y - path[i - 1].y), 2.1f);
|
||||
}
|
||||
|
||||
delete costmapA;
|
||||
}
|
||||
|
||||
TEST(AStarTest, test_se2_single_pose_path)
|
||||
{
|
||||
auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.change_penalty = 0.1;
|
||||
info.non_straight_penalty = 1.1;
|
||||
info.reverse_penalty = 2.0;
|
||||
info.retrospective_penalty = 0.0;
|
||||
info.minimum_turning_radius = 8; // in grid coordinates
|
||||
info.analytic_expansion_max_length = 20.0; // in grid coordinates
|
||||
info.analytic_expansion_ratio = 3.5;
|
||||
unsigned int size_theta = 72;
|
||||
info.cost_penalty = 1.7;
|
||||
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
|
||||
nav2_smac_planner::MotionModel::DUBIN, info);
|
||||
int max_iterations = 100;
|
||||
float tolerance = 10.0;
|
||||
int it_on_approach = 10;
|
||||
double max_planning_time = 120.0;
|
||||
int num_it = 0;
|
||||
|
||||
a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmapA =
|
||||
new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
|
||||
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
// functional case testing
|
||||
a_star.setCollisionChecker(checker.get());
|
||||
a_star.setStart(10u, 10u, 0u);
|
||||
// Goal is one costmap cell away
|
||||
a_star.setGoal(12u, 10u, 0u);
|
||||
nav2_smac_planner::NodeHybrid::CoordinateVector path;
|
||||
EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
|
||||
|
||||
// Check that the path is length one
|
||||
// With the current implementation, this produces a longer path
|
||||
// EXPECT_EQ(path.size(), 1u);
|
||||
EXPECT_GE(path.size(), 1u);
|
||||
|
||||
delete costmapA;
|
||||
}
|
||||
|
||||
TEST(AStarTest, test_constants)
|
||||
{
|
||||
nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown
|
||||
EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Unknown"));
|
||||
mm = nav2_smac_planner::MotionModel::TWOD; // 2d
|
||||
EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("2D"));
|
||||
mm = nav2_smac_planner::MotionModel::DUBIN; // dubin
|
||||
EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Dubin"));
|
||||
mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp
|
||||
EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp"));
|
||||
|
||||
EXPECT_EQ(
|
||||
nav2_smac_planner::fromString(
|
||||
"2D"), nav2_smac_planner::MotionModel::TWOD);
|
||||
EXPECT_EQ(nav2_smac_planner::fromString("DUBIN"), nav2_smac_planner::MotionModel::DUBIN);
|
||||
EXPECT_EQ(
|
||||
nav2_smac_planner::fromString(
|
||||
"REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP);
|
||||
EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN);
|
||||
}
|
||||
@@ -0,0 +1,185 @@
|
||||
// Copyright (c) 2020 Shivang Patel
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
//
|
||||
// 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 <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
|
||||
using namespace nav2_costmap_2d; // NOLINT
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
TEST(collision_footprint, test_basic)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testA");
|
||||
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
|
||||
|
||||
geometry_msgs::msg::Point p1;
|
||||
p1.x = -0.5;
|
||||
p1.y = 0.0;
|
||||
geometry_msgs::msg::Point p2;
|
||||
p2.x = 0.0;
|
||||
p2.y = 0.5;
|
||||
geometry_msgs::msg::Point p3;
|
||||
p3.x = 0.5;
|
||||
p3.y = 0.0;
|
||||
geometry_msgs::msg::Point p4;
|
||||
p4.x = 0.0;
|
||||
p4.y = -0.5;
|
||||
|
||||
nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
|
||||
|
||||
nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
|
||||
collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
|
||||
collision_checker.inCollision(5.0, 5.0, 0.0, false);
|
||||
float cost = collision_checker.getCost();
|
||||
EXPECT_NEAR(cost, 0.0, 0.001);
|
||||
delete costmap_;
|
||||
}
|
||||
|
||||
TEST(collision_footprint, test_point_cost)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testB");
|
||||
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
|
||||
|
||||
nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
|
||||
nav2_costmap_2d::Footprint footprint;
|
||||
collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0);
|
||||
|
||||
collision_checker.inCollision(5.0, 5.0, 0.0, false);
|
||||
float cost = collision_checker.getCost();
|
||||
EXPECT_NEAR(cost, 0.0, 0.001);
|
||||
delete costmap_;
|
||||
}
|
||||
|
||||
TEST(collision_footprint, test_world_to_map)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testC");
|
||||
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
|
||||
|
||||
nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
|
||||
nav2_costmap_2d::Footprint footprint;
|
||||
collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0);
|
||||
|
||||
unsigned int x, y;
|
||||
|
||||
collision_checker.worldToMap(1.0, 1.0, x, y);
|
||||
|
||||
collision_checker.inCollision(x, y, 0.0, false);
|
||||
float cost = collision_checker.getCost();
|
||||
|
||||
EXPECT_NEAR(cost, 0.0, 0.001);
|
||||
|
||||
costmap_->setCost(50, 50, 200);
|
||||
collision_checker.worldToMap(5.0, 5.0, x, y);
|
||||
|
||||
collision_checker.inCollision(x, y, 0.0, false);
|
||||
EXPECT_NEAR(collision_checker.getCost(), 200.0, 0.001);
|
||||
delete costmap_;
|
||||
}
|
||||
|
||||
TEST(collision_footprint, test_footprint_at_pose_with_movement)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testD");
|
||||
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254);
|
||||
|
||||
for (unsigned int i = 40; i <= 60; ++i) {
|
||||
for (unsigned int j = 40; j <= 60; ++j) {
|
||||
costmap_->setCost(i, j, 128);
|
||||
}
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Point p1;
|
||||
p1.x = -1.0;
|
||||
p1.y = 1.0;
|
||||
geometry_msgs::msg::Point p2;
|
||||
p2.x = 1.0;
|
||||
p2.y = 1.0;
|
||||
geometry_msgs::msg::Point p3;
|
||||
p3.x = 1.0;
|
||||
p3.y = -1.0;
|
||||
geometry_msgs::msg::Point p4;
|
||||
p4.x = -1.0;
|
||||
p4.y = -1.0;
|
||||
|
||||
nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
|
||||
|
||||
nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
|
||||
collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
|
||||
|
||||
collision_checker.inCollision(50, 50, 0.0, false);
|
||||
float cost = collision_checker.getCost();
|
||||
EXPECT_NEAR(cost, 128.0, 0.001);
|
||||
|
||||
collision_checker.inCollision(50, 49, 0.0, false);
|
||||
float up_value = collision_checker.getCost();
|
||||
EXPECT_NEAR(up_value, 254.0, 0.001);
|
||||
|
||||
collision_checker.inCollision(50, 52, 0.0, false);
|
||||
float down_value = collision_checker.getCost();
|
||||
EXPECT_NEAR(down_value, 254.0, 0.001);
|
||||
delete costmap_;
|
||||
}
|
||||
|
||||
TEST(collision_footprint, test_point_and_line_cost)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testE");
|
||||
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(
|
||||
100, 100, 0.10000, 0, 0.0, 128.0);
|
||||
|
||||
costmap_->setCost(62, 50, 254);
|
||||
costmap_->setCost(39, 60, 254);
|
||||
|
||||
geometry_msgs::msg::Point p1;
|
||||
p1.x = -1.0;
|
||||
p1.y = 1.0;
|
||||
geometry_msgs::msg::Point p2;
|
||||
p2.x = 1.0;
|
||||
p2.y = 1.0;
|
||||
geometry_msgs::msg::Point p3;
|
||||
p3.x = 1.0;
|
||||
p3.y = -1.0;
|
||||
geometry_msgs::msg::Point p4;
|
||||
p4.x = -1.0;
|
||||
p4.y = -1.0;
|
||||
|
||||
nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
|
||||
|
||||
nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
|
||||
collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
|
||||
|
||||
collision_checker.inCollision(50, 50, 0.0, false);
|
||||
float value = collision_checker.getCost();
|
||||
EXPECT_NEAR(value, 128.0, 0.001);
|
||||
|
||||
collision_checker.inCollision(49, 50, 0.0, false);
|
||||
float left_value = collision_checker.getCost();
|
||||
EXPECT_NEAR(left_value, 254.0, 0.001);
|
||||
|
||||
collision_checker.inCollision(52, 50, 0.0, false);
|
||||
float right_value = collision_checker.getCost();
|
||||
EXPECT_NEAR(right_value, 254.0, 0.001);
|
||||
delete costmap_;
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_smac_planner/costmap_downsampler.hpp"
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
TEST(CostmapDownsampler, costmap_downsample_test)
|
||||
{
|
||||
nav2_util::LifecycleNode::SharedPtr node = std::make_shared<nav2_util::LifecycleNode>(
|
||||
"CostmapDownsamplerTest");
|
||||
nav2_smac_planner::CostmapDownsampler downsampler;
|
||||
|
||||
// create basic costmap
|
||||
nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
|
||||
costmapA.setCost(0, 0, 100);
|
||||
costmapA.setCost(5, 5, 50);
|
||||
|
||||
// downsample it
|
||||
downsampler.on_configure(node, "map", "unused_topic", &costmapA, 2);
|
||||
nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample(2);
|
||||
|
||||
// validate it
|
||||
EXPECT_EQ(downsampledCostmapA->getCost(0, 0), 100);
|
||||
EXPECT_EQ(downsampledCostmapA->getCost(2, 2), 50);
|
||||
EXPECT_EQ(downsampledCostmapA->getSizeInCellsX(), 5u);
|
||||
EXPECT_EQ(downsampledCostmapA->getSizeInCellsY(), 5u);
|
||||
|
||||
// give it another costmap of another size
|
||||
nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.10, 0.0, 0.0, 0);
|
||||
|
||||
// downsample it
|
||||
downsampler.on_configure(node, "map", "unused_topic", &costmapB, 4);
|
||||
downsampler.on_activate();
|
||||
nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample(4);
|
||||
downsampler.on_deactivate();
|
||||
|
||||
// validate size
|
||||
EXPECT_EQ(downsampledCostmapB->getSizeInCellsX(), 1u);
|
||||
EXPECT_EQ(downsampledCostmapB->getSizeInCellsY(), 1u);
|
||||
|
||||
downsampler.resizeCostmap();
|
||||
}
|
||||
@@ -0,0 +1,143 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_subscriber.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_smac_planner/node_2d.hpp"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
TEST(Node2DTest, test_node_2d)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, node);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
// test construction
|
||||
unsigned char cost = static_cast<unsigned char>(1);
|
||||
nav2_smac_planner::Node2D testA(1);
|
||||
testA.setCost(cost);
|
||||
nav2_smac_planner::Node2D testB(1);
|
||||
testB.setCost(cost);
|
||||
EXPECT_EQ(testA.getCost(), 1.0f);
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.cost_penalty = 1.0;
|
||||
unsigned int size = 10;
|
||||
nav2_smac_planner::Node2D::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::TWOD, size, size, size, info);
|
||||
|
||||
// test reset
|
||||
testA.reset();
|
||||
EXPECT_TRUE(std::isnan(testA.getCost()));
|
||||
|
||||
// check collision checking
|
||||
EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
|
||||
testA.setCost(255);
|
||||
EXPECT_EQ(testA.isNodeValid(true, checker.get()), true);
|
||||
testA.setCost(10);
|
||||
|
||||
// check traversal cost computation
|
||||
EXPECT_NEAR(testB.getTraversalCost(&testA), 1.03f, 0.1f);
|
||||
|
||||
// check heuristic cost computation
|
||||
nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0);
|
||||
nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0);
|
||||
EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 11.18, 0.02);
|
||||
|
||||
// check operator== works on index
|
||||
unsigned char costC = '2';
|
||||
nav2_smac_planner::Node2D testC(1);
|
||||
testC.setCost(costC);
|
||||
EXPECT_TRUE(testA == testC);
|
||||
|
||||
// check accumulated costs are set
|
||||
testC.setAccumulatedCost(100);
|
||||
EXPECT_EQ(testC.getAccumulatedCost(), 100.0f);
|
||||
|
||||
// check visiting state
|
||||
EXPECT_EQ(testC.wasVisited(), false);
|
||||
testC.queued();
|
||||
EXPECT_EQ(testC.isQueued(), true);
|
||||
testC.visited();
|
||||
EXPECT_EQ(testC.wasVisited(), true);
|
||||
EXPECT_EQ(testC.isQueued(), false);
|
||||
|
||||
// check index
|
||||
EXPECT_EQ(testC.getIndex(), 1u);
|
||||
|
||||
// check static index functions
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::getIndex(1u, 1u, 10u), 11u);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u);
|
||||
EXPECT_THROW(nav2_smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error);
|
||||
}
|
||||
|
||||
TEST(Node2DTest, test_node_2d_neighbors)
|
||||
{
|
||||
auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
unsigned int size_x = 10u;
|
||||
unsigned int size_y = 10u;
|
||||
unsigned int quant = 0u;
|
||||
// test neighborhood computation
|
||||
size_x = 100u;
|
||||
nav2_smac_planner::Node2D::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::TWOD, size_x, size_y,
|
||||
quant, info);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -100);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 100);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[4], -101);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[5], -99);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[6], 99);
|
||||
EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[7], 101);
|
||||
|
||||
nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, lnode);
|
||||
unsigned char cost = static_cast<unsigned int>(1);
|
||||
nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1);
|
||||
node->setCost(cost);
|
||||
std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> neighborGetter =
|
||||
[&, this](const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn) -> bool
|
||||
{
|
||||
return false;
|
||||
};
|
||||
|
||||
nav2_smac_planner::Node2D::NodeVector neighbors;
|
||||
node->getNeighbors(neighborGetter, checker.get(), false, neighbors);
|
||||
delete node;
|
||||
|
||||
// should be empty since totally invalid
|
||||
EXPECT_EQ(neighbors.size(), 0u);
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_subscriber.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_smac_planner/node_basic.hpp"
|
||||
#include "nav2_smac_planner/node_2d.hpp"
|
||||
#include "nav2_smac_planner/node_hybrid.hpp"
|
||||
#include "nav2_smac_planner/node_lattice.hpp"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
TEST(NodeBasicTest, test_node_basic)
|
||||
{
|
||||
nav2_smac_planner::NodeBasic<nav2_smac_planner::NodeHybrid> node(50);
|
||||
|
||||
EXPECT_EQ(node.index, 50u);
|
||||
EXPECT_EQ(node.graph_node_ptr, nullptr);
|
||||
|
||||
nav2_smac_planner::NodeBasic<nav2_smac_planner::Node2D> node2(100);
|
||||
|
||||
EXPECT_EQ(node2.index, 100u);
|
||||
EXPECT_EQ(node2.graph_node_ptr, nullptr);
|
||||
|
||||
nav2_smac_planner::NodeBasic<nav2_smac_planner::NodeLattice> node3(200);
|
||||
|
||||
EXPECT_EQ(node3.index, 200u);
|
||||
EXPECT_EQ(node3.graph_node_ptr, nullptr);
|
||||
}
|
||||
@@ -0,0 +1,349 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_subscriber.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_smac_planner/node_hybrid.hpp"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
TEST(NodeHybridTest, test_node_hybrid)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.change_penalty = 0.1;
|
||||
info.non_straight_penalty = 1.1;
|
||||
info.reverse_penalty = 2.0;
|
||||
info.minimum_turning_radius = 8; // 0.4m/5cm resolution costmap
|
||||
info.cost_penalty = 1.7;
|
||||
info.retrospective_penalty = 0.1;
|
||||
unsigned int size_x = 10;
|
||||
unsigned int size_y = 10;
|
||||
unsigned int size_theta = 72;
|
||||
|
||||
// Check defaulted constants
|
||||
nav2_smac_planner::NodeHybrid testA(49);
|
||||
EXPECT_EQ(testA.travel_distance_cost, sqrt(2));
|
||||
|
||||
nav2_smac_planner::NodeHybrid::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
|
||||
10, 10, 0.05, 0.0, 0.0, 0);
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, node);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
// test construction
|
||||
nav2_smac_planner::NodeHybrid testB(49);
|
||||
EXPECT_TRUE(std::isnan(testA.getCost()));
|
||||
|
||||
// test node valid and cost
|
||||
testA.pose.x = 5;
|
||||
testA.pose.y = 5;
|
||||
testA.pose.theta = 0;
|
||||
EXPECT_EQ(testA.isNodeValid(true, checker.get()), true);
|
||||
EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
|
||||
EXPECT_EQ(testA.getCost(), 0.0f);
|
||||
|
||||
// test reset
|
||||
testA.reset();
|
||||
EXPECT_TRUE(std::isnan(testA.getCost()));
|
||||
|
||||
// Check motion-specific constants
|
||||
EXPECT_NEAR(testA.travel_distance_cost, 2.08842, 0.1);
|
||||
|
||||
// check collision checking
|
||||
EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
|
||||
|
||||
// check traversal cost computation
|
||||
// simulated first node, should return neutral cost
|
||||
EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088, 0.1);
|
||||
// now with straight motion, cost is 0, so will be neutral as well
|
||||
// but now reduced by retrospective penalty (10%)
|
||||
testB.setMotionPrimitiveIndex(1);
|
||||
testA.setMotionPrimitiveIndex(0);
|
||||
EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088 * 0.9, 0.1);
|
||||
// same direction as parent, testB
|
||||
testA.setMotionPrimitiveIndex(1);
|
||||
EXPECT_NEAR(testB.getTraversalCost(&testA), 2.297f * 0.9, 0.01);
|
||||
// opposite direction as parent, testB
|
||||
testA.setMotionPrimitiveIndex(2);
|
||||
EXPECT_NEAR(testB.getTraversalCost(&testA), 2.506f * 0.9, 0.01);
|
||||
|
||||
// will throw because never collision checked testB
|
||||
EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error);
|
||||
|
||||
// check motion primitives
|
||||
EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u);
|
||||
|
||||
// check operator== works on index
|
||||
nav2_smac_planner::NodeHybrid testC(49);
|
||||
EXPECT_TRUE(testA == testC);
|
||||
|
||||
// check accumulated costs are set
|
||||
testC.setAccumulatedCost(100);
|
||||
EXPECT_EQ(testC.getAccumulatedCost(), 100.0f);
|
||||
|
||||
// check visiting state
|
||||
EXPECT_EQ(testC.wasVisited(), false);
|
||||
testC.visited();
|
||||
EXPECT_EQ(testC.wasVisited(), true);
|
||||
|
||||
// check index
|
||||
EXPECT_EQ(testC.getIndex(), 49u);
|
||||
|
||||
// check set pose and pose
|
||||
testC.setPose(nav2_smac_planner::NodeHybrid::Coordinates(10.0, 5.0, 4));
|
||||
EXPECT_EQ(testC.pose.x, 10.0);
|
||||
EXPECT_EQ(testC.pose.y, 5.0);
|
||||
EXPECT_EQ(testC.pose.theta, 4);
|
||||
|
||||
// check static index functions
|
||||
EXPECT_EQ(nav2_smac_planner::NodeHybrid::getIndex(1u, 1u, 4u, 10u, 72u), 796u);
|
||||
EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).x, 1u);
|
||||
EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).y, 1u);
|
||||
EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).theta, 4u);
|
||||
|
||||
delete costmapA;
|
||||
}
|
||||
|
||||
TEST(NodeHybridTest, test_obstacle_heuristic)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.change_penalty = 0.1;
|
||||
info.non_straight_penalty = 1.1;
|
||||
info.reverse_penalty = 2.0;
|
||||
info.minimum_turning_radius = 8; // 0.4m/5cm resolution costmap
|
||||
info.cost_penalty = 1.7;
|
||||
info.retrospective_penalty = 0.0;
|
||||
unsigned int size_x = 100;
|
||||
unsigned int size_y = 100;
|
||||
unsigned int size_theta = 72;
|
||||
|
||||
nav2_smac_planner::NodeHybrid::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
|
||||
100, 100, 0.1, 0.0, 0.0, 0);
|
||||
// island in the middle of lethal cost to cross
|
||||
for (unsigned int i = 20; i <= 80; ++i) {
|
||||
for (unsigned int j = 40; j <= 60; ++j) {
|
||||
costmapA->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
// path on the right is narrow and thus with high cost
|
||||
for (unsigned int i = 20; i <= 80; ++i) {
|
||||
for (unsigned int j = 61; j <= 70; ++j) {
|
||||
costmapA->setCost(i, j, 250);
|
||||
}
|
||||
}
|
||||
for (unsigned int i = 20; i <= 80; ++i) {
|
||||
for (unsigned int j = 71; j < 100; ++j) {
|
||||
costmapA->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, node);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
nav2_smac_planner::NodeHybrid testA(0);
|
||||
testA.pose.x = 10;
|
||||
testA.pose.y = 50;
|
||||
testA.pose.theta = 0;
|
||||
|
||||
nav2_smac_planner::NodeHybrid testB(1);
|
||||
testB.pose.x = 90;
|
||||
testB.pose.y = 51; // goal is a bit closer to the high-cost passage
|
||||
testB.pose.theta = 0;
|
||||
|
||||
// first block the high-cost passage to make sure the cost spreads through the better path
|
||||
for (unsigned int j = 61; j <= 70; ++j) {
|
||||
costmapA->setCost(50, j, 254);
|
||||
}
|
||||
nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
|
||||
costmapA, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y);
|
||||
float wide_passage_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
|
||||
testA.pose,
|
||||
testB.pose,
|
||||
info.cost_penalty);
|
||||
|
||||
EXPECT_NEAR(wide_passage_cost, 91.1f, 0.1f);
|
||||
|
||||
// then unblock it to check if cost remains the same
|
||||
// (it should, since the unblocked narrow path will have higher cost than the wide one
|
||||
// and thus lower bound of the path cost should be unchanged)
|
||||
for (unsigned int j = 61; j <= 70; ++j) {
|
||||
costmapA->setCost(50, j, 250);
|
||||
}
|
||||
nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
|
||||
costmapA,
|
||||
testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y);
|
||||
float two_passages_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
|
||||
testA.pose,
|
||||
testB.pose,
|
||||
info.cost_penalty);
|
||||
|
||||
EXPECT_EQ(wide_passage_cost, two_passages_cost);
|
||||
|
||||
delete costmapA;
|
||||
}
|
||||
|
||||
TEST(NodeHybridTest, test_node_debin_neighbors)
|
||||
{
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.change_penalty = 1.2;
|
||||
info.non_straight_penalty = 1.4;
|
||||
info.reverse_penalty = 2.1;
|
||||
info.minimum_turning_radius = 4; // 0.2 in grid coordinates
|
||||
info.retrospective_penalty = 0.0;
|
||||
unsigned int size_x = 100;
|
||||
unsigned int size_y = 100;
|
||||
unsigned int size_theta = 72;
|
||||
nav2_smac_planner::NodeHybrid::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
|
||||
|
||||
// test neighborhood computation
|
||||
EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 3u);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 1.731517, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01);
|
||||
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 1.69047, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.3747, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 5, 0.01);
|
||||
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 1.69047, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.3747, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -5, 0.01);
|
||||
}
|
||||
|
||||
TEST(NodeHybridTest, test_node_reeds_neighbors)
|
||||
{
|
||||
auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.change_penalty = 1.2;
|
||||
info.non_straight_penalty = 1.4;
|
||||
info.reverse_penalty = 2.1;
|
||||
info.minimum_turning_radius = 8; // 0.4 in grid coordinates
|
||||
info.retrospective_penalty = 0.0;
|
||||
unsigned int size_x = 100;
|
||||
unsigned int size_y = 100;
|
||||
unsigned int size_theta = 72;
|
||||
nav2_smac_planner::NodeHybrid::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info);
|
||||
|
||||
EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 6u);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 2.088, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01);
|
||||
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 2.070, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.272, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 3, 0.01);
|
||||
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 2.070, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.272, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -3, 0.01);
|
||||
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._x, -2.088, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._y, 0, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._theta, 0, 0.01);
|
||||
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._x, -2.07, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._y, 0.272, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._theta, -3, 0.01);
|
||||
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._x, -2.07, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._y, -0.272, 0.01);
|
||||
EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._theta, 3, 0.01);
|
||||
|
||||
nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0);
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, lnode);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49);
|
||||
std::function<bool(const unsigned int &, nav2_smac_planner::NodeHybrid * &)> neighborGetter =
|
||||
[&, this](const unsigned int & index, nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool
|
||||
{
|
||||
// because we don't return a real object
|
||||
return false;
|
||||
};
|
||||
|
||||
nav2_smac_planner::NodeHybrid::NodeVector neighbors;
|
||||
node->getNeighbors(neighborGetter, checker.get(), false, neighbors);
|
||||
delete node;
|
||||
|
||||
// should be empty since totally invalid
|
||||
EXPECT_EQ(neighbors.size(), 0u);
|
||||
}
|
||||
|
||||
TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
|
||||
{
|
||||
// Tests to check getClosestAngularBin behavior for different input types
|
||||
nav2_smac_planner::HybridMotionTable motion_table;
|
||||
|
||||
{
|
||||
motion_table.bin_size = 3.1415926;
|
||||
motion_table.num_angle_quantization = 2;
|
||||
double test_theta = 3.1415926;
|
||||
unsigned int expected_angular_bin = 1;
|
||||
unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
|
||||
EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
|
||||
}
|
||||
|
||||
{
|
||||
motion_table.bin_size = M_PI;
|
||||
motion_table.num_angle_quantization = 2;
|
||||
double test_theta = M_PI / 2.0 - 0.000001;
|
||||
unsigned int expected_angular_bin = 0;
|
||||
unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
|
||||
EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
|
||||
}
|
||||
|
||||
{
|
||||
motion_table.bin_size = M_PI;
|
||||
motion_table.num_angle_quantization = 2;
|
||||
float test_theta = M_PI;
|
||||
unsigned int expected_angular_bin = 1;
|
||||
unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
|
||||
EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
|
||||
}
|
||||
|
||||
{
|
||||
motion_table.bin_size = 0.0872664675;
|
||||
motion_table.num_angle_quantization = 72;
|
||||
double test_theta = 6.28317530718; // 0.0001 less than 2 pi
|
||||
unsigned int expected_angular_bin = 0; // should be closer to wrap around
|
||||
unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
|
||||
EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,369 @@
|
||||
// Copyright (c) 2021 Joshua Wallace
|
||||
// Copyright (c) 2021 Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <limits>
|
||||
#include "nav2_smac_planner/node_lattice.hpp"
|
||||
#include "gtest/gtest.h"
|
||||
#include "ament_index_cpp/get_package_share_directory.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
|
||||
using json = nlohmann::json;
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
TEST(NodeLatticeTest, parser_test)
|
||||
{
|
||||
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
|
||||
std::string filePath =
|
||||
pkg_share_dir +
|
||||
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
|
||||
"/output.json";
|
||||
std::ifstream myJsonFile(filePath);
|
||||
|
||||
ASSERT_TRUE(myJsonFile.is_open());
|
||||
|
||||
json j;
|
||||
myJsonFile >> j;
|
||||
|
||||
nav2_smac_planner::LatticeMetadata metaData;
|
||||
nav2_smac_planner::MotionPrimitive myPrimitive;
|
||||
nav2_smac_planner::MotionPose pose;
|
||||
|
||||
json jsonMetaData = j["lattice_metadata"];
|
||||
json jsonPrimatives = j["primitives"];
|
||||
json jsonPose = jsonPrimatives[0]["poses"][0];
|
||||
|
||||
nav2_smac_planner::fromJsonToMetaData(jsonMetaData, metaData);
|
||||
|
||||
// Checks for parsing meta data
|
||||
EXPECT_NEAR(metaData.min_turning_radius, 0.5, 0.001);
|
||||
EXPECT_NEAR(metaData.grid_resolution, 0.05, 0.001);
|
||||
EXPECT_NEAR(metaData.number_of_headings, 16, 0.01);
|
||||
EXPECT_NEAR(metaData.heading_angles[0], 0.0, 0.01);
|
||||
EXPECT_EQ(metaData.number_of_trajectories, 80u);
|
||||
EXPECT_EQ(metaData.motion_model, std::string("ackermann"));
|
||||
|
||||
std::vector<nav2_smac_planner::MotionPrimitive> myPrimitives;
|
||||
for (unsigned int i = 0; i < jsonPrimatives.size(); ++i) {
|
||||
nav2_smac_planner::MotionPrimitive newPrimative;
|
||||
nav2_smac_planner::fromJsonToMotionPrimitive(jsonPrimatives[i], newPrimative);
|
||||
myPrimitives.push_back(newPrimative);
|
||||
}
|
||||
|
||||
// Checks for parsing primitives
|
||||
EXPECT_EQ(myPrimitives.size(), 80u);
|
||||
EXPECT_NEAR(myPrimitives[0].trajectory_id, 0, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].start_angle, 0.0, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].end_angle, 13, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].turning_radius, 0.5259, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].trajectory_length, 0.64856, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].arc_length, 0.58225, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].straight_length, 0.06631, 0.01);
|
||||
|
||||
EXPECT_NEAR(myPrimitives[0].poses[0]._x, 0.04981, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].poses[0]._y, -0.00236, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].poses[0]._theta, 6.1883, 0.01);
|
||||
|
||||
EXPECT_NEAR(myPrimitives[0].poses[1]._x, 0.09917, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].poses[1]._y, -0.00944, 0.01);
|
||||
EXPECT_NEAR(myPrimitives[0].poses[1]._theta, 6.09345, 0.015);
|
||||
}
|
||||
|
||||
TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing)
|
||||
{
|
||||
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
|
||||
std::string filePath =
|
||||
pkg_share_dir +
|
||||
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
|
||||
"/output.json";
|
||||
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.minimum_turning_radius = 1.1;
|
||||
info.non_straight_penalty = 1;
|
||||
info.change_penalty = 1;
|
||||
info.reverse_penalty = 1;
|
||||
info.cost_penalty = 1;
|
||||
info.retrospective_penalty = 0.0;
|
||||
info.analytic_expansion_ratio = 1;
|
||||
info.lattice_filepath = filePath;
|
||||
info.cache_obstacle_heuristic = true;
|
||||
info.allow_reverse_expansion = true;
|
||||
|
||||
unsigned int x = 100;
|
||||
unsigned int y = 100;
|
||||
unsigned int angle_quantization = 16;
|
||||
|
||||
nav2_smac_planner::NodeLattice::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
|
||||
|
||||
nav2_smac_planner::NodeLattice aNode(0);
|
||||
aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0));
|
||||
nav2_smac_planner::MotionPrimitivePtrs projections =
|
||||
nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&aNode);
|
||||
|
||||
EXPECT_NEAR(projections[0]->poses.back()._x, 0.5, 0.01);
|
||||
EXPECT_NEAR(projections[0]->poses.back()._y, -0.35, 0.01);
|
||||
EXPECT_NEAR(projections[0]->poses.back()._theta, 5.176, 0.01);
|
||||
|
||||
EXPECT_NEAR(
|
||||
nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata(
|
||||
filePath).grid_resolution, 0.05, 0.005);
|
||||
}
|
||||
|
||||
TEST(NodeLatticeTest, test_node_lattice_conversions)
|
||||
{
|
||||
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
|
||||
std::string filePath =
|
||||
pkg_share_dir +
|
||||
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
|
||||
"/output.json";
|
||||
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.minimum_turning_radius = 1.1;
|
||||
info.non_straight_penalty = 1;
|
||||
info.change_penalty = 1;
|
||||
info.reverse_penalty = 1;
|
||||
info.cost_penalty = 1;
|
||||
info.retrospective_penalty = 0.0;
|
||||
info.analytic_expansion_ratio = 1;
|
||||
info.lattice_filepath = filePath;
|
||||
info.cache_obstacle_heuristic = true;
|
||||
|
||||
unsigned int x = 100;
|
||||
unsigned int y = 100;
|
||||
unsigned int angle_quantization = 16;
|
||||
|
||||
nav2_smac_planner::NodeLattice::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
|
||||
|
||||
nav2_smac_planner::NodeLattice aNode(0);
|
||||
aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0));
|
||||
|
||||
EXPECT_NEAR(aNode.motion_table.getAngleFromBin(0u), 0.0, 0.005);
|
||||
EXPECT_NEAR(aNode.motion_table.getAngleFromBin(1u), 0.46364, 0.005);
|
||||
EXPECT_NEAR(aNode.motion_table.getAngleFromBin(2u), 0.78539, 0.005);
|
||||
|
||||
EXPECT_EQ(aNode.motion_table.getClosestAngularBin(0.0), 0u);
|
||||
EXPECT_EQ(aNode.motion_table.getClosestAngularBin(0.5), 1u);
|
||||
EXPECT_EQ(aNode.motion_table.getClosestAngularBin(1.5), 4u);
|
||||
}
|
||||
|
||||
TEST(NodeLatticeTest, test_node_lattice)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
|
||||
std::string filePath =
|
||||
pkg_share_dir +
|
||||
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
|
||||
"/output.json";
|
||||
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.minimum_turning_radius = 1.1;
|
||||
info.non_straight_penalty = 1;
|
||||
info.change_penalty = 1;
|
||||
info.reverse_penalty = 1;
|
||||
info.cost_penalty = 1;
|
||||
info.retrospective_penalty = 0.1;
|
||||
info.analytic_expansion_ratio = 1;
|
||||
info.lattice_filepath = filePath;
|
||||
info.cache_obstacle_heuristic = true;
|
||||
info.allow_reverse_expansion = true;
|
||||
|
||||
unsigned int x = 100;
|
||||
unsigned int y = 100;
|
||||
unsigned int angle_quantization = 16;
|
||||
|
||||
nav2_smac_planner::NodeLattice::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
|
||||
|
||||
// Check defaults
|
||||
nav2_smac_planner::NodeLattice aNode(0);
|
||||
nav2_smac_planner::NodeLattice testA(49);
|
||||
EXPECT_EQ(testA.getIndex(), 49u);
|
||||
EXPECT_EQ(testA.getAccumulatedCost(), std::numeric_limits<float>::max());
|
||||
EXPECT_TRUE(std::isnan(testA.getCost()));
|
||||
EXPECT_EQ(testA.getMotionPrimitive(), nullptr);
|
||||
|
||||
// Test visited state / reset
|
||||
EXPECT_EQ(testA.wasVisited(), false);
|
||||
testA.visited();
|
||||
EXPECT_EQ(testA.wasVisited(), true);
|
||||
testA.reset();
|
||||
EXPECT_EQ(testA.wasVisited(), false);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
|
||||
10, 10, 0.05, 0.0, 0.0, 0);
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, node);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
// test node valid and cost
|
||||
testA.pose.x = 5;
|
||||
testA.pose.y = 5;
|
||||
testA.pose.theta = 0;
|
||||
EXPECT_EQ(testA.isNodeValid(true, checker.get()), true);
|
||||
EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
|
||||
EXPECT_EQ(testA.getCost(), 0.0f);
|
||||
|
||||
// check collision checking
|
||||
EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
|
||||
|
||||
// check operator== works on index
|
||||
nav2_smac_planner::NodeLattice testC(49);
|
||||
EXPECT_TRUE(testA == testC);
|
||||
|
||||
// check accumulated costs are set
|
||||
testC.setAccumulatedCost(100);
|
||||
EXPECT_EQ(testC.getAccumulatedCost(), 100.0f);
|
||||
|
||||
// check set pose and pose
|
||||
testC.setPose(nav2_smac_planner::NodeLattice::Coordinates(10.0, 5.0, 4));
|
||||
EXPECT_EQ(testC.pose.x, 10.0);
|
||||
EXPECT_EQ(testC.pose.y, 5.0);
|
||||
EXPECT_EQ(testC.pose.theta, 4);
|
||||
|
||||
delete costmapA;
|
||||
}
|
||||
|
||||
|
||||
TEST(NodeLatticeTest, test_get_neighbors)
|
||||
{
|
||||
auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
|
||||
std::string filePath =
|
||||
pkg_share_dir +
|
||||
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
|
||||
"/output.json";
|
||||
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.minimum_turning_radius = 1.1;
|
||||
info.non_straight_penalty = 1;
|
||||
info.change_penalty = 1;
|
||||
info.reverse_penalty = 1;
|
||||
info.cost_penalty = 1;
|
||||
info.analytic_expansion_ratio = 1;
|
||||
info.retrospective_penalty = 0.0;
|
||||
info.lattice_filepath = filePath;
|
||||
info.cache_obstacle_heuristic = true;
|
||||
info.allow_reverse_expansion = true;
|
||||
|
||||
unsigned int x = 100;
|
||||
unsigned int y = 100;
|
||||
unsigned int angle_quantization = 16;
|
||||
|
||||
nav2_smac_planner::NodeLattice::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
|
||||
|
||||
nav2_smac_planner::NodeLattice node(49);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
|
||||
10, 10, 0.05, 0.0, 0.0, 0);
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, lnode);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
std::function<bool(const unsigned int &, nav2_smac_planner::NodeLattice * &)> neighborGetter =
|
||||
[&, this](const unsigned int & index, nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool
|
||||
{
|
||||
// because we don't return a real object
|
||||
return false;
|
||||
};
|
||||
|
||||
nav2_smac_planner::NodeLattice::NodeVector neighbors;
|
||||
node.getNeighbors(neighborGetter, checker.get(), false, neighbors);
|
||||
// should be empty since totally invalid
|
||||
EXPECT_EQ(neighbors.size(), 0u);
|
||||
|
||||
delete costmapA;
|
||||
}
|
||||
|
||||
TEST(NodeLatticeTest, test_node_lattice_custom_footprint)
|
||||
{
|
||||
auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
|
||||
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
|
||||
std::string filePath =
|
||||
pkg_share_dir +
|
||||
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
|
||||
"/output.json";
|
||||
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.minimum_turning_radius = 0.5;
|
||||
info.non_straight_penalty = 1;
|
||||
info.change_penalty = 1;
|
||||
info.reverse_penalty = 1;
|
||||
info.cost_penalty = 1;
|
||||
info.retrospective_penalty = 0.1;
|
||||
info.analytic_expansion_ratio = 1;
|
||||
info.lattice_filepath = filePath;
|
||||
info.cache_obstacle_heuristic = true;
|
||||
info.allow_reverse_expansion = true;
|
||||
|
||||
unsigned int x = 100;
|
||||
unsigned int y = 100;
|
||||
unsigned int angle_quantization = 16;
|
||||
|
||||
nav2_smac_planner::NodeLattice::initMotionModel(
|
||||
nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
|
||||
|
||||
nav2_smac_planner::NodeLattice node(49);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D(
|
||||
40, 40, 0.05, 0.0, 0.0, 0);
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap, 72, lnode);
|
||||
|
||||
// Make some custom asymmetrical footprint
|
||||
nav2_costmap_2d::Footprint footprint;
|
||||
geometry_msgs::msg::Point p;
|
||||
p.x = -0.1;
|
||||
p.y = -0.15;
|
||||
footprint.push_back(p);
|
||||
p.x = 0.35;
|
||||
p.y = -0.15;
|
||||
footprint.push_back(p);
|
||||
p.x = 0.35;
|
||||
p.y = 0.22;
|
||||
footprint.push_back(p);
|
||||
p.x = -0.1;
|
||||
p.y = 0.22;
|
||||
footprint.push_back(p);
|
||||
checker->setFootprint(footprint, false, 0.0);
|
||||
|
||||
// Setting initial robot pose to (1.0, 1.0, 0.0)
|
||||
node.pose.x = 20;
|
||||
node.pose.y = 20;
|
||||
node.pose.theta = 0;
|
||||
// Test that the node is valid though all motion primitives poses for custom footprint
|
||||
nav2_smac_planner::MotionPrimitivePtrs motion_primitives =
|
||||
nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node);
|
||||
EXPECT_GT(motion_primitives.size(), 0u);
|
||||
for (unsigned int i = 0; i < motion_primitives.size(); i++) {
|
||||
EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], false), true);
|
||||
EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], true), true);
|
||||
}
|
||||
|
||||
delete costmap;
|
||||
}
|
||||
@@ -0,0 +1,137 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "gtest/gtest.h"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_subscriber.hpp"
|
||||
#include "nav2_smac_planner/a_star.hpp"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
#include "nav2_smac_planner/node_hybrid.hpp"
|
||||
#include "nav2_smac_planner/smac_planner_2d.hpp"
|
||||
#include "nav2_smac_planner/smac_planner_hybrid.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
// SMAC smoke tests for plugin-level issues rather than algorithms
|
||||
// (covered by more extensively testing in other files)
|
||||
// System tests in nav2_system_tests will actually plan with this work
|
||||
|
||||
TEST(SmacTest, test_smac_2d) {
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node2D =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("Smac2DTest");
|
||||
|
||||
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());
|
||||
|
||||
node2D->declare_parameter("test.smooth_path", true);
|
||||
node2D->set_parameter(rclcpp::Parameter("test.smooth_path", true));
|
||||
node2D->declare_parameter("test.downsample_costmap", true);
|
||||
node2D->set_parameter(rclcpp::Parameter("test.downsample_costmap", true));
|
||||
node2D->declare_parameter("test.downsampling_factor", 2);
|
||||
node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2));
|
||||
|
||||
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;
|
||||
goal.pose.position.x = 7.0;
|
||||
goal.pose.position.y = 0.0;
|
||||
goal.pose.orientation.w = 1.0;
|
||||
auto planner_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>();
|
||||
planner_2d->configure(node2D, "test", nullptr, costmap_ros);
|
||||
planner_2d->activate();
|
||||
try {
|
||||
planner_2d->createPlan(start, goal);
|
||||
} catch (...) {
|
||||
}
|
||||
|
||||
planner_2d->deactivate();
|
||||
planner_2d->cleanup();
|
||||
|
||||
planner_2d.reset();
|
||||
costmap_ros->on_cleanup(rclcpp_lifecycle::State());
|
||||
node2D.reset();
|
||||
costmap_ros.reset();
|
||||
}
|
||||
|
||||
TEST(SmacTest, test_smac_2d_reconfigure) {
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node2D =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("Smac2DTest");
|
||||
|
||||
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_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>();
|
||||
planner_2d->configure(node2D, "test", nullptr, costmap_ros);
|
||||
planner_2d->activate();
|
||||
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
node2D->get_node_base_interface(), node2D->get_node_topics_interface(),
|
||||
node2D->get_node_graph_interface(),
|
||||
node2D->get_node_services_interface());
|
||||
|
||||
auto results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("test.tolerance", 1.0),
|
||||
rclcpp::Parameter("test.cost_travel_multiplier", 1.0),
|
||||
rclcpp::Parameter("test.max_planning_time", 2.0),
|
||||
rclcpp::Parameter("test.downsample_costmap", false),
|
||||
rclcpp::Parameter("test.allow_unknown", false),
|
||||
rclcpp::Parameter("test.downsampling_factor", 2),
|
||||
rclcpp::Parameter("test.max_iterations", -1),
|
||||
rclcpp::Parameter("test.max_on_approach_iterations", -1),
|
||||
rclcpp::Parameter("test.use_final_approach_orientation", false)});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
node2D->get_node_base_interface(),
|
||||
results);
|
||||
|
||||
EXPECT_EQ(node2D->get_parameter("test.tolerance").as_double(), 1.0);
|
||||
EXPECT_EQ(
|
||||
node2D->get_parameter("test.cost_travel_multiplier").as_double(),
|
||||
1.0);
|
||||
EXPECT_EQ(node2D->get_parameter("test.max_planning_time").as_double(), 2.0);
|
||||
EXPECT_EQ(node2D->get_parameter("test.downsample_costmap").as_bool(), false);
|
||||
EXPECT_EQ(node2D->get_parameter("test.allow_unknown").as_bool(), false);
|
||||
EXPECT_EQ(node2D->get_parameter("test.downsampling_factor").as_int(), 2);
|
||||
EXPECT_EQ(node2D->get_parameter("test.max_iterations").as_int(), -1);
|
||||
EXPECT_EQ(node2D->get_parameter("test.use_final_approach_orientation").as_bool(), false);
|
||||
EXPECT_EQ(
|
||||
node2D->get_parameter("test.max_on_approach_iterations").as_int(),
|
||||
-1);
|
||||
|
||||
results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("test.downsample_costmap", true)});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
node2D->get_node_base_interface(),
|
||||
results);
|
||||
}
|
||||
@@ -0,0 +1,149 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_subscriber.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_smac_planner/node_hybrid.hpp"
|
||||
#include "nav2_smac_planner/a_star.hpp"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
#include "nav2_smac_planner/smac_planner_hybrid.hpp"
|
||||
#include "nav2_smac_planner/smac_planner_2d.hpp"
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
// SMAC smoke tests for plugin-level issues rather than algorithms
|
||||
// (covered by more extensively testing in other files)
|
||||
// System tests in nav2_system_tests will actually plan with this work
|
||||
|
||||
TEST(SmacTest, test_smac_se2)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSE2Test");
|
||||
|
||||
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());
|
||||
|
||||
nodeSE2->declare_parameter("test.downsample_costmap", true);
|
||||
nodeSE2->set_parameter(rclcpp::Parameter("test.downsample_costmap", true));
|
||||
nodeSE2->declare_parameter("test.downsampling_factor", 2);
|
||||
nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2));
|
||||
|
||||
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.pose.position.x = 1.0;
|
||||
goal.pose.position.y = 1.0;
|
||||
goal.pose.orientation.w = 1.0;
|
||||
auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>();
|
||||
planner->configure(nodeSE2, "test", nullptr, costmap_ros);
|
||||
planner->activate();
|
||||
|
||||
try {
|
||||
planner->createPlan(start, goal);
|
||||
} catch (...) {
|
||||
}
|
||||
|
||||
planner->deactivate();
|
||||
planner->cleanup();
|
||||
|
||||
planner.reset();
|
||||
costmap_ros->on_cleanup(rclcpp_lifecycle::State());
|
||||
costmap_ros.reset();
|
||||
nodeSE2.reset();
|
||||
}
|
||||
|
||||
TEST(SmacTest, test_smac_se2_reconfigure)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSE2Test");
|
||||
|
||||
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_smac_planner::SmacPlannerHybrid>();
|
||||
planner->configure(nodeSE2, "test", nullptr, costmap_ros);
|
||||
planner->activate();
|
||||
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
nodeSE2->get_node_base_interface(), nodeSE2->get_node_topics_interface(),
|
||||
nodeSE2->get_node_graph_interface(),
|
||||
nodeSE2->get_node_services_interface());
|
||||
|
||||
auto results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("test.downsample_costmap", true),
|
||||
rclcpp::Parameter("test.downsampling_factor", 2),
|
||||
rclcpp::Parameter("test.angle_quantization_bins", 100),
|
||||
rclcpp::Parameter("test.allow_unknown", false),
|
||||
rclcpp::Parameter("test.max_iterations", -1),
|
||||
rclcpp::Parameter("test.minimum_turning_radius", 1.0),
|
||||
rclcpp::Parameter("test.cache_obstacle_heuristic", true),
|
||||
rclcpp::Parameter("test.reverse_penalty", 5.0),
|
||||
rclcpp::Parameter("test.change_penalty", 1.0),
|
||||
rclcpp::Parameter("test.non_straight_penalty", 2.0),
|
||||
rclcpp::Parameter("test.cost_penalty", 2.0),
|
||||
rclcpp::Parameter("test.tolerance", 0.2),
|
||||
rclcpp::Parameter("test.retrospective_penalty", 0.2),
|
||||
rclcpp::Parameter("test.analytic_expansion_ratio", 4.0),
|
||||
rclcpp::Parameter("test.max_planning_time", 10.0),
|
||||
rclcpp::Parameter("test.lookup_table_size", 30.0),
|
||||
rclcpp::Parameter("test.smooth_path", false),
|
||||
rclcpp::Parameter("test.analytic_expansion_max_length", 42.0),
|
||||
rclcpp::Parameter("test.max_on_approach_iterations", 42),
|
||||
rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
nodeSE2->get_node_base_interface(),
|
||||
results);
|
||||
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.downsample_costmap").as_bool(), true);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.downsampling_factor").as_int(), 2);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 100);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.allow_unknown").as_bool(), false);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.max_iterations").as_int(), -1);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.minimum_turning_radius").as_double(), 1.0);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.cache_obstacle_heuristic").as_bool(), true);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.reverse_penalty").as_double(), 5.0);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.change_penalty").as_double(), 1.0);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.non_straight_penalty").as_double(), 2.0);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.cost_penalty").as_double(), 2.0);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.retrospective_penalty").as_double(), 0.2);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.tolerance").as_double(), 0.2);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_ratio").as_double(), 4.0);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.smooth_path").as_bool(), false);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.max_planning_time").as_double(), 10.0);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0);
|
||||
EXPECT_EQ(nodeSE2->get_parameter("test.max_on_approach_iterations").as_int(), 42);
|
||||
EXPECT_EQ(
|
||||
nodeSE2->get_parameter("test.motion_model_for_search").as_string(),
|
||||
std::string("REEDS_SHEPP"));
|
||||
}
|
||||
@@ -0,0 +1,146 @@
|
||||
// Copyright (c) 2021 Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_subscriber.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_smac_planner/node_hybrid.hpp"
|
||||
#include "nav2_smac_planner/a_star.hpp"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
#include "nav2_smac_planner/smac_planner_lattice.hpp"
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
// Simple wrapper to be able to call a private member
|
||||
class LatticeWrap : public nav2_smac_planner::SmacPlannerLattice
|
||||
{
|
||||
public:
|
||||
void callDynamicParams(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
dynamicParametersCallback(parameters);
|
||||
}
|
||||
};
|
||||
|
||||
// SMAC smoke tests for plugin-level issues rather than algorithms
|
||||
// (covered by more extensively testing in other files)
|
||||
// System tests in nav2_system_tests will actually plan with this work
|
||||
|
||||
TEST(SmacTest, test_smac_lattice)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacLatticeTest");
|
||||
|
||||
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.pose.position.x = 1.0;
|
||||
goal.pose.position.y = 1.0;
|
||||
goal.pose.orientation.w = 1.0;
|
||||
auto planner = std::make_unique<nav2_smac_planner::SmacPlannerLattice>();
|
||||
try {
|
||||
// Expect to throw due to invalid prims file in param
|
||||
planner->configure(nodeLattice, "test", nullptr, costmap_ros);
|
||||
} catch (...) {
|
||||
}
|
||||
planner->activate();
|
||||
|
||||
try {
|
||||
planner->createPlan(start, goal);
|
||||
} catch (...) {
|
||||
}
|
||||
|
||||
planner->deactivate();
|
||||
planner->cleanup();
|
||||
|
||||
planner.reset();
|
||||
costmap_ros->on_cleanup(rclcpp_lifecycle::State());
|
||||
costmap_ros.reset();
|
||||
nodeLattice.reset();
|
||||
}
|
||||
|
||||
TEST(SmacTest, test_smac_lattice_reconfigure)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacLatticeTest");
|
||||
|
||||
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<LatticeWrap>();
|
||||
try {
|
||||
// Expect to throw due to invalid prims file in param
|
||||
planner->configure(nodeLattice, "test", nullptr, costmap_ros);
|
||||
} catch (...) {
|
||||
}
|
||||
planner->activate();
|
||||
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
nodeLattice->get_node_base_interface(), nodeLattice->get_node_topics_interface(),
|
||||
nodeLattice->get_node_graph_interface(),
|
||||
nodeLattice->get_node_services_interface());
|
||||
|
||||
auto results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("test.allow_unknown", false),
|
||||
rclcpp::Parameter("test.max_iterations", -1),
|
||||
rclcpp::Parameter("test.cache_obstacle_heuristic", true),
|
||||
rclcpp::Parameter("test.reverse_penalty", 5.0),
|
||||
rclcpp::Parameter("test.change_penalty", 1.0),
|
||||
rclcpp::Parameter("test.non_straight_penalty", 2.0),
|
||||
rclcpp::Parameter("test.cost_penalty", 2.0),
|
||||
rclcpp::Parameter("test.retrospective_penalty", 0.2),
|
||||
rclcpp::Parameter("test.analytic_expansion_ratio", 4.0),
|
||||
rclcpp::Parameter("test.max_planning_time", 10.0),
|
||||
rclcpp::Parameter("test.lookup_table_size", 30.0),
|
||||
rclcpp::Parameter("test.smooth_path", false),
|
||||
rclcpp::Parameter("test.analytic_expansion_max_length", 42.0),
|
||||
rclcpp::Parameter("test.tolerance", 42.0),
|
||||
rclcpp::Parameter("test.rotation_penalty", 42.0),
|
||||
rclcpp::Parameter("test.max_on_approach_iterations", 42),
|
||||
rclcpp::Parameter("test.allow_reverse_expansion", true)});
|
||||
|
||||
try {
|
||||
// All of these params will re-init A* which will involve loading the control set file
|
||||
// which will cause an exception because the file does not exist. This will cause an
|
||||
// expected failure preventing parameter updates from being successfully processed
|
||||
rclcpp::spin_until_future_complete(
|
||||
nodeLattice->get_node_base_interface(),
|
||||
results);
|
||||
} catch (...) {
|
||||
}
|
||||
|
||||
// So instead, lets call manually on a change
|
||||
std::vector<rclcpp::Parameter> parameters;
|
||||
parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI")));
|
||||
EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error);
|
||||
}
|
||||
@@ -0,0 +1,178 @@
|
||||
// Copyright (c) 2020, Samsung Research America
|
||||
//
|
||||
// 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <limits>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_subscriber.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_smac_planner/node_hybrid.hpp"
|
||||
#include "nav2_smac_planner/a_star.hpp"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
#include "nav2_smac_planner/smoother.hpp"
|
||||
#include "ament_index_cpp/get_package_share_directory.hpp"
|
||||
|
||||
using namespace nav2_smac_planner; // NOLINT
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
class SmootherWrapper : public nav2_smac_planner::Smoother
|
||||
{
|
||||
public:
|
||||
explicit SmootherWrapper(const SmootherParams & params)
|
||||
: nav2_smac_planner::Smoother(params)
|
||||
{}
|
||||
|
||||
std::vector<PathSegment> findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path)
|
||||
{
|
||||
return findDirectionalPathSegments(path);
|
||||
}
|
||||
};
|
||||
|
||||
TEST(SmootherTest, test_full_smoother)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSmootherTest");
|
||||
nav2_smac_planner::SmootherParams params;
|
||||
params.get(node, "test");
|
||||
double maxtime = 1.0;
|
||||
|
||||
// Make smoother and costmap to smooth in
|
||||
auto smoother = std::make_unique<SmootherWrapper>(params);
|
||||
smoother->initialize(0.4 /*turning radius*/);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmap =
|
||||
new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0);
|
||||
// island in the middle of lethal cost to cross
|
||||
for (unsigned int i = 20; i <= 30; ++i) {
|
||||
for (unsigned int j = 20; j <= 30; ++j) {
|
||||
costmap->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
|
||||
// Setup A* search to get path to smooth
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.change_penalty = 0.05;
|
||||
info.non_straight_penalty = 1.05;
|
||||
info.reverse_penalty = 2.0;
|
||||
info.cost_penalty = 2.0;
|
||||
info.retrospective_penalty = 0.0;
|
||||
info.analytic_expansion_ratio = 3.5;
|
||||
info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05
|
||||
info.analytic_expansion_max_length = 20.0; // in grid coordinates
|
||||
unsigned int size_theta = 72;
|
||||
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
|
||||
nav2_smac_planner::MotionModel::REEDS_SHEPP, info);
|
||||
int max_iterations = 10000;
|
||||
float tolerance = 10.0;
|
||||
int it_on_approach = 10;
|
||||
double max_planning_time = 120.0;
|
||||
int num_it = 0;
|
||||
|
||||
a_star.initialize(
|
||||
false, max_iterations, std::numeric_limits<int>::max(), max_planning_time, 401, size_theta);
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap, size_theta, node);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
// Create A* search to smooth
|
||||
a_star.setCollisionChecker(checker.get());
|
||||
a_star.setStart(5u, 5u, 0u);
|
||||
a_star.setGoal(45u, 45u, 36u);
|
||||
nav2_smac_planner::NodeHybrid::CoordinateVector path;
|
||||
EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
|
||||
|
||||
// Convert to world coordinates and get length to compare to smoothed length
|
||||
nav_msgs::msg::Path plan;
|
||||
plan.header.stamp = node->now();
|
||||
plan.header.frame_id = "map";
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = plan.header;
|
||||
pose.pose.position.z = 0.0;
|
||||
pose.pose.orientation.x = 0.0;
|
||||
pose.pose.orientation.y = 0.0;
|
||||
pose.pose.orientation.z = 0.0;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
double initial_length = 0.0;
|
||||
double x_m = path[path.size() - 1].x, y_m = path[path.size() - 1].y;
|
||||
plan.poses.reserve(path.size());
|
||||
for (int i = path.size() - 1; i >= 0; --i) {
|
||||
pose.pose = nav2_smac_planner::getWorldCoords(path[i].x, path[i].y, costmap);
|
||||
pose.pose.orientation = nav2_smac_planner::getWorldOrientation(path[i].theta);
|
||||
plan.poses.push_back(pose);
|
||||
initial_length += hypot(path[i].x - x_m, path[i].y - y_m);
|
||||
x_m = path[i].x;
|
||||
y_m = path[i].y;
|
||||
}
|
||||
|
||||
// Check that we accurately detect that this path has a reversing segment
|
||||
EXPECT_EQ(smoother->findDirectionalPathSegmentsWrapper(plan).size(), 2u);
|
||||
|
||||
// Test smoother, should succeed with same number of points
|
||||
// and shorter overall length, while still being collision free.
|
||||
auto path_size_in = plan.poses.size();
|
||||
EXPECT_TRUE(smoother->smooth(plan, costmap, maxtime));
|
||||
EXPECT_EQ(plan.poses.size(), path_size_in); // Should have same number of poses
|
||||
double length = 0.0;
|
||||
x_m = plan.poses[0].pose.position.x;
|
||||
y_m = plan.poses[0].pose.position.y;
|
||||
for (unsigned int i = 0; i != plan.poses.size(); i++) {
|
||||
// Should be collision free
|
||||
EXPECT_EQ(costmap->getCost(plan.poses[i].pose.position.x, plan.poses[i].pose.position.y), 0);
|
||||
length += hypot(plan.poses[i].pose.position.x - x_m, plan.poses[i].pose.position.y - y_m);
|
||||
x_m = plan.poses[i].pose.position.x;
|
||||
y_m = plan.poses[i].pose.position.y;
|
||||
}
|
||||
EXPECT_LT(length, initial_length); // Should be shorter
|
||||
|
||||
// Try again but with failure modes
|
||||
|
||||
// Failure mode: not enough iterations to complete
|
||||
params.max_its_ = 0;
|
||||
auto smoother_bypass = std::make_unique<SmootherWrapper>(params);
|
||||
EXPECT_FALSE(smoother_bypass->smooth(plan, costmap, maxtime));
|
||||
params.max_its_ = 1;
|
||||
auto smoother_failure = std::make_unique<SmootherWrapper>(params);
|
||||
EXPECT_FALSE(smoother_failure->smooth(plan, costmap, maxtime));
|
||||
|
||||
// Failure mode: Not enough time
|
||||
double max_no_time = 0.0;
|
||||
EXPECT_FALSE(smoother->smooth(plan, costmap, max_no_time));
|
||||
|
||||
// Failure mode: Path is in collision, do 2x to exercise overlapping point
|
||||
// attempts to update orientation should also fail
|
||||
pose.pose.position.x = 1.25;
|
||||
pose.pose.position.y = 1.25;
|
||||
plan.poses.push_back(pose);
|
||||
plan.poses.push_back(pose);
|
||||
EXPECT_FALSE(smoother->smooth(plan, costmap, maxtime));
|
||||
EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.z, 1.0, 1e-3);
|
||||
EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.x, 0.0, 1e-3);
|
||||
EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.y, 0.0, 1e-3);
|
||||
EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.w, 0.0, 1e-3);
|
||||
|
||||
delete costmap;
|
||||
}
|
||||
Reference in New Issue
Block a user