CnoidRosプラグイン

1.基本情報

プラグイン名:CnoidRos
依存プラグイン:Grasp

2.概要

PRMやMotionFileで生成した動作をROSで通信する。
ROSノードはpythonスクリプトで書かれており、CnoidRosプラグインとPythonスクリプトはメモリマップドファイルでプロセス間通信を行う。
以下のフォーマットで通信する。
 (命令1);(命令2);.......(命令n)
命令は以下の通り
 goInitial()
 goOff()
 joints[0,0,0,....0]:1.0 (関節角を設定.[]内は関節角列を:のあとは動作時間をあらわしている)
 rhandClose
 lhandClose
 rhandOpen
 lhandOpen

3.動作確認方法

3.1Ubuntuマシン内での動作

3.1.1準備

・ROSのインストール
http://wiki.ros.org/ja/indigo/Installation/Ubuntu を参考にROSをインストールします。
次にHironx/NEXTAGE OPEN APIを以下のコマンドでインストールします。(http://wiki.ros.org/rtmros_nextage/Tutorials/Install%20NEXTAGE%20OPEN%20software%20on%20your%20machine )
 $ sudo apt-get update && sudo apt-get install ros-indigo-rtmros-nextage ros-indigo-rtmros-hironx

・CnoidRos.pyのセットアップ
まずROSのワークスペースを作成します。(http://wiki.ros.org/ja/ROS/Tutorials/InstallingandConfiguringROSEnvironment
 $ mkdir -p ~/catkin_ws/src
 $ cd ~/catkin_ws/src
 $ catkin_init_workspace
 $ cd ~/catkin_ws/
 $ catkin_make

次にワークスペースをOverlayします。
 $ source devel/setup.bash

次にパッケージを作成します
 $ catkin_create_pkg CnoidRos std_msgs rospy

これでCnoidRosパッケージが作成されるのでCnoidRosプラグイン下のCnoidRos.pyをコピーします。
 $ cp "graspPluginのパス"/CnoidRos/ROS/CnoidRos.py ~/catkin_ws/src/CnoidRos/src/

CnoidRos.pyに実行権限を付加します。
 $ chmod a+x ~/catkin_ws/src/CnoidRos/src/CnoidRos.py

3.1.2実行

1.choreonoidを起動します。
2.ターミナルを起動しroscoreを実行

 $ roscore

3. ターミナルを起動しシミュレータを起動
 $ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch

4.ターミナルを起動しCnoidRos.pyを起動
 $ source ~/catkin_ws_devel/setup.bash
 $ rosrun CnoidRos CnoidRos.py

5.choreonoid上で操作。
chorenoidのCnoidRosバーのボタンをクリックするとシミュレータが動作します。

3.2UbuntuとWindowsの通信

3.2.1準備

・Ubuntuマシン
 -ROS2のインストール
  https://github.com/ros2/ros2/wiki/Linux-Install-Binary を参考にインストールを行います。
 -CnoidRosTalker.pyのセットアップ
  CnoidRosTalker.pyをROS2ディレクトリにコピーします。ROS2が~/ros2_installにインストールされたと仮定します。

 $ cp "graspPluginのパス"/CnoidRos/ROS/CnoidRosTalker.py ~/ros2_install/ros2-linux/bin/
 $ chmod a+x ~/ros2_install/ros2-linux/bin/CnoidRosTalker.py

・Windowsマシン
 -ROS2のインストール
 https://github.com/ros2/ros2/wiki/Windows-Development-Setup を参考にソースからビルドします。(バイナリバージョンは正常に動作しませんでした)
 また上記URL(2016/3/10時点)の説明通りではうまくビルドできませんでした。
  ページ中のBuilding the ROS 2 CodeとInstall a binary distribution of OpenCV compatible with Visual Studio 2015の順番を入れ替えてインストールを行ってください。

 ※ROS2インストール時にPython3がインストールされます。python2系が既にインストールされていても、デフォルトでpython3が使用されるようになります。もしデフォルトでpython2系を使用したい場合は環境変数のPATH中のPython3のパスの後にPython2のパスがあるので、Python2のパス,Python3のパス順になるように編集してください。そしてpython3でスクリプトを処理するときは

 > python スクリプトファイル名.py


 > py -3  スクリプトファイル名.py

として実行してください。

 -CnoidRosListener.pyのセットアップ
  CnoidRos/ROS下のCnoidRosListener.py, command_dispatcher.py, nextage_manipulator.pyをC:\dev\ros2\install\binフォルダにコピーします。

3.2.2実行

・Ubuntuマシン
 以下の手順でCnoidRosTalkerを起動します。
 ターミナルを起動します

 $ . ros2_install/ros2-linux/local_setup.bash
 $ CnoidRosTalker.py

・Windowsマシン
 以下の手順でCnoidRosListenerを起動します。
 コマンドプロンプトを管理者権限付きで起動します。
 > cd C:\dev\ros2\install
 > local_setup.bat
 > python bin\CnoidRosListener.py

・choreonoid起動
 Ubuntuマシンでchoreonoideを起動します
 chorenoidのCnoidRosバーのボタンをクリックするとWindowsのコマンドプロンプトにメッセージが表示されます。

・pythonスクリプトについて
 CnoidRosバーのボタンをクリックするとそれに応じてWindowsにメッセージが表示される。Windows側での表示処理はnextage_manipulator.pyに記述されている。
 このファイルを書き換えることで処理内容を変更できる。
 CnoidRosバーのボタンが押されたときに呼ばれるnextage_manipulator.pyの関数を以下に示す。
  [Home] -> goInitial()
  [OffPose]->goOffPose()
  [Move]->動作に応じてrhandOpen(),rhandClose(),lhandOpen(),lhandClose(),setJointAngles(angle,time)が呼ばれる。