ROS シミュレーション上でロボットを移動させる
この記事の目的
ROSシミュレーションでロボットを移動させるスクリプトの作り方をまとめる。(mapserver/Odometry/TFの基礎)
1. ROSで移動ロボットを扱うための仕組み
ROSではロボットを自律移動させるためのパッケージが『Navigation Stack』として提供されている。
Navigation Stackの機能は多岐にわたり、自己位置推定やパスプランニング、SLAMを使った地図生成などのパッケージを設定して使う必要がある。
本記事ではNavigationスタックの基礎として、ROSのシミュレーション上で、地図上を車両ロボットをユーザーが操作した速度/角速度で移動させるシミュレーションを構築する。
2. 地図情報の読み込み(mapserver)
ROS の Navigation Stackではmapserverというパッケージを使って地図をROS上で読み込む。
mapサーバーを使うためには下記コマンドでインストールする。
ros-kinetic-map-server
mapserverは地図情報をyamlファイルと画像データ(白黒画像)のセットとして保持する。 yamlファイルは下記の情報を保持する。
| 設定 | 意味 |
|---|---|
| image | 画像ファイルぺのパス(画像はPNG/BMPなど) |
| resolution | 解像度(1pixelを何mとするか)) |
| origin | 原点座標(x,y,yaw) |
| occupied_thresh | 障害物があると判定する閾値 |
| free_thresh | 障害物がないと判定する閾値 |
| negate | 白黒反転設定 |
下記のサンプルファイルを参考として作成するとよい。 github.com
ファイルを用意したあと、下記のtest_mapserver.launchファイルを作成する。(yamlファイルのパスは実行環境に応じて変更する。)
<launch> <arg name="map_file" default="/home/ubuntu/catkin_ws/src/mapserver_test/launch/glbmap.yaml"/> <group ns="map_server"> <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /> </group> </launch>
このtest_mapserver.launchファイルを実行すると、ROSでmap情報を読み込むことができる。
roslaunch testmapserver.launch
3. 現在位置(Odometry)の生成
ROSのNavigation Stackではロボットの自己位置を"Odometry"というTopicで保持する。 Odometryは下記の信号から構成されている。
| 設定 | 意味 |
|---|---|
| header | 自己位置の計算されたタイムスタンプ/フレームID |
| child_frame_id | オドメトリに関連付けるフレームID |
| pose | ロボットの姿勢 (x[m],y[m],z[m],クォータニオン) |
| twist | ロボットの推定速度(vx[m/s],vy[m/s],vz[m/s],roll[rad/s],pitch[rad/s],yaw[rad/s]) |
ユーザーがtkinterで指定した車両の前進速度[m/s]と角速度[m/s]を指定し、それに従ってOdometryを計算して発行するpythonスクリプトは下記のように記述できる。
#!/usr/bin/env python # -*- coding: utf-8 -*- import math import signal # import for ros function import rospy import tf from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 from std_msgs.msg import Header from Tkinter import * from nav_msgs.msg import Odometry ############################# # Simple Odometry simulator # ############################# class Simple_odom_simulator(): ################## # Initialization # ################## def __init__(self): rospy.init_node('Simple_Odometry_Simlator', anonymous=True) r = rospy.Rate(50) # 50hz #Initialize odometry header self.odom_header = Header() self.odom_header.seq = 0 self.odom_header.stamp = rospy.Time.now() self.odom_header.frame_id = "map" # Initialize pose info self.sim_pose = Pose() self.sim_pose.position.x = 0.0 self.sim_pose.position.y = 0.0 self.sim_pose.position.z = 0.0 self.sim_pose.orientation.x = 0.0 self.sim_pose.orientation.y = 0.0 self.sim_pose.orientation.z = 0.0 # initialize twist info self.sim_twist = Twist() # Initialize odometry info self.sim_odom = Odometry() self.sim_odom.header = self.odom_header self.sim_odom.child_frame_id = "base_link" self.sim_odom.pose.pose = self.sim_pose self.sim_odom.twist.twist = self.sim_twist #configuration for tkinter self.root = Tk() self.root.option_add('*font', ('FixedSys', 14)) self.buffer = StringVar() self.buffer.set('') Label(self.root, text='Simple Odometry Simulator').pack() self.a = Label(self.root, textvariable=self.buffer) self.a.pack() self.a.bind('<Any-KeyPress>', self.cb_keyevent) #initialize publisher self.emu_odom_pub = rospy.Publisher("/odom", Odometry, queue_size=50) #initialize TF self.odom_broadcaster = tf.TransformBroadcaster() self.map_broadcaster = tf.TransformBroadcaster() #set callback for ctrl and c signal.signal(signal.SIGINT, self.ctr_c_interruption) ########################### # Bind Key board callback # ########################### def cb_keyevent(self,event): key = event.keysym #Update User request Speed from Key board if key=="Up": print("Up") self.sim_twist.linear.x = self.sim_twist.linear.x + 0.1 #m/sec pass elif key=="Down": print("Down") self.sim_twist.linear.x = self.sim_twist.linear.x - 0.1 #m/sec pass elif key == "Left": self.sim_twist.angular.z = self.sim_twist.angular.z + 0.1 #rad/sec print("Left") pass elif key == "Right": self.sim_twist.angular.z = self.sim_twist.angular.z - 0.1 #rad/sec print("Right") pass elif key == "Escape": self.sim_twist.linear.x = 0.0 # m/sec self.sim_twist.angular.z = 0.0 #rad/sec print("Escape") #Min-Max GUARD if self.sim_twist.linear.x < -5.0: self.sim_twist.linear.x = -5.0 elif self.sim_twist.linear.x > 5.0: self.sim_twist.linear.x = 5.0 if self.sim_twist.angular.z < -3.14: self.sim_twist.angular.z = -3.14 elif self.sim_twist.angular.z > 3.14 self.sim_twist.angular.z = 3.14 ############################################# # Update odometry form User request cmd_vel # ############################################# def update_odom(self): #Update Vehicle Pose sampletime = 0.1 #calculate by 100msec e = tf.transformations.euler_from_quaternion((self.sim_pose.orientation.x, self.sim_pose.orientation.y, self.sim_pose.orientation.z, self.sim_pose.orientation.w)) yaw_euler = e[2] #update pose self.sim_pose.position.x = self.sim_pose.position.x + self.sim_twist.linear.x*sampletime*math.cos(yaw_euler) self.sim_pose.position.y = self.sim_pose.position.y + self.sim_twist.linear.x*sampletime*math.sin(yaw_euler) updated_yaw = e[2] +self.sim_twist.angular.z*sampletime updated_quaternion =tf.transformations.quaternion_from_euler(0, 0, updated_yaw) self.sim_pose.orientation.x = updated_quaternion[0] self.sim_pose.orientation.y = updated_quaternion[1] self.sim_pose.orientation.z = updated_quaternion[2] self.sim_pose.orientation.w = updated_quaternion[3] #for GUI out_str = '' vel_kmh = "{0:.3f}".format(self.sim_twist.linear.x * 3600.0 / 1000.0) yawrate = "{0:.3f}".format(self.sim_twist.angular.z) pose_x = "{0:.3f}".format(self.sim_pose.position.x) pose_y = "{0:.3f}".format(self.sim_pose.position.y) orientation = "{0:.3f}".format(self.sim_pose.orientation.z) out_str = out_str + "\n velocity[km/h]:" + vel_kmh out_str = out_str + "\n yawrate[rad/s]:" + yawrate out_str = out_str + "\n position[m]:(" + pose_x +"," + pose_y + ")" out_str = out_str + "\n orientation[rad]:" + orientation self.buffer.set(out_str) #update timestamp self.odom_header.seq =self.odom_header.seq + 1 self.odom_header.stamp = rospy.Time.now() self.sim_odom.header = self.odom_header self.sim_odom.pose.pose = self.sim_pose self.sim_odom.twist.twist = self.sim_twist self.emu_odom_pub.publish(self.sim_odom) #update TF self.map_broadcaster.sendTransform( (self.sim_odom.pose.pose.position.x, self.sim_odom.pose.pose.position.y, self.sim_odom.pose.pose.position.z), updated_quaternion, rospy.Time.now(), "odom", "map" ) base_link_quat = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0) self.odom_broadcaster.sendTransform( (0.0, 0.0, 0.0), base_link_quat, rospy.Time.now(), # "base_link", "base_link", "odom" ) self.root.after(100, self.update_odom) #interval for gui update (100msec) ####################### # ctrl and c callabck # ####################### def ctr_c_interruption(self, signum, frame): self.root.quit() self.root.update() print("finish") pass if __name__ == '__main__': print('Simple Odometry Simulator is Started...') test = Simple_odom_simulator() test.a.focus_set() test.update_odom() test.root.mainloop() pass
上記例ではtkinterのUIを100msec毎に動作させてユーザーの速度/角速度目標値を取得する。 そして、目標値に基づいて自車の位置座標(速度、角度)を計算しなおし、Odometryの値を更新してトピックを発行している。
4. 座標変換情報の設定
Odometryを計算する際に、座標系についても設定する必要があるため補足して説明する。 ROSでは複数の座標系(frame)を持つことができ、各フレーム間を座標変換行列で関連付けて設定することができる。 この機能はTFというパッケージで実現される。
今回のシミュレーション実装例では3つの座標系を利用している。
| 設定 | 意味 |
|---|---|
| map | 地図座標 |
| odom | 自己位置 |
| baselink | 車体中心 |
シミュレーションで自己位置として車体中心座標が得られるという前提でodomとbaselinkが同じ座標になるようにTFを発行している。 実際にはロボットのセンサーがOdometryを発行するので、Odomとbaselinkは自己位置認識の方法やデバイスによって変更する必要がある。
5. シミュレーションの実行
コードを下記のgithubにまとめる。(地図ファイルは環境事にパス修正が必要なので注意)
下記コマンドを実行するとtkinterで操作した速度・角速度に従ってロボットが移動することををrviz上で確認できる。
roslaunch dispaly_car_model.launch roslaunch test_mapserver.launch python Simple_odom_simulator.py rviz
