def walkTo3DPose(self, destination):
rospy.loginfo("Staggering to given destination {}".format(str(destination)))
while not self.pose:
print "Pose has not yet been initialized, can not move."
rospy.sleep(2)
destinationPos2d = np.array([destination.position.x,destination.position.y])
euler = RobotInterface.eulerFromQuarternion(self.pose)
heading = euler[2]
locationPos2d = [self.pose.position.x,self.pose.position.y]
destAng = RobotInterface.angle_between2d(destinationPos2d,locationPos2d)
turnAng = destAng - heading
if turnAng > math.pi:
turnAng -= (2*math.pi)
if turnAng < -math.pi:
turnAng += (2*math.pi)
angThreshold = 0.15
distThreshold = 0.4
maxTurnV = 0.2
minTurnV = 0.00
maxFwdV = 1
minFwdV = 0
while True:
heading = RobotInterface.eulerFromQuarternion(self.pose)[2]
locationPos2d = [self.pose.position.x,self.pose.position.y]
destAng = RobotInterface.angle_between2d(destinationPos2d,locationPos2d)
# Determine turning speed
turnAng = destAng - heading
if turnAng > math.pi:
turnAng -= (2*math.pi)
if turnAng < -math.pi:
turnAng += (2*math.pi)
angAbs = abs(turnAng)
if turnAng < 0:
turnDir = -1
else:
turnDir = 1
turnV = angAbs / math.pi * 3
turnV = max(min(math.pow(turnV, 2), maxTurnV), minTurnV)
angV = turnV * turnDir
# Determine forward speed
distAbs = abs(np.linalg.norm(destinationPos2d - locationPos2d))
fwdV = min(math.pow(distAbs, 1.5), 1)
fwdV *= (1 - turnV/maxTurnV)
fwdV = max(min(fwdV, maxFwdV), minFwdV)
if (abs(distAbs) < distThreshold):
fwdV = 0
self.set_walk_velocity(fwdV, 0, angV)
if (abs(turnAng) < angThreshold and abs(distAbs) < distThreshold):
self.set_walk_velocity(0, 0, 0)
rospy.loginfo("Arrived at destination")
break
rospy.sleep(0.2)
def moveTo3DPose(self, destination):
rospy.loginfo("Staggering to given destination {}".format(str(destination)))
while not self.pose:
print "Pose has not yet been initialized, can not move."
rospy.sleep(2)
locationPos = np.array([self.pose.position.x,self.pose.position.y,self.pose.position.z])
destinationPos = np.array([destination.position.x,destination.position.y,destination.position.z])
delta = destinationPos - locationPos
# print delta
destinationPos2d = np.array([destination.position.x,destination.position.y])
euler = RobotInterface.eulerFromQuarternion(self.pose)
heading = euler[2]
# print " My heading is " + str(heading/math.pi*180) + " deg."
locationPos2d = [self.pose.position.x,self.pose.position.y]
# destAng = angle_between(locationPos2d,destinationPos2d)
destAng = RobotInterface.angle_between2d(destinationPos2d,locationPos2d)
# print " My destination is at " + str(destAng/math.pi*180) + " deg."
turnAng = destAng - heading
if turnAng > math.pi:
turnAng -= (2*math.pi)
if turnAng < -math.pi:
turnAng += (2*math.pi)
# print " I have to turn by " + str(turnAng/math.pi*180) + " deg."
angThreshold = 0.03
distThreshold = 0.2
while True:
heading = RobotInterface.eulerFromQuarternion(self.pose)[2]
# # locationPos2d = np.array([self.pose.position.x,self.pose.position.y])
locationPos2d = [self.pose.position.x,self.pose.position.y]
destAng = RobotInterface.angle_between2d(destinationPos2d,locationPos2d)
turnAng = destAng - heading
if turnAng > math.pi:
turnAng -= (2*math.pi)
if turnAng < -math.pi:
turnAng += (2*math.pi)
angAbs = abs(turnAng)
if turnAng < 0 :
turnDir = -1
else:
turnDir = 1
turnV = angAbs / math.pi * 15
maxTurnV = 1
minTurnV = 0.0
turnV = max(min(math.pow(turnV, 2), maxTurnV), minTurnV)
# turnV = min(angAbs,0.5)
angV = turnV * turnDir
distAbs = abs(np.linalg.norm(destinationPos2d - locationPos2d))
fwdV = min(math.pow(distAbs,1.0), 1.5)
if angAbs > (math.pi/8):
fwdV = 0
else:
fwdV *= (1-(angAbs/(math.pi/2)))
if (abs(turnAng) < angThreshold):
angV = 0
if (abs(distAbs) < distThreshold):
fwdV = 0
# print "heading: " + str(heading) + " -- destAng: " + str(destAng) + " -- turnAng: " + str(turnAng) + " -- angV: " + str (angV) + " -- dist: " + str(distAbs) + " -- fwdV: " + str(fwdV)
# break
self.set_move_velocity(fwdV,0,angV)
if (abs(turnAng) < angThreshold and abs(distAbs) < distThreshold):
self.set_move_velocity(0,0,0)
rospy.loginfo("Arrived at destination")
break
rospy.sleep(0.2)
