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

Gazeboで走るRasPiMouse　その2

2016.08.16

こんにちは。佐藤です。

前回はGazeboでガソリンスタンドのモデルデータを表示して、RasPiMouse（の一部）を表示するところまで行いました。

概要

今回は慣性行列を定義し、Gazebo上で正しく表示できることを確認します。
その後、シミュレータ上で走らせてみます！

慣性行列の計算方法

Inventorでそれぞれのパーツについてきちんと質量を設定した後、アセンブリのiPropertyを参照すると慣性行列を確認することができます。
スクリーンショット 2016-08-14 23.39.55
スクリーンショット 2016-08-14 23.40.38

ちなみに、rvizで起動するためのxacroファイルを用意していた際は手動で算出していました。
Raspberry Pi MouseのURDFモデル作成　その2 | RT MicroMouse

Raspberry Pi MouseのURDFモデル作成　その2
こんにちは！佐藤です。 今回は前回の続きでURDFのモデルの作成を行っていきます。 前回はとりあえず動かすことを目的としてパッケージを用意して、URDFを記述するところまで行いました。 URDFファイルについて はじめに記述したURDFファ...
rt-net.jp
2016.08.03

今回必要なデータに関してはすでにGitHubに公開してありますのでこちらをご参照ください。
https://github.com/rt-net/RaspberryPiMouse/tree/master/cad_data

ボディとホイールそれぞれをこのように定義しました。
（その他の詳細な部分については後ほどGitHubに公開する予定ですのでお待ちください。）

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find raspimouse_description)/urdf/base/base.gazebo.xacro"/>
  <xacro:property name="M_PI" value="3.1415926535897931" />
  <xacro:property name="velocity_scale_factor" value="1.0"/>
  <xacro:macro name="base" params="parent *joint_origin">
    <joint name="base_joint" type="fixed">
      <insert_block name="joint_origin"/>
      <parent link="${parent}"/>
      <child link="base_link"/>
    </joint>
    <link name="base_link">
      <visual>
        <geometry>
          <mesh filename="package://raspimouse_description/meshes/dae/base/RasPiMouse_base.dae"/>
        </geometry>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://raspimouse_description/meshes/dae/base/RasPiMouse_base.dae"/>
        </geometry>
      </collision>
      <inertial>
        <origin xyz="-0.005075 -0.000427 0.029879"/>
        <mass value="0.7186"/>
        <inertia ixx="0.001238161833" ixy="0.000000904986" iyy="0.001368529369" ixz="0.000100297310" iyz="0.000022118177" izz="0.000784319117"/>
        <!-- <inertia ixx="0.0012" ixy="0.0" iyy="0.0014" ixz="0.0001" iyz="0.0" izz="0.0008"/> -->
      </inertial>
    </link>
  </xacro:macro>
</robot>

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find raspimouse_description)/urdf/wheel/wheel.transmission.xacro"/>
  <xacro:include filename="$(find raspimouse_description)/urdf/wheel/wheel.gazebo.xacro"/>
  <property name="wheel_radius" value="0.0239"/>
  <property name="wheel_length" value="0.01"/>
  <property name="wheel_mass" value="0.0113"/>
  <xacro:macro name="wheel" params="prefix parent *joint_origin *joint_axis">
  <joint name="${prefix}_wheel_joint" type="continuous">
    <insert_block name="joint_origin"/>
    <parent link="${parent}"/>
    <child link="${prefix}_wheel"/>
    <insert_block name="joint_axis"/>
  </joint>
  <link name="${prefix}_wheel">
    <visual>
      <geometry>
        <mesh filename="package://raspimouse_description/meshes/dae/wheel/RasPiMouse_wheel.dae"/>
      </geometry>
    </visual>
    <collision>
      <!-- <insert_block name="joint_origin"/> -->
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
          <!-- <mesh filename="package://raspimouse_description/meshes/dae/wheel/RasPiMouse_wheel.dae"/> -->
        </geometry>
    </collision>
      <inertial>
        <origin xyz="-0.000107 0 0.002875"/>
        <mass value="${wheel_mass}"/>
        <inertia ixx="0.000002208187" ixy="0.000000000001" iyy="0.00000218412" ixz="0.000000004787" iyz="0.000000000003" izz="0.000004078299"/>
      </inertial>
  </link>
  </xacro:macro>
</robot>

launchファイルの作成

前回作成したraspimouse_gazeboパッケージの中で作業します。

今まではガソリンスタンドを用意したあと、

$ roslaunch raspimouse_gazebo test.launch

spawnするためのurdfファイルを用意（更新）し、

rosrun xacro xacro `rospack find raspimouse_description`/robots/raspimouse.urdf.xacro > `rospack find raspimouse_description`/robots/raspimouse.urdf

それからspawnしていました。

rosrun gazebo_ros spawn_model -file `rospack find raspimouse_description`/robots/raspimouse.urdf -urdf -x 0 -y 0 -z 1 -model raspimouse

正常にspawnできれば最初のtest.launchを起動した方のTerminalで次のような表示が出ています。

[ INFO] [1471184780.056270368, 595.117000000]: Loading gazebo_ros_control plugin
[ INFO] [1471184780.056472361, 595.117000000]: Starting gazebo_ros_control plugin in namespace: raspimouse
[ INFO] [1471184780.058001008, 595.117000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.

毎回起動するのも手間がかかりますので、新たにlaunchファイルを用意します。
今回は何もないempty_worldの中で呼び出してみたいと思います。
raspimouse_with_emptyworld.launchとして保存しました。

<?xml version="1.0"?>
<launch>
  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="model" default="$(find raspimouse_description)/robots/raspimouse.urdf.xacro"/>
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="raspimouse/robot_description" command="$(find xacro)/xacro.py '$(arg model)'" />
  
  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -model raspimouse -param raspimouse/robot_description"/>
</launch>

これで

$ roslaunch raspimouse_gazebo raspimouse_with_emptyworld.launch

で起動できるようになりました。
Screenshot from 2016-08-15 19:10:56

では、ros_controlを定義して、まずはチュートリアルで出てきた亀のように制御できるか確認してみます。

新しいパッケージを作成します。

$ cd ~/catkin_w/src
$ catkin_create_pkg raspimouse_control controller_manager actionlib actionlib_msgs control_msgs sensor_msgs robot_state_publisher
$ cd raspimouse_control
$ mkdir config launch

confingディレクトリの中にcontroller.yamlを以下のように保存しました。

raspimouse:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  diff_drive_controller:
    type        : "diff_drive_controller/DiffDriveController"
    left_wheel  : 'left_wheel_joint'
    right_wheel : 'right_wheel_joint'
    publish_rate: 50               # default: 50
    pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

    # Wheel separation and diameter. These are both optional.
    # diff_drive_controller will attempt to read either one or both from the
    # URDF if not specified as a parameter
    wheel_separation : 0.085
    wheel_radius : 0.024

    # Wheel separation and radius multipliers
    wheel_separation_multiplier: 1.0 # default: 1.0

launchディレクトリの中にraspimouse_control.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"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager"
    type="spawner" ns="raspimouse" 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" ns="/raspimouse">
  </node>
</launch>

最後に、raspimouse_gazeboディレクトリの中にあるlaunch/raspimouse_with_emptyworld.launchの</launch>の前に1行付け足します。

<include file="$(find raspimouse_control)/launch/raspimouse_control.launch"/>

再びGazeboを起動します。

$ roslaunch raspimouse_gazebo raspimouse_with_emptyworld.launch

そして、コマンドを送ってみます。

rostopic pub -1 /raspimouse/diff_drive_controller/cmd_vel geometry_msgs/Twist  '{linear:  {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}'

20160815_3

動きました！！！
ホイールの回転方向が片方逆になってしまっていますが、無事に動かすことができました！

次回はホイールの回転方向を修正して、ros_controlについて書いていきます。

補足

collisionタグについて

linkタグの中のcollisionタグで衝突検出領域を定義することができます。
rvizでモデルを参照できるようにxacroファイルを用意していた際には未定義でしたが、Gazeboでは必要です。
Raspberry Pi MouseのURDFモデル作成　その2 | RT MicroMouse
https://rt-net.jp/mobility/archives/3191

今回、ここの定義を適当に済ませてしまっていたため、ボディが浮いてしまうという困った自体に直面してしまいました。

Screenshot from 2016-08-15 18:31:00

    <collision>
      <!-- <insert_block name="joint_origin"/> -->
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
          <!-- <mesh filename="package://raspimouse_description/meshes/dae/wheel/RasPiMouse_wheel.dae"/> -->
        </geometry>
    </collision>

今はコメントアウトしてあるjoint_originですが、当初はコメントアウトしてなかったのでずいぶんと変な位置に衝突検出領域が定義されていました。
片側のホイールを外した状態で重心を偏らせ、シミュレーションを行うと、非常によくわかります。

20160815_1

回転軸方向から見て明らかに円とは異なる形状の何かがあるように見えます。
meshを直接指定することもできるようでしたのでその方法もありかと思います。

参考文献

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