Yp-Spur を使ったロボット制御
ロボットが，，，，走ったーーーー(∩´∀｀)∩
9/15日の確認走行完了に向けて準備をしてます．今日明日で，ROSの基本パッケージを使って自動走行できるとこまでもっていかないといけないんですが，やらないといけないことは．．．．
1. 確認走行コースのデータ取得：完了
2. 確認走行コースのマップ作成：完了
3. AMCLを使っての自己位置推定：完了
4. Yp-Spurを使ってロボット制御実施．制御パラメータ調整：<- さっき完了！
5. Teb-Local Planner と Yp-Spur を使ってロボット制御実施：これから
6. move_base をつかって，センシングしながらのロボット制御を実施：これから
Yp-Spur を使った制御ですが，推奨パラメータを使ってテストすると大きな音が鳴って全然動かず．．．焦りました．結果的には比例ゲインが大きすぎて不安定になっているだけでした．T-Frog の方が残してくれている下記の Wiki を見ながらパラメータ調整すれば，すんなり動くようになりました．
パラメータファイル [Robot Platform Project Wiki]
さすが研究者の方が設計してくれただけあって，動き方の安定感が半端ないですね．
ROS amcl を使った自己位置推定＠つくば市役所
ということで，gmapping を使ったマップ生成が完了したので，amcl を用いて自己位置推定をしてみました．一応マップを作成したデータとは異なるデータなので，ここでうまく自己位置推定ができていれば，ある程度再現性はあるかと思います．
結果として，，，，ROSはすごいですね．一昨年・昨年と散々苦しんだ自己位置推定ですが，呆気無く完了しました．実際に会場でやるときは周辺環境の動的変化もあるのでここまでうまくは行かないと思うのですが，ひとまず手持ちのデータに関しては amcl を使って Localization 完了です．ということで，明日からは move_baseやplanner に入っていきます．
ROS slam_gmapping を使ったマップ作成＠つくば市役所
オドメトリ作成を終え，ようやく gmapping を使うとこまで来ました！Lidarの精度はやっぱり強烈ですね．．．もともとオドメトリの精度がそこそこ出ていることもあって，かなり綺麗なマップができました．赤のラインはオドメトリラインです．
bag ファイルを再生後，下記のコマンドで pgm ファイルを生成します．
rosrun map_server map_saver
結果，生成されたマップが下記になります．少しゴミが入ってしまっているので，ここは手作業でクリーニングが必要ですかね．
ステップ３ではこのマップを使って，ロボットの自己位置推定をします．
複数の 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)
オドメトリ
確認走行区間データ取得＠つくば市役所
ということで，お盆休みの最低目標だった「つくばチャレンジのコースでのデータ取り」をなんとか完了しました(ﾟДﾟ)ﾉ
結局モータドライバをてなづけたり，ROSのスクリプトを準備してたら朝方までかかってしまい，，，１１時くらいの出発になってしまいましたが，ペーパドライバのへっぽこ運転で首都高を超えてつくばまで行ってきました．ロボットも大型化したこともあり，車に乗せられるか心配してたんですが，近くのTimes Car RentalでダイハツのWakeを借りられることを発見！さすが、「あんちゃん，ここWakeだよ！」というだけあって，すっぽりすべてが収まりました．
で，つくば市役所についたんですが，さすがに何もない日に一人でロボット動かすのは恥ずかしいですね．．．日曜日ということもあってほとんど無人かと思ったんですが，空いている窓口もあって，割と人の出入りもありました．．．案の定、「なにやってるんですか？」とどこかの婦人に声を掛けられ．．．「げ，まさか，怒られる？」と思ってビビった挙句，「筑波大学４年生です！卒業研究のためにデータ取得してます！」って言いかけたんですが（爆），いくら不老不死の自分といえども，２２歳設定はもう無理や．．．と思い，「サラリーマンなんですが，つくばチャレンジに出てまして，くぁwせdrftgyふじこlp．．．」と正直に自分語りしました（笑）
肝心のデータ取得ですが，９／１５の第三回試験走行会での確認走行完了を目標に，確認走行区間のデータ取りをしました．（走行日以外の動力走行は禁止されていたので，手押しでのデータ取得を実施しました．きちんと守りましたよ～．）
明日からLidarデータ使ってMap作成に入りたいと思います．詳しくはまたアップします！
Hokuyo Lidar "UTM-30LX-EW" の ROS への接続方法
ということで，本日会場入りしてデータ取得は間に合わず．．．明日行きます！
出発に向けていろいろと準備をしているんですが，ついに北陽電機さんにお借りした Lidar が接続完了しました！半年も前にお貸し頂いたのに遅くなりました．．．Ethernet Type の使い方を一通り書いてくれてある記事が思いのほか少なかったのでまとめておきます．
0. urg_node のインストール
urg_node という ROSのパッケージが提供されています．こいつをダウンロードします．
$ sudo apt-get install ros-kinetic-urg-node
で，このパッケージには，
1. urg を動かすためのドライバ，ROSのラッパー
2. urg の IP アドレスを変更するための Python スクリプト
が含まれています．Default の IP アドレスから変更する必要がある場合，2 のスクリプトを実行してセンサーのIPを変更します．
1. PINGで疎通確認
UTM-30LX-EWの初期 IP アドレスですが，"192.168.0.10" として設定されています．Lidar を PC に接続して ping をうつと，下記のように Reply が帰ってきます．
（自分のPCのIPを同じネットワークにしてあげないといけないので，適当に192.168.0.100とかにします．）
$ ping 192.168.0.10 PING 192.168.0.10 (192.168.0.10) 56(84) bytes of data. 64 bytes from 192.168.0.10: icmp_seq=1 ttl=128 time=0.129 ms 64 bytes from 192.168.0.10: icmp_seq=2 ttl=128 time=0.098 ms 64 bytes from 192.168.0.10: icmp_seq=3 ttl=128 time=0.140 ms
で，センサーのIPを変更したい場合，自分のPCのIPアドレスを同じネットワークのものに変更してあげる必要があります．
2. IPアドレス変更
で，仮にロボットのローカルネットワークを下記のようにしたいとします．
172.16.x.x
この場合，下記のコマンドでセンサのIPアドレスを変更できます．
rosrun urg_node set_urg_ip.py --nm 255.255.255.0 --ip 192.168.0.10 172.16.0.10 172.16.0.254
右から二番目が変更したいIPアドレス，右端がデフォルトゲートウェイのIPアドレスです．ここまで完了したら，自分のPCのIPアドレスを172.16.x.xに変更します．
3. urg_nodeの起動とrvizでの可視化．
ここまで来たらあとはnodeを起動して，rvizで可視化するだけです．
urg_nodeの起動
rosrun urg_node urg_node _ip_address:="172.16.0.10"
rvizの起動
rosrun rviz rviz
で，rvizで/LaserScanを可視化し，Fixed Frame を laser にしてあげれば，無事 Lidar が動いているのがわかります．
以上，メモメモでした．
