class OdometryHelperRos {
public:
  OdometryHelperRos(std::string odom_topic = "")
  { 
    setOdomTopic( odom_topic ); 
  }
  ~OdometryHelperRos() {}

  void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);

  void getOdom(nav_msgs::Odometry& base_odom);
  void getRobotVel(tf::Stamped<tf::Pose>& robot_vel);

  /** @brief Set the odometry topic.  This overrides what was set in the constructor, if anything.
   *
   * This unsubscribes from the old topic (if any) and subscribes to the new one (if any).
   *
   * If odom_topic is the empty string, this just unsubscribes from the previous topic. */
  void setOdomTopic(std::string odom_topic);

  std::string getOdomTopic() const { return odom_topic_; }

private:
  std::string odom_topic_;
  // we listen on odometry on the odom topic
  ros::Subscriber odom_sub_;
  nav_msgs::Odometry base_odom_;
  boost::mutex odom_mutex_;
  // global tf frame id
  std::string frame_id_; ///< The frame_id associated this data
};

这是base_local_planner局部规划器里起辅助功能的一个类,主要作用就是从/odom的topic中获取机器人的速度状态(包括线速度(x,y)和角速度z)

Costmap2DROS类有一个成员base_local_planner::OdometryHelperRos odom_helper_;，提供给用户获取当前机器人速度的接口

要使用，首先#include <base_local_planner/odometry_helper_ros.h>，

OdometryHelperRos()在初始化的时候，新建了NodeHandle并设置关注的topic的名称odom_topic;
并在回调函数odomCallback()中, 取出速度信息

和Teb算法的关系

类TrajectoryPlannerROS中定义： base_local_planner::OdometryHelperRos odom_helper_;

move_base的yaml中，对局部路径规划配置：teb_local_planner/TebLocalPlannerROS

MoveBase中有一段：

private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
tc_ = blp_loader_.createInstance(local_planner);
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);

获取当前速度

tf::Stamped<tf::Pose> robot_vel_tf;
base_local_planner::OdometryHelperRos odom_helper_;
odom_helper_.getRobotVel(robot_vel_tf);

geometry_msgs::Twist  robot_vel_;
robot_vel_.linear.x = robot_vel_tf.getOrigin().getX();
robot_vel_.linear.y = robot_vel_tf.getOrigin().getY();
robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation());

base_local_planner::stopped

bool stopped(const nav_msgs::Odometry& base_odom, 
      const double& rot_stopped_velocity, const double& trans_stopped_velocity)
{
    return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity 
      && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
      && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
}

参考： OdometryHelperRos类