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