2017-03-06

ROS Navigation Stack ~ Arduino Controller ~

2017/2

Arduino, ROSの制御周りの実装・リファクタが終わりました．
制御にはPIDを用いましたが，等速直線運動，定常円旋回に関してはそこそこうまく動くようになりました．
ちょっとギクシャクしてしまってますが，これは制御器の設定が悪いせいではなく，ハードの作りこみが甘いことが原因のようで，制御パラメータをいくら変えても改善しませんでした．．．

youtu.be

ということで，想定より時間がかかってしまいましたが，ようやく cmd_vel を受けるインターフェースが整ったので，今週から Navigation Stack を使ってロボットを動かしていきます！

2017-03-06

今月のやること．ROS Navigation Stack

2017/3

おはようございます．なんと，はや３月．．．最近時間がたつのが早いです．
さて，今月のロボット目標です．２月中にROS Navigation Stackを動かしてみる予定でしたが，案の情遅れてしまいました．OdometryやArduino周りのコードをリファクタしてたら思いのほか時間がかかってしまったというのが理由ですが，まあ，理解は深まってコードも整理されたので良しとしましょう！

ということで，３月も引き続き ROS Navigation Stackを動かすことを目標にやっていきます．

1. Week of 2017/3/6
Navigation Stackを用いたロボットの前進・右折・左折
2. Week of 2017/3/13
　Odometryフレーム内でのロボットの制御
3. Week of 2017/3/20
　？？　
4. Week of 2017/3/27
　？？

2017-02-28

ROS Navigation Stack ~ Arduino Controller ~

おはようございます．前回の投稿から一週間以上経過して，早くも当初の目標をデコミットしてしまいましたが，，，
なにもしていなかったわけではなく，久々にArduinoのコードを整理していたらわけがわからなくなってしまい，，，，整理ついでにリファクタリングしてました．
あと，つくるものの規模が比較的大きくなってくると，どうしても前に作業したことを覚えておくのが難しくなってしまい，忘れてしまうので，備忘もかねて簡単な設計書を残すことにしました．
（なんか仕事でもないのに，仕事みたいなことしてるな．．．）

Tsukuba exploration rover 制御周り仕様書 from Robotics Engineer

www.slideshare.net

制御のパラメータ調整をしないといけないですが，それはまた後日のエントリで．．．

2017-02-11

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

Week of 2017/2/6

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

2017-02-11

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

Week of 2017/2/6

先ほどの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!

2017-02-11

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

Week of 2017/2/6

今日は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情報をパブリッシュするとこをやります．

2017-02-10

ROS Navigation Stack について１

Week of 2017/2/6

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

move_base周辺のシステム構成図

f:id:rkoichi2001:20170208075021p:plain
引用：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周辺のシステム構成図（今週実験で使うシステム構成図）

f:id:rkoichi2001:20170208081409p:plain

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, θ)を指定すると，グローバル座標系でのパスが生成されます．ここからは，赤矢印のループに従ってある意味でフィードバックループが回り，目的地まで到達することになります．

f:id:rkoichi2001:20170210074532p:plain

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 : リカバリ時の挙動を決定します．

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