1. はじめに

　前回の記事^[1]では、Docker上で構築したROS Noetic環境において、HuskyというロボットにマニピュレーターUR３を搭載し、その先端についているグリッパーで物品を持ち上げるシミュレーションを試みることができました。
　このシリーズでは、Oyediranらの論文^[2]を参考に、Huskyにマニピュレーターを搭載し、フレームなどの資材を把持・運搬する事例を再現することを目指しています。Oyediranらの論文では、マニピュレーターを搭載したHuskyが自律走行をして作業しています。

そこで、本記事ではロボットHusky + マニピュレータUR3で自律走行を試したことについて紹介したいと思います。

2. 実行環境

CPU: CORE i7 7th Gen
メモリ: 32GB
GPU: GeForce RTX 2070
OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
Docker内で構築したROS Noetic環境（シミュレーションした環境は^[3]と^[4]で構築した環境です。構築手順はそちらの記事をご参照ください。）

3. 実行手順

シミュレーションを実行する環境としては、私の持っているPCではあまり大きい環境はつくれないので、よくシミュレーション環境として用いられるTurtleBot3 Houseでシミュレーションをすることにしました。そのため以下の手順では、TurtleBot3 Houseを取り込むところから始めています。

3.1. TurtleBot3 House の導入（Gazeboワールド）

3.1.1. TurtleBot3 シミュレーションのセットアップ

terminal(1)

sudo apt-get update
sudo apt-get install ros-noetic-turtlebot3-gazebo
sudo apt-get install ros-noetic-turtlebot3-navigation

3.1.2. 環境変数の設定

terminal(1)

echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc
source ~/.bashrc

3.2. TurtleBot3 House ワールドを起動して確認（Husky未使用）

terminal(1)

roslaunch turtlebot3_gazebo turtlebot3_house.launch

この状態でGazeboが起動できたので、環境ファイルは正しく読み込めていることを確認できました。

3.3. TurtleBot3 House ワールドに Husky+UR3 を読み込む

3.3.1. シミュレーション環境を起動させる`husky_in_tb3_house.launch`の作成

~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gazebo/launch内のhusky_ur3_HRI_lab2.launchをコピーし、~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gazebo/launch内に以下のhusky_in_tb3_house.launchを保存しました。

husky_in_tb3_house.launch

<?xml version="1.0"?>

<launch>

  <arg name="laser_enabled" default="true"/>
  <arg name="camera_h_enabled" default="true"/>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_house.world"/> #ここを修正
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>
  

  <include file="$(find husky_ur3_gazebo)/launch/spawn_husky_tb3house_world.launch"> #ここを修正
    <arg name="laser_enabled" value="$(arg laser_enabled)"/>
    <arg name="camera_h_enabled" value="$(arg camera_h_enabled)"/>
  </include>
  
  <gazebo>
    <plugin name="link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/>　#ここを修正
  </gazebo>

</launch>

3.3.2. ロボットHusky + マニピュレータUR3を出現させる`spawn_husky_tb3house_world.launch` の作成

~/catkin_ws/src/husky_ur3_simulator/husky_ur3_gazebo/launch内のspawn_husky.launchをコピーしhusky_ur3_gazebo/launch内に以下のspawn_husky_tb3house_world.launchを保存しました。

スポーン位置の初期値を変えただけです。

spawn_husky_tb3house_world.launch

<?xml version="1.0"?>
<launch>

  <arg name="multimaster" default="false"/>

  <arg name="robot_namespace" default="/"/>

  <arg name="x" default="-3.0"/>　#ここを修正
  <arg name="y" default="1.0"/>　#ここを修正
  <arg name="z" default="0.0"/>
  <arg name="yaw" default="0.785"/>　#ここを修正

  <arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED false)"/>
  <arg name="camera_h_enabled" default="$(optenv HUSKY_UR5_ENABLED false)"/>
  <arg name="urdf_extras" default="$(optenv HUSKY_URDF_EXTRAS)"/>


  <group ns="$(arg robot_namespace)">

    <group if="$(arg multimaster)">
      <include file="$(find husky_ur3_gazebo)/launch/description.launch" >
        <arg name="robot_namespace" value="$(arg robot_namespace)"/>
        <arg name="laser_enabled" default="$(arg laser_enabled)"/>
       	<arg name="camera_h_enabled" default="$(arg camera_h_enabled)"/>
        <arg name="urdf_extras" default="$(arg urdf_extras)"/>
      </include>
        
      <include file="$(find multimaster_launch)/launch/multimaster_gazebo_robot.launch">
        <arg name="gazebo_interface" value="$(find husky_ur3_gazebo)/config/gazebo_interface.yaml" />
        <arg name="robot_namespace" value="$(arg robot_namespace)"/>
      </include>

      <!-- For multimaster bringup, need to load the controller config -->
      <rosparam command="load" file="$(find husky_ur3_gazebo)/config/control.yaml" />
    </group>

    <!-- For single master bringup, run robot control on the gazebo master -->
    <!-- Note that husky_description/description.launch is already included in husky_control/control.launch. -->
    <group unless="$(arg multimaster)">
      <include file="$(find husky_ur3_gazebo)/launch/control.launch">
        <arg name="multimaster" value="$(arg multimaster)"/>
        <arg name="laser_enabled" value="$(arg laser_enabled)"/>      
	      <arg name="camera_h_enabled" value="$(arg camera_h_enabled)"/>
        <arg name="urdf_extras" value="$(arg urdf_extras)"/>
      </include>
    </group>
    


    <group if="$(arg camera_h_enabled)">

      <!-- Include poincloud_to_laserscan if simulated Kinect is attached -->
      <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan" output="screen">

          <remap from="cloud_in" to="camera/depth/points"/>
          <remap from="scan" to="scan"/>
          <rosparam>
              target_frame: base_link # Leave empty to output scan in the pointcloud frame
              tolerance: 1.0
              min_height: 0.05
              max_height: 1.0

              angle_min: -0.52 # -30.0*M_PI/180.0
              angle_max: 0.52 # 30.0*M_PI/180.0
              angle_increment: 0.005 # M_PI/360.0
              scan_time: 0.3333
              range_min: 0.45
              range_max: 4.0
              use_inf: true

              # Concurrency level, affects number of pointclouds queued for processing and number of threads used
              # 0 : Detect number of cores
              # 1 : Single threaded
              # 2->inf : Parallelism level
              concurrency_level: 1
          </rosparam>
      </node>

    </group>

    <!-- Spawn robot in gazebo -->
    <node name="spawn_husky_model" pkg="gazebo_ros" type="spawn_model"
        args="-x $(arg x)
              -y $(arg y)
              -z $(arg z)
              -Y $(arg yaw)
              -unpause
              -urdf
              -param robot_description
              -model husky" />

  </group>
  
</launch>

3.3.3. ビルドと実行

本当は不要だったかもしれませんが、以下のコマンドで一旦ビルドしてエラーが出ないか確認しました。ここでは特段エラーなく環境が読み込めました。

terminal(1)

cd ~/catkin_ws
catkin_make
source devel/setup.bash
roslaunch husky_ur3_gazebo husky_in_tb3_house.launch

3.4. 走行用地図の作成

3.4.1. マッピング起動とエラー

次に走行用地図を生成しようとして以下のコマンドを実行したのですが、

terminal(1)

cd ~/catkin_ws
source devel/setup.bash
roslaunch husky_ur3_navigation husky_ur3_mapping.launch

以下のエラーが出ました。

terminal(1)

ERROR: cannot launch node of type [hector_mapping/hector_mapping]: hector_mapping

というエラーがでました。
Hector Mappingが未インストールだったようでしたので、以下のコマンドでインストールしました。

terminal(1)

sudo apt-get install ros-noetic-hector-slam

3.4.2. 再度マッピング起動

以下のコマンドで再度マッピング機能を起動しました。

terminal(1)

cd ~/catkin_ws
source devel/setup.bash
roslaunch husky_ur3_navigation husky_ur3_mapping.launch

3.4.3. Rvizの表示設定

またマッピングは以下のコマンドでrvizを実行して作業をしました。

terminal(1)

cd ~/catkin_ws
source devel/setup.bash
rviz

なお、デフォルトのrvizの状態では何も表示されません。そのため以下のようにtopicを追加し、rviz上で地図が可視化されました。

Fixed Frame → map
Map → /map
TF と RobotModel も追加するとロボットの現在地が分かって便利でした

3.4.4. Huskyの手動移動

また地図作成の際は自律走行させず、以下コマンドでteleopを立ち上げ手動操作をしつつ走行用地図を生成しました。

terminal(2)

cd ~/catkin_ws
source devel/setup.bash
rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/cmd_vel

or

rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/husky_velocity_controller/cmd_vel

3.4.5. 地図の保存

パスを指定して、地図を保存しようとしましたが、うまくいかなかったので、以下のように保存先まで移動してコマンドを実行しました。

terminal(3)

cd ~/catkin_ws/src/husky_ur3_simulator/husky_ur3_navigation/maps
rosrun map_server map_saver -f tb3_house_map

そして生成された地図は以下になります。

3.5. 自律走行（作成した地図を使ってAMCL）するためのlaunchファイル作成

既存の~/catkin_ws/src/husky_ur3_simulator/hhusky_ur3_navigation/launch/husky_ur3_in_HRI_lab_amcl.launchをコピーして、以下のhusky_ur3_in_tb3_house_amcl.launchを作成し、~/catkin_ws/src/husky_ur3_simulator/hhusky_ur3_navigation/launch/に保存しました。

husky_ur3_in_tb3_house_amcl.launch

<?xml version="1.0"?>
<launch>
  <arg name="test_code" default="false"/>

  <!-- Run the map server -->
  <!--arg name="map_file" default="$(find husky_ur3_navigation)/map/gaitech_world.yaml"/-->
  <arg name="map_file" default="$(find husky_ur3_navigation)/map/tb3_house_map.yaml"/> #ここを変更
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!--- Run AMCL -->
  <include file="$(find husky_ur3_navigation)/launch/amcl.launch" />
  <!--include file="$(find husky_ur3_navigation)/launch/amcl_test_code.launch" /-->

  <!--- Run Move Base -->
  <include file="$(find husky_ur3_navigation)/launch/move_base.launch" />
  <!--include file="$(find husky_ur3_navigation)/launch/move_base_test_code.launch" /-->

</launch>

ここまで出来たら、一旦以下のコマンドでビルドを実行しました。

terminal(1)

cd ~/catkin_ws
catkin_make
source devel/setup.bash

3.6. シミュレーション環境起動の手順

シミュレーション環境起動は以下の手順んで行いました。

ロボットHusky + マニピュレータUR3をGazeboで起動

terminal(1)

cd ~/catkin_ws
source devel/setup.bash
roslaunch husky_ur3_gazebo husky_in_tb3_house.launch

MoveIt（モーションプランニング）＋RVizを起動

terminal(2)

cd ~/catkin_ws
source devel/setup.bash
roslaunch husky_ur3_gripper_moveit_config Omni_control.launch

自律移動機能起動（AMCL）

terminal(3)

cd ~/catkin_ws
source devel/setup.bash
roslaunch husky_ur3_navigation husky_ur3_in_tb3_house_amcl.launch

4. シミュレーション結果

シミュレーションした結果が以下の動画になります。rviz上で行先を指定し、ロボットHusky + マニピュレータUR3を自律走行させることができました。

5. まとめ

公開されているリポジトリを活用し、Docker上で構築したROS Noetic環境において、HuskyというロボットにマニピュレーターUR３を搭載し、デフォルトとは異なるシミュレーション環境下において、走行用地図の生成と自律走行をすることができました。

今回成功したことは、以下のとおりです。

TurtleBot3 House ワールドにロボットHusky + マニピュレータUR3 を読み込み ✅
Hector SLAM による地図生成 ✅
map_server による保存 ✅
AMCL＋Move Base による自律移動 ✅

今後も、Oyediranらの論文^[2]を再現を目指して取り組んでいきます。次回は、論文で運ばれていたドアや窓枠などの物体を、Gazebo Link Attacher を用いて把持（正確には「アタッチ」）し、その状態で自律走行ができるかについて検証をする予定です。

参考記事やサイト

[1]Docker上で構築したシミュレーション環境で、ロボットHusky + マニピュレータUR3 を用いた物品の持ち上げを試みてみた（Gazebo Link Attacher 使用） ⬅️ 戻る
[2]Integration of 4D BIM and Robot Task Planning: Creation and Flow of Construction-Related Information for Action-Level Simulation of Indoor Wall Frame Installation　⬅️ 戻る
[3]Docker上で構築したシミュレーション環境で、ロボットHusky + マニピュレータUR3 を用いた物品の把持を試みてみた（ROS Noetic環境）　⬅️ 戻る
[4]Docker でロボットHusky にマニピュレータを搭載して動かしてみた（ROS Noetic環境）　⬅️ 戻る