TurtleBot3のシミュレーション環境で超音波測距センサーHC-SR04を使ってみます。
安価ですが障害物回避用などの距離測定によく使われるセンサーです。
表示用のcollada形式ファイル（hc-sr04.dae）は以下からダウンロードできます。
解凍後、以下のフォルダーにコピーしておきます。
/opt/ros/melodic/share/turtlebot3_description/meshes/sensors
シミュレーション環境でセンサーを使えるようにする
turtlebot3_burger.urdf.xacroファイルを読み書きできるようにしておき、開きます。
|
1 2 3 |
sudo chmod 666 /opt/ros/melodic/share/turtlebot3_description/urdf/turtlebot3_burger.urdf.xacro sudo nano /opt/ros/melodic/share/turtlebot3_description/urdf/turtlebot3_burger.urdf.xacro |
以下を追加
originのｚ値：高さは１０ｃｍ以上を取らないと、有効な値を取得できないようです。
|
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 |
<!-- ~~~~~~~~~~~~~~~~ hc-sr04 ~~~~~~~~~~~~~~~~~~~~~~~~~~ --> <joint name="sonar_front_joint" type="fixed"> <parent link="base_footprint"/> <child link="base_sonar_front"/> <origin xyz="0.030 0.005 0.110" rpy="0 0 0"/> </joint> <link name="base_sonar_front"> <visual> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> <mesh filename="package://turtlebot3_description/meshes/sensors/hc-sr04.dae" scale="1.00 1.00 1.00"/> </geometry> <material name="blue"/> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> <box size="0.010 0.010 0.010"/> </geometry> </collision> <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> |
turtlebot3_burger.gazebo.xacroファイルを読み書きできるようにしておき、開きます。
|
1 2 3 |
sudo chmod 666 /opt/ros/melodic/share/turtlebot3_description/urdf/turtlebot3_burger.gazebo.xacro sudo nano /opt/ros/melodic/share/turtlebot3_description/urdf/turtlebot3_burger.gazebo.xacro |
以下を追加
HC-SR04はFoV（Field of View）超音波センサー（Ultrasonic / Ultrasound Sensor）なのでgazebo_ros_range（libgazebo_ros_range.so）Plugin を使ってみます。
音波を可視化する場合は、以下の<visualize>タグでtrueを設定
各パラメータの値は、すみません、出典を忘れてしまいました（ros.orgのどこかにあるはず）。
|
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 |
<gazebo reference="base_sonar_front"> <sensor type="ray" name="HC-SR04"> <pose>0 0 0 0 0 0</pose> <visualize>false</visualize> <update_rate>5</update_rate> <ray> <scan> <horizontal> <samples>5</samples> <resolution>1.0</resolution> <min_angle>-0.15</min_angle> <max_angle>0.15</max_angle> </horizontal> <vertical> <samples>5</samples> <resolution>1.0</resolution> <min_angle>-0.15</min_angle> <max_angle>0.15</max_angle> </vertical> </scan> <range> <min>0.01</min> <max>0.75</max> <resolution>0.01</resolution> </range> </ray> <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range"> <gaussianNoise>0.005</gaussianNoise> <alwaysOn>true</alwaysOn> <updateRate>5</updateRate> <topicName>sensor/sonar_front</topicName> <frameName>base_sonar_front</frameName> <radiation>ULTRASOUND</radiation> <fov>0.5</fov> </plugin> </sensor> <material>Gazebo/Blue</material> </gazebo> |
このターミナルではBurgerモデルを使うので、MODELを指定します。
|
1 2 3 |
export TURTLEBOT3_MODEL=burger roslaunch turtlebot3_gazebo turtlebot3_world.launch |
青いパーツがセンサー
音波を可視化しています。
別ターミナル開いて、以下のコマンドでセンサーの値が配信されていることを確認
|
1 |
rostopic echo /sensor/sonar_front |
別ターミナルを開いて、以下のコマンドでBot3を移動して
障害物に接近すると距離に応じてセンサーのrange値が変化するのを確認します。
|
1 |
rosrun turtlebot3_teleop turtlebot3_teleop_key |
以下の3つの位置ではrangeの値が異なるのが分かります。
センサーの測定値を取得するためのPythonコードを作成します。
依存パッケージは、rospy、geometry_msgs、sensor_msgsの３つ。
topic のtype を確認
rostopic type /sensor/sonar_front
sensor_msgs/Range
rostopic type /cmd_vel
geometry_msgs/Twist
gazebo起動
|
1 2 3 |
export TURTLEBOT3_MODEL=burger roslaunch turtlebot3_gazebo turtlebot3_world.launch |
別のターミナルでpython3 コンソールを開いて以下のコードを実行します。
sensor_valueに測定結果（m）
Twistのlinearが並進方向の速度（m/秒）、angularが軸回りの回転速度（rad/秒）
|
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 |
#!/usr/bin/env python3 import rospy from rospy.exceptions import ROSInterruptException from geometry_msgs.msg import Twist from sensor_msgs.msg import Range def sonar_callback(msg): global sensor_value sensor_value = float(msg.range) def avoidance_callback(event): twist = Twist() rate = rospy.Rate(10) rospy.loginfo(sensor_value) if sensor_value > 0.1 and sensor_value < 0.5: twist.angular.z = 1.0 elif sensor_value < 0.1: twist.linear.x = -1.0 else: twist.linear.x = 1.5 twist.angular.z = 0.0 for num in range(5): cmd_vel_pub.publish(twist) rate.sleep() def shutdown_tb3(): rospy.loginfo('Stop') cmd_vel_pub.publish(Twist()) def walk_around(): global cmd_vel_pub rospy.init_node('walkaround') rospy.on_shutdown(shutdown_tb3) sensor_value = 0.0 cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) scan_sub = rospy.Subscriber('/sensor/sonar_front', Range, sonar_callback, queue_size=1) rospy.Timer(rospy.Duration(0.5), avoidance_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 cmd_vel_pub.publish(twist) try: walk_around() except ROSInterruptException:pass |
こんな感じ (夏場になると出てくる昆虫みたいです)
Bot3はごく単純なルールで動き回っていて、ターミナルにはHC-SR04と障害物との距離を表示しています。
途中で障害物を出し入れしてみます。
Leave a Reply