目的
前回の講義では，ROSで用意されているパッケージを利用して地図の作成とナビゲーションを行いました．ナビゲーションの際には，データを可視化するツールであるrvizから目標位置を設定することで，経路計画及び経路に沿った走行が行われることを確認しました．完全な自律移動を行うために，今回は目標位置をプログラムから与えましょう．
準備
前回の講義にてナビゲーションに関連するノードを起動するためにnavigation.launchを作成しました．このlaunchファイルでノードを起動しましょう．ロボットシミュレータstageとデータ可視化ツールrvizが起動したら，ロボットの位置をrvizから修正してください．事前に作成した地図の障害物と2次元レーザースキャンの点群の位置がおおよそ一致すれば準備完了です．
プログラムからナビゲーションを実行する
1.トピックの確認
まずはトピックを確認しましょう．
$ rostopic list /amcl/parameter_descriptions /amcl/parameter_updates /amcl_pose /base_pose_ground_truth /base_scan /clicked_point /clock /cmd_vel /initialpose /map /map_metadata /map_updates /move_base/cancel /move_base/feedback /move_base/goal /move_base/result /move_base/status /move_base_node/NavfnROS/plan /move_base_node/TrajectoryPlannerROS/cost_cloud /move_base_node/TrajectoryPlannerROS/global_plan /move_base_node/TrajectoryPlannerROS/local_plan /move_base_node/TrajectoryPlannerROS/parameter_descriptions /move_base_node/TrajectoryPlannerROS/parameter_updates /move_base_node/current_goal /move_base_node/global_costmap/costmap /move_base_node/global_costmap/costmap_updates /move_base_node/global_costmap/footprint /move_base_node/global_costmap/inflation_layer/parameter_descriptions /move_base_node/global_costmap/inflation_layer/parameter_updates /move_base_node/global_costmap/obstacle_layer/clearing_endpoints /move_base_node/global_costmap/obstacle_layer/parameter_descriptions /move_base_node/global_costmap/obstacle_layer/parameter_updates /move_base_node/global_costmap/parameter_descriptions /move_base_node/global_costmap/parameter_updates /move_base_node/global_costmap/static_layer/parameter_descriptions /move_base_node/global_costmap/static_layer/parameter_updates /move_base_node/local_costmap/costmap /move_base_node/local_costmap/costmap_updates /move_base_node/local_costmap/footprint /move_base_node/local_costmap/inflation_layer/parameter_descriptions /move_base_node/local_costmap/inflation_layer/parameter_updates /move_base_node/local_costmap/obstacle_layer/clearing_endpoints /move_base_node/local_costmap/obstacle_layer/parameter_descriptions /move_base_node/local_costmap/obstacle_layer/parameter_updates /move_base_node/local_costmap/obstacle_layer/voxel_grid /move_base_node/local_costmap/parameter_descriptions /move_base_node/local_costmap/parameter_updates /move_base_node/parameter_descriptions /move_base_node/parameter_updates /move_base_simple/goal /odom /particlecloud /rosout /rosout_agg /tf /tf_static
配信されているトピックが少ない場合には，rostopic infoやrostopic echoでトピックの詳細を調べても手間にはなりませんが，非常に多くのトピックが配信されているため，このリストから目標位置を扱っているトピックを探すことは困難です．そこで，可視化ツールであるrvizから目標位置を設定していることに着目します．rvizが配信しているトピックを調べましょう．
$ rosnode info /rviz -------------------------------------------------------------------------------- Node [/rviz] Publications: * /clicked_point [geometry_msgs/PointStamped] * /initialpose [geometry_msgs/PoseWithCovarianceStamped] * /move_base_simple/goal [geometry_msgs/PoseStamped] * /rosout [rosgraph_msgs/Log] Subscriptions: * /base_scan [sensor_msgs/LaserScan] * /clock [rosgraph_msgs/Clock] * /map [nav_msgs/OccupancyGrid] * /map_updates [unknown type] * /move_base_node/global_costmap/footprint [geometry_msgs/PolygonStamped] * /tf [tf2_msgs/TFMessage] * /tf_static [unknown type] Services: * /rviz/get_loggers * /rviz/reload_shaders * /rviz/set_logger_level ------------------------------以下省略------------------------------
上記からrvizが配信しているトピックは4つあることがわかります．このうち，/rosoutは入出力やログを扱うので除外できます．残り3つのトピックから目標位置を扱うトピックを探しますが，今回はトピックの名前から/move_base_simple/goalであることが想像できます．そこで，このトピックを購読してみましょう．ターミナルで/move_base_simple/goalを購読し，rvizの「2D Nav Goal」ボタンで目標位置を設定すると，下記のような出力が得られます．
$ rostopic echo /move_base_simple/goal
WARNING: no messages received and simulated time is active.
Is /clock being published?
header:
seq: 0
stamp:
secs: 2113
nsecs: 600000000
frame_id: "map"
pose:
position:
x: 0.248686745763
y: 1.27372550964
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.00707567377133
w: 0.999974967107
---
上記コマンドを実行しても何も購読できませんが，rvizで目標位置を設定すると1度だけ上記のようなトピックを購読できます．このことから/move_base_simple/goalが目標位置を表すトピックであることが確認できました．次にこのトピックのメッセージの型を調べましょう．
$ rostopic type /move_base_simple/goal geometry_msgs/PoseStamped
最後にメッセージの中身を調べます．
$ rosmsg show geometry_msgs/PoseStamped
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
トピックの中身はシーケンス番号，タイムスタンプ，基準となる座標系，ロボットの位置と姿勢となります．
2.パッケージの作成
適当なワークスペースの/srcディレクトリに移動し，パッケージを作成してください．パッケージの名前は自由ですが，以下では説明を簡単にするために「set_goal」という名前のパッケージ名を作成します．上記でメッセージを調べてわかるとおり，今回はgeometry_msgsパッケージに含まれるメッセージを使用します．従って，依存パッケージは下記の2つのみです．
rospy geometry_msgs
3.プログラミング
今回は下記のプログラムを記述してください．
#! /usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped
class Goal:
def __init__(self):
rospy.init_node('set_goal', anonymous=True)
self.ps_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1)
def setGoal(self, px, py, pz, ow):
rospy.sleep(1.0)
now = rospy.Time.now()
goal_point = PoseStamped()
goal_point.pose.position.x = px
goal_point.pose.position.y = py
goal_point.pose.position.z = pz
goal_point.pose.orientation.w = ow
goal_point.header.stamp = now
goal_point.header.frame_id = 'map'
self.ps_pub.publish(goal_point)
if __name__ == '__main__':
try:
goal_ob = Goal()
goal_ob.setGoal(-20.0, 21.0, 0.0, 1.0)
rospy.spin()
except rospy.ROSInterruptException: pass
権限の変更とビルド
作成したプログラムファイルに対して実行権限を与え，ビルドしてください．
4.実行
これまで通り，ノードの実行にはrosrunを使用します．
$ rosrun set_goal set_goal
下記は実行した際の動画です．
今回はロボットの目標位置・姿勢をプログラムにて決めうちの値(マジックナンバー)を与えました．自律移動ロボットでは，事前に目標位置・姿勢を決定することができないケースもありますので，実行中に動的に変えてあげる必要があります．
課題
1. ロボットの目標位置・姿勢を変えてサンプルプログラムを実行しなさい．
2. トピック/move_base/resultをコマンドラインツールで購読し，配信しているノード，配信される条件，配信される情報を確認しなさい．
3. プログラムにてトピック/move_base/resultを購読できるようにしなさい．
4. 予め決めた2つの目標位置・姿勢を往復するようなプログラムを作成しなさい．