7.6.2 Pythonスクリプトでロボットを動かしてみる（シミュレーション編）

timed_out_and_back.py を参考にPythonスクリプトでロボットを動かすプログラムを見てみましょう。

1 #!/usr/bin/env python
2
3　import rospy

この2行はPythonスクリプトでROSプログラム書くときのお決まりです

4　from geometry_msgs.msg import Twist5　from math import pi

Twist型のメッセージを使います

7　class OutAndBack():
8 　　def __init__(self):

OutAndBackクラスを作ります

9 　　　 # Give the node a name
10 　　　rospy.init_node('out_and_back', anonymous=False)
11
12 　　　# Set rospy to execute a shutdown function when exiting
13 　　　rospy.on_shutdown(self.shutdown)

out_and_backノードを初期化します。
スクリプトが終了するときに呼ばれるコールバックon_shutdown()を宣言します。例えば、CtrlCtrl+cキーが押された時に呼ばれます。

15 　　　# Publisher to control the robot's speed
16 　　　self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)
17
18 　　　# How fast will we update the robot's movement?
19 　　　rate = 50
20
21 　　　# Set the equivalent ROS rate variable
22 　　　r = rospy.Rate(rate)

/cmd_velトピックにTwist型の型のメッセージ（コマンド？）を送る宣言をします。

一秒間に50回のタイミングでコマンドをアップデートします。

24 　　 # Set the forward linear speed to 0.2 meters per second
25 　　 linear_speed = 0.2
26
27 　　 # Set the travel distance to 1.0 meters
28 　　 goal_distance = 1.0
29
30 　　 # How long should it take us to get there?
31 　　 linear_duration = linear_distance / linear_speed

ロボットの前進速度を秒速0.2メートルに設定します。

ゴールまでの距離を1.0メートルに設定します。

ゴールまでの到達時間を求めます。1.0／0.2＝5秒

33 　　　# Set the rotation speed to 1.0 radians per second
34 　　　angular_speed = 1.0
35
36 　　　# Set the rotation angle to Pi radians (180 degrees)
37 　　　goal_angle = pi
38
39 　　　# How long should it take to rotate?
40 　　　angular_duration = angular_distance / angular_speed
この行は間違えなので、以下のように修正する。付属のスクリプトは正しい
40      angular_duration = goal_angle / angular_speed

同じように、ロボットの回転角速度を毎秒1.0ラジアンに設定し、

目標とする回転角度を180度（πラジアン）に設定すると、

目標とする回転角度までの時間は pi (3.14159263) / 1.0 = 3.14秒

42 　　　# Loop through the two legs of the trip
43 　　　for i in range(2):
44 　　　　# Initialize the movement command
45 　　　　move_cmd = Twist()
46
47 　　　　# Set the forward speed
48 　　　　move_cmd.linear.x = linear_speed
49
50 　　　　# Move forward for a time to go the desired distance
51 　　　　ticks = int(linear_duration * rate)
52
53 　　　　for t in range(ticks):
54 　　　　　self.cmd_vel.publish(move_cmd)
55 　　　　　r.sleep()

このループで実際にロボット動かします。一連の動きを２回繰り返します。

ロボットを１秒の間に、linear_speedの速度で、linear_distanceの距離だけ動かすために、1/rate 秒の間隔でTwist型のmove_cmd メッセージを出力し続けます。

 r.sleep()では非常に短い時間（1/rate）だけスリープします。1/rateという時間は、r=rospy.Rate(rate)で設定された値になります。

57 　　　　　# Stop the robot before the rotation
58 　　　　　move_cmd = Twist()
59 　　　　　self.cmd_vel.publish(move_cmd)
60 　　　　　rospy.sleep(1)

ここではループの後半に入る前に、ロボットを一旦停止させ、スクリプトを1秒間停止させます。

62 　　　　　　# Now rotate left roughly 180 degrees
63
64 　　　　　　# Set the angular speed
65 　　　　　　move_cmd.angular.z = angular_speed
66
67 　　　　　　# Rotate for a time to go 180 degrees
68 　　　　　　ticks = int(goal_angle * rate)
69
70 　　　　　　for t in range(ticks):
71 　　　　　　　　self.cmd_vel.publish(move_cmd)
72 　　　　　　　　r.sleep()

ループの後半はロボットを回転させます。

１秒間で180度回転します。

74 　　　　# Stop the robot before the next leg
75 　　　　move_cmd = Twist()
76 　　　　self.cmd_vel.publish(move_cmd)
77 　　　　rospy.sleep(1)

2度めのループに入る前に、ロボットを一旦停止させ、スクリプトを1秒間停止させます。

79 　　　# Stop the robot.
80 　　　self.cmd_vel.publish(Twist())

2度のループが終わったら、ロボットに空（すべてが0）のTwistメッセージ送る。

82 def shutdown(self):
83 　　# Always stop the robot when shutting down the node.
84 　　rospy.loginfo("Stopping the robot...")
85 　　self.cmd_vel.publish(Twist())
86 　　rospy.sleep(1)

何らかの理由でシャットダウン（Ctrl+cが押された）が実行された時はこのコールバック関数が実行される。

Tearminalウィンドウに”Stopping the robot..." が表示され、空のTwistメッセージが送られ、1秒間ループしたのちにスクリプトは終了する。

さて、timed_out_and_back.pyの動作をシミュレータで確認してみましょう。

$roslaunch rbx1_bringup fake_turtlebot.launch

$rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz

$rosrun rbx1_nav timed_out_and_back.py

この3つのコマンドをそれぞれ別のTerminalウィンドウで実行して下さい。もう一つ別のウィンドウで $roscore を実行するのを忘れないでください。