ROS Navigation Stack について3 ~ Goalを送信するノードの作成 ~

move_baseとodometryに関しては整ったので，ここではmove_baseに対してゴールを送信するノードを作成します．
引用：http://wiki.ros.org/ja/navigation/Tutorials/SendingSimpleGoals

ただ，今回作成したシステムでは，オドメトリ情報がでたらめなので，実際にロボットでmove_baseを動かすには，ここを調整してあげる必要があります．
実際のodometryノードに関しては，あとのポストで詳しく書ければと思います．

1. Goal送信ノード用のパッケージ作成

Goal送信ノード用のパッケージを生成します．ここでは，simple_goal_generatorパッケージとします．

cd ~/reps/catkin_ws/src
catkin_create_pkg simple_goal_generator move_base_msgs actionlib roscpp

2. Goal送信ノードの作成

ここでは，チュートリアル通りのコードsrc/simple_goal_generator.cppを作成します．

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char** argv){
  ros::init(argc, argv, "simple_goal_generator");

  //tell the action client that we want to spin a thread by default
  MoveBaseClient ac("move_base", true);

  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  move_base_msgs::MoveBaseGoal goal;

  //we'll send a goal to the robot to move 1 meter forward
  goal.target_pose.header.frame_id = "base_footprint";
  goal.target_pose.header.stamp = ros::Time::now();

  goal.target_pose.pose.position.x = 1.0;
  goal.target_pose.pose.orientation.w = 1.0;

  ROS_INFO("Sending goal");
  ac.sendGoal(goal);

  ac.waitForResult();

  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Hooray, the base moved 1 meter forward");
  else
    ROS_INFO("The base failed to move forward 1 meter for some reason");

  return 0;
}

3. CMakeLists.txtの生成・編集

いつものように，catkinビルドのためにCMakeLists.txtを変更します．

cmake_minimum_required(VERSION 2.8.3)
project(simple_goal_generator)

find_package(catkin REQUIRED COMPONENTS
  actionlib
  move_base_msgs
  roscpp
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES simple_goal_generator
  CATKIN_DEPENDS actionlib move_base_msgs roscpp
  DEPENDS system_lib
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(simple_goal_generator_node src/simple_goal_generator.cpp)

target_link_libraries(simple_goal_generator_node
  ${catkin_LIBRARIES}
)

4. ノードの起動

roscore
rosrun simple_goal_generator simple_goal_generator_node

move_baseノードが起動している状態で上記ノードを起動すると，goal座標をmove_baseに送ってくれます．
送信後は，下記のようなログが出力されます．

[ INFO] [1486816759.533561032]: Sending goal

ROS Navigation Stack について2 ~ Odometry生成ノードの作成 ~

先ほどのmove_base生成のエントリに引き続き，Odometryを生成するノードを作成します．コードはほぼROSのチュートリアルのままです．

引用：http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom

1. Odometry生成ノード用のパッケージ作成

Odometry生成ノード用のパッケージを生成します．ここでは，simple_odom_generatorとします．

cd ~/reps/catkin_ws/src
catkin_create_pkg simple_odom_generator tf nav_msgs
roscd simple_odom_generator
mkdir src

2. Odometryノードのソース作成

下記，ほぼチュートリアルのコードのままですが，ロボットの土台の座標系をbase_linkからbase_footprintに変えてあります．あと，簡単のために速度も位置も常に0にしてあります．
Publish Rateは10にしてます．デフォルトだと1なのですが，move_baseのデフォルトの設定だとタイムアウトしてしまうので，変えました．

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster odom_broadcaster;

  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  double vx = 0.0;
  double vy = 0.0;
  double vth = 0.0;

  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();

  ros::Rate r(10);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages
    current_time = ros::Time::now();

    //compute odometry in a typical way given the velocities of the robot
    //double dt = (current_time - last_time).toSec();
    double dt = 0;
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_footprint";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);

    //next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    //set the velocity
    odom.child_frame_id = "base_footprint";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    //publish the message
    odom_pub.publish(odom);

    last_time = current_time;
    r.sleep();
  }
}

3. CMakeLists.txtの生成

次にcatkinビルドのためにCMakeLists.txtを変更します．

cmake_minimum_required(VERSION 2.8.3)
project(simple_odom_generator)

find_package(catkin REQUIRED COMPONENTS
  nav_msgs
  tf
)

catkin_package(
  CATKIN_DEPENDS nav_msgs tf
  DEPENDS system_lib
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(simple_odom_generator_node src/simple_odom_generator.cpp)

target_link_libraries(sample_odom_generator_node
   ${catkin_LIBRARIES}
)

4. ノードの起動

roscore
rosrun simple_odom_generator simple_odom_generator_node

5. move_baseノードの起動

rosrun sample_odom_generator sample_odom_generator_node

ここまで来ると，move_baseがエラー・警告をはかなくなります．以下，実行した時のログ．
次は，このmove_baseにたいして，目的地を指定するプログラムを作ります．といっても，位置・速度は永遠に0なので，目的地に対して進むことはないプログラムですが．．．

core service [/rosout] found
process[navigation_velocity_smoother-1]: started with pid [14221]
process[kobuki_safety_controller-2]: started with pid [14222]
process[move_base-3]: started with pid [14228]
[ INFO] [1486805952.117027681]: Using plugin "obstacle_layer"
[ INFO] [1486805952.137293082]:     Subscribed to Topics: scan bump
[ INFO] [1486805952.167710860]: Using plugin "inflation_layer"
[ERROR] [1486805952.176414612]: You must specify at least three points for the robot footprint, reverting to previous footprint.
[ INFO] [1486805952.217033572]: Using plugin "obstacle_layer"
[ INFO] [1486805952.233590024]:     Subscribed to Topics: scan bump
[ INFO] [1486805952.261528950]: Using plugin "inflation_layer"
[ERROR] [1486805952.270706401]: You must specify at least three points for the robot footprint, reverting to previous footprint.
[ INFO] [1486805952.304290698]: Created local_planner dwa_local_planner/DWAPlannerROS
[ INFO] [1486805952.306532365]: Sim period is set to 0.20
[ INFO] [1486805952.509251511]: Recovery behavior will clear layer obstacles
[ INFO] [1486805952.528223121]: Recovery behavior will clear layer obstacles
[ INFO] [1486805952.551962298]: odom received!

ROS Navigation Stack について1 ~ move_baseの起動スクリプト作成 ~

今日はROSのチュートリアルに従って，move_baseに対してシンプルな移動コマンドを発行するところをやりたいと思います．
が，まずはmove_baseを必要なパラメータを提供しつつ起動してあげないといけないので，move_baseの起動スクリプトを作成します．

1. turtlebot_navigationのlaunchファイルを参考に，必要な箇所だけ抜き出す．

まずは，move_baseを起動するためのlaunchファイルをturtlebot_navigationから持ってきます．
turtlebot_navigationのファイルはローカリゼーションまでやっているものなので，
今回は，3D Sensor，Map Server, AMCL無しの構成なので，不要な箇所を削除します．
結局，一つにまとめると，下記のようにすればmove_baseが単体で立ち上がります．
使用するパッケージに関しては，自作パッケージになるので，turtlebot_navigationのところは変えないといけないです．
今回は，tsukuba_exploration_rover_navigationパッケージを自作する前提で作ります．

<launch>
  <include file="$(find tsukuba_exploration_rover_navigation)/launch/includes/velocity_smoother.launch.xml"/>
  <include file="$(find tsukuba_exploration_rover_navigation)/launch/includes/safety_controller.launch.xml"/>
  
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_footprint"/>
  <arg name="global_frame_id" default="odom"/>
  <arg name="odom_topic" default="odom" />

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find tsukuba_exploration_rover_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find tsukuba_exploration_rover_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />   
    <rosparam file="$(find tsukuba_exploration_rover_navigation)/param/local_costmap_params.yaml" command="load" />   
    <rosparam file="$(find tsukuba_exploration_rover_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find tsukuba_exploration_rover_navigation)/param/dwa_local_planner_params.yaml" command="load" />
    <rosparam file="$(find tsukuba_exploration_rover_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find tsukuba_exploration_rover_navigation)/param/global_planner_params.yaml" command="load" />
    <rosparam file="$(find tsukuba_exploration_rover_navigation)/param/navfn_global_planner_params.yaml" command="load" />
    
    <!-- reset frame_id parameters using user input data -->
    <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
    <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
    <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>

    <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>
    <remap from="odom" to="$(arg odom_topic)"/>
  </node>
</launch>

上記のファイルをひとまずmove_base.launchと命名して保存します．保存したものは，自分のパッケージ下のlaunchにでも入れておけばいいかと．
で，その専用のパッケージを作ります．

2. 変更したスクリプトを保存するパッケージを作成する．

cd ~/reps/catkin_ws/src/
catkin_create_pkg tsukuba_exploration_rover_navigation

もともとのturtlebot_navigationからパラメータファイル等々一式をコピーします．

roscd turtlebot_navigation
cp -r launch/ ~/reps/catkin_ws/src/tsukuba_exploration_rover_navigation
cp -r param/ ~/reps/catkin_ws/src/tsukuba_exploration_rover_navigation

３. local_costmapの内容をglobal_costmapにコピーする．

　今回はオドメトリ情報のみを頼りにロボットを動かすので，global_costmapは使いません．が，move_base自体はglobal_costmapを必要とするので，local_costmap_params.yamlの内容をglobal_costmap_params.yamlに上書きします．一行目のglobal_costmap:はそのままにします．

4. move_base.launchの実行

　作成したlaunchファイルを早速実行します．すると，下記の警告が出てきますが，これはまだodom情報を作成・パブリッシュしてないからですね．

[ WARN] [1486792398.759467327]: Timed out waiting for transform from base_footprint to odom to become available before running costmap, tf error: . canTransform returned after 0.100692 timeout was 0.1.

ということで，ひとまずmove_baseを起動するところまでは来ました．次はOdometry情報をパブリッシュするとこをやります．

ROS Navigation Stack について１

以下，ROS Navigation Stackに関して調べたことをまとめておきます．
マッピングを実装して使えるようになるには，何らかのセンシング手段が必要で，まだちょっとハードルが高そうなので，ひとまずオドメトリのみを用いたロボットの自立制御ができるようにします．

move_base周辺のシステム構成図

引用：http://wiki.ros.org/move_base

上記ダイアグラムは本家から持ってきたものですが，今回深くいじるところは"move_base"と書かれている正方形のボックス内の要素になります．move_baseというコンポーネント自体は，各要素をバインドしているコンポーネントになってまして，細かい挙動の実装を持っているというよりは，必要に応じてそれぞれのコンポーネントのAPIをコールしてあげるようなつくりになってます．で，上記の図の登場人物をざっくり説明しますと．．．．

move_baseとつながる部品群

amcl : モンテカルロローカリゼーション

　自己位置推定用の部品です．オドメトリ（超ざっくりいうと車輪の回転数の積分）だけだと当然ながら誤差が少しずつ大きくなってくるので，自分以外の情報を使って実世界に対する自分の相対位置を更新する必要があるのですが，この部品がそれをやってます．ROSのチュートリアルを見る限り，現状2Dレーザスキャナのみの対応になるみたいです．事前に作った地図に対して，レーザスキャナで走行中にとった周囲環境の形状をマッチングしながら自分の位置を推定します．が，今回は使いません．まだ未完成なので．．．

sensor transform : センサとロボットの座標変換

　センサはふつう車両の基準位置からずれているとおもいますが，この部品が座標変換してくれます．つまり，センサでとった値（センサの座標系からみた値）をロボット基準（ロボットの座標系から見た値）に引き戻してくれます．今回はオドメトリ情報しか使わないので，使いません．

odometry source : オドメトリ生成

　超ざっくりいうと，車輪の回転数の積分をしながら，ロボットがどのくらいすすんだか，どこに行ったかを推定します．今回必須です．

base controller : ロボットの制御調整コントローラ

　move_baseから出てくる情報は，「1m/sですすめ」とか「1deg/sでまわれ」とかっていう割とハイレベルな指令ですが，当然この指令をもとにモータを駆動させないといけません．ここではArduinoで作ったモータコントローラを使います．

map server : 走行環境に関する地図情報保持部品

　amclの説明で少し触れましたが，マッチングするもととなる地図情報です．ただ，地図が大きくなってくると必要な部分だけ持ってこないと大きすぎるので，map serverは現状走っていると思われる周辺の地図を返してくれます．今回は使いません．

sensor sources : センサー

　そのまんま，センサーです．今回は使いません．

ということで難しい要素はすべて排除して，「なにもない平面世界でロボットを意図した通りに動かす．ただし，オドメトリの誤差は無視する」ということを今週の目標とします．
簡略化したシステム図は下記のようになります．すっきりしましたね（笑）

move_base周辺のシステム構成図（今週実験で使うシステム構成図）

move_baseへの入力／出力情報

"odom" nav_msgs/Odometry : 文字通りオドメトリ情報です．2D平面上の座標(x, y, θ)です．この情報自体は，自作エンコーダとヨーレートセンサーの値を積分して求めます．

"move_base_simple/goal" geometry_msgs/PoseStamped : 最終目的座標(xgoal, ygoal, θgoal)です．

"cmd_vel" geometry_msgs/Twist : 駆動部に対する制御指令です．Twistというのは，つまるところ(vx, vy, vθ)です．

　ざっくり入出力の説明をすると，move_baseに対してはユーザリクエストとして言ってほしい場所の座標を指定します．そうすると，あとは move_base がうまいことやってくれます（笑）．というのはざっくりすぎるのでちょっとフローをまとめて見ました．下記の図に記されているように，ユーザが目的地点の(x, y, θ)を指定すると，グローバル座標系でのパスが生成されます．ここからは，赤矢印のループに従ってある意味でフィードバックループが回り，目的地まで到達することになります．

move_baseによってバインドされている構成要素とその役割

global_planner : 与えられた地図上の一点に対して，そこへ移動するための経路を生成します．

　ここで生成される経路はglobal_costmapに基づいて生成されるまっぷなので，動的な障害物（人・自動車とか）は気にしません．

local_planner : global_plannnerが静的な地図に基づいてゴールまでの経路を求めるのに対して，local_plannerはもっと小さいウィンドウでの軌道を生成します．

　また，local_plannerはlocal_costmapに基づいて生成されますが，この地図上にはセンサーで取得した動的な障害物も含まれます．global_plannerで軌道を生成しているのに，なぜもう一度軌道を作成するのか？という点ですが，global_plannerで生成した軌道はロボットの制御精度や動的な障害物を考慮できていません．なので，ロボットが完璧に制御できて，なおかつ動的な障害物が全くなければglobal_plannerの生成した軌道のみで事足りるのかもしれませんが，実際にはそんなことはないわけで．local_plannerは対象領域をもっと絞って，動的な障害物等も考慮しながらその領域内て軌道を生成します．ただ，local_plannerで生成した軌道はある程度global_plannerに沿ったもの出ないと意味がないので，そこはglobal_plannerの生成軌道とlocal_plannerの生成軌道の差分に重みをつけて一つの要素として考慮しています．

global_costmap : 静的地図．動的な障害物は考慮されてません．

local_costmap : 動的地図．ロボット周辺のその時その時の地図．センサー取得値もマッピングされているので，動的な障害物も考慮されてます．

recovery_behaviors : リカバリ時の挙動を決定します．

各要素の詳細説明は後述します．

今月のやること．ROS Navigation Stack

　おはようございます．昨日宣言した通り，これから毎月／毎週の目標と結果を書いていきます．

　昨年のつくばチャレンジでは，画像処理の実装に時間がかかりすぎて制御周りが間に合わなかったので，今年はまずここから始めたいと思います．ロボットの足回りはとりあえずそれとなく動くものができているので，今月はロボットの足回りを ROS の Navigation Stack を用いて動かすようにします．各週の細かいTODOはやりながらおいおい更新します．

1. Week of 2017/2/6
Navigation StackのWiki読み．システム構成の理解．ロボット側のインターフェースの実装．
2. Week of 2017/2/13
　Navigation Stackを用いたロボットの前進・右折・左折
3. Week of 2017/2/20
　Odometryフレーム内でのロボットの制御
4. Week of 2017/2/27
　？？

　というとこで，乞うご期待！

Road To つくばチャレンジ2017 ~ 完走へ向けて！~

　ということで，「２０１６年を振り返って」という若干後ろ向きなエントリを書いた数分後に，「完走へ向けて」なんていう無謀なエントリを書いてしまいます．．．．

　２０１７年が明けてもう一か月が経過しようとしているのですが，年始に会社の研修発表会があった関係でしばらくロボットはお預けになってました．先週末からようやく時間が取れ始めたので，またロボットいじりを始めたのですが，早速一つ壁にぶち当たりました．．．．

「モチベーションが上がってこない．．．」

　我ながら困っているのですが，本番まで９ヶ月ある現在，ちょっと時間がありすぎでやる気がいまいちわいてきません．．．が，昨年いやというほど思い知ったのですが，この時期からコツコツ積み上げないと，とてもじゃないが１１月の本番に間に合わないのもこれまた事実で．．．やる気がいまいちわいてこない理由を自分なりに考えてみたのですが，

　いまさらですが，「自作で自律走行するロボットを作成する．」というハードルが思った以上に高く，何から手を付けていいかわからない状態になっていることが原因なんだろうなと思います．知り合いには，「ちゃんと計画立てないからだよ．」と言われたのですが，全体像がすっきり見えてないものに対して，計画立てるのって結構しんどいんですよね．．．アルゴリズム実装するにしても，予想してたほど精度が出なくて，ボツになったりすると，速攻で計画くるってしまうし．．．

　ということで，今年の進めかたを考えました．

• 月ごとに目標・プランニングをしてブログで公表する．
• 週ごとに達成内容をブログで残す．（備忘録・整理目的）

　自分の知識不足もあり，年単位のプランニングがやっぱり難しいので，月ごとに取り組む内容を決めて，ブログで公表します．で，週ごとの成果をブログで公表します．

• まず，できる限りありものを使って，走るところまで持っていく．

　フレームワークとかライブラリとかの使い方を勉強するのがめんどくさくて，昨年はもっとシンプルなものを自分で作れないかと思っていたのですが，まずはありものをできる限りパクる・再利用するという方針で作り上げていきたいと思います．

　ということで，３日坊主になる可能性もありそうですが，，温かく見守ってください．次のエントリで書きますが，今月はＲＯＳのＮａｖｉｇａｔｉｏｎスタックの勉強・実装をします．

2016年を振り返って．．．

　現在，朝７時．．．今年は早寝・早起きを習慣にしようととりあえず毎朝５時に起きてます．昨日から（笑）

　２０１６年は転職から始まり，つくばチャレンジ２０１６参加，ＳＩ２０１６でのつくばチャレンジ取り組み発表，会社の研修・発表会等，いろいろありました．ばたばたといろいろ動きましたが，反省点も多々あり，，，というか，振り返ると反省点ばかりだったような気がします．

　結局のところ，２０１６年もやっぱりつくばチャレンジが一番のメインイベントだったのですが，なかなか思うようにいかず．．．参加２年目ということで，「２０１６年は完走するぞ！」と，ガチで（年初は）意気込んでいたのですが，結果的には全然間に合わず２０１５年の内容とほぼ同じになってしまいました．
　
　ハード作成の時は，トライアンドエラーでとにかく手を動かして，無理くりいろんなものをくっつけていったらなんとなく動くところまでは行ったのですが，ソフト作成に関しては思ったようにいきませんでした．まず，ROSとかOpenCVなんかのライブラリ・フレームワークのキャッチアップをして，それからアルゴリズムの勉強をして，，，，という感じで，「作る」前に「理解」することが前提で，全然間に合いませんでした．

　２０１７年は取り組み方をもう少し工夫しないといけないなと思ってます．この辺は，次の投稿で考えをまとめられれば！今年は，一週間に最低一つは記事を書きます！（書くよう努力します（笑））下のスライドにも書いてありますが，「素人がGoogle先生だけを頼りに，０からロボットを作ったらどうなるか？」をテーマに今年も頑張りますので，一つよろしくお願いします．<(_ _)>

　
下記，ＳＩ２０１６での発表資料です．今回は何か技術的に中身のある発表にしたかったのですが，結局取り組み紹介で終わってしまいましたが．．．

Si2016 発表スライド from Robotics Engineer

www.slideshare.net

写真は，ＳＩ２０１６で訪れた札幌の写真．

以上，旧正月の挨拶でした！