本記事の目的

本記事ではwaypoint navigationを実行したいと思います！基本的には以下のリポジトリの使用方法についてまとめた記事になります。

環境

本記事は以下の環境で実験しています.

項目	バージョン
Ubuntu	18.04
ROS	Melodic
RPLidar	A1 M8
Arduino	Mega2560

Install

まずは必要なパッケージをインストールします。

$ sudo apt-get install ros-melodic-jsk-recognition-msgs
$ sudo apt-get install ros-melodic-jsk-rviz-plugins

パッケージをcloneし、build及びパスを設定します。

$ git clone https://github.com/TSUKUBA-CHALLENGE/cirkit_waypoint_manager.git
$ git clone https://github.com/TSUKUBA-CHALLENGE/cirkit_waypoint_manager_msgs.git
$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash

うまく通ればbuild成功です！

waypoint generator

次にcirkit_waypoint_generatorを使用して事前に生成した地図上にwaypointを設定していきます。基本的にはrviz上でのwaypointの設定とcsvファイルへの保存の二段階の工程を踏みます。

rvizの起動

以下のコマンドでrvizを立ち上げて、waypointを設定したい地図をインポートします。

$ roslaunch cirkit_waypoint_generator waypoint_generator.launch

launchファイルの中身は以下のような構成になっています。

1.map_serverによりwaypointを設定したいmapを読み込む
2.cirkit_waypoint_generatorをnodeとして実行
3.rviz環境の立ち上げ

waypoint_generator.launch

<launch> 
  <!-- map -->
  <node pkg="map_server" type="map_server" name="map_server" args="$(find cirkit_waypoint_navigator)/maps/home/two_floor.yaml" output="screen">
    <!-- Topic Name -->
    <remap from="map" to="/map" />
    <remap from="map_metadata" to="/map_metadata" />
    <!-- Frame Name -->
    <param name="frame_id" value="map" />
  </node>

  <node name="cirkit_waypoint_generator" pkg="cirkit_waypoint_generator" type="cirkit_waypoint_generator" output="screen">
  </node>

  <!-- rviz -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find cirkit_waypoint_generator)/rviz/waypoint_generator.rviz"/>
</launch>

waypointの設定と保存

次にrviz上のpublish pointを選択して、waypointにしたい位置にカーソルを合わせてクリックしていきます。

waypointにはID番号があり、ID番号順に経路を辿っていくことになります。これを順に繰り返して、waypointを保存したい場合は以下のコマンドを実行します。

$ cd ~/catkin_ws
$ source devel/setup.bash
$ roscd cirkit_waypoint_navigator
$ cd waypoints/
$ rosrun cirkit_waypoint_generator cirkit_waypoint_saver

そうすると以下のような形式のcsvファイルが生成されます。

   x, 　　　　  y,     z,  qx, qy, qz, qw, is_searching_area, reach_threshold
1.29414,   -1.75889,  0,  0,  0,  0,  1,        0,                 3
0.638452,  -1.44282,  0,  0,  0,  0,  1,        0,                 3
-0.134379, -1.71576,  0,  0,  0,  0,  1,        0,                 3

waypoint navigator

次に設定したwaypointに順に追従するようなnavigationを実行します。

$ roslaunch cirkit_waypoint_navigator waypoint_navigator.launch

launchファイルの中身は以下のような構成になっています。

1.事前に設定したwaypointのCSVファイルを読み込む
2.読み込んだCSVファイルをcirkit_waypoint_navigator_nodeに渡す
3.rviz環境の立ち上げ

waypoint_navigator.launch

<launch> 
  <!-- way point -->
  <arg name="waypoint_filename" default="$(find cirkit_waypoint_navigator)/waypoints/home_environment/room.csv" />
  <arg name="start_waypoint" default="0"/>

  <node name="cirkit_waypoint_navigator_node" pkg="cirkit_waypoint_navigator" type="cirkit_waypoint_navigator_node" output="screen">
    <param name="waypointsfile" value="$(arg waypoint_filename)" />
    <param name="start_waypoint" value="$(arg start_waypoint)"/>
  </node>

  <node pkg="cirkit_waypoint_generator" name="cirkit_waypoint_server" type="cirkit_waypoint_server" args="--load $(arg waypoint_filename)" output="screen"/>

  <!-- rviz -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find cirkit_waypoint_navigator)/rviz/way_point_publish.rviz"/>
</launch>

ここでwaypointのgoal判定範囲設定はcirkit_waypoint_navigatorのreadWaypoint関数内で行います。関数内のdata配列の各要素は上記で設定したwaypoint一つに対する9つの要素(x, y, z, qx, qy, qz, qw, is_searching_area, reach_threshold)に相当します。**data[8]がreach_thresholdなので、generatorの方で調整するか、navigator側で任意の値で割る事で値を調整します。**各種閾値は使用する環境に大きく依存するので、その都度調整する必要があります(もちろん他のパラメータも)

move_base_msgs::MoveBaseGoal waypoint;
waypoint.target_pose.pose.position.x    = data[0];
waypoint.target_pose.pose.position.y    = data[1];
waypoint.target_pose.pose.position.z    = data[2];
waypoint.target_pose.pose.orientation.x = data[3];
waypoint.target_pose.pose.orientation.y = data[4];
waypoint.target_pose.pose.orientation.z = data[5];
waypoint.target_pose.pose.orientation.w = data[6];
waypoints_.push_back(WayPoint(waypoint, (int)data[7], data[8]/30.0));

またロボットの位置情報取得はcirkit_waypoint_navigatorのgetRobotCurrentPosition関数内で行います。この際、mapフレームとロボットフレーム間でtfを用いて位置情報を取得しますが、ロボットフレームについては設計者によって変わる所なので(/base_link, /base_footprintなど)適宜変更が必要です。

geometry_msgs::Pose getRobotCurrentPosition() {
    tf::StampedTransform transform;
    geometry_msgs::Pose pose;
    try {
      listener_.lookupTransform("/map", "/base_link", ros::Time(0), transform);
    } catch (tf::TransformException ex) {
      ROS_ERROR("%s", ex.what());
    }
    pose.position.x = transform.getOrigin().x();
    pose.position.y = transform.getOrigin().y();
   
    return pose;
}

上記の位置情報を基にロボットがwaypointに到達したかどうかを判定し、到達していれば次のwaypointをpublishしてnavigationを繰り返します。

動作確認

上記の流れに従って実際のロボットを使用してwaypoint navigationを実行してみます！今回はlaser_scan_matcherベースのnavigationを適用して動作確認を行います。laser_scan_matcherについては以下の記事を参照してください。

また実環境で使用する実験機の制作やソフトウェアについては以下の記事を参照してください。

今回実験機としては以下の画像のロボットを使用しております。

laser_scan_matcher navigation

それでは以下のコマンドを順に実行します。

$ sudo chmod 666 /dev/ttyACM0
$ sudo chmod 666 /dev/ttyUSB0
$ roslaunch laser_scan_matcher_navigation navigation.launch
$ rosrun rosserial_python serial_node.py /dev/ttyACM0
$ roslaunch cirkit_waypoint_navigator waypoint_navigator.launch

ここでroslaunch laser_scan_matcher_navigation navigation.launchを実行した後にrviz上で2D_Pose_Estimateを選択して初期位置を設定してから、navigationを実行してください！

navigation結果

精度は少々悪いですが、先ほど設定したwaypoint 3点を順に追従する事が出来ています！より広大な環境ではwaypointの個数を増やしてnavigationすることになりますが、waypointの設定方法やnavigation方法については上記と変わりません。ただし自己位置推定の破綻などが原因でnavigationに異常をきたすことがあると思うので、各種パラメータ調整や別途ソフトウェアを追加するなどが必要になる思います。

まとめ

CIR-KITのcirkit_waypoint_managerを使用してwaypoint navigationを実行しました

参考文献