ROS JAPAN Users Group Meetup 03

第三回ROS勉強会 @東京
２０１４年8月30日　
千葉工業大学　工学部　４年　
ロボット設計・制御研究室　前川大輝

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

自己紹介
■千葉工業大学のロボカップ開発チーム「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

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

■必要なパッケージのインストール
＄ 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!のユーザーインタフェース
■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の解説も含む

■パッケージの作成
$ 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.xml
src/
パッケージ名
依存パッケージ

■package.xml
catkinに対応したパッケージのルートフォルダにある
・・・
<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="dev@opensource-robotics.tokyo.jp">Isaac Saito</maintainer>
<author email="iiysaito@opensource-robotics.tokyo.jp">Isaac IY Saito</author>
<author email="method_aspect_card@yahoo.co.jp">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>
<build_depend>geometry_msgs</build_depend>
<build_depend>leap_motion</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>tf</run_depend>
<export>
<rosdoc config="rosdoc.yaml" />
</export>
</package>

<package>
Industries.
</description>
<export>
</export>
</package>
■パッケージの名前
■バージョン

<package>
Industries.
</description>
<export>
　■パッケージの説明
　<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.
</description>
</export>
</package>

<package>
Industries.
</description>
<export>
</export>
</package>
■作成者のメールアドレスと名前
　<maintainer email="dev@opensource-robotics.tokyo.jp">Isaac Saito</maintainer>
■ライセンス

<package>
Industries.
</description>
<export>
</export>
</package>
■リンク付け
　<url type="website">http://wiki.ros.org/hironx_tutorial</url>

■依存パッケージのインストール
$ rosdep install nextage_moveit_planning_execution
package.xml
<build_depend>nextage_moveit_config</build_depend>
<run_depend>nextage_moveit_config</run_depend>

■moveit_commanderのサンプルコード作成
$ vi scripts/moveit_command_sender.py

■スクリプトをPythonとして実行させる（ROSノードに必須）
#!/usr/bin/env python
■必要なモジュールをインポート
import moveit_commander
import rospy
import geometry_msgs.msg
import 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")

def main():
ロボット全体に対するインタフェース
print "=" * 10

def main():
print robot.get_ロボcurrent_ット内のstate()
すべてのグループのリストを取得
print "=" * 10

def main():
print "=" * 10
larm = moveit_デバッグの際coにm有m効anなdロerボ.Mッoトve全G体roのup状C態omをm取a得nder("left_arm")

def main():
print "=" * 10
特定のグループのための単純なコマンドの実行を行うクラス

■sleep中のCtrl - cで投げられる例外をキャッチする
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass
テキストを保存する

■実行権限を付加
$ chmod +x scripts/moveit_commander_sender.py

■MoveIt!の設定ファイルを追加
　#デフォルト値で使うなら不要
joint_limits:
0 ～ 5
RARM_JOINT0:
has_velocity_limits: true
max_velocity: 15.0
has_acceleration_limits: true
max_acceleration: 17.0
0 ～ 5
LARM_JOINT0:
has_velocity_limits: true
max_velocity: 15.0
has_acceleration_limits: true
max_acceleration: 17.0
config/joint_limit.yaml

■設定ファイルを読み込む
launch/planning_context.launch
<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ファイルを読み込む
<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>
joint_limit.yamlを読み込む

■move_groupの起動とコンテキストの設定を行う
<launch>
<master auto="start"/>
<include file="$(find
nextage_moveit_config)/launch/move_group.launch">
launch/planning_execution.launch
<arg name="publish_monitored_planning_scene"
value="true" />
</include>
nextage_moveit_planning_execution)/launch/planning_co
ntext.launch">
<arg name="load_robot_description" value="true"/>
</include>
</launch>

■move_groupの起動とコンテキストの設定を行う
move_groupを起動
<launch>
<master auto="start"/>
nextage_moveit_config)/launch/move_group.launch">
<arg name="publish_monitored_planning_scene"
value="true" />
</include>
nextage_moveit_planning_execution)/launch/planning_co
ntext.launch">
<arg name="load_robot_description" value="true"/>
</include>
</launch>
コンテキストの設定

■動作確認
$ 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 state
joint_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 ", "=" * 15
print "=" * 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().pose
print "=" * 10, " Printing initial pose: "
print rarm_initial_pose

すべての計画が実行された
フレームの名前を取得
def main()
・・・
print "=" * 15, " Right arm ", "=" * 15
print "=" * 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().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)

エンドエフェクタの姿勢を設定
print "=" * 10, " plan1..."
rarm.go()
rospy.sleep(1)
指定された目標にグループを移動

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

■左アームも動かす
print "=" * 15, " Left arm ", "=" * 15
print "=" * 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().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
]l
arm.set_pose_target(target_pose_l)
print "=" * 10, " plan2..."
larm.go()
rospy.sleep(1)

■左アームも動かす
print "=" * 15, " Left arm ", "=" * 15
print "=" * 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().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
]l
arm.set_pose_target(target_pose_l)
print "=" * 10, " plan2..."
larm.go()
rospy.sleep(1)
Poseメッセージではなくリストを使用

■関節空間で目標を設定
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)

■関節空間で目標を設定
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)
すべての変数の値をリストで渡して
目標を設定

■中間地点を結ぶ経路を計画
print "=" * 10, " Cartesian Paths"
waypoints = []
waypoints.append(larm.get_current_pose().pose)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y - 0.15
wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))
wpose.position.z -= 0.1
wpose.position.y -= 0.05
(plan, fraction) = larm.compute_cartesian_path(waypoints, 0.01, 0.0)
print "=" * 10, " plan4..."
larm.execute(plan)
rospy.sleep(5)

waypoints = []
compute_cartesian_path(waypoints, eef_step,
jump_threshold, avoid_collisions = True)
wpose.position.x = waypoints[* waypoints
0].position.x
wpose.position.y = waypoints[中間地点
0].position.y - 0.15
wpose.position.z = waypoints[* eef_step
0].position.z
waypoints.append(copy.deepcopy(補完する分解wpose))
能[m]
* jump_threshold
waypoints.append(copy.deepcopy(経路内の点wpose))
間の最大距離
print "=" * 10, " plan4..."
larm.execute(plan)
rospy.sleep(5)

waypoints = []
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y - 0.15
wpose.position.z = waypoints[0].position.z
print "=" * 10, " plan4..."
larm.execute(plan)
動作計画の結果(RobotTrajectoryクラス)
rospy.sleep(5)
に従って経路を追従

■初期姿勢に戻す
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_commander
import rospy
import 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メッセージを購読するクラスを作成
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のコールバックメソッド
・・・
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のコールバックメソッド
　・・・
def delta_pose_r_cb(self, pose):
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のコールバックメソッド
　・・・
def delta_pose_l_cb(self, pose):
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()
　・・・
def run(self):
rospy.spin()
　・・・

■rospy.spin()
　・・・
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の実装は完了

■スクリプトをPythonとして実行させる（ROSノードに必須）
#!/usr/bin/env python
scripts/nextage_teleop_key.py
import rospy, geometry_msgs.msg
import sys, select, termios, tty

■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メッセージを配信するクラスを作成
・・・
def __init__(self):
self._settings = termios.tcgetattr(sys.stdin)
self._delta_pose_pub = rospy.Publisher('right_arm/delta_pose',
geometry_msgs.msg.Pose)
・・・

■ユーザーに押されたキーを取得するメソッド
・・・
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メッセージを発行
・・・
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()
・・・

■終了処理
・・・
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
nextage_command_server.py
nextage_teleop_key.py

アームを自由に動かせるようになった
足りないのは環境認識
次は「NEXTAGEのカメラとtf」

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

ROS JAPAN Users Group Meetup 03

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to ROS JAPAN Users Group Meetup 03

Similar to ROS JAPAN Users Group Meetup 03 (20)

ROS JAPAN Users Group Meetup 03