【ROS总结】 ROS接口——Odometry

版本：Indigo

  这篇文章主要是把平时用到的一些ROS接口梳理一下，避免无法和ros进行对接，首先ROS中相对重要的是里程计（Odometry），里程计的重要性不言而喻，如果没有里程计，不管是建立地图还是导航都不会很好的工作，相应的Odometry更新也有一些节点可以调用，本文主要用到的是ros_controllers中的差分驱动控制器，介绍文档如下，该控制器用处广泛，基本差分驱动都可以用到，从底层得到左右电机编码器数据，通过机器人运动学转换成Odometry消息发送出去，其他节点得到Odometry消息就可以在rviz或者Gazebo中更新机器人位置信息，也可以加上imu数据让机器人更准确。

   安装方法为：

$ sudo apt-get install ros-indigo-ros-controllers

   本文并没有直接使用Odometry消息，而是通过ros_controls中的hardware_interface来更新Odometry。相应教程说明如下，链接地址。直接更新hardware_interface中的joint就可以更新Odometry。在本地声明一个类为myrobot，继承于hardware_interface::RobotHw。

#include //关节命令接口，用于接收/cmd_vel数据
#include //关节状态接口，用于更新Odometry
#include 

class MyRobot : public hardware_interface::RobotHW
{
public:
  MyRobot() 
 { 
   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_a("A", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_a);

   hardware_interface::JointStateHandle state_handle_b("B", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_b);

   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("A"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_a);

   hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("B"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_b);

   registerInterface(&jnt_pos_interface);
  }

private:
  hardware_interface::JointStateInterface jnt_state_interface;
  hardware_interface::PositionJointInterface jnt_pos_interface;
  double cmd[2];
  double pos[2];
  double vel[2];
  double eff[2];
};

  其中"A"为机器人模型中的关节，一般为wheel_left。同理“B"也是轮子关节，一般为wheel_right。pos为当前机器人的位置，vel为机器人目前速度，eff为初始状态时机器人位置，cmd为接收cmd_vel消息用于控制机器人运动。需要注意的是pos为轮子所走过的弧度，因此上传时需要上传弧度值。

 Odometry.cpp文件中更新Odometry的代码为：

 bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
  {
    /// Get current wheel joint positions:
    const double left_wheel_cur_pos  = left_pos  * wheel_radius_;
    const double right_wheel_cur_pos = right_pos * wheel_radius_;

    /// Estimate velocity of wheels using old and current position:
    const double left_wheel_est_vel  = left_wheel_cur_pos  - left_wheel_old_pos_;
    const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;

    /// Update old position with current:
    left_wheel_old_pos_  = left_wheel_cur_pos;
    right_wheel_old_pos_ = right_wheel_cur_pos;

    /// Compute linear and angular diff:
    const double linear  = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
    const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;

    /// Integrate odometry:
    integrate_fun_(linear, angular);

    /// We cannot estimate the speed with very small time intervals:
    const double dt = (time - timestamp_).toSec();
    if (dt < 0.0001)
      return false; // Interval too small to integrate with

    timestamp_ = time;

    /// Estimate speeds using a rolling mean to filter them out:
    linear_acc_(linear/dt);
    angular_acc_(angular/dt);

    linear_ = bacc::rolling_mean(linear_acc_);
    angular_ = bacc::rolling_mean(angular_acc_);

    return true;
  }

  其中

 const double left_wheel_cur_pos  = left_pos  * wheel_radius_;

  当前的轮子所走过的弧度*轮子半径，得到当前轮子走过的距离，因此，这就是为什么pos要为弧度值。