概要
ROSのチュートリアル等では、RvizのGUIで指定した場所に自律走行させるところまでで終わっていることが多く、広い環境下でロボットをウェイポイントに沿って自律走行させる方法がまとまっていない。
そこで、本稿では、RvizのGUI上で指定したウェイポイントをリストとして出力し、そのウェイポイントを順にたどっていく、マルチウェイポイント走行のプログラムサンプルを紹介する。
サンプル化するために、必要最小限のコードで動くことを考慮した。美しい実装ではないが、わかりやすいはず。
本稿の前提として、以下ブログのように、Rviz上で2D Nav Goalを指定することでロボットが自律走行できる状態にあるものとする。
ros-robot.blogspot.jp
2D Nav GoalをSubscribeし、リストで出力する。
まずは、想定する環境を2D Nav Goalを逐次人手で与えることにより、自律走行させる。(ここでは必ずしも現地で走行させる必要はなく、rosbagでログを再生しながらでも良い。)
その際、以下のスクリプトを起動させておき、2D Nav Goalで与えたゴールをリストとしてコマンドラインに出力させる。
サンプルコードは以下の通り。
これを実行すると、ターミナル上に以下のようなリストが表示される。
これは、2D Nav Goalで与えた座標(map座標系)、クォータニオンが1ポイント一行で表示されている。
[出力]
[(244.145950317,-122.487724304,0.0),(0.0,0.0,-0.172345248339,0.985036606109)],
[(266.074157715,-131.417816162,0.0),(0.0,0.0,-0.293727794279,0.955889105947)],
[(289.220703125,-136.964599609,0.0),(0.0,0.0,0.0289358052563,0.99958127192)],
[(316.21270752,-131.119720459,0.0),(0.0,0.0,-0.0128675861889,0.999917209186)],
[(344.102478027,-130.938964844,0.0),(0.0,0.0,0.681444056207,0.731870205883)][サンプルコード]
#!/usr/bin/env python
import rospy
from move_base_msgs.msg import MoveBaseActionGoal
def callback(data):
pos = data.goal.target_pose.pose
print "[({0},{1},0.0),(0.0,0.0,{2},{3})],".format(pos.position.x,pos.position.y,pos.orientation.z,pos.orientation.w)
def listener():
rospy.init_node('goal_sub', anonymous=True)
rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, callback)
rospy.spin()
if __name__ == '__main__':
listener()
2D Nav Goalをスクリプトから逐次与える
次に、2D Nav Goalをスクリプトから逐次与え、ウェイポイントに沿って逐次走行させる。
actionlibでは、ウェイポイントに到達したことをメッセージとして取得することもできるが、actionlibを使ってウェイポイント到達を認識させると、ウェイポイントに到達するたびにロボットが停止してしまう。
ロボットがいちいち停止するのも美しくないため、tfを使ってロボットの位置を逐次把握し、ウェイポイントに近づいたら、ゴールをオーバーライドさせる実装とした。
#!/usr/bin/env python
import rospy
import actionlib
import tf
from nav_msgs.msg import Odometry
import math
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
# ここに先のプログラムで出力されたリストを書き込む
waypoints = [
[(229.966918945,115.991737366,0.0),(0.0,0.0,0.998713073099,-0.0507168376415)],
[(344.102478027,-130.938964844,0.0),(0.0,0.0,0.681444056207,0.731870205883)]
]
def goal_pose(pose):
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = pose[0][0]
goal_pose.target_pose.pose.position.y = pose[0][1]
goal_pose.target_pose.pose.position.z = pose[0][2]
goal_pose.target_pose.pose.orientation.x = pose[1][0]
goal_pose.target_pose.pose.orientation.y = pose[1][1]
goal_pose.target_pose.pose.orientation.z = pose[1][2]
goal_pose.target_pose.pose.orientation.w = pose[1][3]
return goal_pose
if __name__ == '__main__':
rospy.init_node('patrol')
listener = tf.TransformListener()
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
listener.waitForTransform("map", "base_link", rospy.Time(), rospy.Duration(4.0))
while True:
for pose in waypoints:
goal = goal_pose(pose)
client.send_goal(goal)
while True:
now = rospy.Time.now()
listener.waitForTransform("map", "base_link", now, rospy.Duration(4.0))
# map座標系の現在位置をｔｆから取得する
position, quaternion = listener.lookupTransform("map", "base_link", now)
# ウェイポイントのゴールの周囲１ｍ以内にロボットが来たら、次のウェイポイントを発行する
if(math.sqrt((position[0]-goal.target_pose.pose.position.x)**2 + (position[1]-goal.target_pose.pose.position.y)**2 ) <= 1):
print "next!!"
break
else:
rospy.sleep(0.5)
