urdfのテスト

testbot.urdf

<?xml version="1.0" ?>
<robot name="testbot">
    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0"/>
    </material>
    <material name="orange">
        <color rgba="1.0 0.4 0.0 1.0"/> 
    </material>

    <link name="base"/>
    <joint name="fixed" type="fixed">
        <parent link="base"/>
        <child link="link1"/> 
    </joint>

    <link name="link1"> 
        <collision>
            <origin xyz="0 0 0.25" rpy="0 0 0"/>
            <geometry>
                <box size="0.1 0.1 0.5"/> 
            </geometry>
        </collision> 
        <visual>
            <origin xyz="0 0 0.25" rpy="0 0 0"/> 
            <geometry>
                <box size="0.1 0.1 0.5"/> 
            </geometry>
            <material name="black"/>
        </visual> 
        <inertial>
            <origin xyz="0 0 0.25" rpy="0 0 0"/> 
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial> 
    </link>

    <joint name="joint1" type="revolute"> 
        <parent link="link1"/>
        <child link="link2"/>
        <origin xyz="0 0 0.5" rpy="0 0 0"/> 
        <axis xyz="0 0 1"/>
        <limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/> 
    </joint>

    <link name="link2"> 
        <collision>
            <origin xyz="0 0 0.25" rpy="0 0 0"/> 
            <geometry>
                <box size="0.1 0.1 0.5"/> 
            </geometry>
        </collision>
        <visual>
            <origin xyz="0 0 0.25" rpy="0 0 0" />
            <geometry>
                <box size="0.1 0.1 0.5" />
            </geometry>
            <material name="orange" />
        </visual>
        <inertial>
            <origin xyz="0 0 0.25" rpy="0 0 0"/>
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>

    <joint name="joint2" type="revolute">
        <parent link="link2"/>
        <child link="link3" />
        <origin xyz="0 0 0.5" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <limit effort="30" lower="-2.617" upper="2.617" velocity="1.571" />
    </joint>

    <link name="link3">
        <collision>
            <origin xyz="0 0 0.5" rpy="0 0 0" />
            <geometry>
                <box size="0.1 0.1 1" />
            </geometry>
        </collision>
        <visual>
            <origin xyz="0 0 0.5" rpy="0 0 0" />
            <geometry>
                <box size="0.1 0.1 1" />
            </geometry>
            <material name="black" />
        </visual>
        <inertial>
            <origin xyz="0 0 0.5" rpy="0 0 0" />
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>
    
    <joint name="joint3" type="revolute">
        <parent link="link3"/>
        <child link="link4"/>
        <origin xyz="0 0 1.0" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <limit effort="30" lower="-2.617" upper="2.617" velocity="1.571" />
    </joint>

    <link name="link4">
        <collision>
            <origin xyz="0 0 0.25" rpy="0 0 0" />
            <geometry>
                <box size="0.1 0.1 0.5" />
            </geometry>
        </collision>
        <visual>
            <origin xyz="0 0 0.25" rpy="0 0 0" />
            <geometry>
                <box size="0.1 0.1 0.5" />
            </geometry>
            <material name="orange" />
        </visual>
        <inertial>
            <origin xyz="0 0 0.25" rpy="0 0 0" />
            <mass value="1" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </link>

</robot>
$ urdf_to_graphiz testbot.urdf

f:id:seinzumtode:20201123220541p:plain

launch/testbot.launch

<launch>
<arg name="model" default="$(find testbot_description)/urdf/testbot.urdf" />
<arg name="gui" default="True" />
<param name="robot_description" textfile="$(arg model)" /> 
<param name="use_gui" value="$(arg gui)"/>
<node pkg="joint_state_publisher" type="joint_state_publisher"
  name="joint_state_publisher" />
<node pkg="robot_state_publisher" type="robot_state_publisher"
  name="robot_state_publisher" />
</launch>

起動
$ roslaunch testbot_description testbot.launch
$ rviz

f:id:seinzumtode:20201123222137p:plain


GitHubレポジトリに上がっていた
github.com

上のGitHubをそのまま実行すると以下のエラーがでるので、testbot.launch内のstate_publisherをrobot_state_publisherに変更する
ERROR: cannot launch node of type [robot_state_publisher/state_publisher]: Cannot locate node of type [state_publisher] in package [robot_state_publisher]. Make sure file exists in package path and permission is set to executable (chmod +x)

Rvizに衛星画像タイルを読み込む

以下を使う
github.com

catkin_make installする。

起動

$ roslaunch rviz_satellite demo.launch

OpenstreetmapとTomtomではSatellite imageが降ってこなかった
mapboxを使うといけた

Object URIは以下を使う

https://api.mapbox.com/styles/v1/mapbox/satellite-v9/tiles/256/{z}/{x}/{y}?access_token=pk.eyJ1IjoidG9tbW90dG9tIiwiYSI6ImNraHViMWlwYjFid28ycW1jeTBhbTJscXYifQ.vaFdoLCL6sGgbz4R9Qa0JQ

URIを更新したらRvizの左下のResetボタンを押すと地図がリロードされる

f:id:seinzumtode:20201123175021p:plain

SimulinkとGazeboで協調シミュレーションを行う

ここを参考に。
www.mathworks.com

MatlabでGazeboPluginとコマンドを打つと、GazeboPlugin.zipというフォルダが生成されるので、これをUbuntuに移動して解凍する。
自分は~/Downloadsに配置した。

./buildPlugin.shを実行してビルドする

ビルドが終わると最終的にexport/lib/ 配下に以下のライブラリが生成される。
libGazeboCoSimCustom.so libGazeboCoSimServer.so libmsgproto.so
libGazeboCoSimPlugin.so libGazeboCoSimTransport.so

テスト
exportフォルダに移動する。
$ cd ~/Downloads/GazeboPlugin/export

Gazeboの起動
(初回の起動はWorldのダウンロードで時間がかかる)

$ gazebo ../world/multiSensorPluginTest.world --verbose

Simulinkサンプルを起動する

>> open_system("performCoSimulationWithGazebo")

Simlin上のGazeboのポートを14581に設定する。
ちなみにこの14581というポート番号はWorldファイル(今回はmultiSensorPluginTest.world)で指定されている
f:id:seinzumtode:20201123155226p:plain
f:id:seinzumtode:20201123154107p:plain

world/multiSensorPluginTest.world(ポート番号14581が記載)

(中略)
   <plugin name="GazeboPlugin" filename="lib/libGazeboCoSimPlugin.so"><portNumber>14581</portNumber></plugin>

Parallel desktopで14581をポートフォワーディングした
f:id:seinzumtode:20201123155036p:plain

Simulinkでシミュレーションを実行する
動いた
f:id:seinzumtode:20201123154538p:plain

ROSでCoppeliaSim(旧V-REP)を使う:ライブラリのインストール

ライブラリとしてsimExtROSInterfaceをビルドする必要があるのだが、これが鬼門だった(ドキュメントがない)

simExtROSInterfaceライブラリ本体のレポジトリは以下だが、これのビルドにlibPluginが必要
github.com

理由→普通にビルドすると、CmakeLists.txt仲のcoppeliasim_add_pluginというコマンドが未定義といエラーでコケる。これはsimExtROSInterfaceレポジトリではなく、libPluginレポジトリのcmakeファイル、FindCoppeliaSim.cmakeのなかで定義されている。l

libPluginをどうやってビルドするのかが不明だったが、以下のように試行錯誤したらビルドできた。

事前準備

依存関係のインストール

$ sudo apt install xsltproc
$ python3 -m pip install xmlschema

0. 環境変数の設定(CoppeliasimをUbunutuにインストールした前提。インストールはCoppeliasimをダウンロードしてきて適当な場所に解凍する)
~/.bashrc

export COPPELIASIM_ROOT_DIR=~/Documents/CoppeliaSim/

1. simExtROSInterfaceをクローン(フォルダ名をsim_ros_interfaceに変更しないとビルドが失敗する)

$ cd ~/catkin_ws/src
$ git clone --recursive https://github.com/CoppeliaRobotics/simExtROSInterface.git sim_ros_interface

2. libPluginをクローン

$ cd sim_ros_interface
$ git clone  https://github.com/CoppeliaRobotics/libPlugin

3. フォルダの構成をいじる

$ mv libPlugin/cmake .
$ mv libPlugin/simPlusPlus .
$ mv libPlugin/simStubsGen .
$ mv libPlugin/simTest .
$ rm -rf liPlugin

4. CMakeの編集

$ vim CMakeLists.txt
  1 cmake_minimum_required(VERSION 3.16)
  2 project(sim_ros_interface)
  3 #ここから追加
  4 set (CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};${CMAKE_CURRENT_SOURCE_DIR}/cmake"    )
  5 include(FindCoppeliaSim)
  6 #ここまで追加

5. ビルド

$ cd ~/catkin_ws/src
$ catkin_make


ビルドが成功した。
これでsimExtROSInterface.soができるのかと思ったけど、フォルダのなかに見当たらない。

install.shというスクリプトがあったので、実行してみた

$ chmod +x install.sh
$ ./install.sh

buildというフォルダができたので、この中に入ってみた。
makeファイルがあったので、makeしてみたところ、makeが完了した。

devel/lib/のなかに、libsimExtROSInterface.soができていたので、これをCoppeliaSimフォルダの下のコピーして使うことにする。

と思ったら!

CoppeliaSimのフォルダの中に、すでにlibsimExtROSInterface.soが存在した。。しかもlibsimExtROS2Interface.soというROS2用のライブラリもあった。

GazeboにCADモデルを読み込む

ここを参考に
gazebosim.org

COLLADA形式(.dae)のファイルを読み込める。

CADからSTLでエクスポートし、Blenderでいったん読み込んだ後、.daeでエクスポートするのが良さそう。


1)CADでSTLにエクスポート
f:id:seinzumtode:20201122223132p:plain

2)Blenderにインポートして.daeでエクスポート
f:id:seinzumtode:20201122223222p:plain

3)Gazeboでロード
my_mesh.world

<?xml version="1.0"?>
<sdf version="1.4">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>
    <model name="my_mesh">
      <pose>0 0 0  0 0 0</pose>
      <static>true</static>
      <link name="body">
        <visual name="visual">
          <geometry>
            <mesh><uri>file://my_mesh.dae</uri></mesh>
          </geometry>
        </visual>
      </link>
    </model>
  </world>
</sdf>

Gazebo起動

$ gazebo my_mesh.world

f:id:seinzumtode:20201122222737p:plain