差動二輪ロボットのsdfを作成

http://gazebosim.org/tutorials?tut=ros_gzplugins&cat=connect_ros

今日は上記を見ながら、ロボットを作成していきます。ただし、記述が古いのでそのままだとうまく行きません。
まずは、GazeboからROS2を動かすデモを起動して理解を深めます。
次のコマンドでGazeboを起動します。

gazebo --verbose /opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_diff_drive_demo.world

teleopで操作してみます。

ros2 run teleop_twist_keyboard teleop_twist_keyboard cmd_vel:=/demo/cmd_demo

今回は差動二輪ロボットであるので、Differential Driveを実装します。gazebo_ros_diff_drive_demo.worldの中身をみて、そのまま真似て作成します。以下のような記述をロボットのsdfに追加します。

    <!-- プラグインをロードします。 filenameは共有ライブラリのファイル名です。 -->
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <ros>
        <namespace>/momo</namespace>
      </ros>

      <!-- 左右の車輪のジョイントを指定します。 このジョイントにアクチュエータが追加され、制御可能になります。 -->
      <left_joint>wheel_left_joint</left_joint>
      <right_joint>wheel_right_joint</right_joint>
      <!-- ロボットのオドメトリの中心となるフレームです。 rviz等でロボットの位置を可視化するときにこのフレームを利用します。 -->
      <robot_base_frame>base_link</robot_base_frame>
      <!-- 車輪の間の距離です。 -->
      <wheel_separation>0.540</wheel_separation>
      <!-- 車輪の直径です。ホイールの直径（メートル単位）は、デフォルトで0.15mです。 -->
      <wheel_diameter>0.615</wheel_diameter>
      <!-- ホイールの加速度（rad / s ^ 2）は、デフォルトで0.0 rad / s ^ 2になります。-->
      <max_wheel_acceleration>1.0</max_wheel_acceleration>
      <!-- ホイールが生成できる最大トルク（Nm）は、デフォルトで5Nmです。 -->
      <max_wheel_torque>20</max_wheel_torque>
      <!-- オドメトリフレーム、デフォルトは `odom` -->
      <odometry_frame>odom</odometry_frame>
      <!-- オドメトリを計算するロボットフレーム。デフォルトは `base_footprint`です。 -->
      <robotBaseFrame>base_footprint</robotBaseFrame>
      <!-- オドメトリソース、エンコーダの場合は0、WORLDの場合は1、デフォルトはWORLD -->
      <odometrySource>1</odometrySource>
      <!-- ホイールリンクの変換を公開するにはtrueに設定する。デフォルト設定はfalse。-->
      <publish_wheel_tf>true</publish_wheel_tf>
      <!-- オドメトリを公開するにはtrueに設定する。 -->
      <publish_odom>true</publish_odom>
      <!-- オドメトリの変換を公開するにはtrueに設定する。 -->
      <publish_odom_tf>true</publish_odom_tf>

    </plugin>

では実際に動作させてみます。空のworldに入れてチェックします。

<?xml version="1.0"?>
<sdf version="1.6">
  <world name="default">

    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://sun</uri>
    </include>

    <scene>
      <shadows>false</shadows>
    </scene>

    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
        <view_controller>orbit</view_controller>
        <projection_type>perspective</projection_type>
      </camera>
    </gui>

    <physics type="ode">
      <real_time_update_rate>1000.0</real_time_update_rate>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <ode>
        <solver>
          <type>quick</type>
          <iters>150</iters>
          <precon_iters>0</precon_iters>
          <sor>1.400000</sor>
          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
        </solver>
        <constraints>
          <cfm>0.00001</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
          <contact_surface_layer>0.01000</contact_surface_layer>
        </constraints>
      </ode>
    </physics>

    <include>
      <uri>model://momo</uri>
    </include>

  </world>
</sdf>

実行します。

$ gazebo --verbose momo_ws/src/momo_gazebo/worlds/static_map_plugin_get.world 
Gazebo multi-robot simulator, version 11.5.1
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
Gazebo multi-robot simulator, version 11.5.1
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.86.42
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.86.42
[Msg] Loading world file [/home/hiroakikaneda/momo_ws/src/momo_gazebo/worlds/static_map_plugin_get.world]
[Wrn] [ColladaLoader.cc:1928] Triangle input semantic: 'COLOR' is currently not supported
[Wrn] [ColladaLoader.cc:1928] Triangle input semantic: 'COLOR' is currently not supported
[Wrn] [ColladaLoader.cc:1928] Triangle input semantic: 'COLOR' is currently not supported
[Wrn] [ColladaLoader.cc:1928] Triangle input semantic: 'COLOR' is currently not supported
[Wrn] [ColladaLoader.cc:1928] Triangle input semantic: 'COLOR' is currently not supported
[Wrn] [ColladaLoader.cc:1928] Triangle input semantic: 'COLOR' is currently not supported
[Wrn] [ColladaLoader.cc:1928] Triangle input semantic: 'COLOR' is currently not supported
[Wrn] [ColladaLoader.cc:1928] Triangle input semantic: 'COLOR' is currently not supported
[INFO] [1632923283.195835717] [gazebo_ros_node]: ROS was initialized without arguments.
[INFO] [1632923283.214245446] [momo.differential_drive_controller]: Wheel pair 1 separation set to [0.540000m]
[INFO] [1632923283.214581649] [momo.differential_drive_controller]: Wheel pair 1 diameter set to [0.615000m]
[INFO] [1632923283.216477829] [momo.differential_drive_controller]: Subscribed to [/momo/cmd_vel]
[INFO] [1632923283.217386722] [momo.differential_drive_controller]: Advertise odometry on [/momo/odom]
[INFO] [1632923283.218196407] [momo.differential_drive_controller]: Publishing odom transforms between [odom] and [base_link]
[INFO] [1632923283.218218133] [momo.differential_drive_controller]: Publishing wheel transforms between [base_link], [wheel_left_joint] and [wheel_right_joint]
^C[INFO] [1632924658.173345571] [rclcpp]: signal_handler(signal_value=2)
[Wrn] [Publisher.cc:136] Queue limit reached for topic /gazebo/default/visual, deleting message. This warning is printed only once.
hiroakikaneda@:~$ gazebo --verbose momo_ws/src/momo_gazebo/worlds/empty.world 
Gazebo multi-robot simulator, version 11.5.1
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
Gazebo multi-robot simulator, version 11.5.1
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.86.42
[Msg] Loading world file [/home/hiroakikaneda/momo_ws/src/momo_gazebo/worlds/empty.world]
[INFO] [1632924661.728514279] [gazebo_ros_node]: ROS was initialized without arguments.
[INFO] [1632924661.747010815] [momo.differential_drive_controller]: Wheel pair 1 separation set to [0.540000m]
[INFO] [1632924661.747259540] [momo.differential_drive_controller]: Wheel pair 1 diameter set to [0.615000m]
[INFO] [1632924661.748479147] [momo.differential_drive_controller]: Subscribed to [/momo/cmd_vel]
[INFO] [1632924661.749557110] [momo.differential_drive_controller]: Advertise odometry on [/momo/odom]
[INFO] [1632924661.750442843] [momo.differential_drive_controller]: Publishing odom transforms between [odom] and [base_link]
[INFO] [1632924661.750501497] [momo.differential_drive_controller]: Publishing wheel transforms between [base_link], [wheel_left_joint] and [wheel_right_joint]
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.86.42

teleopで操作します。

$ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=/momo/cmd_vel

This node takes keypresses from the keyboard and publishes them
as Twist messages. It works best with a US keyboard layout.
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

currently:  speed 0.5   turn 1.0 

新型コロナウィルスによる休校とKindle キッズモデル 1,000冊以上の子ども向けの本が読み放題！

世界での感染拡大が懸念される新型コロナウィルス。 今週から私もリモートワークとなり自宅で作業を行っています。たまたま、父親がリモートワークをはじめるのと同じタイミングで学校も臨時休校となり、子どもたちは一足早い春休み気分です。 休校中だとしても、親としては子どもたちにテレビやゲームでダラダラと過ごしてほしくはありません。そこで子どもたちと次の約束をしています。

• 臨時休校中も、学校と同じスケジュールで勉強しましょう。
• 父も就業時間をまもって仕事します。 父の仕事中は邪魔をしないこと。

とはいえ、家では授業を受けられるわけでもなく、プリントをやるだけでは子どもたちもすぐに飽きてしまいます。 図書館で本を借りようにも、肝心の図書館も休館なわけで。そこで、新しく登場したkindleのキッズモデルを購入して渡すことにしました。

https://amzn.to/2vCzJSg ちょうど、1,000冊以上の子ども向けの本を1年間追加料金なしで読み放題なのと、その中に長女の読みたがっているハリー・ポッターシリーズもあったのが決め手になりました。子供二人それぞれに読みたいだろうから、仕方なく2台購入。もちろん1台をアカウントを切り替えながら二人でシェアして使うこともできるのでしょうが、それだと今読みたい！という気持ちを阻害しそうだったのでそれぞれ買いました。小学校3年の下の子のほうは活字よりも図鑑がお気に入りみたい。 設定した子どもの年齢に合った本が表示されるのもマルですね。 内容物はkindle本体とカバー、それに充電用USBケーブル。充電アダプタはついていません。 これはスペースステーションカバー。中は赤色です。 開いてみるとすでに表示されています。そっか、電子ペーパーだから電気がなくても表示されつづけるんでしたね。電源ONを促されます。

設定を終え、早速読み始めてます。これを機会に本を読むようになって欲しいなぁ。まずは興味を持ってくれているのでよしとしましょう。

ROS Wrapper 2.0 for Intel RealSense Devices (build 2.1.0)を D435で使用する

まず情報源ですが、ことのおりやればOKです。ただし今回はタイミング悪く、最新バージョンがリリースされてしまってうまくいかないという罠にはまりましたので、手順を残しておきます。 https://github.com/intel-ros/realsense インストールするものは

1. LibRealsense
2. RealsenseのROSラッパー

の順ですが、先に、「RealsenseのROSラッパー」を確認します。 https://github.com/intel-ros/realsense/releases で、ROSラッパーの条件を確認します。 現在の最新は2.1.0です。 https://github.com/intel-ros/realsense/releases/tag/2.1.0 上記のURLを見ると、2.1.0に必要な libRealSense のバージョンはv2.16.1だと書いてあります。 なので、 https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md を見ながらlibRealSenseをインストールすのですが、この通りやると、現在の最新(執筆時点では2.16.2)がインストールされてしまいます。そこで次のように2.16.1をバージョン指定してインストールします。 ※もし、2.16.2をインストールしてROSラッパーの2.1.0をビルドすると、次のエラーが出ちゃいます。これで3時間は無駄な時間を費やしました。

CMake Error at /usr/lib/x86_64-linux-gnu/cmake/realsense2/realsense2Targets.cmake:84 (message):
 The imported target "realsense2::tm" references the file
 "/usr/lib/x86_64-linux-gnu/libtm.a"
 but this file does not exist. Possible reasons include:
 * The file was deleted, renamed, or moved to another location.
 * An install or uninstall procedure did not complete successfully.
 * The installation package was faulty and contained
 "/usr/lib/x86_64-linux-gnu/cmake/realsense2/realsense2Targets.cmake"
 but not all the files it references.
Call Stack (most recent call first):
 /usr/lib/x86_64-linux-gnu/cmake/realsense2/realsense2Config.cmake:33 (include)
 realsense2_camera/CMakeLists.txt:35 (find_package)
-- Configuring incomplete, errors occurred!
See also "/home/hiroakikaneda/catkin_ws/build/CMakeFiles/CMakeOutput.log".
See also "/home/hiroakikaneda/catkin_ws/build/CMakeFiles/CMakeError.log".

上のようなエラーが出たら、ROSラッパーのバージョンと、それに対応しているlibrealsenseがちゃんとインストールできているかバージョン整合性を確認してみて下さい。 インストールは次の手順で行います。

sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE
sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u
sudo apt-get update
sudo apt-get install -y librealsense2=2.16.1-0~realsense0.88
sudo apt-get install -y librealsense2-dkms
sudo apt-get install -y librealsense2-utils=2.16.1-0~realsense0.88
sudo apt-get install -y librealsense2-dev=2.16.1-0~realsense0.88
sudo apt-get install -y librealsense2-dbg=2.16.1-0~realsense0.88

  次に、ROSラッパー（ROS Wrapper 2.0 for Intel® RealSense™ Devices (build 2.1.0)）をインストールします。 https://github.com/intel-ros/realsense の手順に従うだけです。ただし、私の場合は既にROSがインストールされておりwork spaceも設定済みだったので、以下のように手順を変更しています。

cd ~/catkin_ws/
wget https://github.com/intel-ros/realsense/archive/2.1.0.tar.gz -O realsense-2.1.0.tar.gz
tar xzvf realsense-2.1.0.tar.gz
catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release --source realsense-2.1.0
catkin_make install --source realsense-2.1.0
#ビルドした結果が~/catkin_ws/installに配置されますので、パスを通します。
echo "source ~/catkin_ws/install/setup.bash" >> ~/.bashrc
#installが不完全なので、足りないファイルは手でインストールします。
cp -r realsense-2.1.0/realsense2_camera/cfg install/share/realsense2_camera/
cp -r realsense-2.1.0/realsense2_camera/meshes install/share/realsense2_camera/
cp -r realsense-2.1.0/realsense2_camera/scripts install/share/realsense2_camera/
cp -r realsense-2.1.0/realsense2_camera/urdf install/share/realsense2_camera/
#ビルド後のソースコードは不要なので削除して、ビルドしなおし、realsense-2.1.0フォルダもキレイに消去します
rm -rf realsense-2.1.0
rm -rf build
rm -rf devel
catkin_make clean
catkin_make
#反映
source ~/.bashrc
#installにパス通ったか確認してみる
echo $ROS_PACKAGE_PATH

    これで、realsenseのrosラッパーをビルドして 、~/catkin_ws/installの中にインストールし、パスを通すことができていると思います。 テスト実行してみます。  

sudo apt-get install ros-kinetic-rgbd-launch
roslaunch realsense2_camera rs_camera.launch

macOS high sierra Inkscape 起動しない

inkscape fails to start on macos 10.13. うーん困った。 インクスケープが起動しません。インストール直後の1回だけは起動するのですが、その後起動しなくなります・・・トホホ。 In this case, そのような場合は、アプリケーション、の「その他」にある「ターミナル」を起動し、  

/Applications/Inkscape.app/Contents/MacOS/Inkscape

と打ってやると起動するそうです。 https://www.reddit.com/r/Inkscape/comments/73tcj6/inkscape_fails_to_start_on_macos_1013_high_sierra/ 毎回打ち込むのは面倒なんでオートメーションを使います。

シェルスクリプトに先程のコマンドを打ち込み、デスクトップに保存します。

Cmd+Iで情報を表示します。/Application/Incscape.appを選択し同じようにCmd＋Iで情報を表示します。

アイコンをコピーします。

  これで、デスクトップのIncsapeをクリックすると起動するようになりました。

    以上、macOS High SierraでIncscapeが起動しない時の対処法でした。

UbuntuでApple Magic Trackpadの移動速度を最速にする

こんな感じでトラックパッドの速度を変更できます。 Apple Magic Trackpadが、認識されているトラックパッドの名前です。 最後の１が、マウスの速度。大きくすると遅くなります。 xinput --set-prop "Apple Magic Trackpad" "Device Accel Constant Deceleration" 1 これを自動起動するアプリケーションに登録します。

しかし、速度１にすると、速くて快適なのですが、シビアになりすぎて、領域選択とか出来ません（笑）  

Outdated Kotlin Runtime

Outdated Kotlin Runtimeワーニングが出ました。Intellij ideaの画面ですが、おそらくAndroid Studioも同じでしょう。

 Your version of Kotlin runtime in 'Gradle: org.jetbrains.kotlin:kotlin-stdlib:1.1.3-2' library is 1.1.3-2, while plugin version is 1.1.4-release-IJ2017.2-3.
 Runtime library should be updated to avoid compatibility problems.

ここで、Update Runtimeボタンをクリックしてしてみましたが、次のメッセージが表示されるだけです。

Automatic library version update for Gradle projects is currently unsupported. Please update your build.gradle manually

この場合はbuild.gradleファイルを開き、手動でバージョンを上げます。

 ext.kotlin_version = '1.1.3-2'

となっている所を

ext.kotlin_version = '1.1.4-3'

とします。

最新のバージョンはリポジトリページで見つけることができます。

これでOutdated Kotlin Runtimeワーニングが消えるはずです。

参考： https://stackoverflow.com/questions/43928118/outdated-kotlin-runtime-warning-kotlin-plugin-1-1-2-release-studio2-3-3

Lenovo Phab 2 Pro で Project Tango 入門3

本日はPointCloudのJavaサンプルを見てみましょう。 Android StudioのでInport projectを選択し、java_point_cloud_exampleを選択してOKボタンをクリックします。

ファイル構成は以下のようになっています。

PointCloudActivity.java	Javaによる点群表示サンプル。メインアクティビティです。
PointCloudRajawaliRenderer.java	点群データのレンダラーです。
TouchViewHandler.java	標準的なパン操作とズームタッチ操作の処理を含む、VR設定のトップダウンビューと3人称ビューを追加するヘルパークラスです。
rajawali/FrustumAxes.java	public class FrustumAxes extends Line3D FrustumとAxesの組み合わせを表すプリミティブ（基本的なもの）。錐台（すいだい、英: Frustum）は、錐体から、頂点を共有し相似に縮小した錐体を取り除いた立体図形の事。 Axesとは軸の事。
rajawali/Grid.java	public class Grid extends Line3D 現在のシーンの「床」を表すRajawaliオブジェクト。 これはシーンに配置された静的グリッドで、さまざまなビューにパースペクティブを提供します。
rajawali/PointCloud.java	public class PointCloud extends Points 色を使用して深度センサまでの距離を示すポイントクラウドをレンダリングします。 着色は光のスペクトルに基づいています。最も近い点は赤色で、最も遠くは紫色です。
rajawali/Points.java	public class Points extends Object3D Rajawaliの点のプリミティブ（基本型）です。

それでは詳しくみていきましょう。

PointCloudActivity

onCreateでは、TangoPointCloudManagerを生成しています。また、PointCloudRajawaliRendererクラス（点群データのレンダラー）のインスタンス生成を行なっています。また、DisplayManagerを取得し、画面の回転方向をmDisplayRotationメンバ変数へ保持しています。 onStartでは、bindTangoServiceメソッド内で、Tangoサービスを通常のAndroidサービスとして初期化します。 onStopでは、OpenGLスレッドまたはUIスレッドで使用されているサービスが切断するまで同期します。Tangoコールバックスレッドでこの同じオブジェクトに対してロックしないでください。すべてのTangoコールバックコールが終了するまで、Tango.disconnectはここでブロックされます。Tangoのコールバックスレッドでこのオブジェクトをロックすると、デッドロックが発生します。 bindTangoServiceではTangoサービスを通常のAndroidサービスとして初期化します。 onPauseでmTango.disconnect（）を呼び出すので、これはTango Serviceのバインドを解除するので、onResumeが呼び出されるたびに新しいTangoオブジェクトを作成する必要があります。サービスがOpenGLスレッドまたはUIスレッドで使用されているときに切断に対して同期させます。 setupTangoConfigはbindTangoService内で呼ばれるメソッドで、Tango設定オブジェクトを設定します。 この呼び出しを行う前に、mTangoオブジェクトが初期化されていることを確認してください。これはデフォルトの設定を使用して、さらに深度センサーを追加します。 startupTangoもbindTangoServiceメソッド内で呼ばれるメソッドで、Tangoサービスのコールバックリスナーを設定し、Tango接続後に必要なその他のパラメータを取得します。ポイントクラウドとTangoのイベントとポーズの更新をリッスンします。 setupRendererはonCreate時に呼ばれるメソッドで、Rajawaliサーフェイスビューとそのレンダラーを設定します。 これは、理想的にはonCreateで1回だけ呼び出されます。 setupTangoUxAndLayoutもonCreate時に呼ばれるメソッドでTangoUXの設定とそのリスナーの設定を行います。イベントは、例外条件が観測されたときに「DETECTED」とみなされ、根本原因が解決されたときに「RESOLVED」とみなされます。 画面のFirstボタンを押すとカメラが現在位置に設定されます。いわゆる一人称視点になります。 Thirdボタンを押すとカメラを現在位置に設定し、Z軸に45度回転し、更にカメラをY軸に45度回した位置に固定します。いわゆる第三者視点です。 Topボタンを押すとカメラを現在位置の上部に固定し上からの俯瞰表示にします。これらボタンの処理はTouchViewHandler内で行われます。 画面上には、getAveragedDepthメソッドで計算した平均深度とポイントクラウドの数が表示されています。

PointCloudRajawaliRenderer

ポイントクラウドのデータはXYZC形式で提供されるため、ポイントあたり4つのfloatを示します。 レンダリングされたポイントクラウドを更新します。 このためには、クラウドデータを取得した時点でポイントクラウドデータとデバイスポーズが必要です。 注：これは、OpenGLレンダリングスレッドから呼び出す必要があります。 Rajawaliは左手系を使用するため、共役四元数が必要です。（共役四元数はこちらのサイトの説明がわかりやすい） 現在のデバイスポーズに関する情報を更新します。 注：これは、OpenGLレンダリングスレッドから呼び出す必要があります。 Rajawaliは四元数に対して左手系を使用するため、共役四元数が必要です。

TouchViewHandler

標準的なパン操作とズームタッチ操作の処理を含む、VR設定のトップダウンビューと3人称ビューを追加するヘルパークラスです。  

実行する

では実行してみましょう。実行すると次の画像のような点群表示が得られます。リアルタイムでかなり高速に点群表示できている事が確認できます。ただ、点群の更新はちょっと遅れるようですね。そんなに応答性が良い感じではありませんが、スマホで使うには申し分ない速度でしょう。