def execute_location(self, location, req):
dur = req.dur
print "Preparing to send goal..."
srv = rospy.ServiceProxy("set_pose", MoveBaseToPose)
resp = srv(location)
print "DONE!"
return ExecuteLocationResponse(True)
def execute_location(self,location,req):
dur = req.dur
print "Preparing to send goal..."
srv = rospy.ServiceProxy("set_pose", MoveBaseToPose)
resp = srv(location)
print "DONE!"
return ExecuteLocationResponse(True)
def execute_stance(self,stance,req):
dur = req.dur
if stance.arms == "l" or stance.arms == "b":
target_left = GripTarget()
target_left.point.header.frame_id = stance.frame_left
target_left.point.header.stamp = rospy.Time.now()
target_left.point.point.x = stance.x_left
target_left.point.point.y = stance.y_left
target_left.point.point.z = stance.z_left
target_left.roll = stance.roll_left
target_left.pitch = stance.pitch_left
target_left.yaw = stance.yaw_left
target_left.grip = stance.grip_left
target_left.arm = "l"
if stance.arms == "r" or stance.arms == "b":
target_right = GripTarget()
target_right.point.header.frame_id = stance.frame_right
target_right.point.header.stamp = rospy.Time.now()
target_right.point.point.x = stance.x_right
target_right.point.point.y = stance.y_right
target_right.point.point.z = stance.z_right
target_right.roll = stance.roll_right
target_right.pitch = stance.pitch_right
target_right.yaw = stance.yaw_right
target_right.grip = stance.grip_right
target_right.arm = "r"
success = True
print "Preparing to send goal..."
if stance.set_torso_height:
try:
srv = rospy.ServiceProxy("move_torso",MoveTorso)
resp = srv(MoveTorsoRequest(height=stance.torso_height))
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
success = False
def send_traj(self,fold_traj):
try:
srv = rospy.ServiceProxy('execute_fold',ExecuteFold)
resp = srv(ExecuteFoldRequest(fold_traj=fold_traj))
return True
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
return False
def adjustFoldline(self,intended):
#intended.expand(10)
start = self.convert_to_world_frame(intended.start())
end = self.convert_to_world_frame(intended.end())
try:
srv = rospy.ServiceProxy('adjust_fold',AdjustFold)
resp = srv(start,end)
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
return False
def sendTarget(self, dur, target1, target2=False,grab=False):
resp = False
if grab:
point = target1.point
pitch = target1.pitch
roll = target1.roll
yaw = target1.yaw
GripUtils.grab_point(point=point,pitch=pitch,roll=roll,yaw=yaw,arm=target1.arm,approach=False)
return
target1.point.header.stamp = rospy.Time.now()
if not target2:
try:
srv = rospy.ServiceProxy("move_one_arm",MoveOneArm)
resp = srv(MoveOneArmRequest(target=target1,dur=dur))
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
def sendTarget(self, dur, target1, target2=False, grab=False):
resp = False
if grab:
point = target1.point
pitch = target1.pitch
roll = target1.roll
yaw = target1.yaw
GripUtils.grab_point(point=point,
pitch=pitch,
roll=roll,
yaw=yaw,
arm=target1.arm,
approach=False)
return
target1.point.header.stamp = rospy.Time.now()
if not target2:
try:
srv = rospy.ServiceProxy("move_one_arm", MoveOneArm)
resp = srv(MoveOneArmRequest(target=target1, dur=dur))
except rospy.ServiceException, e:
rospy.loginfo("Service Call Failed: %s" % e)
except rospy.ServiceException, e:
rospy.loginfo("Service Call Failed: %s" % e)
else:
target2.point.header.stamp = rospy.Time.now()
assert target1.arm != target2.arm
try:
srv = rospy.ServiceProxy("move_both_arms", MoveBothArms)
target_left = target_right = []
if target1.arm == "l":
target_left = target1
target_right = target2
else:
target_left = target2
target_right = target1
resp = srv(
MoveBothArmsRequest(target_left=target_left,
target_right=target_right,
dur=dur))
except rospy.ServiceException, e:
rospy.loginfo("Service Call Failed: %s" % e)
def rotate(self, x, y, tilt):
new_y = cos(tilt) * y + sin(tilt) * x
new_x = -sin(tilt) * y + cos(tilt) * x
return (new_x, new_y)
def gripPoints(self,
point,
arm,
tilt,
grip,
preferred_pitch="DEFAULT",
target_right.yaw = stance.yaw_right
target_right.grip = stance.grip_right
target_right.arm = "r"
success = True
print "Preparing to send goal..."
if stance.set_torso_height:
try:
srv = rospy.ServiceProxy("move_torso",MoveTorso)
resp = srv(MoveTorsoRequest(height=stance.torso_height))
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
success = False
if stance.set_head_angle:
try:
srv = rospy.ServiceProxy("angle_head",AngleHead)
resp = srv(AngleHeadRequest(pan=stance.head_pan,tilt=stance.head_tilt))
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
success = False
if stance.arms == "b":
print "Getting both goals..."
try:
srv = rospy.ServiceProxy("move_both_arms",MoveBothArms)
resp = srv(MoveBothArmsRequest(target_left = target_left,target_right=target_right,dur=dur))
success &= resp.success
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
success = False
elif stance.arms == "r":
try:
srv = rospy.ServiceProxy("move_one_arm",MoveOneArm)
resp = srv(MoveOneArmRequest(target=target1,dur=dur))
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
else:
target2.point.header.stamp = rospy.Time.now()
assert target1.arm != target2.arm
try:
srv = rospy.ServiceProxy("move_both_arms",MoveBothArms)
target_left = target_right = []
if target1.arm == "l":
target_left = target1
target_right = target2
else:
target_left = target2
target_right = target1
resp = srv(MoveBothArmsRequest(target_left=target_left,target_right=target_right,dur=dur))
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
def rotate(self,x,y,tilt):
new_y = cos(tilt)*y + sin(tilt)*x
new_x = -sin(tilt)*y + cos(tilt)*x
return (new_x,new_y)
def gripPoints (self,point,arm,tilt,grip,preferred_pitch="DEFAULT",preferred_roll="DEFAULT"
,point2=None,arm2="n",tilt2=0,grip2=0,preferred_pitch2=0,preferred_roll2=0
,red=False,grab=False):
(target1,mode1) = self.gripAt(point=point,arm=arm,tilt=tilt,grip=grip,preferred_pitch=preferred_pitch,preferred_roll=preferred_roll,red=red)
target2 = False
mode2 = ""
if point2:
