'').replace('\\', '')
print float(vision_x)
print float(vision_y)
print float(vision_z)
print float(vision_Rx)
print float(vision_Ry)
print float(vision_Rz)
start_program()
release()
stop_program()
try:
rospy.init_node('move_action_client')
goal = ActionGoal()
goal.goal_goal1 = float(vision_x)
goal.goal_goal2 = float(vision_y) + 100
goal.goal_goal3 = float(vision_z) + 350
goal.goal_goal4 = float(vision_Rx)
goal.goal_goal5 = float(vision_Ry)
goal.goal_goal6 = float(vision_Rz) - 180
result = call_server()
print 'The result is:', result
print "moved to pickup location"
except rospy.ROSInterruptException as e:
print 'Something went wrong:', e
start_program()
grip()
print "gripped object"
stop_program()
try:
rospy.init_node('move_action_client')
goal = ActionGoal()
client.wait_for_result()
result = client.get_result()
return result
if __name__ == "__main__":
rospy.init_node('moma')
print "REMEMBER TO PUT TM ARM IN SAFE POSITION"
start_program()
from tm_motion.msg import ActionAction, ActionGoal
print "tm moving to pick location to scan tm landmark"
try:
goal = ActionGoal()
goal.goal_goal1 = 576.27
goal.goal_goal2 = -241.48
goal.goal_goal3 = 613.98
goal.goal_goal4 = -179.85
goal.goal_goal5 = -1.43
goal.goal_goal6 = -42.93
result = call_server()
print 'The result is:', result
except rospy.ROSInterruptException as e:
print 'Something went wrong:', e
print "scanning for tm landmark location"
print landmark_location_service_client()
rate = rospy.Rate(10.0)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
while not rospy.is_shutdown():
try:
goal = ActionGoal()
goal.goal_goal = "pickup"
result = call_ld_server()
print 'The result is:', result
print "ld moved to pickup location"
except rospy.ROSInterruptException as e:
print 'Something went wrong:', e
start_program()
from tm_motion.msg import ActionAction, ActionGoal
print "tm moving to pick location to scan tm landmark"
try:
goal = ActionGoal()
goal.goal_goal1 = 440.52
goal.goal_goal2 = 362.69
goal.goal_goal3 = 706.25
goal.goal_goal4 = 178.59
goal.goal_goal5 = 0.93
goal.goal_goal6 = 135.04
result = call_server()
print 'The result is:', result
except rospy.ROSInterruptException as e:
print 'Something went wrong:', e
print "scanning for tm landmark location"
print landmark_location_service_client()
rate = rospy.Rate(10.0)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
while not rospy.is_shutdown():
try:
client.wait_for_server()
client.send_goal(goal, feedback_cb=feedback_cb)
client.wait_for_result()
result = client.get_result()
return result
if __name__ == "__main__":
rospy.init_node('test_tf')
print "moving to initial location to scan tm landmark"
try:
goal = ActionGoal()
goal.goal_goal1 = -326.47
goal.goal_goal2 = -53.09
goal.goal_goal3 = 646.33
goal.goal_goal4 = -177.09
goal.goal_goal5 = -1.64
goal.goal_goal6 = -90.20
result = call_server()
print 'The result is:', result
except rospy.ROSInterruptException as e:
print 'Something went wrong:', e
print "scanning for tm landmark location"
print landmark_location_service_client()
rate = rospy.Rate(10.0)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
while not rospy.is_shutdown():
try:
# print 'Something went wrong:', e
start_program()
from tm_motion.msg import ActionAction, ActionGoal
print "tm moving to pick location to scan tm landmark"
try:
goal = ActionGoal()
# goal.goal_goal1 = float(values[0])
# goal.goal_goal2 = float(values[1])
# goal.goal_goal3 = float(values[2])
# goal.goal_goal4 = float(values[3])
# goal.goal_goal5 = float(values[4])
# goal.goal_goal6 = float(values[5])
goal.goal_goal1 = 440.52
goal.goal_goal2 = 362.69
goal.goal_goal3 = 706.25
goal.goal_goal4 = 178.59
goal.goal_goal5 = 0.93
goal.goal_goal6 = 135.04
result = call_server()
print 'The result is:', result
except rospy.ROSInterruptException as e:
print 'Something went wrong:', e
print "scanning for tm landmark location"
print landmark_location_service_client()
rate = rospy.Rate(10.0)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
while not rospy.is_shutdown():
try:
goal.goal_goal = "pickup"
result = call_ld_server()
print 'The result is:', result
print "ld moved to pickup location"
except rospy.ROSInterruptException as e:
exit(0)
print 'Something went wrong:', e
start_program()
from tm_motion.msg import ActionAction, ActionGoal
print "tm moving to pick location to scan tm landmark"
try:
goal = ActionGoal()
goal.goal_goal1 = 373.52
goal.goal_goal2 = -165
goal.goal_goal3 = 683.37
goal.goal_goal4 = -178.86
goal.goal_goal5 = 0.57
goal.goal_goal6 = 47.18
result = call_server()
print 'The result is:', result
except rospy.ROSInterruptException as e:
print 'Something went wrong:', e
print "scanning for tm landmark location"
print landmark_location_service_client()
rate = rospy.Rate(10.0)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
while not rospy.is_shutdown():
try:
client.send_goal(goal, feedback_cb=feedback_cb)
client.wait_for_result()
result = client.get_result()
return result
if __name__ == "__main__":
rospy.init_node('vision_pick_place_client')
values = rospy.get_param("pick_scan_location").split(",")
print "pick_scan_location: ", values
print "moving to pick location to scan tm landmark"
try:
goal = ActionGoal()
goal.goal_goal1 = float(values[0])
goal.goal_goal2 = float(values[1])
goal.goal_goal3 = float(values[2])
goal.goal_goal4 = float(values[3])
goal.goal_goal5 = float(values[4])
goal.goal_goal6 = float(values[5])
# goal.goal_goal1 = -326.47
# goal.goal_goal2 = -53.09
# goal.goal_goal3 = 646.33
# goal.goal_goal4 = -177.09
# goal.goal_goal5 = -1.64
# goal.goal_goal6 = -90.20
result = call_server()
print 'The result is:', result
except rospy.ROSInterruptException as e:
print 'Something went wrong:', e
print "scanning for tm landmark location"
Rz = math.degrees(euler[2])
print trans.transform.translation.x
print trans.transform.translation.y
print trans.transform.translation.z
print Rx
print Ry
print Rz
exit(0)
from tm_motion.msg import ActionAction, ActionGoal
print "tm moving to entry position"
try:
goal = ActionGoal()
goal.goal_goal1 = trans.transform.translation.x
goal.goal_goal2 = trans.transform.translation.y
goal.goal_goal3 = trans.transform.translation.z
# goal.goal_goal4 = Rx
# goal.goal_goal5 = Ry
# goal.goal_goal6 = Rz
goal.goal_goal4 = 179.81
goal.goal_goal5 = -1.51
goal.goal_goal6 = -43.95
result = call_server()
print 'The result is:', result
print "moved to position to pick object"
except rospy.ROSInterruptException as e:
print 'Something went wrong:', e
exit(0)