【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 <hardware_interface/joint_command_interface.h>//關節命令接口，用於接收/cmd_vel數據
#include <hardware_interface/joint_state_interface.h>//關節狀態接口，用於更新Odometry
#include <hardware_interface/robot_hw.h>

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要爲弧度值。