Class SimpleSmoother

Inheritance Relationships

Base Type

  • public nav2_core::Smoother

Class Documentation

class SimpleSmoother : public nav2_core::Smoother

A path smoother implementation.

Public Functions

SimpleSmoother() = default

A constructor for nav2_smoother::SimpleSmoother.

~SimpleSmoother() override = default

A destructor for nav2_smoother::SimpleSmoother.

void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr&, std::string name, std::shared_ptr<tf2_ros::Buffer>, std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>, std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>) override
inline virtual void cleanup() override

Method to cleanup resources.

inline virtual void activate() override

Method to activate smoother and any threads involved in execution.

inline virtual void deactivate() override

Method to deactivate smoother and any threads involved in execution.

virtual bool smooth(nav_msgs::msg::Path &path, const rclcpp::Duration &max_time) override

Method to smooth given path.

Parameters:
  • path – In-out path to be smoothed

  • max_time – Maximum duration smoothing should take

Returns:

If smoothing was completed (true) or interrupted by time limit (false)

Protected Functions

bool smoothImpl(nav_msgs::msg::Path &path, bool &reversing_segment, const nav2_costmap_2d::Costmap2D *costmap, const double &max_time)

Smoother method - does the smoothing on a segment.

Parameters:
  • path – Reference to path

  • reversing_segment – Return if this is a reversing segment

  • costmap – Pointer to minimal costmap

  • max_time – Maximum time to compute, stop early if over limit

Returns:

If smoothing was successful

inline double getFieldByDim(const geometry_msgs::msg::PoseStamped &msg, const unsigned int &dim)

Get the field value for a given dimension.

Parameters:
  • msg – Current pose to sample

  • dim – Dimension ID of interest

Returns:

dim value

inline void setFieldByDim(geometry_msgs::msg::PoseStamped &msg, const unsigned int dim, const double &value)

Set the field value for a given dimension.

Parameters:
  • msg – Current pose to sample

  • dim – Dimension ID of interest

  • value – to set the dimention to for the pose

Protected Attributes

double tolerance_
double data_w_
double smooth_w_
int max_its_
int refinement_ctr_
bool do_refinement_
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_
rclcpp::Logger logger_ = {rclcpp::get_logger("SimpleSmoother")}