Nav2 Navigation Stack - jazzy  jazzy
ROS 2 Navigation Stack
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
nav2_util::OdomSmoother Class Reference

#include <nav2_util/include/nav2_util/odometry_utils.hpp>

Collaboration diagram for nav2_util::OdomSmoother:
Collaboration graph
[legend]

Public Member Functions

 OdomSmoother (const rclcpp::Node::WeakPtr &parent, double filter_duration=0.3, const std::string &odom_topic="odom")
 Constructor that subscribes to an Odometry topic. More...
 
 OdomSmoother (const nav2_util::LifecycleNode::WeakPtr &parent, double filter_duration=0.3, const std::string &odom_topic="odom")
 Overloadded Constructor for nav_util::LifecycleNode parent that subscribes to an Odometry topic. More...
 
geometry_msgs::msg::Twist getTwist ()
 Get twist msg from smoother. More...
 
geometry_msgs::msg::TwistStamped getTwistStamped ()
 Get twist stamped msg from smoother. More...
 

Protected Member Functions

void odomCallback (nav_msgs::msg::Odometry::SharedPtr msg)
 Callback of odometry subscriber to process. More...
 
void updateState ()
 Update internal state of the smoother after getting new data.
 

Protected Attributes

rclcpp::Subscription< nav_msgs::msg::Odometry >::SharedPtr odom_sub_
 
nav_msgs::msg::Odometry odom_cumulate_
 
geometry_msgs::msg::TwistStamped vel_smooth_
 
std::mutex odom_mutex_
 
rclcpp::Duration odom_history_duration_
 
std::deque< nav_msgs::msg::Odometry > odom_history_
 

Detailed Description

Wrapper for getting smooth odometry readings using a simple moving avergae. Subscribes to the topic with a mutex.

Definition at line 41 of file odometry_utils.hpp.

Constructor & Destructor Documentation

◆ OdomSmoother() [1/2]

nav2_util::OdomSmoother::OdomSmoother ( const rclcpp::Node::WeakPtr &  parent,
double  filter_duration = 0.3,
const std::string &  odom_topic = "odom" 
)
explicit

Constructor that subscribes to an Odometry topic.

Parameters
parentNodeHandle for creating subscriber
filter_durationDuration for odom history (seconds)
odom_topicTopic on which odometry should be received

Definition at line 26 of file odometry_utils.cpp.

References odomCallback().

Here is the call graph for this function:

◆ OdomSmoother() [2/2]

nav2_util::OdomSmoother::OdomSmoother ( const nav2_util::LifecycleNode::WeakPtr &  parent,
double  filter_duration = 0.3,
const std::string &  odom_topic = "odom" 
)
explicit

Overloadded Constructor for nav_util::LifecycleNode parent that subscribes to an Odometry topic.

Parameters
parentNodeHandle for creating subscriber
filter_durationDuration for odom history (seconds)
odom_topicTopic on which odometry should be received

Definition at line 46 of file odometry_utils.cpp.

References odomCallback().

Here is the call graph for this function:

Member Function Documentation

◆ getTwist()

geometry_msgs::msg::Twist nav2_util::OdomSmoother::getTwist ( )
inline

Get twist msg from smoother.

Returns
twist Twist msg

Definition at line 71 of file odometry_utils.hpp.

◆ getTwistStamped()

geometry_msgs::msg::TwistStamped nav2_util::OdomSmoother::getTwistStamped ( )
inline

Get twist stamped msg from smoother.

Returns
twist TwistStamped msg

Definition at line 77 of file odometry_utils.hpp.

◆ odomCallback()

void nav2_util::OdomSmoother::odomCallback ( nav_msgs::msg::Odometry::SharedPtr  msg)
protected

Callback of odometry subscriber to process.

Parameters
msgOdometry msg to smooth

Definition at line 66 of file odometry_utils.cpp.

References updateState().

Referenced by OdomSmoother().

Here is the call graph for this function:
Here is the caller graph for this function:

The documentation for this class was generated from the following files: