f:id:kuwamai:20190507205934p:plain 前回は動力と摩擦係数の設定をして、走らせる準備をした。

今回は動力へ速度指令を送るControl packageと、GAZEBOシミュレータを起動するGazebo packageを作成する。

動作環境

Ubuntu16.04
ROS Kinetic Kame
GAZEBO 7.0.0

練習の記録

今回やってることはこちらのリポジトリに公開してます。

下記手順はnavigation_practiceディレクトリで行う前提で話を進める。

Control packageの作成

速度指令値を受け取り、ロボットの車輪を制御するPackageを作成する。

$ catkin_create_pkg mabot_control controller_manager actionlib actionlib_msgs control_msgs sensor_msgs robot_state_publisher

今回作成する対向二輪型ロボットは、受け取った速度指令値を左右のタイヤの角速度に変換する必要がある。幸いその計算を勝手にしてくれるControllerがあるのでありがたく使わせていただく。

diff_drive_controller - ROS wiki

$ mkdir mabot_control/config
$ vim mabot_control/config/controller.yaml

mabot:
  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.34
    wheel_radius : 0.1
    
    # Wheel separation and radius multipliers
    wheel_separation_multiplier: 1.0 # default: 1.0
    wheel_radius_multiplier: 1.0 #default: 1.0

Controllerを起動するlaunchファイルを作成する。

$ mkdir mabot_control/launch
$ vim mabot_control/launch/mabot_control.launch

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

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

Gazebo packageの作成

GAZEBO上にWorldを作成し、ロボットを置くPackageを作成する。

$ catkin_create_pkg mabot_gazebo

Worldは様々な種類があるし自分でも作れるが、とりあえず何もないemptyworldを生成してロボットを置くlaunchファイルを作成する。

$ mkdir mabot_gazebo/launch
$ vim mabot_gazebo/launch/mabot_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 mabot_description)/robots/mabot.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="mabot/robot_description" command="$(find xacro)/xacro --inorder '$(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 mabot -param mabot/robot_description" />

  <!-- ros_control motoman launch file -->
  <include file="$(find mabot_control)/launch/mabot_control.launch" />
</launch>

GAZEBOの起動

作成したlaunchファイルを実行するとGAZEBOが起動する。

$ roslaunch mabot_gazebo mabot_with_emptyworld.launch

f:id:kuwamai:20190507205934p:plain

動作確認

実際に速度指令値を送ってロボットが動作するか確認する。速度指令値は/mabot/diff_drive_controller/cmd_vel topicにgeometry_msgs/Twist形式でmessageを送る。

下記コマンドを実行するとロボットが前進する。x軸がロボットにとって前後方向になる。ちなみに対向二輪型なので左右方向(y軸)や上下方向(z軸)には進まない。

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

f:id:kuwamai:20190508010012g:plain

また、このように入力すると旋回する。z軸が左右旋回方向になる。

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

f:id:kuwamai:20190508005703g:plain

こんな感じで速度指令値をdiff_drive_controllerに送るだけで勝手に左右の車輪をいい感じに動かしてくれる。台車が無事にできたので、センサーを載せたり制御したり色々できるようになった。

クワマイでもできる

クワマイでもわかる

xacroでロボットを書く[3] - ロボットをGAZEBOに置く