Cost function for path smoothing with multiple terms including curvature, smoothness, distance from original and obstacle avoidance. More...
#include <nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp>
Collaboration diagram for nav2_constrained_smoother::SmootherCostFunction:
Public Member Functions | |
| SmootherCostFunction (const Eigen::Vector2d &original_pos, double next_to_last_length_ratio, bool reversing, const nav2_costmap_2d::Costmap2D *costmap, const std::shared_ptr< ceres::BiCubicInterpolator< ceres::Grid2D< unsigned char >>> &costmap_interpolator, const SmootherParams ¶ms, double costmap_weight) | |
| A constructor for nav2_constrained_smoother::SmootherCostFunction. More... | |
| ceres::CostFunction * | AutoDiff () |
| void | setCostmapWeight (double costmap_weight) |
| double | getCostmapWeight () |
| template<typename T > | |
| bool | operator() (const T *const pt, const T *const pt_next, const T *const pt_prev, T *pt_residual) const |
| Smoother cost function evaluation. More... | |
Protected Member Functions | |
| template<typename T > | |
| void | addSmoothingResidual (const double &weight, const Eigen::Matrix< T, 2, 1 > &pt, const Eigen::Matrix< T, 2, 1 > &pt_next, const Eigen::Matrix< T, 2, 1 > &pt_prev, T &r) const |
| Cost function term for smooth paths. More... | |
| template<typename T > | |
| void | addCurvatureResidual (const double &weight, const Eigen::Matrix< T, 2, 1 > &pt, const Eigen::Matrix< T, 2, 1 > &pt_next, const Eigen::Matrix< T, 2, 1 > &pt_prev, T &r) const |
| Cost function term for maximum curved paths. More... | |
| template<typename T > | |
| void | addDistanceResidual (const double &weight, const Eigen::Matrix< T, 2, 1 > &xi, const Eigen::Matrix< T, 2, 1 > &xi_original, T &r) const |
| Cost function derivative term for steering away changes in pose. More... | |
| template<typename T > | |
| void | addCostResidual (const double &weight, const Eigen::Matrix< T, 2, 1 > &pt, const Eigen::Matrix< T, 2, 1 > &pt_next, const Eigen::Matrix< T, 2, 1 > &pt_prev, T &r) const |
| Cost function term for steering away from costs. More... | |
Protected Attributes | |
| const Eigen::Vector2d | original_pos_ |
| double | next_to_last_length_ratio_ |
| bool | reversing_ |
| SmootherParams | params_ |
| double | costmap_weight_ |
| Eigen::Vector2d | costmap_origin_ |
| double | costmap_resolution_ |
| std::shared_ptr< ceres::BiCubicInterpolator< ceres::Grid2D< unsigned char > > > | costmap_interpolator_ |
Detailed Description
Cost function for path smoothing with multiple terms including curvature, smoothness, distance from original and obstacle avoidance.
Definition at line 42 of file smoother_cost_function.hpp.
Constructor & Destructor Documentation
◆ SmootherCostFunction()
|
inline |
A constructor for nav2_constrained_smoother::SmootherCostFunction.
- Parameters
-
original_path Original position of the path node next_to_last_length_ratio Ratio of next path segment compared to previous. Negative if one of them represents reversing motion. reversing Whether the path segment after this node represents reversing motion. costmap A costmap to get values for collision and obstacle avoidance params Optimization weights and parameters costmap_weight Costmap cost weight. Can be params.costmap_weight or params.cusp_costmap_weight
Definition at line 55 of file smoother_cost_function.hpp.
Member Function Documentation
◆ addCostResidual()
template<typename T >
|
inlineprotected |
Cost function term for steering away from costs.
- Parameters
-
weight Weight to apply to function value Point Xi's cost' params computed values to reduce overhead r Residual (cost) of term
Definition at line 201 of file smoother_cost_function.hpp.
◆ addCurvatureResidual()
template<typename T >
|
inlineprotected |
Cost function term for maximum curved paths.
- Parameters
-
weight Weight to apply to function pt Point Xi for evaluation pt_next Point Xi+1 for calculating Xi's cost pt_prev Point Xi-1 for calculating Xi's cost curvature_params A struct to cache computations for the jacobian to use r Residual (cost) of term
Definition at line 153 of file smoother_cost_function.hpp.
◆ addDistanceResidual()
template<typename T >
|
inlineprotected |
Cost function derivative term for steering away changes in pose.
- Parameters
-
weight Weight to apply to function xi Point Xi for evaluation xi_original original point Xi for evaluation r Residual (cost) of term
Definition at line 184 of file smoother_cost_function.hpp.
◆ addSmoothingResidual()
template<typename T >
|
inlineprotected |
Cost function term for smooth paths.
- Parameters
-
weight Weight to apply to function pt Point Xi for evaluation pt_next Point Xi+1 for calculating Xi's cost pt_prev Point Xi-1 for calculating Xi's cost r Residual (cost) of term
Definition at line 130 of file smoother_cost_function.hpp.
◆ operator()()
template<typename T >
|
inline |
Smoother cost function evaluation.
- Parameters
-
pt X, Y coords of current point pt_next X, Y coords of next point pt_prev X, Y coords of previous point pt_residual array of output residuals (smoothing, curvature, distance, cost)
- Returns
- if successful in computing values
Definition at line 99 of file smoother_cost_function.hpp.
The documentation for this struct was generated from the following file:
- nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp
