【自律型ロボット製作記】#4 ROS ゴール到着を検知して最終ゴールまで走行させる
こんにちは、InomaCreateです。
前回、HectorSLAMとmove_baseを使い、目的地へ自律走行させることができました。
ただし、カーブ走行がうまくいかず、遠くの目的地へ行くには、こまめに目的地を指定しないといけないという課題がありました。
そこで、今回は、以下のように中間地点を設け、中間地点に到着したら、次の中間地点を指定、それを繰り返し、最終的に遠くのゴールを目指して走行させるようにしてみました。
Goalについたことを検知
中間地点をゴール指定し、ゴールについたら、次の中間地点を指定するためには、まずゴールについたことを検知しなければなりません。ナビゲーションを制御しているmove_baseは、常に/move_base/statusというトピックを配信しており、このトピックをサブスクライブしておけば、ゴールに到着したことも判定できそうです。
move_base/statusトピックは、具体的には以下のようなステータスが配信されます。
〇ステータスの説明
1 2 3 4 5 6 7 8 9 10 11 |
int status_id = 0; //uint8 PENDING = 0 //uint8 ACTIVE = 1 //uint8 PREEMPTED = 2 //uint8 SUCCEEDED = 3 //uint8 ABORTED = 4 //uint8 REJECTED = 5 //uint8 PREEMPTING = 6 //uint8 RECALLING = 7 //uint8 RECALLED = 8 //uint8 LOST = 9 |
ゴールに着いた時、上記のSUCCEEDED=3が通知されます。
Goalトピックを配信する
今までは、rviz上でGoalを指定していましたが、中間地点に到着するごとに、手動でゴールを指定していくのは面倒です。よって、スタート位置を原点とし、中間地点の座標を事前に測定しておき、中間地点到着する毎にプログラムから次のゴール座標を指定するようにします。
※ゴールは、”move_base_simple/goal”トピックを配信します。
中間地点を経由して最終ゴールまで走行させるサンプルプログラム
以下のようなプログラムを作成し、中間地点を経由して最終ゴールまで走行させるようにしてみました。中間地点の座標、姿勢は自宅の環境下で設定してますので、適当に変更してください。
※サンプルプログラム
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 |
#include <ros/ros.h> #include <move_base_msgs/MoveBaseAction.h> #include <actionlib_msgs/GoalStatusArray.h> #include <actionlib/client/simple_action_client.h> #include <geometry_msgs/PoseStamped.h> // add goal navi // Global int target_pos = 0; int robot_state = 0; // 0: idle 1:moving 2:goal class Goal { public: Goal(double px, double py, double pz, double ow); ~Goal(); private: ros::Publisher pub; ros::NodeHandle nh; }; Goal::Goal(double px, double py, double pz, double ow){ pub = nh.advertise<geometry_msgs::PoseStamped>("move_base_simple/goal", 1); ros::Rate one_sec(1); one_sec.sleep(); ros::Time time = ros::Time::now(); geometry_msgs::PoseStamped goal_point; goal_point.pose.position.x = px; goal_point.pose.position.y = py; // goal_point.pose.position.z = pz; goal_point.pose.orientation.z = pz; goal_point.pose.orientation.w = ow; goal_point.header.stamp = time; goal_point.header.frame_id = "map"; pub.publish(goal_point); } Goal::~Goal(){ } void navStatusCallBack(const actionlib_msgs::GoalStatusArray::ConstPtr &status) { int status_id = 0; //uint8 PENDING = 0 //uint8 ACTIVE = 1 //uint8 PREEMPTED = 2 //uint8 SUCCEEDED = 3 //uint8 ABORTED = 4 //uint8 REJECTED = 5 //uint8 PREEMPTING = 6 //uint8 RECALLING = 7 //uint8 RECALLED = 8 //uint8 LOST = 9 if (!status->status_list.empty()){ actionlib_msgs::GoalStatus goalStatus = status->status_list[0]; status_id = goalStatus.status; } ROS_INFO("status=%d\n",status_id); if(status_id==1){ //移動中 ROS_INFO("Target = %d MOVING!!\n",target_pos); robot_state = 1; } if((status_id==3)||(status_id==0)){ //ゴールに到達・もしくはゴールに到達して待機中。 if(status_id==3){ if(robot_state!=1){ return; } robot_state = 2; ROS_INFO("Target = %d Goal reached!!!!!\n",target_pos); if(target_pos==1) { // target1到着時 target_pos++; Goal goal_ob(1.5, -0.5, -0.3, 1.0); // target2へ } else if(target_pos==2) { // target2到着時 target_pos++; Goal goal_ob(1.9, -1.2, -0.7, 0.7); // add goal navi } else if(target_pos==3) { target_pos++; Goal goal_ob(2.0, -1.79, 0, 1.0); // add goal navi } /* else if(target_pos==4) { target_pos++; Goal goal_ob(3.0, -1.8, 0, 1.0); // add goal navi } else if(target_pos==5) { target_pos++; Goal goal_ob(4.0, -1.69, 0, 1.0); // add goal navi } */ else if(target_pos==4){ target_pos++; Goal goal_ob(5.0, -1.69, 0, 1.0); // add goal navi } else if(target_pos==5){ target_pos++; Goal goal_ob(7.5, -1.69, 1.5, 1.0); // add goal navi } else if(target_pos==6){ ROS_INFO("Final Goal reached!!!!!\n"); } } else { ROS_INFO("Idle!\n"); robot_state = 0; } } } int main(int argc, char **argv) { ros::init(argc, argv, "move_base_goal_state"); ros::NodeHandle nh; ros::Subscriber switch_sub; ros::Subscriber move_base_status_sub; move_base_status_sub = nh.subscribe<actionlib_msgs::GoalStatusArray>("/move_base/status", 10, &navStatusCallBack); target_pos = 1; Goal goal_ob(1.13, -0.04, 0, 1.0); // add goal navi // goal_ob(1.13, -0.04, 0, 1.0); // add goal navi ros::spin(); return 0; } |
rosのビルド環境へ移動して、rosのパッケージを作ります。
上記のプログラムを作成したパッケージのsrcフォルダにアップロードします。
作成したパッケージの中に入っているCmakeLists.txtを編集します。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
cmake_minimum_required(VERSION 2.8.3) project(inobo_navi) find_package(catkin REQUIRED COMPONENTS actionlib move_base_msgs roscpp std_msgs geometry_msgs ) catkin_package( INCLUDE_DIRS include LIBRARIES inobo_navi CATKIN_DEPENDS actionlib move_base_msgs roscpp std_msgs geometry_msgs DEPENDS system_lib ) add_executable(${PROJECT_NAME}_node src/inobo_navi_node.cpp) target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ) |
catkinビルドを実施します。
ビルドが成功したら、ナビゲーションさせるためmove_baseを起動します。(以下の記事「実行方法」参照)
【自律型ロボット製作記】#3 Hector SLAMとmove_baseで自律走行させる
作成したパッケージ「inobo_navi」を起動します。
$ rosrun inobo_navi inobo_navi_node
これで、中間地点を経由しながら、最終ゴールを目指して自律走行します。
動画
動画では、中間地点を経由してゴールを目指すプログラムをAndroidアプリ上に実装し、動かしてみました。
最後に
まだまだ、SLAMの精度は改善が必要ですが、少しずつ自律走行が形になってきました。
SLAMの技術は本当に難しく、中身を理解するのにかなり苦戦しています。以下のSLAMの本を読んだのですが、確率論や統計学などの知識が必要でなかなか難しく完全に理解できなかったです。
ということで、別の本も読んで勉強中です。これらで得たノウハウもブログでアウトプットしていこうと考えています。
そのうち記事にしたいと思います。
次は、カメラや他のセンサー、上半身の制御など色々アイデアを出して、試作を繰り返していこうと思います。
それでは!!
スポンサーリンク
コメント
動画を拝見し、面白いと思いました、初めにSlamでの地図が出ますが全体の地図を保存しておいて全体地図を表示しながら同様のSlamでの画面は出来ますでしょうか
コメントありがとうございます
今回採用しているHector SLAMがリアルタイムに地図を作成しながら、自律走行するシステムとなっています。
事前に地図をロードしてマップを作成していく方法もあるようですが、すみません。。勉強不足でそのあたりの検証ができていません。
そのあたりやり方が分かったらまた記事にしたいと思います。