概要
rvizのGUIで指定したウェイポイント通りにロボットを自律走行させる（Python,ROS） - 技術メモ集にて、ウェイポイントを保存し、その順番通りにロボットを自律走行させる方法を記載した。
しかし、実際に自律走行させてみると、ウェイポイントを追加したり、ウェイポイントの位置をずらしたり・・・と、ウェイポイントの中身を弄りたくなる。
その際に、座標の羅列だけではどう変更してよいかわからないため、
・ ウェイポイントの座標と番号をrviz上に表示
・ 2D Nav Goalで指定した座標をターミナル上に表示
する機能を持つスクリプトを作成した。
座標番号と位置がひと目でわかるだけでも、かなりデバッグが楽になる。
＃本当はGUIでウェイポイントを修正できるようにしたかったが、実装に時間がかかりそうだったので、修正はCSVファイル上からて入力で数値を入れることとした。
地図情報の保存
前提として、ウェイポイントファイルを以下の通り記述し、スクリプトと同じフォルダに置くこととする。（ファイル名waypoint.csv）
フォーマットやファイル名を変更したければ、スクリプトを適宜修正のこと。
ウェイポイントファイル詳細
・ numは０から始まるウェイポイント番号
・ xyzはウェイポイントの座標
・ q1,q2,q3,q4はクォータニオンで示されるロボットの方位
num,x,y,z,q1,q2,q3,q4 0,229.966918945,115.991737366,0,0,0,0.9987130731,-0.0507168376 1,178.294799805,118.079750061,0,0,0,0.9998951068,0.0144836224 2,133.862442017,119.17339325,0,0,0,0.9999969209,0.0024815768
ウェイポイント描画スクリプトの実行方法と、Rvizへの表示画面
スクリプトは最後に記載することとして、ここでは使い方と結果を示す。
まず、Map Server、Rvizと、本スクリプトを起動する。
次に、Rvizの画面から
Add→By topic→Waypoint→Markerをクリックする。
すると、以下の通りRviz上に地図と、矢印、ウェイポイント番号が表示される。
# ウェイポイントが重なると見難いが、心の目で見るしかない・・・。
また、この状態で2D Nav Goalを指定すると、以下のようにターミナル上に座標が表示されるので、この数値を元にcsvファイルを編集することで、ウェイポイントファイルと適宜修正できる。
スクリプト
スクリプトとしてはシンプルで、Marker型で矢印と数字をPublishしているだけ。
矢印等の描画方法は以下を参考のこと。
import rospy
import csv
from visualization_msgs.msg import Marker
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)
rospy.init_node("waypoint_manager")
pub = rospy.Publisher("waypoint", Marker, queue_size = 10)
rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, callback)
rate = rospy.Rate(25)
while not rospy.is_shutdown():
with open('waypoint.csv', 'r') as f:
counter = 0
reader = csv.reader(f)
header = next(reader)
for row in reader:
# Mark arrow
marker_data = Marker()
marker_data.header.frame_id = "map"
marker_data.header.stamp = rospy.Time.now()
marker_data.ns = "basic_shapes"
marker_data.id = counter
marker_data.action = Marker.ADD
marker_data.pose.position.x = map(float,row)[1]
marker_data.pose.position.y = map(float,row)[2]
marker_data.pose.position.z = map(float,row)[3]
marker_data.pose.orientation.x=map(float,row)[4]
marker_data.pose.orientation.y=map(float,row)[5]
marker_data.pose.orientation.z=map(float,row)[6]
marker_data.pose.orientation.w=map(float,row)[7]
marker_data.color.r = 1.0
marker_data.color.g = 0.0
marker_data.color.b = 0.0
marker_data.color.a = 1.0
marker_data.scale.x = 5
marker_data.scale.y = 1
marker_data.scale.z = 1
marker_data.lifetime = rospy.Duration()
marker_data.type = 0
pub.publish(marker_data)
counter +=1
# Mark num
marker_data = Marker()
marker_data.header.frame_id = "map"
marker_data.header.stamp = rospy.Time.now()
marker_data.ns = "basic_shapes"
marker_data.id = counter
marker_data.action = Marker.ADD
marker_data.pose.position.x = map(float,row)[1]
marker_data.pose.position.y = map(float,row)[2]
marker_data.pose.position.z = map(float,row)[3]
marker_data.pose.orientation.x=map(float,row)[4]
marker_data.pose.orientation.y=map(float,row)[5]
marker_data.pose.orientation.z=map(float,row)[6]
marker_data.pose.orientation.w=map(float,row)[7]
marker_data.color.r = 0.0
marker_data.color.g = 0.0
marker_data.color.b = 0.0
marker_data.color.a = 1.0
marker_data.scale.x = 5
marker_data.scale.y = 5
marker_data.scale.z = 5
marker_data.lifetime = rospy.Duration()
marker_data.type = Marker.TEXT_VIEW_FACING
marker_data.text = str(int(map(float,row)[0]))
pub.publish(marker_data)
counter +=1
rate.sleep()
rospy.spin()
