複数の Input と ROS message_filter

前回のエントリでオドメトリの話をしましたが，自分のオドメトリを生成するために必要な Input は下記になります．

Input :

1. sensor_msgs/JointState.h : i-Cart Middleのモータドライバからの出力
2. kmsgs/Imu.h : 自作 IMU からのヨーレート出力

ここで，1の入力が5ms，2の入力が10msなんですが，普通にROSのcallbackを使うと1の入力が5ms，2の入力が10msごとに届いてその都度コールバックが呼ばれる形になると思います．オドメトリを計算するときには，バッファに対してロックをかけてヨーレート，車輪の回転角度をコピーしないといけないですが，このやり方だとInputの数が増えるに従ってロックをかける数が多くなってしまいます．最初はこの方法でやろうとしていたんですが，どうもロックのオーバーヘッドが大きすぎて，計算が回りきってないようだったので，message_filterを使いました．（自分のロックのかけ方が下手糞だったこともあります．（笑））

オドメトリモジュールの In/Out

いちいちLockをしてバッファコピーしていた時のオドメトリ結果（0.5倍速再生）

いちいちLockをしてバッファコピーしていた時のオドメトリ結果（3倍速再生）

上記の写真を比較するとわかると思うんですが，3倍速再生だとヨーレートの積分計算が十分に間に合ってなく，コーナーを迎えるたびに実際のヨー角と計算上のヨー角の開きが大きくなってしまいます．

Lockフリーオドメトリ作成

ということで，ROSのノード内でLockフリーにするにはどうすればよいか検討しました．調べてみた結果，message_filterを使えば所望の実装が簡単にできることがわかりました．
http://wiki.ros.org/message_filters

で，今回の自分の場合，最終的にオドメトリ計算に使用される入力の組み合わせイメージは下記のようになります．

結局 JointState が 5ms 毎に届いているのに，二回に一回は捨ててしまっていることになりますが，10ms毎に計算できれば十分かと．
結果的に3倍速で実行しても何とか回りきるようになりました．ちょっとヨー角が出すぎてしまってますが，，，まあ，こんなもんでしょう．

最終的な実装のイメージは下記のようになりました．

//
#include <sys/time.h>
#include <cstddef>

//
#include <nodelet/nodelet.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>
#include <string>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

//
#include "ros/ros.h"
#include "odometry.h"

// 3rd party include
#include "kmsgs/Imu.h"

namespace koichi_robotics_lib {

class OdometryNodelet : public nodelet::Nodelet {

public:

  OdometryNodelet();

  ~OdometryNodelet();

  virtual void onInit();

private:

  void publishTransform(const nav_msgs::Odometry &odomMsg, std::string frameName);

  void sensorCallback(const kmsgs::ImuConstPtr &imuMsg, const sensor_msgs::JointStateConstPtr &jointPos);

  void calcAndPublishOdom(const ros::Time &stamp);

  void readParams(OdomParams &odomParam);

  void readParam(std::string paramName, bool result);

  void parseJointStatesMsg(const sensor_msgs::JointStateConstPtr &jointPos, 
    double &rRad, double &lRad, double &rVel, double &lVel);

  void fillOdomMessage(nav_msgs::Odometry &odomMsg, std::string frameName, const ros::Time &stamp, 
    const OdomPos &odomPos, const OdomVel &odomVel);

private:

  // Topic Name
  std::string odomImuFrameName, odomWhlFrameName, baselinkFrameName;

  int maxSyncDiff;

  ros::NodeHandle nh, nh_ns;
  nav_msgs::Odometry curOdom;

  // ROS Subsceibers
  message_filters::Subscriber<kmsgs::Imu> *subImu;
  message_filters::Subscriber<sensor_msgs::JointState> *subWhl;

  // ROS Sync Policy
  typedef message_filters::sync_policies::ApproximateTime<
    kmsgs::Imu, sensor_msgs::JointState
  > OdomSyncPolicy;
  message_filters::Synchronizer<OdomSyncPolicy> *odomSync;

  // ROS Publisher
  ros::Publisher odomPubWhl, odomPubImu;
  tf::TransformBroadcaster *odomBroadCaster;

  // Class
  Odometry odometry;
};

}

using namespace std;
using namespace koichi_robotics_lib;

OdometryNodelet::OdometryNodelet() {
}

OdometryNodelet::~OdometryNodelet() {
  odometry.Finalize();
  delete this->odomBroadCaster;
  delete this->subImu;
  delete this->subWhl;
  delete this->odomSync;
}

void OdometryNodelet::onInit() {
  NODELET_DEBUG("Initializing Odometry Nodelet....");

  nh = getMTNodeHandle();
  nh_ns = getMTPrivateNodeHandle();
  odomBroadCaster = new tf::TransformBroadcaster();

  OdomParams params;
  readParams(params);
  odometry.Initialize(params);

  subImu = new message_filters::Subscriber<kmsgs::Imu>(nh, "imu", 100);
  subWhl = new message_filters::Subscriber<sensor_msgs::JointState>(nh, "joint_states", 100);
  
  odomSync = new message_filters::Synchronizer<OdomSyncPolicy>(OdomSyncPolicy(maxSyncDiff), *subImu, *subWhl);
  odomSync->registerCallback(boost::bind(&OdometryNodelet::sensorCallback, this, _1, _2));

  odomPubWhl = nh.advertise<nav_msgs::Odometry>("odom_whl", 100);
  odomPubImu = nh.advertise<nav_msgs::Odometry>("odom_imu", 100);
}

void OdometryNodelet::readParams(OdomParams &odomParams) {

  // Topic Name
  readParam("odometry_imu_frame_name", nh.param<std::string>("odometry_imu_frame_name", odomImuFrameName, ""));
  readParam("odometry_whl_frame_name", nh.param<std::string>("odometry_whl_frame_name", odomWhlFrameName, ""));
  readParam("base_link_frame_name", nh.param<std::string>("base_link_frame_name", baselinkFrameName, ""));

  readParam("gyro_1bit_in_rad", nh.param<double>("gyro_1bit_in_rad", odomParams.gyro1BitInRad, 0.0));

  double r_whl_dia, l_whl_dia;
  readParam("l_whl_diameter", nh.param<double>("l_whl_diameter", l_whl_dia, 0.0));
  odomParams.leftDistPerRad = l_whl_dia / 2.0;

  readParam("r_whl_diameter", nh.param<double>("r_whl_diameter", r_whl_dia, 0.0));
  odomParams.rightDistPerRad = r_whl_dia / 2.0;

  readParam("odometry/standstill_thresh", nh.param<double>("odometry/standstill_thresh", odomParams.standstillThresh, 0.0));
  readParam("wheel_base", nh.param<double>("wheel_base", odomParams.wheelBase, 1.0));
  readParam("odometry/use_left_pls", nh.param<bool>("odometry/use_left_pls", odomParams.useLeftPls, false));
  readParam("odometry/use_right_pls", nh.param<bool>("odometry/use_right_pls", odomParams.useRightPls, false));
  readParam("cycle_time", nh_ns.param<double>("cycle_time", odomParams.cycleTime, 0.0));
  readParam("max_sync_diff", nh_ns.param<int>("max_sync_diff", maxSyncDiff, 8));

  // Buffer Size
  readParam("imu_yaw_buffer", nh_ns.param<int>("imu_yaw_buffer", odomParams.imuYawBufferSize, 0.0));
  readParam("whl_yaw_buffer", nh_ns.param<int>("whl_yaw_buffer", odomParams.whlYawBufferSize, 0.0));
  readParam("dist_buffer", nh_ns.param<int>("dist_buffer", odomParams.distBufferSize, 0));

}

void OdometryNodelet::readParam(string paramName, bool result) {
  if (!result) {
    ROS_FATAL("Param read failure. : %s", paramName.c_str());
  }
}

void OdometryNodelet::publishTransform(const nav_msgs::Odometry &odomMsg, std::string odomFrameName) {

  tf::Transform odomTrans;
  tf::Pose tfPose;

  tf::poseMsgToTF(odomMsg.pose.pose, odomTrans);
  odomBroadCaster->sendTransform(tf::StampedTransform(odomTrans, ros::Time::now(), odomFrameName, baselinkFrameName));
}

void OdometryNodelet::sensorCallback(const kmsgs::ImuConstPtr &imuMsg, const sensor_msgs::JointStateConstPtr &jointPos) {

  double rRad, lRad, rVel, lVel;
  parseJointStatesMsg(jointPos, rRad, lRad, rVel, lVel);
  odometry.SetCurrentPosition(rRad, lRad, rVel, lVel);
  odometry.SetCurrentGyroZ(imuMsg->omegaZ);
  calcAndPublishOdom(imuMsg->header.stamp);
}

void OdometryNodelet::parseJointStatesMsg(const sensor_msgs::JointStateConstPtr &jointPos, double &rRad, double &lRad, double &rVel, double &lVel) {

  for (int i = 0; i < jointPos->name.size(); i++) {
    
    string name = jointPos->name[i];
    double jntPos = jointPos->position[i];
    double jntVel = jointPos->velocity[i];

    if (name == "right") {
      rRad = -jntPos;
      rVel = -jntVel;
    } else if (name == "left") {
      lRad = jntPos;
      lVel = jntVel;
    } else {
      ROS_ERROR("Unexpected Joint Name : %s", name);
    }
  }
}

void OdometryNodelet::calcAndPublishOdom(const ros::Time &stamp) {

  static double lastStamp = stamp.toSec();
  double newStamp = stamp.toSec();
  double dT = newStamp - lastStamp;
  lastStamp = newStamp;

  OdomPos odomWhl, odomImu;
  OdomVel velWhl, velImu;
  odometry.CalculateOdometry(odomWhl, odomImu, velWhl, velImu, dT);

  // Published IMU based odometry.
  {
    nav_msgs::Odometry odomImuMsg;
    fillOdomMessage(odomImuMsg, odomImuFrameName, stamp, odomImu, velImu);
    publishTransform(odomImuMsg, odomImuFrameName);
    odomPubImu.publish(odomImuMsg);
  }

  // Published Whl Based Odometry.
  {
    nav_msgs::Odometry odomWhlMsg;
    fillOdomMessage(odomWhlMsg, odomWhlFrameName, stamp, odomWhl, velWhl);
    publishTransform(odomWhlMsg, odomWhlFrameName);
    odomPubWhl.publish(odomWhlMsg);
  }

}

void OdometryNodelet::fillOdomMessage(nav_msgs::Odometry &odomMsg, std::string odomFrameName,
    const ros::Time &stamp, const OdomPos &odomPos, const OdomVel &odomVel) {

  // Header
  odomMsg.header.frame_id = odomFrameName;
  odomMsg.header.stamp = stamp;

  // Pose
  odomMsg.pose.pose.position.x = odomPos.x;
  odomMsg.pose.pose.position.y = odomPos.y;
  odomMsg.pose.pose.position.z = 0.0;
  odomMsg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(odomPos.angz);

  odomMsg.pose.covariance[0] = odomPos.totDist;
  // Twist
  odomMsg.twist.twist.linear.x = odomVel.vx;
  odomMsg.twist.twist.linear.y = odomVel.vy;
  odomMsg.twist.twist.linear.z = 0;
  odomMsg.twist.twist.angular.x = 0;
  odomMsg.twist.twist.angular.y = 0;
  odomMsg.twist.twist.angular.z = odomVel.wz;

}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(koichi_robotics_lib::OdometryNodelet, nodelet::Nodelet)