Raspberry Pi Mouse（移行中）
GazeboRaspberry PiRaspberry Pi MouseRasPiMouseROSマイクロマウス

Gazeboで走るRasPiMouse　その5

2016.08.24

こんにちは。佐藤です。

前回はGazeboでまっすぐに走るRasPiMouseをコントロールすることができるようになりました。

概要

今回はセンサを取り付けてみます。さらに試験用に3×3のGazebo用のフィールドを用意して走行させてみます。

センサは実機と同様に近づけば近づくほど大きくなるような値を返すようにします。

センサの取り付け

センサを取り付けます。
実機と同様に光らせることができればかっこいいのですが、見た目にしか影響しません。
ですから今回は直方体をセンサとして取り付けることにします。
センサはレーザーレンジセンサなので返ってくる値はセンサから物体までの距離です。
raspimouse_descriptionのパッケージ内でモデルを定義します。
lightsens.urdf.xacroとして保存します。

<?xml version="1.0"?>
<robot name="light_sensor" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find raspimouse_description)/urdf/sensors/lightsens.gazebo.xacro"/>

  <xacro:macro name="light_sensor" params="prefix parent *joint_origin">
    <joint name="${prefix}_joint" type="fixed">
      <insert_block name="joint_origin"/>
      <parent link="${parent}"/>
      <child link="${prefix}"/>
    </joint>

    <link name="${prefix}">
      <visual>
        <geometry>
          <box size="0.01 0.005 0.007" />
        </geometry>
      </visual>
      <material name="grey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
      <inertial>
        <mass value="0.000001" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
          iyy="0.0001" iyz="0.0"
          izz="0.0001" />
      </inertial>
    </link>

    <!-- Set up laser gazebo details -->
    <hokuyo_laser />
  </xacro:macro>
</robot>

こちらはGazebo用の設定値ファイルです。同様にしてlightsens.gazebo.xacroとして保存します。

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:macro name="lightsensor_gazebo" params="prefix base_rad rad_range min_range max_range">
    <gazebo reference="${prefix}">
      <sensor type="ray" name="${prefix}_lightsensor">
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <update_rate>20</update_rate>
        <ray>
          <scan>
            <horizontal>
              <!-- <samples>720</samples> -->
              <samples>1</samples>
              <resolution>0.001</resolution>
              <min_angle>${base_rad-rad_range/2}</min_angle>
              <max_angle>${base_rad+rad_range/2}</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>${min_range}</min>
            <max>${max_range}</max>
            <resolution>0.001</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.03</stddev>
          </noise>
        </ray>
        <plugin name="gazebo_ros_${prefix}_lightsensor_controller" filename="libgazebo_ros_laser.so">
          <topicName>/raw_raspimouse/${prefix}/scan</topicName>
          <frameName>${prefix}</frameName>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>
</robot>

これらを呼び出すための設定を書いていきます。
raspimouse.urdf.xacroとして保存していたものにセンサの設定を書き加えています。
センサの位置が前とサイドに向くように合わせます。

<?xml version="1.0"?>
<robot name="raspimouse"
       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:include filename="$(find raspimouse_description)/urdf/body/body.urdf.xacro"/>
  <xacro:include filename="$(find raspimouse_description)/urdf/wheel/wheel.urdf.xacro"/>
  <xacro:include filename="$(find raspimouse_description)/urdf/sensors/lightsens.urdf.xacro"/>

  <!-- =============== Link & Joint =============== -->
  <!-- Base -->
  <link name="base_footprint"/>

  <xacro:base parent="base_footprint">
    <origin xyz="0 0 0.00185"/>
  </xacro:base>

  <!-- Wheel -->
  <xacro:wheel prefix="right" parent="base_link">
    <origin xyz="0 -0.0425 0.02215" rpy="1.57 0 0"/>
    <axis xyz="0 0 -1"/>
  </xacro:wheel>
  <xacro:wheel prefix="left" parent="base_link">
    <origin xyz="0 0.0425 0.02215" rpy="-1.57 0 0"/>
    <axis xyz="0 0 1"/>
  </xacro:wheel>

  <!-- Sensors -->
  <xacro:light_sensor prefix="rf" parent="base_link">
    <origin xyz="0.04 -0.045 0.032" rpy="0 0 0"/>
  </xacro:light_sensor>

  <xacro:light_sensor prefix="rs" parent="base_link">
    <origin xyz="0.059 -0.01 0.032" rpy="0 0 -1.04"/>
  </xacro:light_sensor>

  <xacro:light_sensor prefix="ls" parent="base_link">
    <origin xyz="0.059 0.01 0.032" rpy="0 0 1.04"/>
  </xacro:light_sensor>

  <xacro:light_sensor prefix="lf" parent="base_link">
    <origin xyz="0.04 0.045 0.032" rpy="0 0 0"/>
  </xacro:light_sensor>

  <!-- ===============  Transmission =============== -->
  <!-- <xacro:wheel_trans prefix="right" interface="EffortJointInterface"/> -->
  <!-- <xacro:wheel_trans prefix="left" interface="EffortJointInterface"/> -->
  <xacro:wheel_trans prefix="right" interface="VelocityJointInterface"/>
  <xacro:wheel_trans prefix="left" interface="VelocityJointInterface"/>

  <!-- =============== Gazebo =============== -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>raspimouse</robotNamespace>
    </plugin>
  </gazebo>

  <!-- Base -->
  <xacro:base_gazebo/>

  <!-- Wheel -->
  <xacro:wheel_gazebo prefix="right"/>
  <xacro:wheel_gazebo prefix="left"/>

  <!-- Sensors -->
  <xacro:lightsensor_gazebo prefix="rf" base_rad="0" rad_range="0.3" min_range="0.01" max_range="0.6"/>
  <xacro:lightsensor_gazebo prefix="rs" base_rad="-1.04" rad_range="0.3" min_range="0.01" max_range="0.6"/>
  <xacro:lightsensor_gazebo prefix="ls" base_rad="1.04" rad_range="0.3" min_range="0.01" max_range="0.6"/>
  <xacro:lightsensor_gazebo prefix="lf" base_rad="0" rad_range="0.3" min_range="0.01" max_range="0.6"/>

</robot>

センサ値変換スクリプトの作成

先ほどまでの設定だと返ってくる値はセンサから物体までの距離なので実際のセンサと同じような値に変換する必要があります。
変換するために、実機で距離ごとに測定値を記録し、近似式を求めました。

実機で記録したグラフ

raspimouse_controlのパッケージの中にscriptsディレクトリを作成します。

cd ~/catkin_ws/src/raspimouse_control
mkdir scripts
cd scripts

作成led_converter.pyとして保存します。

#!/usr/bin/env python

import sys, time, math, rospy
from raspimouse_control.msg import LightSensorValues
from sensor_msgs.msg import LaserScan

def sensor1_callback(data):
    sensor1.ranges = data.ranges

def sensor2_callback(data):
    sensor2.ranges = data.ranges

def sensor3_callback(data):
    sensor3.ranges = data.ranges

def sensor4_callback(data):
    sensor4.ranges = data.ranges

def range2led(range_value):
    try:
        distance = int(range_value[0]*1000) # distance[mm]
        if distance < 4:
            distance = 8 - distance

        led_value = int( 761000 / math.pow(distance,1.66) )

        if led_value > 4000:
            led_value = 4000
        if led_value < 15:
            led_value = 15
    except:
        led_value = 15 
    # rospy.loginfo(led_value)
    return led_value

def listener():
    sub1 = rospy.Subscriber('/raw_raspimouse/rf/scan', LaserScan, sensor1_callback)
    sub2 = rospy.Subscriber('/raw_raspimouse/rs/scan', LaserScan, sensor2_callback)
    sub3 = rospy.Subscriber('/raw_raspimouse/ls/scan', LaserScan, sensor3_callback)
    sub4 = rospy.Subscriber('/raw_raspimouse/lf/scan', LaserScan, sensor4_callback)

def talker():
    rospy.init_node('lightsensors')
    pub = rospy.Publisher('lightsensors', LightSensorValues, queue_size=1)
    r = rospy.Rate(10)
    if not rospy.is_shutdown():
        try:
            # rospy.loginfo(sensor1.ranges[0] * 1000)
            d = LightSensorValues()
            d.right_forward = range2led(sensor1.ranges)
            d.right_side = range2led(sensor2.ranges)
            d.left_side = range2led(sensor3.ranges)
            d.left_forward = range2led(sensor4.ranges)
            pub.publish(d)
        except:
            rospy.logerr("Converting Sensor Data Failed")

        r.sleep()

if __name__ == "__main__":
    # sensor[4] = [0.0,0.0,0.0,0.0]
    sensor1 = LaserScan()
    sensor2 = LaserScan()
    sensor3 = LaserScan()
    sensor4 = LaserScan()
    try:
        while not rospy.is_shutdown():
            listener()
            talker()
    except rospy.ROSInterruptException:
        pass

ROSのノードが使用するメッセージ形式を定義する必要がありますので、raspimouse_control以下に”msg”ディレクトリを作成し、
raspimouse_rosパッケージの”msg”ディレクトリの”LightSensorValues.msg”をコピーしてきます。

その他CMakeLists.txtに以下の依存関係を追記しておきます。

 add_message_files(
    FILES
    LightSensorValues.msg
    Switches.msg
 　  LeftRightFreq.msg
  )

publisherを作成した際にどのような変更を加えればよいかを知るためにはraspimouse_rosのこのコミット3d08155cを参考にしました。

このままだと毎回手動でスクリプトを実行する必要があります。Gazebo起動時に同時に起動するようlaunchファイルを書き換えます。

<?xml version="1.0"?>
<launch>
  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find raspimouse_control)/config/controller.yaml" command="load"/>

  <group ns="raspimouse">
    <!-- load the controllers -->
    <node name="controller_spawner" pkg="controller_manager"
      type="spawner" output="screen" 
      args="joint_state_controller 
      diff_drive_controller 
      "/>
    <!-- convert joint states to TF transforms for rviz, etc -->
    <node name="robot_state_publisher" pkg="robot_state_publisher"
      type="robot_state_publisher"
      respawn="false" output="screen">
    </node>
    <!-- convert sensor values -->
    <node pkg="raspimouse_control" name="lightsensors" type="led_converter.py"/>
  </group>
</launch>

これで起動時に/raspimouse/lightsensorsに距離センサの情報がpublishされるようになります。
ためしにempyworldで起動してみるとセンサの値が確認できました

$ roslaunch raspimouse_gazebo raspimouse_with_emptyworld.launch

$ rostopic echo /raspimouse/lightsensors

empyworldで起動

サンプル迷路データの作成

emptyworldやガソリンスタンドではマイクロマウスらしくないので、迷路のサンプルを作成します。
worldデータの作成もxacroのマクロを利用します。おそらくxacroファイルを直接読み込むことも可能なのだと思いますが、
launchファイルでの記述方法についての資料を見つけることができませんでした。今回はworldファイルに変換して読み込みます。

まず、壁のデータを作成します。samplemaze.wall.xacroとして保存しました。

<?xml version="1.0" ?>
<sdf version="1.4" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:macro name="wall" params="*pose">
    <model name='wall'>
      <!-- <pose>&#045;0.09 0 0.5 0 0 1.57</pose> -->
      <insert_block name="pose"/>
      <link name='link'>
        <inertial>
          <mass>10</mass>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size>0.180 0.012 0.050</size>
            </box>
          </geometry>
          <max_contacts>10</max_contacts>
          <surface>
            <bounce/>
            <friction>
              <ode/>
            </friction>
            <contact>
              <ode/>
            </contact>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size>0.180 0.012 0.050</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/White</name>
            </script>
          </material>
        </visual>
        <velocity_decay>
          <linear>0</linear>
          <angular>0</angular>
        </velocity_decay>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
        <gravity>1</gravity>
      </link>
      <static>0</static>
    </model>
  </xacro:macro>
</sdf>

次に壁を呼び出すxacroファイルを用意します。

<?xml version="1.0" ?>
<sdf version="1.4" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- <sdf version="1.4"> -->
<xacro:include filename="$(find raspimouse_gazebo)/materials/sample_maze.wall.xacro"/>
<world name="default">
  <include>
    <uri>model://ground_plane</uri>
  </include>

  <light name='sun' type='directional'>
    <cast_shadows>1</cast_shadows>
    <pose>0 0 5 0 0 0</pose>
    <diffuse>0.8 0.8 0.8 1</diffuse>
    <specular>0.2 0.2 0.2 1</specular>
    <attenuation>
      <range>1000</range>
      <constant>0.9</constant>
      <linear>0.01</linear>
      <quadratic>0.001</quadratic>
    </attenuation>
    <direction>-0.5 0.1 -0.9</direction>
  </light>

  <physics type='ode'>
    <max_step_size>0.001</max_step_size>
    <real_time_factor>1</real_time_factor>
    <real_time_update_rate>1000</real_time_update_rate>
    <gravity>0 0 -9.8</gravity>
  </physics>


  <gui fullscreen='0'>
    <camera name='user_camera'>
      <pose>-0.556842 -0.914575 1.68773 0 1.05964 0.520195</pose>
      <view_controller>orbit</view_controller>
    </camera>
  </gui>

  <!-- horizontal -->
  <xacro:wall>
    <pose>-0.090 0.006 0.5 0 0 1.57</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>-0.090 -0.174 0.5 0 0 1.57</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>-0.090 -0.354 0.5 0 0 1.57</pose>
  </xacro:wall>
  <!-- vertical -->
  <xacro:wall>
    <pose>0.006 0.090 0.5 0 0 0</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>0.186 0.090 0.5 0 0 0</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>0.366 0.090 0.5 0 0 0</pose>
  </xacro:wall>
  <!-- horizontal -->
  <xacro:wall>
    <pose>0.450 0.006 0.5 0 0 1.57</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>0.450 -0.174 0.5 0 0 1.57</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>0.450 -0.354 0.5 0 0 1.57</pose>
  </xacro:wall>
  <!-- vertical -->
  <xacro:wall>
    <pose>0.006 -0.450 0.5 0 0 0</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>0.186 -0.450 0.5 0 0 0</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>0.366 -0.450 0.5 0 0 0</pose>
  </xacro:wall>
  <!-- plus alpha -->
  <xacro:wall>
    <pose>0.186 -0.270 0.5 0 0 0</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>0.366 -0.090 0.5 0 0 0</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>0.270 -0.174 0.5 0 0 1.57</pose>
  </xacro:wall>
  <xacro:wall>
    <pose>0.006 -0.090 0.5 0 0 0</pose>
  </xacro:wall>
</world>
</sdf>

そしてworldファイルに変換するスクリプトを用意し、実行します。

#!/bin/bash

WORKINGDIR="$HOME/catkin_ws/src/raspimouse_sim/raspimouse_gazebo"

rosrun xacro xacro.py $WORKINGDIR/materials/sample_maze.world.xacro > $WORKINGDIR/worlds/sample_maze.world

これで3×3の迷路が用意出来ました。

3×3の迷路

これで起動してみます。

前回紹介したvel_publisherの値を以下のように書き換えると、前進/後進、左右の超信地旋回ができます。

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist

rospy.init_node('vel_publisher')
pub = rospy.Publisher('/raspimouse/diff_drive_controller/cmd_vel', Twist, queue_size=10)
while not rospy.is_shutdown():
    vel = Twist()
    direction = raw_input('w: forward, s: backward, a: left, d: right > ')
    if 'w' in direction:
        vel.linear.x = 0.35
    if 's' in direction:
        vel.linear.x = -0.35
    if 'a' in direction:
        vel.angular.z = 3.21
    if 'd' in direction:
        vel.angular.z = -3.21
    if 'q' in direction:
        break
    print vel
    pub.publish(vel)

これで簡単なシミュレーションを行うことができるようになりました。

簡単ではありますが、これで作り方の紹介兼メモは今回で終了となります。
GitHubに公開できるよう準備しています。公開したらまたブログで紹介したいと思います。

2016年9月13日追記
公開しました。
https://github.com/rt-net/raspimouse_sim
紹介記事も公開しました。
Gazeboで走るRasPiMouse　その6 | RT MicroMouse
https://www.rt-net.jp/mobility/archives/3544

　

補足

セットアップしたばかりの新しい環境だとパッケージが不足することがよくあります。

ros-control不足1

Error [Plugin.hh:156] Failed to load plugin libgazebo_ros_control.so: libgazebo_ros_control.so: cannot open shared object file: No such file or directory

このようなエラーが出る場合は以下のパッケージのインストールで解決できます。

sudo apt-get install ros-indigo-gazebo-ros-control

ros-control不足1

[INFO] [WallTime: 1471688489.798805] [0.487000] Loading controller: joint_state_controller
[ERROR] [1471688489.800796277, 0.488000000]: Could not load controller 'joint_state_controller' because controller type 'joint_state_controller/JointStateController' does not exist.
[ERROR] [1471688489.800835514, 0.488000000]: Use 'rosservice call controller_manager/list_controller_types' to get the available types
[ERROR] [WallTime: 1471688490.802091] [1.266000] Failed to load joint_state_controller
[INFO] [WallTime: 1471688490.802519] [1.267000] Loading controller: diff_drive_controller
[ERROR] [1471688490.804952548, 1.269000000]: Could not load controller 'diff_drive_controller' because controller type 'diff_drive_controller/DiffDriveController' does not exist.
[ERROR] [1471688490.804998101, 1.269000000]: Use 'rosservice call controller_manager/list_controller_types' to get the available types
[ERROR] [WallTime: 1471688491.806314] [2.029000] Failed to load diff_drive_controller
[INFO] [WallTime: 1471688491.806731] [2.029000] Controller Spawner: Loaded controllers:

このようなエラーが出る場合は以下のパッケージのインストールで解決できます。

sudo apt-get install ros-indigo-ros-controllers

Gazeboのモデルデータ不足

[INFO] [WallTime: 1471687453.480123] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1471687453.483211] [0.000000] Waiting for service /gazebo/spawn_urdf_model
Warning [ModelDatabase.cc:334] Getting models from[http://gazebosim.org/models/]. This may take a few seconds.
[INFO] [WallTime: 1471687453.649846] [0.000000] Controller Spawner: Waiting for service controller_manager/load_controller
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
Error [gazebo.cc:220] Waited 11 seconds for namespaces. Giving up.
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
Error [Node.cc:90] No namespace found
[WARN] [WallTime: 1471687483.877793] [0.000000] Controller Spawner couldn't find the expected controller_manager ROS interface.

このようなエラーが出て、Gazeboのウィンドウが真っ黒な場合、モデルデータが不足しています。
インターネットに接続されていれば自動的にデータをダウンロードできますので、しばらく待ってみてください。
環境によっては数分かかることもあるようです。

worldファイル変換スクリプト

worldファイルに変換するスクリプトを実行する際、zshを使っているとファイルを上書きできない場合があります。
その場合は最終行を以下のように変更したスクリプトを実行することで解決できます。

#!/bin/bash

WORKINGDIR="$HOME/catkin_ws/src/raspimouse_sim/raspimouse_gazebo"

rosrun xacro xacro.py $WORKINGDIR/materials/sample_maze.world.xacro |> $WORKINGDIR/worlds/sample_maze.world

参考文献

タイトルとURLをコピーしました