障害物認識と回避
TurtleBot3は、LDSのデータによって移動・停止できます。TurtleBot3が移動している際に、前方の障害物を検知した場合は停止します。
実際のロボットを使う場合： Turtlebot3を起動します（参照）。
次にサンプルプログラムから続きます。
シミュレーションで試したい場合：
下記の実習を行うために、まずROS_MASTER_URIとROS_HOSTNAMEをlocalhostに戻します（参照）。
下記のROSパケージをダウンロードすることをおすすめします（コンビニのシミュレーション環境）。
1
2
3
4
$ cd ~/catkin_ws/src/
$ git clone https://github.com/igra9/rsj_seminar_2021_navigation.git
$ cd ..
$ catkin_make
シミュレーションを起動します。
1
$ roslaunch rsj_seminar_2021_navigation turtlebot3_convini.launch
PCの性能によってシミュレータを起動するのに多少時間がかかります。 Gazeboの画面が表示されるまでに待ちます。
サンプルプログラムを実行
リモートPCで obstacle fileを起動します。
1
$ roslaunch turtlebot3_example turtlebot3_obstacle.launch
このプログラムのソースコードが簡単な処理を行なっています。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
import rospy
import math
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
LINEAR_VEL = 0.22
STOP_DISTANCE = 0.2
LIDAR_ERROR = 0.05
SAFE_STOP_DISTANCE = STOP_DISTANCE + LIDAR_ERROR
class Obstacle():
def __init__(self):
self._cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.obstacle()
def get_scan(self):
scan = rospy.wait_for_message('scan', LaserScan)
scan_filter = []
samples = len(scan.ranges) # The number of samples is defined in
# turtlebot3_<model>.gazebo.xacro file,
# the default is 360.
samples_view = 1 # 1 <= samples_view <= samples
if samples_view > samples:
samples_view = samples
if samples_view is 1:
scan_filter.append(scan.ranges[0])
else:
left_lidar_samples_ranges = -(samples_view//2 + samples_view % 2)
right_lidar_samples_ranges = samples_view//2
left_lidar_samples = scan.ranges[left_lidar_samples_ranges:]
right_lidar_samples = scan.ranges[:right_lidar_samples_ranges]
scan_filter.extend(left_lidar_samples + right_lidar_samples)
for i in range(samples_view):
if scan_filter[i] == float('Inf'):
scan_filter[i] = 3.5
elif math.isnan(scan_filter[i]):
scan_filter[i] = 0
return scan_filter
def obstacle(self):
twist = Twist()
turtlebot_moving = True
while not rospy.is_shutdown():
lidar_distances = self.get_scan()
min_distance = min(lidar_distances)
if min_distance < SAFE_STOP_DISTANCE:
if turtlebot_moving:
twist.linear.x = 0.0
twist.angular.z = 0.0
self._cmd_pub.publish(twist)
turtlebot_moving = False
rospy.loginfo('Stop!')
else:
twist.linear.x = LINEAR_VEL
twist.angular.z = 0.0
self._cmd_pub.publish(twist)
turtlebot_moving = True
rospy.loginfo('Distance of the obstacle : %f', min_distance)
def main():
rospy.init_node('turtlebot3_obstacle')
try:
obstacle = Obstacle()
except rospy.ROSInterruptException:
pass
上記のプログラム（/opt/ros/kinetic/lib/turtlebot3_example/turtlebot3_obstacle.pyあるいは/home/user/catkin_ws/src/turtlebot3/turtlebot3_example/nodes/turtlebot3_obstacle.py）を新しいファイルにコピーします。
コピーした後にスクリプットの１行目に!#/usr/bin/pythonが書いてあることを確認します。
新しいROS packageを作成するかrsj_seminar_2021_navigationのパケージに追加します。
1
2
$ cd ~/catkin_ws/src/rsj_seminar_2021_navigation
$ mkdir src
上記のフォルダーに作成したファイルname.pyを保存します（適切に名前を変更してください）。
新しいパケージを作成するには（package-nameがパケージ名を指定して適切に変更してください）：
1
2
3
4
$ cd ~/catkin_ws/src
$ catkin_create_pkg package-name message_generation rospy sensor_msgs geometry_msgs
$ cd package-name
$ mkdir src
上記のフォルダーに作成したファイルを保存します（拡張子はpyにします）。
1
$ chmod u+x ~/catkin_ws/src/package-name/src/name.py
~/catkin_ws/src/package-name/CMakeLists.txtのファイルに下記を追加します。
catkin_install_python(PROGRAMS src/name.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
ファイルを保存して閉じます。
ビルドします。
1
2
$ cd ~/catkin_ws
$ catkin_make
実行を確認します。
1
$ rosrun package-name name.py
課題 ロボットを停止するのではなくて障害物を避けて引き続いて移動できるようにプログラムname.pyを変更します。
座標指令
TurtleBot3は、2次元上の点(x, y)とz-angularで移動することができる。たとえば、(0.5, 0.3, 60)を入れると、TurtleBot3は点（x = 0.5m, y = 0.3m）に移動し、60度回転します。
このプログラムを実行するには
1
roslaunch turtlebot3_example turtlebot3_pointop_key.launch
実行すると下記の表示があります。
1
2
3
4
5
6
7
8
9
10
control your Turtlebot3!
-------------------------
Insert xyz - coordinate
x : position x (m)
y : position y (m)
z : orientation z (degree: -180 ~ 180)
If you want to close, insert 's'
-------------------------
| x | y | z |
適切な座標を入力し、Enterキーを押すとロボットが動きます。
課題 上記のプログラムを参考しながら指定したポイントまで障害物を回避しながらロボットを移動させます。
turtlebot3_pointtop_key.pyは /opt/ros/kinetic/lib/turtlebot3_example/にあります。