def cd(data):
global flag, arm
if data.data == "Artifice" and flag:
rospy.loginfo("Start Artifice")
flag = False
# -------------------
pub = rospy.Publisher("report", Int32, queue_size=1) # 動作報告パブリッシャ
# --------------------
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
# --------------------
arm_initial_pose = arm.get_current_pose().pose # アーム初期ポーズを表示
# -------------------
#紙を見つける
arm.set_named_target("home")
arm.go()
joint_move(5, -60)
joint_move(5, 60)
arm_move(0.2, 0.05, 0.3)
#周囲に誰もいないか確認
arm.set_named_target("home")
arm.go()
joint_move(0, -80)
joint_move(0, 160)
joint_move(0, -160)
# --------------------
# 動作終了報告
pub.publish(True)
rospy.loginfo("Finish Greet")
def cd(data):
global flag, arm
put_x = 0.20 # x座標[m]
put_y = 0.0 # y座標[m]
put_before_z = 0.20 # 押す前 z座標[m]
put_z = 0.12 # 押す z座標[m]
push_z = 0.008 # 押し込みz座標[m]
put_after_z = 0.20 # 押す後 z座標[m]
if data.data == "Seal" and flag:
rospy.loginfo("Start Seal")
flag = False
# -------------------
pub = rospy.Publisher("report", Int32, queue_size=1) # 動作報告パブリッシャ
# --------------------
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
# --------------------
arm_initial_pose = arm.get_current_pose().pose # アーム初期ポーズを表示
# --------------------
# 捺印
arm_move(put_x, put_y, put_before_z)
arm_move(put_x, put_y, put_z)
arm_move(put_x, put_y, put_z - push_z)
arm_move(put_x, put_y, put_z)
rospy.sleep(0.5)
arm_move(put_x, put_y, put_after_z)
# --------------------
# 動作終了報告
pub.publish(True)
rospy.loginfo("Finish Seal")
def cd(data):
global flag, arm
if data.data == "Check" and flag:
rospy.loginfo("Start check")
flag = False
# -------------------
pub = rospy.Publisher("report", Int32, queue_size=1) # 動作報告パブリッシャ
# --------------------
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
# --------------------
arm_initial_pose = arm.get_current_pose().pose # アーム初期ポーズを表示
# --------------------
# 文書確認の動作
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
arm_move(0.24, 0.10, 0.2)
arm_move(0.24, 0.0, 0.2)
# --------------------
# 動作終了報告
pub.publish(True)
rospy.loginfo("Finish Check")
def pick_seal():
print("持つ")
global see_x, see_y, pick_z, close, flag
pub = rospy.Publisher("report", Int32, queue_size = 1)
see_y += 0.025
arm_move(see_x, see_y, pick_z)
hand_move(close)
flag = False
pub.publish(True)
rospy.loginfo("Finish Detect seal")
def cd(data):
global flag, arm
inkpad_x = 0.20 # x座標[m]
inkpad_y = -0.15 # y座標[m]
inkpad_before_z = 0.30 # 押す前 z座標[m]
inkpad_z = 0.13 # 押す z座標[m]
inkpad_after_z = 0.30 # 押す後 z座標[m]
inkpad_up_z = 0.01 # ポンポンする高さ[m]
check_deg = 50 # 確認時回転角度[deg]
if data.data == "PushCheck" and flag:
rospy.loginfo("Start Push and Check")
flag = False
# -------------------
pub = rospy.Publisher("report", Int32, queue_size=1) # 動作報告パブリッシャ
# --------------------
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
# --------------------
arm_initial_pose = arm.get_current_pose().pose # アーム初期ポーズを表示
# --------------------
# 朱肉につけ確認する
for i in range(2):
arm_move(inkpad_x, inkpad_y, inkpad_before_z)
# --------------------
# 朱肉にはんこを数回押し付ける
arm.set_max_velocity_scaling_factor(1.0)
arm_move(inkpad_x, inkpad_y, inkpad_z)
for j in range(2):
arm_move(inkpad_x, inkpad_y, inkpad_z + inkpad_up_z)
arm_move(inkpad_x, inkpad_y, inkpad_z)
# --------------------
arm.set_max_velocity_scaling_factor(0.1)
# 持ち上げる
arm_move(inkpad_x, inkpad_y, inkpad_after_z)
# 確認する
joint_move(4, check_deg)
# --------------------
# 動作終了報告
pub.publish(True)
rospy.loginfo("Finish Push and Check")
def cd(data):
global flag, arm
wipe_x = 0.20 # x座標[m]
wipe_y = 0.30 # y座標[m]
wipe_before_z = 0.2 # 拭く前 z座標[m]
wipe_z = 0.12 # z座標[m]
wipe_after_z = 0.2 # 拭いた後 z座標[m]
if data.data == "Wipe" and flag :
rospy.loginfo("Start Wipe")
flag = False
# -------------------
pub = rospy.Publisher("report", Int32, queue_size = 1) # 動作報告パブリッシャ
# --------------------
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
# --------------------
arm_initial_pose = arm.get_current_pose().pose # アーム初期ポーズを表示
# --------------------
# 拭く
arm_move(wipe_x, wipe_y, wipe_before_z)
for i in range(6):
arm_move(wipe_x, wipe_y, wipe_z)
rospy.sleep(0.5)
if i % 2 == 0:
wipe_x = wipe_x + 0.02
else:
wipe_x = wipe_x - 0.02
arm_move(wipe_x, wipe_y, wipe_after_z)
# --------------------
# 動作終了報告
pub.publish(True)
rospy.loginfo("Finish Wipe")
def pickcallback(pose):
limit = pose.theta
pose_x = pose.x
pose_y = pose.y
global see_x , see_y, mode, x_max, y_max, first
if(first):
print("serch position")
arm_move(see_x, see_y, see_z)
hand_move(0.7)
rospy.sleep(0.5)
first = False
if(mode == True):
hand_move(0.7)
print("zahyou", pose_x, pose_y)
if(limit == 0):
print("探す")
arm_move(see_x, see_y, see_z)
see_x += 0.01
if(see_x >= x_max):
see_y += 0.01
see_x = 0.10
if(see_y >= y_max):
see_y = -0.25
elif(pose_x < 20):
print(1)
see_x -= 0.01
elif(pose_x > 50):
print(2)
see_x += 0.01
elif(pose_y < -80):
print(3)
see_y -= 0.01
elif(pose_y > -50):
print(4)
see_y += 0.01
else:
print(5)
mode = False
pick_seal()
print(see_x, see_y, see_z)
arm_move(see_x, see_y, see_z)
rospy.sleep(1.0)
print("pickcallback,end")
else:
pass
def cd(data):
global flag0, flag1, arm
hand_open = math.pi / 4 # ハンド 開く角度[rad]
pub = rospy.Publisher("report", Int32, queue_size=1) # 動作報告パブリッシャ
if data.data == "Exclusion" and flag0:
rospy.loginfo("Start Exclusion")
flag0 = False
# --------------------
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
# --------------------
arm_initial_pose = arm.get_current_pose().pose # アーム初期ポーズを表示
# --------------------
# 跳ね除ける
push_x1 = 0
push_x2 = 0.10
push_x3 = 0.20
push_y1 = -0.25
push_y2 = -0.20
push_before_z = 0.20
z_after = 0.14
push_after_z = 0.12
arm.set_named_target("home")
arm.go()
arm_move(push_x1, push_y1, push_before_z)
hand_move(hand_open)
arm_move(push_x1, push_y1, z_after)
rospy.sleep(0.5)
arm_move(push_x1, push_y1, push_before_z)
for n in range(4):
if n % 2 == 0:
arm_move(push_x2, push_y1, push_before_z)
arm_move(push_x2, push_y1, z_after)
rospy.sleep(0.5)
arm_move(push_x2, push_y1, push_before_z)
else:
arm_move(push_x3, push_y1, push_before_z)
rospy.sleep(0.5)
hand_move(0.1)
arm_move(push_x3, push_y2, push_before_z)
for i in range(10):
arm_move(push_x3, push_y2, push_after_z)
push_y2 = push_y2 - 0.01
# --------------------
# 動作終了報告
pub.publish(True)
rospy.loginfo("Finish Exclusion")
# --------------------
if data.data == "Release" and flag1:
rospy.loginfo("Start Release")
flag1 = False
# --------------------
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
# --------------------
arm_initial_pose = arm.get_current_pose().pose # アーム初期ポーズを表示
# --------------------
# 戻すの動作
seal_x = 0.30 # x座標[m]
seal_y = -0.15 # y座標[m]
seal_before_z = 0.30 # 掴む前 Z座標[m]
seal_z = 0.135 # 掴む Z座標[m]
arm_move(seal_x, seal_y, seal_before_z) # はんこをはなす前の位置まで移動
arm_move(seal_x, seal_y, seal_z) # はんこをはなす位置まで移動
hand_move(hand_open) # はんこをはなす
# --------------------
# 動作終了報告
pub.publish(True)
rospy.loginfo("Finish Release")