GazeboのTurtlebot3に無理やり超音波センサーをつけてみた
前回はgazeboのTurtlebot3を動かしてみました。
でこの記事の後半ではLiDARセンサーを使って障害物回避をしてみました。
LiDARセンサーはもともとマッピング、ナビゲーション用に使うセンサーなので、目の前の障害物を回避するのには向いていません。
とはいえgazebo上で参照できるセンサーはこれぐらいしかありません(公式ドキュメントでは実機で接続すればできるよってぐらいしか書いてないという不親切さｗ)。
というわけで今回は無理やりTurtlebot3に超音波センサーをつけて目の前の障害物をちゃんと回避できるようにしてみたいと思います。
事前準備
前回の前提条件とパッケージの追加に沿って実行環境を整えておきましょう。
xacroの修正
まずはxacroを修正します。xacroはモデルの定義ファイルです。
~/wanderbot_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.gazebo.xacroを開き、<robot>タグ内に以下の記述を追加します。
ここでは超音波センサーのプラグインを定義しています。
<gazebo reference="base_sonar_front">
<sensor type="ray" name="TeraRanger">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>50</update_rate>
<ray>
<scan>
<horizontal>
<samples>10</samples>
<resolution>1</resolution>
<min_angle>-0.14835</min_angle>
<max_angle>0.14835</max_angle>
</horizontal>
<vertical>
<samples>10</samples>
<resolution>1</resolution>
<min_angle>-0.14835</min_angle>
<max_angle>0.14835</max_angle>
</vertical>
</scan>
<range>
<min>0.01</min>
<max>2</max>
<resolution>0.02</resolution>
</range>
</ray>
<plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>50</updateRate>
<topicName>sensor/sonar_front</topicName>
<frameName>base_sonar_front</frameName>
<radiation>ULTRASOUND</radiation>
<fov>0.2967</fov>
</plugin>
</sensor>
</gazebo>
続いて~/wanderbot_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.urdf.xacroを開き、<robot>タグ内に以下の記述を追加します。
これはロボット上でのセンサーモデルのオブジェクト、位置を定義してTurtlebot3のモデルと結合しています。
<joint name="sonar_front_joint" type="fixed">
<axis xyz="0 1 0" />
<origin rpy="0 0 0" xyz="0.03 0 0.15" />
<parent link="base_footprint"/>
<child link="base_sonar_front"/>
</joint>
<link name="base_sonar_front">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
今回はセンサーの機能をシミュレートできればいいので、センサーの形状はシンプルな立方体にして、色はデフォルトのままにしています。
センサーの位置は目分量で調整してそれっぽい位置にしています。
モデルの確認
それでは実際にgazeboを起動してセンサーの位置を確認してみます。
以下のコマンドを実行してシミュレータを起動してみます。
cd ~/wanderbot_ws/
source devel/setup.bash
roslaunch turtlebot3_gazebo turtlebot3_world.launch
実行後、以下ように立方体のオブジェクトがTurtlebot3にくっついていたら成功です。
青い放射線状のオブジェクトはセンサーの認識範囲を示しています。
デバッグ用に表示しているので、非表示にしたければturtlebot3_burger.gazebo.xacroの<visualize>タグの値をfalseに設定します。
センサーが使えるか確認
モデルの配置ができたのでセンサーがしっかり認識するか確認してみます。
別ターミナルでトピックでセンサーの値が配信されていることを確認するために以下のコマンドを実行します。
cd ~/wanderbot_ws/
source devel/setup.bash
rostopic echo /sensor/sonar_front
さらに別ターミナルを起動して以下のコマンドでteleopkeyを起動します。
cd ~/wanderbot_ws/
source devel/setup.bash
rosrun turtlebot3_teleop turtlebot3_teleop_key
起動後以下のように距離に応じてセンサーの値を配信していることを確認できると思います。
Pythonの実装
次にPythonでセンサを使えるようにしていきます。
~/wanderbot_ws/src/wanderbot/src/wander_sonar.pyを作成し、以下の内容を記述します。
前回のソースコードからカッコつけてロジック部分をクラス文にしましたw。
ソースコード
#!/usr/bin/env python3
import rospy
from rospy.exceptions import ROSInterruptException
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Range
class Wanderbot:
def __init__(self):
rospy.init_node('wander')
rospy.on_shutdown(self.shutdown_turtlebot)
self.sensor_value = 1.0
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
# センサーメッセージを購読
self.scan_sub = rospy.Subscriber('/sensor/sonar_front', Range, self.scan_callback, queue_size=1)
# 1秒ごとにコールバック関数を実行
rospy.Timer(rospy.Duration(1.0), self.timer_callback)
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
self.cmd_vel_pub.publish(twist)
def scan_callback(self, msg):
self.sensor_value = float(msg.range)
def timer_callback(self, event):
twist = Twist()
rate = rospy.Rate(10)
rospy.loginfo(self.sensor_value)
if self.sensor_value < 0.5:
twist.angular.z = 1.0
else:
twist.linear.x = 0.5
twist.angular.z = 0.0
for num in range(5):
self.cmd_vel_pub.publish(twist)
rate.sleep()
def shutdown_turtlebot(self):
rospy.loginfo('Stop Turtlebot')
self.cmd_vel_pub.publish(Twist())
if __name__ == '__main__':
try:
Wanderbot()
rospy.spin()
except ROSInterruptException:
pass
Turtlebot3用に超音波センサーのメッセージオブジェクトが用意されていますが、今回はROSにデフォルトで用意されている距離センサーのメッセージオブジェクトを使っているので、以下のモジュールをインポートしています。
from sensor_msgs.msg import Range
While文がなくなった分、かなりシンプルになりました。
代わりに以下の1行で1秒毎にコールバック関数を実行します。
rospy.Timer(rospy.Duration(1.0), self.timer_callback)
そのコールバック関数では、1回メッセージをパブリッシュさせるだけだと動作しないので5回メッセージをパブリッシュさせてロボットを動作させます。
def timer_callback(self, event):
twist = Twist()
rate = rospy.Rate(10)
rospy.loginfo(self.sensor_value)
if self.sensor_value < 0.5:
twist.angular.z = 1.0
else:
twist.linear.x = 0.5
twist.angular.z = 0.0
for num in range(5):
self.cmd_vel_pub.publish(twist)
rate.sleep()
そして、メイン文の以下の1行でプログラムが止まらないようにループを実行しています。
rospy.spin()
動作確認
それでは、再び動作確認です。
以下のコマンドでシミュレータを実行してみましょう。
cd ~/wanderbot_ws/
source devel/setup.bash
cd ~/wanderbot_ws/src/wanderbot/src/
chmod u+x wander_sonar.py
rosrun wanderbot wander_sonar.py
実行すると、以下のようにセンサーの値によって障害物を避けられるようになりました。
前回のLiDARよりもだいぶスムーズです。
今回はTurtlebot3に無理やりセンサーをつける方法を紹介しました。
センサーはデフォルトのメッセージオブジェクトを使って通信をしているので、他のロボットにセンサーを拡張するときにも応用できそうな気がします。
いつかは自作ロボットのシミュレーションするときにも活かしてみたいですね。
Discussion