複数の 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)