LoginSignup
5
4

More than 1 year has passed since last update.

@sfc_nakanishi_lab(慶應義塾大学・環境情報学部・中西研究室)

ROS2で道案内ロボットをつくる〜ナビゲーション編（後編）〜

Last updated at Posted at 2022-04-14

はじめに

　本記事では、移動型ロボットが事前に登録されたパスをたどりつつも後方から追随するユーザーを置いてけぼりにしないような「道案内ロボット」の実装について紹介しています。
　後半では、得られたデータをサブスクライブし、ゴールを更新したりキャンセルしたりする方法について紹介します。
　なお前半の記事はこちら

1.指標物体の情報をサブスクライブする

　前編で紹介した独自のメッセージを立てるので、ワークスペース直下にimgProc_msgsディレクトリを用意し、ビルドしたあとで反映されるようにしましょう。
　今回は半径の大きさのみをもって、認識されているかいないかの判定を行うのでコールバック関数ではピンポン玉の半径を取得します。

callback.func
//抜粋
void topic_callback(const imgproc_msgs::msg::Imgproc::SharedPtr msg) const
{
  ballRadius = msg->radius;
  RCLCPP_INFO(this->get_logger(), "radius: '%f'", ballRadius);
}

　C++で書く際にコールバック関数の属性がconstになっているため、外部で宣言した変数への代入ができないです。これを解決するのに、メンバ変数のballRadiusの属性をmutableにしています。

2.決められたパスをたどる

　ノードの中でActionクライアントを立てて、逐一ゴールをPoseStamped型で送信しています。今回は便宜的に座標だけを引数に指定して、回転は定数にしています。設定してあるパスは以下のような形でVectorで定義しています。

//座標地点用の構造体
struct Vec2
{
    float x;
    float y;

    Vec2()
        : x(0.0)
        , y(0.0) {}

    Vec2(float _x, float _y)
        : x(_x)
        , y(_y) {}
};

//ゴールとなるポイントを設定したVector配列
std::vector<Vec2> wayPoints;
wayPoints.push_back(Vec2(2.0, 2.0));
wayPoints.push_back(Vec2(2.0, 0.0));
wayPoints.push_back(Vec2(0.0, 0.0));

　地図によって適宜目標地点を設定してください。

3.パスをキャンセルする

　実行中のゴールをキャンセルするには以下のメソッドから実行可能です

client_ptr_->async_cancel_all_goals();

4.コード

　なお、Navigation2に対してゴールを送信するのにこちらの記事にあるコードを参考にしました。基本的には、ボールが検出されている（＝半径が０以上）場合にはゴールを送ることができ、されていない場合にはゴールをキャンセルします。また、ボールが検出されていて、ゴールに一定距離近づいたら次のゴールを送信します。

navigation_manager.cpp
#include <vector>  
#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "rclcpp/time.hpp"

#include "imgproc_msgs/msg/imgproc.hpp"

using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_2;
using std::placeholders::_3;

struct Vec2
{
    float x;
    float y;

    Vec2()
        : x(0.0)
        , y(0.0) {}

    Vec2(float _x, float _y)
        : x(_x)
        , y(_y) {}
};

class NavManagerClass : public rclcpp::Node
{
public:
  using NavigateToPose = nav2_msgs::action::NavigateToPose;
  using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
  rclcpp_action::Client<NavigateToPose>::SharedPtr client_ptr_;

  int counter = 0;
  float dist = 1.0;
  std::vector<Vec2> wayPoints;
  mutable std::vector<float> radiusBuffer;
  mutable int max_buffer_size = 10;
  mutable float radius_avg = 0.0;

  bool isAborted = false;
  bool isCanceled = false;
  bool moveFlag = false;
  mutable float ballRadius = 0.0;
  explicit NavManagerClass(): Node("navigation_manager")
  {
    //waypoints
    /*
    wayPoints.push_back(Vec2(2.0, 2.0));
    wayPoints.push_back(Vec2(2.0, 0.0));
    wayPoints.push_back(Vec2(4.0, 2.5));
    wayPoints.push_back(Vec2(-2.0, 2.0));
    wayPoints.push_back(Vec2(0.0, 0.0));
    */

    wayPoints.push_back(Vec2(0.0, 2.0));
    wayPoints.push_back(Vec2(2.0, 0.0));
    wayPoints.push_back(Vec2(0.0, -2.0));
    wayPoints.push_back(Vec2(-2.0, 0.0));

    //create action client
    this->client_ptr_  = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");
    //timer call back
    timer_ = this->create_wall_timer(2000ms, std::bind(&NavManagerClass::timer_callback, this));
    //subscriber
    subscription_ = this->create_subscription<imgproc_msgs::msg::Imgproc>("img_data", 10, std::bind(&NavManagerClass::topic_callback, this, _1));

  }

  void sendGoal(float x, float y) {
    int counter= 0;
    bool entryFlag = false;

    while (!entryFlag)
    {
      counter ++;
      if(this->client_ptr_->wait_for_action_server()) 
      {
        entryFlag = true;
      }
      else
      {
        if(counter > 100000)
        {
          RCLCPP_INFO(get_logger(), "No connection to server");
          exit(0);
        } 
      }
    }
    
    auto goal_msg = NavigateToPose::Goal();
    goal_msg.pose.header.stamp = this->now();
    goal_msg.pose.header.frame_id = "map";

    goal_msg.pose.pose.position.x = x;
    goal_msg.pose.pose.position.y = y;
    goal_msg.pose.pose.orientation.x = 0.0;
    goal_msg.pose.pose.orientation.y = 0.0;
    goal_msg.pose.pose.orientation.w = 1.0;
    goal_msg.pose.pose.orientation.z = 0.0;

    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    send_goal_options.feedback_callback = std::bind(&NavManagerClass::feedbackCallback, this, _1, _2);
    send_goal_options.result_callback = std::bind(&NavManagerClass::resultCallback, this, _1);
    
    client_ptr_->async_send_goal(goal_msg, send_goal_options);
    
  }
  void feedbackCallback(GoalHandleNavigateToPose::SharedPtr,const std::shared_ptr<const NavigateToPose::Feedback> feedback)
  {
    RCLCPP_INFO(get_logger(), "Distance remaininf = %f", feedback->distance_remaining);

    dist = feedback->distance_remaining;
  
  }
  void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        RCLCPP_ERROR(get_logger(), "Success!!!");
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(get_logger(), "Goal was aborted");
        isAborted = true;
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(get_logger(), "Goal was canceled");
        isCanceled = true;
        return;
      default:
        RCLCPP_ERROR(get_logger(), "Unknown result code");
        return;
    }
  }

  private:
    void timer_callback()
    {
      
      RCLCPP_INFO(this->get_logger(), "TIMERCALLBACK");

      if(ballRadius > 0)
      {
        moveFlag = true;
      }
      else
      {
        moveFlag = false;
      }

      if(moveFlag)
      {
        if(dist < 0.35)
        {
            counter ++;
            sendGoal(wayPoints[counter%wayPoints.size()].x, wayPoints[counter%wayPoints.size()].y);  
        }

        if(isAborted || isCanceled)
        {
          sendGoal(wayPoints[counter%wayPoints.size()].x, wayPoints[counter%wayPoints.size()].y);  
          isAborted = false;
          isCanceled = false;
        }
      }
      else
      {
         client_ptr_->async_cancel_all_goals();
      }      
    }

    void topic_callback(const imgproc_msgs::msg::Imgproc::SharedPtr msg) const
    {
      ballRadius = msg->radius;
      RCLCPP_INFO(this->get_logger(), "radius: '%f'", ballRadius);

      
    }
    
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<imgproc_msgs::msg::Imgproc>::SharedPtr subscription_;
    
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<NavManagerClass>();
  node->sendGoal(-0.70, 0.0);
  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

なお、画像データをサブスクライブしてナビゲーションのアクションサーバへ送るノードが含まれるパッケージは以下のリポジトリに投稿しています。よろしければご参照ください。

5.Gazeboで試す

ターミナルの画面を5つ立てて実行します。
　まずは、Gazebo

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

　次に、Nav2のブリングアップ

ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True map:=/opt/ros/foxy/share/nav2_bringup/maps/turtlebot3_world.yaml default_bt_xml_filename:=$HOME/btdata/sample.xml param_file:=$HOME/my_param/test_param01.yaml

　そして、Rviz

ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/rviz/nav2_default_view.rviz

　ここまで出来たら初期位置をRviz上で設定しておきましょう。

　画像認識ノードを立てます。

cd ~/(your_ws)
. install/setup.bash
ros2 run image_processor colball_detection

　ナビゲーション管理のノードを立てます。

cd ~/(your_ws)
. install/setup.bash
ros2 run guide_operation nav_manager

　この挙動をRviz上で試したものです。カメラはPCの備え付けのウェブカメラを使用してボールを見せたり隠したりしています。

6.実機によるデモ

　実機のブリングアップと、画像認識ノードをrunさせた上で、Gazebo以外の先程のコマンドを叩いています。なお、実機に対するNavigation2のブリングアップ等については、以前投稿したこちらの記事をご参考下さい。


犬の散歩のようです（笑）

所感・展望

　今回は指標物体としてピンポン玉を使用しましたが、Yoloなどを用いて精度のより高い人物検知やデプスカメラを用いた検知をしてみても良いかなーと少し思っています。

参考資料

5
4
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
5
4