ros japan users group meetup 03

第三回ROS勉強会 @東京

２０１４年8月30日　千葉工業大学　工学部　４年　ロボット設計・制御研究室　前川大輝

目次

1. 自己紹介2. 今日やること３. NEXTAGEについて４. 下準備5. シミュレータ環境のテスト6. MoveIt! + RViz7. MoveIt!のPython API8. 目標値をトピックでやりとりする

自己紹介

■千葉工業大学のロボカップ開発チーム「CITBrains」　のリーダー

■教育用マイコンボードの開発プロジェクトのリーダー　→　ソフトウェアのライブラリ開発（２０１１年～２０１２年）

■農場用メータの自動読み取り技術　→　企業と共同開発（２０１３年11月～２０１４年8月）

■今年からつくばチャレンジの開発に参加

■この勉強会を含む、OSSの活動

ロボカップ世界大会の様子

ブラジルで行われたロボカップ世界大会２０１４の成績

■テクニカルチャレンジ1位■4on4（トーナメント形式）優勝■ベストヒューマノイド1位（ルイ・ヴィトン・カップ）

今日やること

13：00 ～１５：50　NEXTAGEで学ぶMoveIt!入門（前川）

１６：００～１７：50　NEXTAGEのtfとカメラ(近藤)

１８：３０～ 20：30　懇親会　鳥一新宿西口店

次回のNEXTAGEハッカソンに繋がる話をします

＃ついでにROSに関する基礎的なことも

前回のNEXTAGEハッカソンの様子

NEXTAGEについて

※詳しくはNEXTAGEの解説スライド（第2回ROS勉強会）https://speakerdeck.com/youtalk/nextage-openwoshi-tutarospuroguramingu

ハードウェアの構成

ソフトウェアの構成

下準備

最低限必要なセットアップを行う

■rosdepの初期設定を行っていない場合＄ sudo rosdep init＄ rosdep update

■ROSの環境変数を追加するコマンドをbashrcに追記していない場合

＄ echo “source /opt/ros/hydro/setup.bash” >> ~/.bashrc

■ワークスペースを作成していない場合公式ドキュメント： http://wiki.ros.org/catkin/Tutorials/create_a_workspace

$ source /opt/ros/hydro/setup.bash$ mkdir -p ~/catkin_ws/src$ cd ~/catkin_ws/src$ catkin_init_workspace$ cd ~/catkin_ws/$ catkin_make$ echo “source ~/catkin_ws/devel/setup.bash” >> ~/.bashrc$ source ~/.bashrc

http://wiki.ros.org/catkin/Tutorials/create_a_workspace

シミュレータ環境のテスト

■必要なパッケージのインストール

＄ sudo apt-get install ros-hydro-rtmros-nextage ros-hydro-rtshell-core

■シミュレータの起動などに使うコマンドのエイリアスに関する設定$ echo "source `rospack find openrtm_tools`/scripts/rtshell-setup.sh" >> ~/.bashrc$ source ~/.bashrc

■NEXTAGEのシミュレータを起動

＄ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch

MoveIt! + RViz

RVizとMoveIt!を使ってグラフィカルなモーションプランニング環境を構築する

MoveIt!の概要

MoveIt!のユーザーインタフェース

■move_groupが提供する機能にアクセスする方法　→　move_group_interfaceパッケージを用いてC＋＋で書く　→　moveit_commanderパッケージを用いてPythonで書く　→　Motion Planningプラグインを用いてRVizでグラフィカル　　　に制御

■シミュレータを起動した状態で以下のコマンドを実行

＄ roslaunch nextage_moveit_config moveit_planning_execution.launch

目標値をグラフィカルに入力できる状態でRVizが起動

■球体や矢印をマウスで動かして目標値を変更　→　プランニング結果がリアルタイムで表示される

■Executeボタンを押してプランを実行

■プランを実行するとMoveGroupActionGoalメッセージ　が発行される　→　シミュレータ（または実機）のアームが動作

■Planning Groupをright_armに変更　→　右腕も操作できるようになる

■干渉している場合二つのリンクが赤く表示される　→　プランを実行できない

■Planned PathのLoop Animationを有効に　→　開始状態から終了状態までのアームの軌跡を　　　繰り返し表示

■Show Tailを有効にすると経路計画を可視化

MoveIt!のPython API

MoveIt!のPython APIを使ってNEXTAGEを動作させる

　＃rospyの解説も含む

MoveIt!のユーザーインタフェース

■move_groupが提供する機能にアクセスする方法　→　move_group_interfaceパッケージを用いてC＋＋で書く　→　moveit_commanderパッケージを用いてPythonで書く　→　Motion Planningプラグインを用いてRVizでグラフィカル　　　に制御

■パッケージの作成$ cd ~/catkin_ws/src$ catkin_create_pkg nextage_moveit_planning_execution moveit_commander nextage_ros_bridge nextage_moveit_config rospy

$ ls nextage_moveit_planning_execution

CMakeLists.txt package.xmlsrc/

パッケージ名

依存パッケージ

■package.xmlcatkinに対応したパッケージのルートフォルダにある

・・・ <buildtool_depend>catkin</buildtool_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_moveit_config</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_moveit_config</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend>・・・

雛形は自動生成されるので必要なものを後で追記する

■package.xmlの例<?xml version="1.0"?><package> <name>hironx_tutorial</name> <version>0.1.1</version> <description>Sample / demo / tutorial programs for <a href = "http://wiki.ros.org/hironx_ros_bridge">Hiro</a> and <a href = "http://wiki.ros.org/rtmros_nextage">NEXTAGE OPEN (NXO)</a> robot by Kawada Industries. All sample code that works on Hiro robot should work also on NEXTAGE OPEN. The opposite is, however, not always true (e.g. gripper operation is different in NXO). </description> <maintainer email="[email protected]">Isaac Saito</maintainer> <author email="[email protected]">Isaac IY Saito</author> <author email="[email protected]">Daiki Maekawa</author> <license>BSD</license> <license>MIT</license> <url type="website">http://wiki.ros.org/hironx_tutorial</url> <url type="website">http://docs.ros.org/hydro/api/hironx_tutorial/html/</url> <url type="repository">https://github.com/tork-a/hironx_tutorial</url> <url type="bugtracker">https://github.com/tork-a/hironx_tutorial/issues</url> <buildtool_depend>catkin</buildtool_depend> <build_depend>geometry_msgs</build_depend> <build_depend>leap_motion</build_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <build_depend>tf</build_depend> <run_depend>hironx_ros_bridge</run_depend> <run_depend>geometry_msgs</run_depend> <run_depend>leap_motion</run_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend> <run_depend>tf</run_depend> <export> <rosdoc config="rosdoc.yaml" /> </export> </package>


■パッケージの名前 <name>hironx_tutorial</name>

■バージョン <version>0.1.1</version>


　■パッケージの説明

　<description>Sample / demo / tutorial programs for <a href = "http://wiki.ros.org/hironx_ros_bridge">Hiro</a> and <a href = "http://wiki.ros.org/rtmros_nextage">NEXTAGE OPEN (NXO)</a> robot by Kawada Industries. All sample code that works on Hiro robot should work also on NEXTAGE OPEN. The opposite is, however, not always true (e.g. gripper operation is different in NXO). </description>


■作成者のメールアドレスと名前　<maintainer email="[email protected]">Isaac Saito</maintainer> <author email="[email protected]">Isaac IY Saito</author> <author email="[email protected]">Daiki Maekawa</author>■ライセンス <license>BSD</license> <license>MIT</license>


■リンク付け　<url type="website">http://wiki.ros.org/hironx_tutorial</url> <url type="website">http://docs.ros.org/hydro/api/hironx_tutorial/html/</url> <url type="repository">https://github.com/tork-a/hironx_tutorial</url> <url type="bugtracker">https://github.com/tork-a/hironx_tutorial/issues</url>

■依存パッケージのインストール

<buildtool_depend>catkin</buildtool_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_moveit_config</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_moveit_config</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend>

$ rosdep install nextage_moveit_planning_execution

package.xml

■moveit_commanderのサンプルコード作成

$ vi scripts/moveit_command_sender.py

■スクリプトをPythonとして実行させる（ROSノードに必須）#!/usr/bin/env python

■必要なモジュールをインポート

import moveit_commanderimport rospyimport geometry_msgs.msgimport copy

■セットアップ & 基本情報の取得

def main(): rospy.init_node("moveit_command_sender") robot = moveit_commander.RobotCommander() print "=" * 10, " Robot Groups:" print robot.get_group_names()

print "=" * 10, " Printing robot state" print robot.get_current_state() print "=" * 10

rarm = moveit_commander.MoveGroupCommander("right_arm") larm = moveit_commander.MoveGroupCommander("left_arm")





ロボット全体に対するインタフェース





ロボット内のすべてのグループのリストを取得




rarm = moveit_commander.MoveGroupCommander("right_arm") larm = moveit_commander.MoveGroupCommander("left_arm")デバッグの際に有効なロボット全体の状態を取得





特定のグループのための単純なコマンドの実行を行うクラス

■sleep中のCtrl - cで投げられる例外をキャッチする

if __name__ == '__main__': try: main() except rospy.ROSInterruptException: pass

テキストを保存する

■実行権限を付加

$ chmod +x scripts/moveit_commander_sender.py

■MoveIt!の設定ファイルを追加　#デフォルト値で使うなら不要

joint_limits: RARM_JOINT0: has_velocity_limits: true max_velocity: 15.0 has_acceleration_limits: true max_acceleration: 17.0

LARM_JOINT0: has_velocity_limits: true max_velocity: 15.0 has_acceleration_limits: true max_acceleration: 17.0

0 ～ 5

0 ～ 5

config/joint_limit.yaml

■設定ファイルを読み込む

<?xml version="1.0"?><launch> <arg name="load_robot_description" default="false"/> <param if="$(arg load_robot_description)" name="robot_description" textfile="$(find nextage_description)/urdf/NextageOpen.urdf"/>

<param name="robot_description_semantic" textfile="$(find nextage_moveit_config)/config/NextageOpen.srdf"/> <group ns="robot_description_planning"> <rosparam command="load" file="$(find nextage_moveit_planning_execution)/config/joint_limit.yaml"/> </group>

</launch>

launch/planning_context.launch

■設定ファイルを読み込む

<?xml version="1.0"?><launch> <arg name="load_robot_description" default="false"/> <param if="$(arg load_robot_description)" name="robot_description" textfile="$(find nextage_description)/urdf/NextageOpen.urdf"/>

<param name="robot_description_semantic" textfile="$(find nextage_moveit_config)/config/NextageOpen.srdf"/> <group ns="robot_description_planning"> <rosparam command="load" file="$(find nextage_moveit_planning_execution)/config/joint_limit.yaml"/> </group>

</launch>

URDFファイルを読み込む

joint_limit.yamlを読み込む

■move_groupの起動とコンテキストの設定を行う

<?xml version="1.0"?>

<launch> <master auto="start"/> <include file="$(find nextage_moveit_config)/launch/move_group.launch"> <arg name="publish_monitored_planning_scene" value="true" /> </include> <include file="$(find nextage_moveit_planning_execution)/launch/planning_context.launch"> <arg name="load_robot_description" value="true"/> </include></launch>

launch/planning_execution.launch

■move_groupの起動とコンテキストの設定を行う

<?xml version="1.0"?>

<launch> <master auto="start"/> <include file="$(find nextage_moveit_config)/launch/move_group.launch"> <arg name="publish_monitored_planning_scene" value="true" /> </include> <include file="$(find nextage_moveit_planning_execution)/launch/planning_context.launch"> <arg name="load_robot_description" value="true"/> </include></launch>

move_groupを起動

コンテキストの設定

■動作確認

$ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch

$ roslaunch nextage_moveit_planning_execution planning_execution.launch

$ rosrun nextage_moveit_planning_execution moveit_command_sender.py

■出力結果

[ INFO] [1409329353.399066034]: Loading robot model 'NextageOpen'...========== Robot Groups:['left_arm', 'right_arm']========== Printing robot statejoint_state: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: /WAIST_Link name: ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'] position: [0.0, 0.0, 0.0, 0.010472, 0.0, -1.74533, -0.26529, 0.164061, -0.055851, -0.010472, 0.0, -1.74533, 0.26529, 0.164061, 0.055851] velocity: [] effort: []multi_dof_joint_state: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: /WAIST_Link joint_names: [] transforms: [] twist: [] wrench: []attached_collision_objects: []is_diff: False==========[ INFO] [1409329354.980926234, 832.749999999]: Ready to take MoveGroup commands for group right_arm.[ INFO] [1409329355.324125746, 833.209999999]: Ready to take MoveGroup commands for group left_arm.

■目標姿勢への移動　#main関数に追記

def main()・・・print "=" * 15, " Right arm ", "=" * 15print "=" * 10, " Reference frame: %s" % rarm.get_planning_frame() print "=" * 10, " Reference frame: %s" % rarm.get_end_effector_link() rarm_initial_pose = rarm.get_current_pose().poseprint "=" * 10, " Printing initial pose: "print rarm_initial_pose


def main()・・・print "=" * 15, " Right arm ", "=" * 15print "=" * 10, " Reference frame: %s" % rarm.get_planning_frame() print "=" * 10, " Reference frame: %s" % rarm.get_end_effector_link() rarm_initial_pose = rarm.get_current_pose().poseprint "=" * 10, " Printing initial pose: "print rarm_initial_pose

すべての計画が実行されたフレームの名前を取得

エンドエフェクタであるリンクの名前を取得存在しない場合空の文字列を返す

エンドエフェクタの現在の姿勢を取得エンドエフェクタが存在しない場合は例外を投げる


target_pose_r = geometry_msgs.msg.Pose() target_pose_r.position.x = 0.2035 target_pose_r.position.y = -0.5399 target_pose_r.position.z = 0.0709 target_pose_r.orientation.x = 0.000427 target_pose_r.orientation.y = 0.000317 target_pose_r.orientation.z = -0.000384 target_pose_r.orientation.w = 0.999999 rarm.set_pose_target(target_pose_r) print "=" * 10, " plan1..." rarm.go() rospy.sleep(1)


エンドエフェクタの姿勢を設定

指定された目標にグループを移動




set_pose_target(pose, end_effector_link = “”)poseは4種類の入力に対応

* Poseメッセージ * PoseStampedメッセージ * [x, y, z, rot_x, rot_y, rot_z] * [x, y, z, qx, qy, qz, qw]

■動作確認　$ rosrun nextage_moveit_planning_execution moveit_command_sender.py

■左アームも動かす　#main関数に追記

print "=" * 15, " Left arm ", "=" * 15print "=" * 10, " Reference frame: %s" % larm.get_planning_frame()print "=" * 10, " Reference frame: %s" % larm.get_end_effector_link()larm_initial_pose = larm.get_current_pose().poseprint "=" * 10, " Printing initial pose: "print larm_initial_pose target_pose_l = [ target_pose_r.position.x, -target_pose_r.position.y, target_pose_r.position.z, target_pose_r.orientation.x, target_pose_r.orientation.y, target_pose_r.orientation.z, target_pose_r.orientation.w]larm.set_pose_target(target_pose_l) print "=" * 10, " plan2..."larm.go()rospy.sleep(1)

■左アームも動かす　#main関数に追記

print "=" * 15, " Left arm ", "=" * 15print "=" * 10, " Reference frame: %s" % larm.get_planning_frame()print "=" * 10, " Reference frame: %s" % larm.get_end_effector_link()larm_initial_pose = larm.get_current_pose().poseprint "=" * 10, " Printing initial pose: "print larm_initial_pose target_pose_l = [ target_pose_r.position.x, -target_pose_r.position.y, target_pose_r.position.z, target_pose_r.orientation.x, target_pose_r.orientation.y, target_pose_r.orientation.z, target_pose_r.orientation.w]larm.set_pose_target(target_pose_l) print "=" * 10, " plan2..."larm.go()rospy.sleep(1)

Poseメッセージではなくリストを使用

■関節空間で目標を設定　#main関数に追記

print "=" * 10, " Planning to a joint-space goal"rarm.clear_pose_targets()print "=" * 10, " Joint values: ", rarm.get_current_joint_values() rarm_variable_values = [ 1.4377544509919726, -1.3161643133168621, -2.126307271452489, 1.4335761224859305, 0.02359653211486051, 0.55989121526186]

rarm.set_joint_value_target(rarm_variable_values) print "=" * 10, " plan3..."rarm.go()rospy.sleep(1)

■関節空間で目標を設定　#main関数に追記

print "=" * 10, " Planning to a joint-space goal"rarm.clear_pose_targets()print "=" * 10, " Joint values: ", rarm.get_current_joint_values() rarm_variable_values = [ 1.4377544509919726, -1.3161643133168621, -2.126307271452489, 1.4335761224859305, 0.02359653211486051, 0.55989121526186]

rarm.set_joint_value_target(rarm_variable_values) print "=" * 10, " plan3..."rarm.go()rospy.sleep(1)

すべての目標をクリア

すべての変数の値をリストで渡して目標を設定

■中間地点を結ぶ経路を計画　#main関数に追記

print "=" * 10, " Cartesian Paths"waypoints = []waypoints.append(larm.get_current_pose().pose)wpose = geometry_msgs.msg.Pose()wpose.orientation.w = 1.0wpose.position.x = waypoints[0].position.xwpose.position.y = waypoints[0].position.y - 0.15wpose.position.z = waypoints[0].position.zwaypoints.append(copy.deepcopy(wpose))wpose.position.z -= 0.1waypoints.append(copy.deepcopy(wpose))wpose.position.y -= 0.05waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = larm.compute_cartesian_path(waypoints, 0.01, 0.0) print "=" * 10, " plan4..."larm.execute(plan)rospy.sleep(5)



compute_cartesian_path(waypoints, eef_step, jump_threshold, avoid_collisions = True)

* waypoints 中間地点* eef_step 補完する分解能[m]* jump_threshold 経路内の点間の最大距離



動作計画の結果(RobotTrajectoryクラス)に従って経路を追従

■初期姿勢に戻す　#main関数に追記

print "=" * 10, " Moving to an initial pose"rarm.set_pose_target(rarm_initial_pose)larm.set_pose_target(larm_initial_pose)rarm.go()larm.go()rospy.sleep(2)

プログラム開始時の姿勢に戻して終了

■アームの移動速度を変更してみよう

config/joint_limit.txtを書き換える

　→　max_velocity　→　max_acceleration

■planning_execution.launchを再度立ち上げ直す

$ roslaunch nextage_moveit_planning_execution planning_execution.launch

目標値をトピックでやりとりする

Poseメッセージの送信・受信を二つのノードに分ける

キーボード操作でアームの動きを制御するプログラムを作成


scripts/nextage_command_server.py

import moveit_commanderimport rospyimport geometry_msgs.msg

■Poseメッセージを購読するクラスを作成


class NextageCommandServer: def __init__(self): self._robot = moveit_commander.RobotCommander()

rospy.Subscriber("right_arm/delta_pose", geometry_msgs.msg.Pose, self.delta_pose_r_cb) rospy.Subscriber("left_arm/delta_pose", geometry_msgs.msg.Pose, self.delta_pose_l_cb)

self._rarm = moveit_commander.MoveGroupCommander("right_arm") self._larm = moveit_commander.MoveGroupCommander("left_arm")

・・・

■Poseメッセージを購読するクラスを作成


class NextageCommandServer: def __init__(self): self._robot = moveit_commander.RobotCommander()

rospy.Subscriber("right_arm/delta_pose", geometry_msgs.msg.Pose, self.delta_pose_r_cb) rospy.Subscriber("left_arm/delta_pose", geometry_msgs.msg.Pose, self.delta_pose_l_cb)

self._rarm = moveit_commander.MoveGroupCommander("right_arm") self._larm = moveit_commander.MoveGroupCommander("left_arm")

・・・

rospy.Subscriber(トピック名, メッセージ, コールバック関数)

■right_arm/delta_poseのコールバックメソッド


class NextageCommandServer: ・・・ def delta_pose_r_cb(self, pose): rospy.loginfo(rospy.get_caller_id()) target_pose_r = self._rarm.get_current_pose().pose

target_pose_r.position.x += pose.position.x target_pose_r.position.y += pose.position.y target_pose_r.position.z += pose.position.z target_pose_r.orientation.w += pose.orientation.w target_pose_r.orientation.x += pose.orientation.x target_pose_r.orientation.y += pose.orientation.y target_pose_r.orientation.z += pose.orientation.z

self._rarm.set_pose_target(target_pose_r) self._rarm.go()　・・・

■right_arm/delta_poseのコールバックメソッド


class NextageCommandServer:　・・・ def delta_pose_r_cb(self, pose): rospy.loginfo(rospy.get_caller_id()) target_pose_r = self._rarm.get_current_pose().pose

target_pose_r.position.x += pose.position.x target_pose_r.position.y += pose.position.y target_pose_r.position.z += pose.position.z target_pose_r.orientation.w += pose.orientation.w target_pose_r.orientation.x += pose.orientation.x target_pose_r.orientation.y += pose.orientation.y target_pose_r.orientation.z += pose.orientation.z

self._rarm.set_pose_target(target_pose_r) self._rarm.go()　・・・

right_arm/delta_poseトピックを受け取った際に呼び出される

■left_arm/delta_poseのコールバックメソッド


class NextageCommandServer:　・・・ def delta_pose_l_cb(self, pose): rospy.loginfo(rospy.get_caller_id()) target_pose_l = self._larm.get_current_pose().pose target_pose_l.position.x += pose.position.x target_pose_l.position.y += pose.position.y target_pose_l.position.z += pose.position.z target_pose_l.orientation.w += pose.orientation.w target_pose_l.orientation.x += pose.orientation.x target_pose_l.orientation.y += pose.orientation.y target_pose_l.orientation.z += pose.orientation.z self._larm.set_pose_target(target_pose_l) self._larm.go()　・・・

■rospy.spin()


class NextageCommandServer:　・・・ def run(self): rospy.spin()　・・・

■rospy.spin()


class NextageCommandServer:　・・・ def run(self): rospy.spin()　・・・

ノードが終了するまでキープ

■ノードの初期化


・・・

def main(): rospy.init_node("nextage_command_server") nextage_command_server = NextageCommandServer() nextage_command_server.run()

if __name__ == '__main__': main()

nextage_command_serverの実装は完了


scripts/nextage_teleop_key.py

import rospy, geometry_msgs.msgimport sys, select, termios, tty

■スクリプトをPythonとして実行させる（ROSノードに必須）

#!/usr/bin/env python

■Poseメッセージを配信するクラスを作成


class TeleopNextage: MOVE_BINDINGS = { 'h' : (1, 0.1), 'j' : (0, -0.1), 'k' : (0, 0.1), 'l' : (1, -0.1), }

MSG = """ ********** h : y += 0.1 j : x += -0.1 k : x += 0.1 l : y += -0.1 ********** """ ・・・

■Poseメッセージを配信するクラスを作成


class TeleopNextage:・・・

def __init__(self): self._settings = termios.tcgetattr(sys.stdin) self._delta_pose_pub = rospy.Publisher('right_arm/delta_pose', geometry_msgs.msg.Pose)・・・

■ユーザーに押されたキーを取得するメソッド


class TeleopNextage:・・・ def get_key(self): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) key = sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self._settings) return key・・・

■押されたキーによって異なるPoseメッセージを発行


class TeleopNextage:・・・ def run(self): r = rospy.Rate(10) while not rospy.is_shutdown(): key = self.get_key() print key if key in TeleopNextage.MOVE_BINDINGS.keys(): move = TeleopNextage.MOVE_BINDINGS[key] diff_list = [move[1] if index is move[0] else 0 for index in range(7)] delta_pose = geometry_msgs.msg.Pose() delta_pose.position.x = diff_list[0] delta_pose.position.y = diff_list[1] delta_pose.position.z = diff_list[2] delta_pose.orientation.x = diff_list[3] delta_pose.orientation.y = diff_list[4] delta_pose.orientation.z = diff_list[5] delta_pose.orientation.w = diff_list[6] self._delta_pose_pub.publish(delta_pose) elif key == '\x03': print "Quit" break else: print "Unknown type"

r.sleep()・・・

■終了処理


class TeleopNextage:・・・ def __del__(self): termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self._settings)・・・

■ノードの初期化など


def main(): rospy.init_node('teleop_pose_key') teleop_nextage = TeleopNextage() print TeleopNextage.MSG teleop_nextage.run()

if __name__ == "__main__": try: main() except rospy.ROSInterruptException: pass

■動作確認

$ chmod +x scripts/nextage_command_server.py scripts/nextage_teleop_key.py

$ rosrun nextage_moveit_planning_execution nextage_command_server.py

$ rosrun nextage_moveit_planning_execution nextage_teleop_key.py

アームを自由に動かせるようになった

足りないのは環境認識

次は「NEXTAGEのカメラとtf」

ご清聴ありがとうございました!

ros japan users group meetup 03

Documents