Nav2 Navigation Stack - jazzy  jazzy
ROS 2 Navigation Stack
options.hpp
1 // Copyright (c) 2021 RoboTech Vision
2 // Copyright (c) 2020, Samsung Research America
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License. Reserved.
15 
16 #ifndef NAV2_CONSTRAINED_SMOOTHER__OPTIONS_HPP_
17 #define NAV2_CONSTRAINED_SMOOTHER__OPTIONS_HPP_
18 
19 #include <map>
20 #include <string>
21 #include <vector>
22 #include "rclcpp_lifecycle/lifecycle_node.hpp"
23 #include "nav2_util/node_utils.hpp"
24 #include "ceres/ceres.h"
25 
26 namespace nav2_constrained_smoother
27 {
28 
33 struct SmootherParams
34 {
38  SmootherParams()
39  {
40  }
41 
47  void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
48  {
49  std::string local_name = name + std::string(".");
50 
51  // Smoother params
52  double minimum_turning_radius;
53  nav2_util::declare_parameter_if_not_declared(
54  node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4));
55  node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius);
56  max_curvature = 1.0f / minimum_turning_radius;
57  nav2_util::declare_parameter_if_not_declared(
58  node, local_name + "w_curve", rclcpp::ParameterValue(30.0));
59  node->get_parameter(local_name + "w_curve", curvature_weight);
60  nav2_util::declare_parameter_if_not_declared(
61  node, local_name + "w_cost", rclcpp::ParameterValue(0.015));
62  node->get_parameter(local_name + "w_cost", costmap_weight);
63  double cost_cusp_multiplier;
64  nav2_util::declare_parameter_if_not_declared(
65  node, local_name + "w_cost_cusp_multiplier", rclcpp::ParameterValue(3.0));
66  node->get_parameter(local_name + "w_cost_cusp_multiplier", cost_cusp_multiplier);
67  cusp_costmap_weight = costmap_weight * cost_cusp_multiplier;
68  nav2_util::declare_parameter_if_not_declared(
69  node, local_name + "cusp_zone_length", rclcpp::ParameterValue(2.5));
70  node->get_parameter(local_name + "cusp_zone_length", cusp_zone_length);
71  nav2_util::declare_parameter_if_not_declared(
72  node, local_name + "w_dist", rclcpp::ParameterValue(0.0));
73  node->get_parameter(local_name + "w_dist", distance_weight);
74  nav2_util::declare_parameter_if_not_declared(
75  node, local_name + "w_smooth", rclcpp::ParameterValue(2000000.0));
76  node->get_parameter(local_name + "w_smooth", smooth_weight);
77  nav2_util::declare_parameter_if_not_declared(
78  node, local_name + "cost_check_points", rclcpp::ParameterValue(std::vector<double>()));
79  node->get_parameter(local_name + "cost_check_points", cost_check_points);
80  if (cost_check_points.size() % 3 != 0) {
81  RCLCPP_ERROR(
82  rclcpp::get_logger(
83  "constrained_smoother"),
84  "cost_check_points parameter must contain values as follows: "
85  "[x1, y1, weight1, x2, y2, weight2, ...]");
86  throw std::runtime_error("Invalid parameter: cost_check_points");
87  }
88  // normalize check point weights so that their sum == 1.0
89  double check_point_weights_sum = 0.0;
90  for (size_t i = 2u; i < cost_check_points.size(); i += 3) {
91  check_point_weights_sum += cost_check_points[i];
92  }
93  for (size_t i = 2u; i < cost_check_points.size(); i += 3) {
94  cost_check_points[i] /= check_point_weights_sum;
95  }
96  nav2_util::declare_parameter_if_not_declared(
97  node, local_name + "path_downsampling_factor", rclcpp::ParameterValue(1));
98  node->get_parameter(local_name + "path_downsampling_factor", path_downsampling_factor);
99  nav2_util::declare_parameter_if_not_declared(
100  node, local_name + "path_upsampling_factor", rclcpp::ParameterValue(1));
101  node->get_parameter(local_name + "path_upsampling_factor", path_upsampling_factor);
102  nav2_util::declare_parameter_if_not_declared(
103  node, local_name + "reversing_enabled", rclcpp::ParameterValue(true));
104  node->get_parameter(local_name + "reversing_enabled", reversing_enabled);
105  nav2_util::declare_parameter_if_not_declared(
106  node, local_name + "keep_goal_orientation", rclcpp::ParameterValue(true));
107  node->get_parameter(local_name + "keep_goal_orientation", keep_goal_orientation);
108  nav2_util::declare_parameter_if_not_declared(
109  node, local_name + "keep_start_orientation", rclcpp::ParameterValue(true));
110  node->get_parameter(local_name + "keep_start_orientation", keep_start_orientation);
111  }
112 
113  double smooth_weight{0.0};
114  double costmap_weight{0.0};
115  double cusp_costmap_weight{0.0};
116  double cusp_zone_length{0.0};
117  double distance_weight{0.0};
118  double curvature_weight{0.0};
119  double max_curvature{0.0};
120  double max_time{10.0}; // adjusted by action goal, not by parameters
121  int path_downsampling_factor{1};
122  int path_upsampling_factor{1};
123  bool reversing_enabled{true};
124  bool keep_goal_orientation{true};
125  bool keep_start_orientation{true};
126  std::vector<double> cost_check_points{};
127 };
128 
133 struct OptimizerParams
134 {
135  OptimizerParams()
136  : debug(false),
137  max_iterations(50),
138  param_tol(1e-8),
139  fn_tol(1e-6),
140  gradient_tol(1e-10)
141  {
142  }
143 
149  void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
150  {
151  std::string local_name = name + std::string(".optimizer.");
152 
153  // Optimizer params
154  nav2_util::declare_parameter_if_not_declared(
155  node, local_name + "linear_solver_type", rclcpp::ParameterValue("SPARSE_NORMAL_CHOLESKY"));
156  node->get_parameter(local_name + "linear_solver_type", linear_solver_type);
157  if (solver_types.find(linear_solver_type) == solver_types.end()) {
158  std::stringstream valid_types_str;
159  for (auto type = solver_types.begin(); type != solver_types.end(); type++) {
160  if (type != solver_types.begin()) {
161  valid_types_str << ", ";
162  }
163  valid_types_str << type->first;
164  }
165  RCLCPP_ERROR(
166  rclcpp::get_logger("constrained_smoother"),
167  "Invalid linear_solver_type. Valid values are %s", valid_types_str.str().c_str());
168  throw std::runtime_error("Invalid parameter: linear_solver_type");
169  }
170  nav2_util::declare_parameter_if_not_declared(
171  node, local_name + "param_tol", rclcpp::ParameterValue(1e-15));
172  node->get_parameter(local_name + "param_tol", param_tol);
173  nav2_util::declare_parameter_if_not_declared(
174  node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7));
175  node->get_parameter(local_name + "fn_tol", fn_tol);
176  nav2_util::declare_parameter_if_not_declared(
177  node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10));
178  node->get_parameter(local_name + "gradient_tol", gradient_tol);
179  nav2_util::declare_parameter_if_not_declared(
180  node, local_name + "max_iterations", rclcpp::ParameterValue(100));
181  node->get_parameter(local_name + "max_iterations", max_iterations);
182  nav2_util::declare_parameter_if_not_declared(
183  node, local_name + "debug_optimizer", rclcpp::ParameterValue(false));
184  node->get_parameter(local_name + "debug_optimizer", debug);
185  }
186 
187  const std::map<std::string, ceres::LinearSolverType> solver_types = {
188  {"DENSE_QR", ceres::DENSE_QR},
189  {"SPARSE_NORMAL_CHOLESKY", ceres::SPARSE_NORMAL_CHOLESKY}};
190 
191  bool debug;
192  std::string linear_solver_type;
193  int max_iterations; // Ceres default: 50
194 
195  double param_tol; // Ceres default: 1e-8
196  double fn_tol; // Ceres default: 1e-6
197  double gradient_tol; // Ceres default: 1e-10
198 };
199 
200 } // namespace nav2_constrained_smoother
201 
202 #endif // NAV2_CONSTRAINED_SMOOTHER__OPTIONS_HPP_
nav2_constrained_smoother::OptimizerParams
Definition: options.hpp:134
nav2_constrained_smoother::OptimizerParams::get
void get(rclcpp_lifecycle::LifecycleNode *node, const std::string &name)
Get params from ROS parameter.
Definition: options.hpp:149
nav2_constrained_smoother::SmootherParams
Definition: options.hpp:34
nav2_constrained_smoother::SmootherParams::SmootherParams
SmootherParams()
A constructor for nav2_smac_planner::SmootherParams.
Definition: options.hpp:38
nav2_constrained_smoother::SmootherParams::get
void get(rclcpp_lifecycle::LifecycleNode *node, const std::string &name)
Get params from ROS parameter.
Definition: options.hpp:47
Generated by doxygen 1.9.1