add humble-navigation2
This commit is contained in:
X-lanni
@@ -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_
|
||||
Reference in New Issue
Block a user