【自律型ロボット製作記】#4 ROS ゴール到着を検知して最終ゴールまで走行させる

こんにちは、InomaCreateです。

前回、HectorSLAMとmove_baseを使い、目的地へ自律走行させることができました。
ただし、カーブ走行がうまくいかず、遠くの目的地へ行くには、こまめに目的地を指定しないといけないという課題がありました。

そこで、今回は、以下のように中間地点を設け、中間地点に到着したら、次の中間地点を指定、それを繰り返し、最終的に遠くのゴールを目指して走行させるようにしてみました。

Goalについたことを検知

中間地点をゴール指定し、ゴールについたら、次の中間地点を指定するためには、まずゴールについたことを検知しなければなりません。ナビゲーションを制御しているmove_baseは、常に/move_base/statusというトピックを配信しており、このトピックをサブスクライブしておけば、ゴールに到着したことも判定できそうです。
move_base/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

ゴールに着いた時、上記のSUCCEEDED=3が通知されます。

Goalトピックを配信する

今までは、rviz上でGoalを指定していましたが、中間地点に到着するごとに、手動でゴールを指定していくのは面倒です。よって、スタート位置を原点とし、中間地点の座標を事前に測定しておき、中間地点到着する毎にプログラムから次のゴール座標を指定するようにします。
※ゴールは、”move_base_simple/goal”トピックを配信します。

中間地点を経由して最終ゴールまで走行させるサンプルプログラム

以下のようなプログラムを作成し、中間地点を経由して最終ゴールまで走行させるようにしてみました。中間地点の座標、姿勢は自宅の環境下で設定してますので、適当に変更してください。
※サンプルプログラム

#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を編集します。

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」を起動します。

これで、中間地点を経由しながら、最終ゴールを目指して自律走行します。

動画

動画では、中間地点を経由してゴールを目指すプログラムをAndroidアプリ上に実装し、動かしてみました。

最後に

まだまだ、SLAMの精度は改善が必要ですが、少しずつ自律走行が形になってきました。
SLAMの技術は本当に難しく、中身を理解するのにかなり苦戦しています。以下のSLAMの本を読んだのですが、確率論や統計学などの知識が必要でなかなか難しく完全に理解できなかったです。

ということで、別の本も読んで勉強中です。これらで得たノウハウもブログでアウトプットしていこうと考えています。
そのうち記事にしたいと思います。

次は、カメラや他のセンサー、上半身の制御など色々アイデアを出して、試作を繰り返していこうと思います。
それでは！！