def process():
pose2.main()
rospy.sleep(2)
#n = rospy.wait_for_message('hand_gesture', Int32)
#n = n.data
print(n)
if n > 0:
if n == 1:
gu.main()
elif n == 2:
choki.main()
elif n == 5:
par.main()
print(n)
else:
print('not find gesture')
print('done')
def process():
pose4.main()
rospy.sleep(1)
n = rospy.wait_for_message('hand_gesture', Int32, 0.1)
n = n.data
print(n)
if n > 0:
if n == 1:
choki.main()
elif n == 2:
par.main()
elif n == 3:
par_new.main()
elif n == 5:
gu.main()
else:
print('not find gesture')
if not n == 3:
hand_action.main()
print('done')
def main():
z = 0.2 #移動するときの高さ
min_z = 0.15 #掴むときのアームの高さ
y = 0.0
max_z = 0.25
x1 = 0.1
x2 = 0.15
x3 = 0.2
x4 = 0.25
x5 = 0.3
i = random.randint(1, 3)
#rospy.init_node("gripper_action_client")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
#アームの動きの速さを示している
arm.set_max_velocity_scaling_factor(1.0)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
gripper.set_joint_value_target([0.01, 0.01])
gripper.go()
#繰り返し呼び出すのでmove関数を定義する
def move(x,y,z):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = x
target_pose.position.y = y
target_pose.position.z = z
q = quaternion_from_euler(-3.14, 0.0, 3.14)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
arm.go()
#最初はグーの動き
move(x1, y, z)
rospy.sleep(1.0)
move(x1, y, min_z)
# move(x2, y, z)
move(x3, y, max_z)
# move(x4, y, z)
move(x5, y, min_z)
move(x3, y, max_z)
move(x1, y, min_z)
move(x3, y, max_z)
arm.set_named_target("home")
arm.go()
if i==1:
choki.main()
elif i == 2:
gu.main()
else:
par.main()
print("done")