def intialPoseCallback(data):
if data.pose.pose.position.x == 0 and data.pose.pose.position.y == 0:
return
# do full rotation on pose estimate, to hone-in amcl (if not docked)
rospy.sleep(0.5) # let amcl settle
oculusprimesocket.clearIncoming() # why?
oculusprimesocket.sendString("right 360")
oculusprimesocket.waitForReplySearch("<state> direction stop")
def intialPoseCallback(data):
if data.pose.pose.position.x == 0 and data.pose.pose.position.y == 0:
return
# do full rotation on pose estimate, to hone-in amcl (if not docked)
rospy.sleep(0.5) # let amcl settle
oculusprimesocket.clearIncoming() # why?
oculusprimesocket.sendString("right 360")
oculusprimesocket.waitForReplySearch("<state> direction stop")
def intialPoseCallback(data):
# do full rotation on pose estimate, to hone-in amcl
rospy.sleep(0.5) # let amcl settle
oculusprimesocket.sendString("speed " + str(turnspeed))
oculusprimesocket.sendString("move right")
rospy.sleep(secondspertwopi) # full rotation
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
def intialPoseCallback(data):
# do full rotation on pose estimate, to hone-in amcl
global initturn
initturn = True
rospy.sleep(0.5) # let amcl settle
oculusprimesocket.sendString("speed "+str(turnspeed) )
oculusprimesocket.sendString("move right")
rospy.sleep(secondspertwopi) # full rotation
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
rospy.sleep(1)
initturn = False
def directionListenerThread():
global turning
while oculusprimesocket.connected:
s = oculusprimesocket.waitForReplySearch("<state> direction")
direction = s.split()[2]
if direction == "left" or direction == "right":
turning = True
else:
turning = False
rospy.init_node('arcmove_globalpath_follower', anonymous=False)
listener = tf.TransformListener()
oculusprimesocket.connect()
rospy.on_shutdown(cleanup)
rospy.Subscriber("odom", Odometry, odomCallback)
rospy.Subscriber("move_base/DWAPlannerROS/local_plan", Path, pathCallback)
rospy.Subscriber("move_base/goal", MoveBaseActionGoal, goalCallback)
rospy.Subscriber("move_base/status", GoalStatusArray, goalStatusCallback)
rospy.Subscriber("move_base/DWAPlannerROS/global_plan", Path,
globalPathCallback)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped, intialPoseCallback)
oculusprimesocket.sendString("log arcmove_globalpath_follower.py connected")
oculusprimesocket.sendString("readsetting usearcmoves")
s = oculusprimesocket.waitForReplySearch("setting usearcmoves")
if re.search("true", s):
oculusprimesocket.sendString("state rosarcmove true")
else:
oculusprimesocket.sendString("state rosarcmove false")
while not rospy.is_shutdown():
t = rospy.get_time()
if t >= nextmove:
if goalseek and (followpath or goalpose):
oculusprimesocket.sendString("state rosarcmove")
s = oculusprimesocket.waitForReplySearch("<state> rosarcmove")
if re.search("true", s):
rosarcmove = True
def move(ox, oy, oth, tx, ty, tth, gth):
global followpath, goalpose, tfth, nextmove
global odomx, odomy, odomth
# determine xy deltas for move
distance = 0
if followpath:
dx = tx - ox
dy = ty - oy
distance = math.sqrt(pow(dx, 2) + pow(dy, 2))
goalrotate = False
if distance > 0:
th = math.acos(dx / distance)
if dy < 0:
th = -th
elif goalpose:
th = gth - tfth
goalrotate = True
else:
th = tth
# determine angle delta for move
dth = th - oth
if dth > math.pi:
dth = -math.pi * 2 + dth
elif dth < -math.pi:
dth = math.pi * 2 + dth
# force minimums
if distance > 0 and distance < 0.05:
distance = 0.05
# supposed to reduce zig zagging
if dth < minturn * 0.3 and dth > -minturn * 0.3:
dth = 0
elif dth >= minturn * 0.3 and dth < minturn:
dth = minturn
elif dth <= -minturn * 0.3 and dth > -minturn:
dth = -minturn
if dth > 0:
oculusprimesocket.sendString("speed " + str(turnspeed))
oculusprimesocket.sendString("move left")
rospy.sleep(dth / (2.0 * math.pi) * secondspertwopi)
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
elif dth < 0:
oculusprimesocket.sendString("speed " + str(turnspeed))
oculusprimesocket.sendString("move right")
rospy.sleep(-dth / (2.0 * math.pi) * secondspertwopi)
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
if distance > 0:
oculusprimesocket.sendString("speed " + str(linearspeed))
oculusprimesocket.sendString("move forward")
rospy.sleep(distance * secondspermeter)
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
if goalrotate:
rospy.sleep(1)
def move(ox, oy, oth, tx, ty, tth, gth):
global followpath, initialturn, waitonaboutface, nextmove
currentpathid = pathid
# determine xy deltas for move
distance = 0
if followpath:
dx = tx - ox
dy = ty - oy
distance = math.sqrt(pow(dx, 2) + pow(dy, 2))
goalrotate = False
if distance > 0:
th = math.acos(dx / distance)
if dy < 0:
th = -th
elif goalpose:
try:
(trans, rot) = listener.lookupTransform('/map', '/odom',
rospy.Time(0))
quaternion = (rot[0], rot[1], rot[2], rot[3])
th = gth - tf.transformations.euler_from_quaternion(quaternion)[2]
goalrotate = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
th = tth
# th = gth - tfth
else: # does this ever get called? just use th = math.acos(dx/distance), same?
th = tth
# determine angle delta for move
dth = th - oth
if dth > math.pi:
dth = -math.pi * 2 + dth
elif dth < -math.pi:
dth = math.pi * 2 + dth
# force minimums
if distance > 0 and distance < minlinear:
distance = minlinear
if distance > maxlinear:
distance = maxlinear
# supposed to reduce zig zagging (was 0.3)
if dth < minturn * 0.5 and dth > -minturn * 0.5:
dth = 0
elif dth >= minturn * 0.5 and dth < minturn:
dth = minturn
elif dth <= -minturn * 0.5 and dth > -minturn:
dth = -minturn
oculusprimesocket.clearIncoming()
# if turning more than 120 deg, inch forward, make sure not transient obstacle (like door transfer)
if abs(
dth
) > 1.57 and not goalrotate and not initialturn and waitonaboutface < 1:
if goalDistance() > 0.9:
oculusprimesocket.sendString("forward 0.26")
oculusprimesocket.waitForReplySearch("<state> direction stop")
waitonaboutface += 1 # only do this once
rospy.sleep(1.5)
nextmove = rospy.get_time() + listentime
return
waitonaboutface = 0
initialturn = False
if not pathid == currentpathid:
nextmove = rospy.get_time() + listentime
return
if dth > 0:
oculusprimesocket.sendString("left " + str(int(math.degrees(dth))))
oculusprimesocket.waitForReplySearch("<state> direction stop")
elif dth < 0:
oculusprimesocket.sendString("right " + str(int(math.degrees(-dth))))
oculusprimesocket.waitForReplySearch("<state> direction stop")
if distance > 0:
oculusprimesocket.sendString("forward " + str(distance))
rospy.sleep(distance / meterspersec)
# initialturn = False
nextmove = rospy.get_time() + listentime
def move(ox, oy, oth, tx, ty, tth, gth):
global followpath, goalpose, tfth, pathid, initialturn, waitonaboutface
global odomx, odomy, odomth
currentpathid = pathid
# determine xy deltas for move
distance = 0
if followpath:
dx = tx - ox
dy = ty - oy
distance = math.sqrt(pow(dx, 2) + pow(dy, 2))
goalrotate = False
if distance > 0:
th = math.acos(dx / distance)
if dy < 0:
th = -th
elif goalpose:
th = gth - tfth
goalrotate = True
else:
th = tth
# determine angle delta for move
dth = th - oth
if dth > math.pi:
dth = -math.pi * 2 + dth
elif dth < -math.pi:
dth = math.pi * 2 + dth
# force minimums
if distance > 0 and distance < minlinear:
distance = minlinear
if distance > maxlinear:
distance = maxlinear
# supposed to reduce zig zagging (was 0.3)
if dth < minturn * 0.5 and dth > -minturn * 0.5:
dth = 0
elif dth >= minturn * 0.5 and dth < minturn:
dth = minturn
elif dth <= -minturn * 0.5 and dth > -minturn:
dth = -minturn
oculusprimesocket.clearIncoming()
# if turning more than 120 deg, inch forward, make sure not transient obstacle (like door transfer)
if abs(
dth
) > 2.0944 and not goalrotate and not initialturn and waitonaboutface < 1:
oculusprimesocket.sendString("forward 0.25")
oculusprimesocket.waitForReplySearch("<state> direction stop")
waitonaboutface += 1 # only do this once
rospy.sleep(1)
return
waitonaboutface = 0
if not pathid == currentpathid:
return
if dth > 0:
oculusprimesocket.sendString("left " + str(int(math.degrees(dth))))
oculusprimesocket.waitForReplySearch("<state> direction stop")
elif dth < 0:
oculusprimesocket.sendString("right " + str(int(math.degrees(-dth))))
oculusprimesocket.waitForReplySearch("<state> direction stop")
if distance > 0:
oculusprimesocket.sendString("forward " + str(distance))
rospy.sleep(distance / meterspersec)
initialturn = False
# rospy.init_node('dwa_base_controller', anonymous=False)
rospy.init_node('arcmove_globalpath_follower', anonymous=False)
listener = tf.TransformListener()
oculusprimesocket.connect()
rospy.on_shutdown(cleanup)
rospy.Subscriber("odom", Odometry, odomCallback)
rospy.Subscriber("move_base/DWAPlannerROS/local_plan", Path, pathCallback)
rospy.Subscriber("move_base/goal", MoveBaseActionGoal, goalCallback)
rospy.Subscriber("move_base/status", GoalStatusArray, goalStatusCallback)
rospy.Subscriber("move_base/DWAPlannerROS/global_plan", Path, globalPathCallback)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped, intialPoseCallback)
oculusprimesocket.sendString("log arcmove_globalpath_follower.py connected")
oculusprimesocket.sendString("readsetting usearcmoves")
s = oculusprimesocket.waitForReplySearch("setting usearcmoves")
if re.search("true", s):
oculusprimesocket.sendString("state rosarcmove true")
else:
oculusprimesocket.sendString("state rosarcmove false")
while not rospy.is_shutdown():
t = rospy.get_time()
if t >= nextmove:
if goalseek and (followpath or goalpose):
oculusprimesocket.sendString("state rosarcmove")
s = oculusprimesocket.waitForReplySearch("<state> rosarcmove")
if re.search("true", s):
def move(ox, oy, oth, tx, ty, tth, gth):
global followpath, goalpose, tfth, pathid, initialturn, waitonaboutface
global odomx, odomy, odomth
currentpathid = pathid
# determine xy deltas for move
distance = 0
if followpath:
dx = tx - ox
dy = ty - oy
distance = math.sqrt( pow(dx,2) + pow(dy,2) )
goalrotate = False
if distance > 0:
th = math.acos(dx/distance)
if dy <0:
th = -th
elif goalpose:
th = gth - tfth
goalrotate = True
else:
th = tth
# determine angle delta for move
dth = th - oth
if dth > math.pi:
dth = -math.pi*2 + dth
elif dth < -math.pi:
dth = math.pi*2 + dth
# force minimums
if distance > 0 and distance < minlinear:
distance = minlinear
if distance > maxlinear:
distance = maxlinear
# supposed to reduce zig zagging (was 0.3)
if dth < minturn*0.5 and dth > -minturn*0.5:
dth = 0
elif dth >= minturn*0.5 and dth < minturn:
dth = minturn
elif dth <= -minturn*0.5 and dth > -minturn:
dth = -minturn
oculusprimesocket.clearIncoming()
# if turning more than 120 deg, inch forward, make sure not transient obstacle (like door transfer)
if abs(dth) > 2.0944 and not goalrotate and not initialturn and waitonaboutface < 1:
oculusprimesocket.sendString("forward 0.25")
oculusprimesocket.waitForReplySearch("<state> direction stop")
waitonaboutface += 1 # only do this once
rospy.sleep(1)
return
waitonaboutface = 0
if not pathid == currentpathid:
return
if dth > 0:
oculusprimesocket.sendString("left " + str(int(math.degrees(dth))) )
oculusprimesocket.waitForReplySearch("<state> direction stop")
elif dth < 0:
oculusprimesocket.sendString("right " +str(int(math.degrees(-dth))) )
oculusprimesocket.waitForReplySearch("<state> direction stop")
if distance > 0:
oculusprimesocket.sendString("forward "+str(distance))
rospy.sleep(distance/meterspersec)
initialturn = False
#### recovery routine
oculusprimesocket.sendString(
"messageclients recovery rotation")
oculusprimesocket.clearIncoming()
# cancel goal, clear costmaps, generally reset as much as possible
move_base.cancel_goal()
rospy.wait_for_service('/move_base/clear_costmaps')
rospy.ServiceProxy('/move_base/clear_costmaps', Empty)()
# wait for cpu
rospy.sleep(2)
oculusprimesocket.sendString("waitforcpu")
oculusprimesocket.waitForReplySearch(
"<state> waitingforcpu false")
# check if cancelled by user while waiting
oculusprimesocket.sendString("state rosgoalcancel")
s = oculusprimesocket.waitForReplySearch(
"<state> rosgoalcancel")
if re.search("rosgoalcancel true", s):
goalcancel()
continue
# resend pose, full rotate
publishinitialpose(
str(xoffst + odomx) + "_" + str(yoffst + odomy) + "_" +
str(thoffst + odomth))
# oculusprimesocket.sendString("right 360")
oculusprimesocket.waitForReplySearch("<state> direction stop")
recoveryrotate = True
#### recovery routine
oculusprimesocket.sendString("messageclients recovery rotation")
oculusprimesocket.clearIncoming()
# cancel goal, clear costmaps, generally reset as much as possible
move_base.cancel_goal()
rospy.wait_for_service('/move_base/clear_costmaps')
rospy.ServiceProxy('/move_base/clear_costmaps', Empty)()
# wait for cpu
rospy.sleep(2)
oculusprimesocket.sendString("waitforcpu")
oculusprimesocket.waitForReplySearch("<state> waitingforcpu false")
# check if cancelled by user while waiting
oculusprimesocket.sendString("state rosgoalcancel")
s = oculusprimesocket.waitForReplySearch("<state> rosgoalcancel")
if re.search("rosgoalcancel true", s):
goalcancel()
continue
# resend pose, full rotate
publishinitialpose(str(xoffst+odomx)+"_"+str(yoffst+odomy)+"_"+str(thoffst+odomth))
# oculusprimesocket.sendString("right 360")
oculusprimesocket.waitForReplySearch("<state> direction stop")
# wait for cpu
rospy.sleep(2)
def move(ox, oy, oth, tx, ty, tth, gth):
global followpath, goalpose, tfth
# print "odom: "+str(ox)+", "+str(oy)+", "+str(oth)
# print "target: "+str(tx)+", "+str(ty)+", "+str(tth)
distance = 0
if followpath:
dx = tx - ox
dy = ty - oy
distance = math.sqrt( pow(dx,2) + pow(dy,2) )
if distance > 0:
th = math.acos(dx/distance)
if dy <0:
th = -th
elif goalpose:
th = gth - tfth
else:
th = tth
#if goalpose:
#th = gth
#distance = 0
dth = th - oth
if dth > math.pi:
dth = -math.pi*2 + dth
elif dth < -math.pi:
dth = math.pi*2 + dth
# force minimums
if distance > 0 and distance < 0.05:
distance = 0.05
# supposed to reduce zig zagging
if dth < minturn*0.3 and dth > -minturn*0.3:
dth = 0
elif dth >= minturn*0.3 and dth < minturn:
dth = minturn
elif dth <= -minturn*0.3 and dth > -minturn:
dth = -minturn
if dth > 0:
oculusprimesocket.sendString("speed "+str(turnspeed) )
oculusprimesocket.sendString("move left")
rospy.sleep(dth/(2.0*math.pi) * secondspertwopi)
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
elif dth < 0:
oculusprimesocket.sendString("speed "+str(turnspeed) )
oculusprimesocket.sendString("move right")
rospy.sleep(-dth/(2.0*math.pi) * secondspertwopi)
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
if distance > 0:
oculusprimesocket.sendString("speed "+str(linearspeed) )
oculusprimesocket.sendString("move forward")
rospy.sleep(distance*secondspermeter)
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
def arcmove(ox, oy, oth, gpx, gpy, gpth, gth, lpx, lpy, lpth):
global initialturn, waitonaboutface, nextmove
# global path targets
gpdx = gpx - ox
gpdy = gpy - oy
gpdistance = math.sqrt(pow(gpdx, 2) + pow(gpdy, 2))
if not gpdistance == 0:
gpth = math.acos(gpdx / gpdistance)
if gpdy < 0:
gpth = -gpth
# local path targets
lpdx = lpx - ox
lpdy = lpy - oy
lpdistance = math.sqrt(pow(lpdx, 2) + pow(lpdy, 2))
if not lpdistance == 0:
lpth = math.acos(lpdx / lpdistance)
if lpdy < 0:
lpth = -lpth
# find arclength if any, and turn radians, depending on scenario
arclength = 0
goalrotate = False
if followpath and not ox == gpx and not oy == gpy and not initialturn: # normal arc move
if abs(
lpth - gpth
) > dpmthreshold: # 90 degrees local path disparity, use global instead
dth = gpth
distance = gpdistance / 2 # prevent oscillation, better than distance = 0
# distance = 0
# print("using global path")
else:
dth = lpth
distance = lpdistance
dth = dth - oth
if dth > math.pi:
dth = -math.pi * 2 + dth
elif dth < -math.pi:
dth = math.pi * 2 + dth
radius = (distance / 2) / math.sin(dth / 2)
arclength = radius * dth # *should* work out to always be > 0
if not arclength == 0:
if abs(dth / arclength) > dpmthreshold: # 1.57:
arclength = 0
# print ("high dpm")
elif goalpose: # final goal rotate move
try:
(trans, rot) = listener.lookupTransform('/map', '/odom',
rospy.Time(0))
quaternion = (rot[0], rot[1], rot[2], rot[3])
dth = (
gth -
tf.transformations.euler_from_quaternion(quaternion)[2]) - oth
goalrotate = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
dth = lpth
else: # initial turn move (always?)
dth = gpth - oth # point to global path
# determine angle delta for move TODO: cleanup
if dth > math.pi:
dth = -math.pi * 2 + dth
elif dth < -math.pi:
dth = math.pi * 2 + dth
# if turning more than 120 deg, inch forward, make sure not transient obstacle (like door transfer)
if abs(
dth
) > 1.57 and not goalrotate and not initialturn and waitonaboutface < 1: # was 2.094 120 deg
if goalDistance() > 0.9: # skip if close to goal
oculusprimesocket.clearIncoming()
oculusprimesocket.sendString("forward 0.25")
oculusprimesocket.waitForReplySearch("<state> direction stop")
waitonaboutface += 1 # only do this once
rospy.sleep(1.5)
nextmove = rospy.get_time() + listentime
# print("pausing...")
return
waitonaboutface = 0
initialturn = False
if arclength > 0: # arcmove
if arclength < minlinear:
arclength = minlinear
oculusprimesocket.sendString("arcmove " + str(arclength) + " " +
str(int(math.degrees(dth))))
rospy.sleep(arclength / 0.35)
nextmove = rospy.get_time()
return
# rotate only
# force minimum
if dth > 0 and dth < minturn:
dth = minturn
elif dth < 0 and dth > -minturn:
dth = -minturn
oculusprimesocket.clearIncoming()
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
if dth > 0:
oculusprimesocket.sendString("left " + str(int(math.degrees(dth))))
oculusprimesocket.waitForReplySearch("<state> direction stop")
elif dth < 0:
oculusprimesocket.sendString("right " + str(int(math.degrees(-dth))))
oculusprimesocket.waitForReplySearch("<state> direction stop")
nextmove = rospy.get_time() + listentime
oculusprimesocket.sendString(s)
if len(globalpath) > 0:
sendGlobalPath()
if goalseek:
state = move_base.get_state()
if state == GoalStatus.SUCCEEDED: # error if not seeking goal
oculusprimesocket.sendString("messageclients navigation goal reached")
oculusprimesocket.sendString("state delete roscurrentgoal")
goalseek = False
elif state == GoalStatus.ABORTED:
if not recoveryrotate:
recoveryrotate = True
oculusprimesocket.sendString("messageclients recovery rotation")
oculusprimesocket.sendString("speed "+str(turnspeed) )
oculusprimesocket.sendString("move right")
rospy.sleep(secondspertwopi)
oculusprimesocket.sendString("move stop")
oculusprimesocket.waitForReplySearch("<state> direction stop")
rospy.sleep(1)
move_base.send_goal(goal)
else:
oculusprimesocket.sendString("messageclients navigation goal ABORTED")
oculusprimesocket.sendString("state delete roscurrentgoal")
goalseek = False
rospy.sleep(0.01) # this really helps with cpu load
if motion == 1:
oculusprimesocket.sendString("gotowaypoint " + wp)
elif data.data == 1: # 1 means raise e-stop
stop = 1
if motion == 1:
oculusprimesocket.sendString("rosgoalcancel TRUE")
# start the Subscriber threads
def action():
rospy.Subscriber("destination", String, loco)
rospy.Subscriber("master_control", Int64, emergency_stop)
rospy.spin()
t1 = Thread(target=action)
t1.start()
# track the state of the platform
while True:
while stop != 1: # loop
time.sleep(1) # wait one second, don't clog the telnet stream!
s = oculusprimesocket.waitForReplySearch("navigation").lower().split()[-1]
if s == "reached":
print "waypoint " + wp + " reached"
motion = 0
if final == 1:
pub1.publish("reached")
def move(ox, oy, oth, tx, ty, tth, gth):
global followpath, initialturn, waitonaboutface, nextmove
currentpathid = pathid
# determine xy deltas for move
distance = 0
if followpath:
dx = tx - ox
dy = ty - oy
distance = math.sqrt( pow(dx,2) + pow(dy,2) )
goalrotate = False
if distance > 0:
th = math.acos(dx/distance)
if dy <0:
th = -th
elif goalpose:
try:
(trans,rot) = listener.lookupTransform('/map', '/odom', rospy.Time(0))
quaternion = (rot[0], rot[1], rot[2], rot[3])
th = gth - tf.transformations.euler_from_quaternion(quaternion)[2]
goalrotate = True
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
th = tth
# th = gth - tfth
else: # does this ever get called? just use th = math.acos(dx/distance), same?
th = tth
# determine angle delta for move
dth = th - oth
if dth > math.pi:
dth = -math.pi*2 + dth
elif dth < -math.pi:
dth = math.pi*2 + dth
# force minimums
if distance > 0 and distance < minlinear:
distance = minlinear
if distance > maxlinear:
distance = maxlinear
# supposed to reduce zig zagging (was 0.3)
if dth < minturn*0.5 and dth > -minturn*0.5:
dth = 0
elif dth >= minturn*0.5 and dth < minturn:
dth = minturn
elif dth <= -minturn*0.5 and dth > -minturn:
dth = -minturn
oculusprimesocket.clearIncoming()
# if turning more than 120 deg, inch forward, make sure not transient obstacle (like door transfer)
if abs(dth) > 1.57 and not goalrotate and not initialturn and waitonaboutface < 1:
if goalDistance() > 0.9:
oculusprimesocket.sendString("forward 0.26")
oculusprimesocket.waitForReplySearch("<state> direction stop")
waitonaboutface += 1 # only do this once
rospy.sleep(1.5)
nextmove = rospy.get_time() + listentime
return
waitonaboutface = 0
initialturn = False
if not pathid == currentpathid:
nextmove = rospy.get_time() + listentime
return
if dth > 0:
oculusprimesocket.sendString("left " + str(int(math.degrees(dth))) )
oculusprimesocket.waitForReplySearch("<state> direction stop")
elif dth < 0:
oculusprimesocket.sendString("right " +str(int(math.degrees(-dth))) )
oculusprimesocket.waitForReplySearch("<state> direction stop")
if distance > 0:
oculusprimesocket.sendString("forward "+str(distance))
rospy.sleep(distance/meterspersec)
# initialturn = False
nextmove = rospy.get_time() + listentime
def arcmove(ox, oy, oth, gpx, gpy, gpth, gth, lpx, lpy, lpth):
global initialturn, waitonaboutface, nextmove
# global path targets
gpdx = gpx - ox
gpdy = gpy - oy
gpdistance = math.sqrt( pow(gpdx,2) + pow(gpdy,2) )
if not gpdistance == 0:
gpth = math.acos(gpdx/gpdistance)
if gpdy <0:
gpth = -gpth
# local path targets
lpdx = lpx - ox
lpdy = lpy - oy
lpdistance = math.sqrt( pow(lpdx,2) + pow(lpdy,2) )
if not lpdistance == 0:
lpth = math.acos(lpdx/lpdistance)
if lpdy <0:
lpth = -lpth
# find arclength if any, and turn radians, depending on scenario
arclength = 0
goalrotate = False
if followpath and not ox==gpx and not oy==gpy and not initialturn: # normal arc move
if abs(lpth-gpth) > dpmthreshold: # 90 degrees local path disparity, use global instead
dth = gpth
distance = gpdistance/2 # prevent oscillation, better than distance = 0
# distance = 0
# print("using global path")
else:
dth = lpth
distance = lpdistance
dth = dth - oth
if dth > math.pi:
dth = -math.pi*2 + dth
elif dth < -math.pi:
dth = math.pi*2 + dth
radius = (distance/2)/math.sin(dth/2)
arclength = radius * dth # *should* work out to always be > 0
if not arclength == 0:
if abs(dth/arclength) > dpmthreshold: # 1.57:
arclength = 0
# print ("high dpm")
elif goalpose: # final goal rotate move
try:
(trans,rot) = listener.lookupTransform('/map', '/odom', rospy.Time(0))
quaternion = (rot[0], rot[1], rot[2], rot[3])
dth = (gth - tf.transformations.euler_from_quaternion(quaternion)[2]) - oth
goalrotate = True
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
dth = lpth
else: # initial turn move (always?)
dth = gpth - oth # point to global path
# determine angle delta for move TODO: cleanup
if dth > math.pi:
dth = -math.pi*2 + dth
elif dth < -math.pi:
dth = math.pi*2 + dth
# if turning more than 120 deg, inch forward, make sure not transient obstacle (like door transfer)
if abs(dth) > 1.57 and not goalrotate and not initialturn and waitonaboutface < 1: # was 2.094 120 deg
if goalDistance() > 0.9: # skip if close to goal
oculusprimesocket.clearIncoming()
oculusprimesocket.sendString("forward 0.25")
oculusprimesocket.waitForReplySearch("<state> direction stop")
waitonaboutface += 1 # only do this once
rospy.sleep(1.5)
nextmove = rospy.get_time() + listentime
# print("pausing...")
return
waitonaboutface = 0
initialturn = False
if arclength > 0: # arcmove
if arclength < minlinear:
arclength = minlinear
oculusprimesocket.sendString("arcmove " + str(arclength) + " " + str(int(math.degrees(dth))) )
rospy.sleep(arclength/0.35)
nextmove = rospy.get_time()
return
# rotate only
# force minimum
if dth > 0 and dth < minturn:
dth = minturn
elif dth < 0 and dth > -minturn:
dth = -minturn
oculusprimesocket.clearIncoming()
oculusprimesocket.sendString("move stop")
# below waits forever with new java stopbetweenmoves rotate code!! state never arrives
# oculusprimesocket.waitForReplySearch("<state> direction stop")
rospy.sleep(0.5)
if dth > 0:
oculusprimesocket.sendString("left " + str(int(math.degrees(dth))) )
oculusprimesocket.waitForReplySearch("<state> direction stop")
elif dth < 0:
oculusprimesocket.sendString("right " +str(int(math.degrees(-dth))) )
oculusprimesocket.waitForReplySearch("<state> direction stop")
nextmove = rospy.get_time() + listentime
