ロボットプログラミングⅡ：第8週 ウェイポイントナビゲーション（ActionLib）

この記事は私が担当している講義ロボットプログラミングⅡ用です。今回は第６週のプログラムを少し改良してウェイポイントナビゲーションのプログラムを作りましょう。これを応用するだけで、つくばチャレンジに出場できますよ。

ソース

 
// 本プログラムは
// http://wiki.ros.org/ja/navigation/Tutorials/SendingSimpleGoals
// を元に作成しています。
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/transform_broadcaster.h>

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

struct MyPose {
  double x;
  double y;
  double yaw;
};

int main(int argc, char** argv){
  MyPose way_point[] = {
    {-2.0, 3.0,-0.5 * M_PI},
    { 3.0, 3.0, 0.0 * M_PI},
    { 3.0,-4.5, 0.5 * M_PI},
    { 0.0,-4.5, 1.0 * M_PI},
    { 0.0, 0.0, 0.0 * M_PI},
    {999, 999, 999}};

  ros::init(argc, argv, "wp_navigation");

  // アクションクライアンを作成。1番目の引数は接続するアクションサーバー名。          
  // ２番目の引数はtrueならスレッドを自動的に回す(ros::spin()。                       
  MoveBaseClient ac("move_base", true);

  // アクションサーバーが起動するまで待つ。引数はタイムアウトする時間(秒）。          
  // この例では５秒間待つ(ブロックされる)                                             
  while(!ac.waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the move_base action server to come up");
  }

  ROS_INFO("The server comes up");
  move_base_msgs::MoveBaseGoal goal;
  // map(地図)座標系(第６週のプログラムとの大きな変更点）                             
  goal.target_pose.header.frame_id = "map";
  // 現在時刻                                                                         
  goal.target_pose.header.stamp = ros::Time::now();


  int i = 0;
  while (ros::ok()) {
    // ROSではロボットの進行方向がx座標、左方向がy座標、上方向がz座標                 
    goal.target_pose.pose.position.x =  way_point[i].x;
    goal.target_pose.pose.position.y =  way_point[i].y;

    if (goal.target_pose.pose.position.x == 999) break;

    goal.target_pose.pose.orientation
      = tf::createQuaternionMsgFromYaw(way_point[i].yaw);

    ROS_INFO("Sending goal: No.%d", i+1);
    // サーバーにgoalを送信                                                           
    ac.sendGoal(goal);

    // 結果が返ってくるまで60.0[s] 待つ。ここでブロックされる。                       
    bool succeeded = ac.waitForResult(ros::Duration(60.0));

    // 結果を見て、成功ならSucceeded、失敗ならFailedと表示                            
    actionlib::SimpleClientGoalState state = ac.getState();
   
    if(succeeded) {
      ROS_INFO("Succeeded: No.%d (%s)",i+1, state.toString().c_str());
    }
    else {
      ROS_INFO("Failed: No.%d (%s)",i+1, state.toString().c_str());
    }
    i++;
  }
  return 0;
}

• packageのディレクトリ毎、圧縮したファイル
  • wp_navigation.tgz
• 展開・ビルド方法
  • $ cp wp_navigation.tgz ~/catkin_ws/src/.
  • $ cd ~/catkin_ws/src
  • $ tar xvzf wp_navigation.tgz
  • $ cd ~/catkin_ws
  • $ catkin_make

演習

• 上で展開したwp_navigationを次の要領で実行しよう。
  • $ roslaunch turtlebot_gazebo turtlebot_world.launch
  • $ roslaunch turtlebot_gazebo amcl_demo.launch map_file:=/opt/ros/indigo/share/turtlebot_gazebo/maps/playground.yaml
  • $ roslaunch turtlebot_rviz_launchers view_navigation.launch
  • $ rosrun wp_navigation wp_navigation

プチプロジェクト

• もう少し規模の大きい建物内でのナビゲーションを考えましょう。以下のワールドファイルを使い、ロボットの初位置から右奥にあるポールまでウェイポイントナビゲーションをするプログラムをつくりましょう。
• 準備
  • empty_worldを読み込んでからでないとエラーが出ると報告があったので、以下のコマンドをまず実行する。
    • $ roslaunch  gazebo_ros  empty_world.launch
  • $ cd
  • $ mkdir  downloads
  • fmt_world.tgzをダウンロードして~/downloadsの中に保存。
  • $ cd downloads
  • $ tar xvzf fmt_world.tgz
  • $ cd fmt_world
  • $ sudo cp fmt_world.launch  /opt/ros/indigo/share/turtlebot_gazebo/launch
  • $ sudo cp fmt.world  /usr/share/gazebo-2.2/worlds
  • $ cp -r  fmt  ~/.gazebo/models
  • $ roslaunch turtlebot_gazebo  fmt_world.launch
• 下のような建物とロボットが表示されたら終わり。右奥のボールがゴール。