def execute_cb(goal):
try:
succeeded = False
if goal.down:
#goal.pose.pose.position.z += FUDGE
try:
arm.tfl.waitForTransform('base_link', goal.pose.header.frame_id, goal.pose.header.stamp, rospy.Duration(2.0))
rospy.loginfo("Placing bridge at " + str(goal.pose) + "\n\n" +
"Incidentally, this is also at: " +
str(arm.tfl.transformPose('base_link', goal.pose)))
except:
pass
b_marker = Marker()
b_marker.ns = "place_bridge_action"
b_marker.id = 1;
b_marker.type = Marker.ARROW
b_marker.header.frame_id = goal.pose.header.frame_id
b_marker.header.stamp = rospy.get_rostime()
b_marker.pose = goal.pose.pose
b_marker.scale = Vector3(0.2, 0.2, 0.2)
b_marker.color = ColorRGBA(1, 0, 1, 1)
print "=== Publishing bridge desi marker:"
print b_marker
marker_pub.publish(b_marker)
bridge_t = pm.fromMsg(goal.pose.pose)
#r_in_bridge = arm.fromRaw(-1.093, 0.180, -0.425, 0.157, -0.177, -0.630, 0.740)
#r_in_bridge = arm.fromRaw(-1.148, 0.333, -0.300, 0.076, -0.048, -0.721, 0.687)
# First one we used in driving around tests. Too high (angle puts cue into table)
r_in_bridge = arm.fromRaw(-1.236, 0.260, -0.170, 0, 0, -0.7071, 0.7071)
r_in_bridge = arm.fromRaw(-1.236, 0.260, -0.250, 0, 0, -0.7071, 0.7071)
# This one is from the measuring session, trying to get it flat
r_in_bridge = arm.fromRaw(-1.0, 0.17, -0.300, 0, 0, -0.7071, 0.7071)
# Switching to the shorter bridge
r_in_bridge = arm.fromRaw(-1.0, 0.17, -0.280, 0, 0, -0.7071, 0.7071)
r_pose_t = bridge_t * r_in_bridge
r_pose = PoseStamped()
r_pose.header = goal.pose.header
r_pose.pose = pm.toMsg(r_pose_t)
#r_pose = arm.PS('bridge_frame', -1.148, 0.333, -0.300, 0.076, -0.048, -0.721, 0.687)
#r_pose = arm.PS('bridge_frame', -1.1, 0.333, -0.300, 0, 0, -0.7071, 0.7071)
rospy.loginfo("Lining up right arm for shot at: " + str(r_pose))
r_marker = Marker()
r_marker.ns = "place_bridge_action"
r_marker.id = 0;
r_marker.type = Marker.ARROW
r_marker.header.frame_id = r_pose.header.frame_id
r_marker.header.stamp = rospy.get_rostime()
r_marker.pose = r_pose.pose
r_marker.scale = Vector3(0.2, 0.2, 0.2)
r_marker.color = ColorRGBA(1, 0, 1, 1)
marker_pub.publish(r_marker)
q_r = arm.ik_r(r_pose, arm.q_stage)
if not q_r:
raise Exception("IK failed for lining up right arm")
# Adjusting the arm position to stage the shot
q_r[6] += arm.WRIST_BACK_OFF
if not arm.gotor(q_r):
raise Exception("Failed to line up right arm")
#arm.gotor(arm.q_stage)
if not goal.pose.header.frame_id:
arm.gotol(arm.BRIDGE)
succeeded = arm.gotol(arm.BRIDGE_PUSH)
else:
#goal.pose.pose.position.z -= 0.025
goal_above = copy.deepcopy(goal)
goal_above.pose.pose.position.z += 0.05
tool = arm.PS('bridge_frame', 0,0,0, 0,0,0,1)
rospy.loginfo("Moving the bridge above its goal: " + str(goal_above))
q = arm.ik_l(goal_above.pose, arm.BRIDGE, tool)
if not q:
raise Exception("IK failed for moving above: " + str(goal_above))
if not arm.gotol(q):
raise Exception("Moving to above failed")
rospy.loginfo("Moving the bridge to its goal: " + str(goal))
q = arm.ik_l(goal.pose, arm.BRIDGE, tool)
if not q:
raise Exception("IK failed for placing bridge: " + str(goal_above))
if not arm.gotol(q):
raise Exception("Moving to place the bridge failed")
goal_below = copy.deepcopy(goal)
goal_below.pose.pose.position.z += PUSH
rospy.loginfo("Pushing into the table, to:\n" + str(goal_below))
q = arm.ik_l(goal_below.pose, arm.BRIDGE, tool)
if not q:
rospy.logerr("IK failed for the pushing goal. Just leaving it here")
elif not arm.gotol(q):
raise Exception("Moving to push location failed")
# Bridge has been placed. Let's print out where it got to.
try:
t_bridge = arm.tfl.lookupTransform('table', 'bridge_frame', rospy.Time(0.0))
rospy.loginfo("Final resting place of the bridge (in table frame): " + str(t_bridge))
except Exception as ex:
rospy.logerr("Printing out final info failed: " + str(ex))
else: # Lifting up
# Tries to lift directly up first
try:
tool = arm.PS('bridge_frame', 0,0,0, 0,0,0,1)
bridge_pose = arm.tfl.transformPose('base_link', tool)
z0 = bridge_pose.pose.position.z
bridge_pose.pose.position.z = z0 + 0.07
q = arm.ik_l(bridge_pose, arm.BRIDGE, tool)
if not q:
bridge_pose.pose.position.z = z0 + 0.04
q = arm.ik_l(bridge_pose, arm.BRIDGE, tool)
if q:
rospy.logwarn('Only raising directly upwards a little bit')
else:
raise Exception("IK failed for lifting the bridge up directly")
if not arm.gotol(q):
raise Exception("OK, so IK worked but I just couldn't move up. Weird")
except Exception as ex:
rospy.logerr("Couldn't lift directly up. Just lifting the bridge by joint values.\n" + str(ex))
if not arm.gotol(arm.BRIDGE_UP):
raise Exception("Moving arm to lift bridge failed")
server.set_succeeded()
except Exception as ex:
server.set_aborted()
rospy.logerr("Place bridge failed:\n" + str(ex))
def execute_cb(goal):
try:
succeeded = False
if goal.down:
#### FIXED BRIDGE POSE ####
goal.pose = arm.PS('base_link',
Constants.BRIDGE_IN_BASE_X,
Constants.BRIDGE_IN_BASE_Y,
Constants.TABLE_HEIGHT - 0.051,
Constants.BRIDGE_IN_BASE_QX,
Constants.BRIDGE_IN_BASE_QY,
Constants.BRIDGE_IN_BASE_QZ,
Constants.BRIDGE_IN_BASE_QW)
b_marker = Marker()
b_marker.ns = "place_bridge_action"
b_marker.id = 1;
b_marker.type = Marker.ARROW
b_marker.header.frame_id = goal.pose.header.frame_id
b_marker.header.stamp = rospy.get_rostime()
b_marker.pose = goal.pose.pose
b_marker.scale = Vector3(0.2, 0.2, 0.2)
b_marker.color = ColorRGBA(1, 0, 1, 1)
print "=== Publishing bridge desi marker:"
print b_marker
marker_pub.publish(b_marker)
bridge_t = pm.fromMsg(goal.pose.pose)
#r_in_bridge = arm.fromRaw(-1.093, 0.180, -0.425, 0.157, -0.177, -0.630, 0.740)
#r_in_bridge = arm.fromRaw(-1.148, 0.333, -0.300, 0.076, -0.048, -0.721, 0.687)
# First one we used in driving around tests. Too high (angle puts cue into table)
r_in_bridge = arm.fromRaw(-1.236, 0.260, -0.170, 0, 0, -0.7071, 0.7071)
r_in_bridge = arm.fromRaw(-1.236, 0.260, -0.250, 0, 0, -0.7071, 0.7071)
# This one is from the measuring session, trying to get it flat
r_in_bridge = arm.fromRaw(-1.0, 0.17, -0.300, 0, 0, -0.7071, 0.7071)
r_pose_t = bridge_t * r_in_bridge
r_pose = PoseStamped()
r_pose.header = goal.pose.header
r_pose.pose = pm.toMsg(r_pose_t)
#r_pose = arm.PS('bridge_frame', -1.148, 0.333, -0.300, 0.076, -0.048, -0.721, 0.687)
#r_pose = arm.PS('bridge_frame', -1.1, 0.333, -0.300, 0, 0, -0.7071, 0.7071)
rospy.loginfo("Lining up right arm for shot at: " + str(r_pose))
r_marker = Marker()
r_marker.ns = "place_bridge_action"
r_marker.id = 0;
r_marker.type = Marker.ARROW
r_marker.header.frame_id = r_pose.header.frame_id
r_marker.header.stamp = rospy.get_rostime()
r_marker.pose = r_pose.pose
r_marker.scale = Vector3(0.2, 0.2, 0.2)
r_marker.color = ColorRGBA(1, 0, 1, 1)
marker_pub.publish(r_marker)
q_r = arm.ik_r(r_pose, arm.q_stage)
if not q_r:
raise Exception("IK failed for lining up right arm")
if not arm.gotor(q_r):
raise Exception("Failed to line up right arm")
#arm.gotor(arm.q_stage)
if not goal.pose.header.frame_id:
arm.gotol(arm.BRIDGE)
succeeded = arm.gotol(arm.BRIDGE_PUSH)
else:
#goal.pose.pose.position.z -= 0.025
goal_above = copy.deepcopy(goal)
goal_above.pose.pose.position.z += 0.03
tool = arm.PS('bridge_frame', 0,0,0, 0,0,0,1)
rospy.loginfo("Moving the bridge above its goal: " + str(goal_above))
q = arm.ik_l(goal_above.pose, arm.BRIDGE, tool)
if not q:
raise Exception("IK failed for moving above: " + str(goal_above))
if not arm.gotol(q):
raise Exception("Moving to above failed")
rospy.loginfo("Moving the bridge to its goal: " + str(goal))
q = arm.ik_l(goal.pose, arm.BRIDGE, tool)
if not q:
raise Exception("IK failed for placing bridge: " + str(goal_above))
if not arm.gotol(q):
raise Exception("Moving to place the bridge failed")
# TODO: push
else:
if not arm.gotol(arm.BRIDGE_UP):
raise Exception("Moving arm to lift bridge failed")
server.set_succeeded()
except Exception as ex:
server.set_aborted()
rospy.logerr("Place bridge failed:\n" + str(ex))
