def mapcallBack(data): global lockfilepath lockfilepath = "/run/shm/map.raw.lock" framefilepath ="/run/shm/map.raw" if os.path.exists(lockfilepath): return open(lockfilepath, 'w') # creates lockfile framefile = open(framefilepath, 'w') packeddata = struct.pack('%sb' %len(data.data), *data.data) framefile.write(packeddata) framefile.close() if os.path.exists(lockfilepath): os.remove(lockfilepath) quaternion = ( data.info.origin.orientation.x, data.info.origin.orientation.y, data.info.origin.orientation.z, data.info.origin.orientation.w ) th = tf.transformations.euler_from_quaternion(quaternion)[2] # width height res originx originy originth updatetime s = "state rosmapinfo "+str(data.info.width)+","+str(data.info.height)+"," s += str(data.info.resolution)+","+str(data.info.origin.position.x)+"," s += str(data.info.origin.position.y)+","+str(th)+","+str(rospy.get_time()) oculusprimesocket.sendString(s) oculusprimesocket.sendString("state rosmapupdated true")
def mapcallBack(data): global lockfilepath lockfilepath = "/run/shm/map.raw.lock" framefilepath = "/run/shm/map.raw" if os.path.exists(lockfilepath): return open(lockfilepath, 'w') # creates lockfile framefile = open(framefilepath, 'w') packeddata = struct.pack('%sb' % len(data.data), *data.data) framefile.write(packeddata) framefile.close() if os.path.exists(lockfilepath): os.remove(lockfilepath) quaternion = (data.info.origin.orientation.x, data.info.origin.orientation.y, data.info.origin.orientation.z, data.info.origin.orientation.w) th = tf.transformations.euler_from_quaternion(quaternion)[2] # width height res originx originy originth updatetime s = "state rosmapinfo " + str(data.info.width) + "," + str( data.info.height) + "," s += str(data.info.resolution) + "," + str( data.info.origin.position.x) + "," s += str(data.info.origin.position.y) + "," + str(th) + "," + str( rospy.get_time()) oculusprimesocket.sendString(s) oculusprimesocket.sendString("state rosmapupdated true")
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 goalCallback(d): data = d.goal.target_pose x = data.pose.position.x y = data.pose.position.y quaternion = ( data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w ) th = tf.transformations.euler_from_quaternion(quaternion)[2] oculusprimesocket.sendString("state roscurrentgoal "+str(x)+","+str(y)+","+str(th)) oculusprimesocket.sendString("messageclients new navigation goal received");
def emergency_stop(data): global stop if data.data == 0: # 0 means clear e-stop stop = 0 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")
def cleanup(): # ser.write("p\n") # stop lidar rotation # ser.write("n\n") # disable broadcast # ser.write("0\n") # disable lidar ser.write("f\n") # stop lidar ser.close() removelockfiles() rospy.sleep(3) print("lidar disabled, shutdown") oculusprimesocket.sendString("state lidar false")
def sendScan(): global scanpoints s = "state rosscan " step = 8 size = len(scanpoints) i = 0 while i < size-step: s += str(round(scanpoints[i],3))+"," i += step s += str(round(scanpoints[size-1],3)) oculusprimesocket.sendString(s)
def sendScan(): global scanpoints s = "state rosscan " step = 8 size = len(scanpoints) i = 0 while i < size - step: s += str(round(scanpoints[i], 3)) + "," i += step s += str(round(scanpoints[size - 1], 3)) oculusprimesocket.sendString(s)
def sendGlobalPath(path): s = "state rosglobalpath " step = 5 size = len(path) i = 0 while i < size - step: s = s + str(round(path[i].pose.position.x, 2)) + "," s = s + str(round(path[i].pose.position.y, 2)) + "," i += step s = s + str(round(path[size - 1].pose.position.x, 2)) s = s + str(round(path[size - 1].pose.position.y, 2)) oculusprimesocket.sendString(s)
def sendGlobalPath(path): s = "state rosglobalpath " step = 5 size = len(path) i = 0 while i < size - step: s = s + str(round(path[i].pose.position.x,2))+"," s = s + str(round(path[i].pose.position.y,2))+"," i += step s = s + str(round(path[size-1].pose.position.x,2)) s = s + str(round(path[size-1].pose.position.y,2)) oculusprimesocket.sendString(s)
def action(data): global last_command, status # if the last message is different from the current message received from app cmd = data.data if cmd != last_command[-1]: # if the last message is different from the current one last_command.append(cmd) # store command status = "ongoing" # not reached serial_push.publish(apptoxbee[cmd]) # send request for XBee oculusprimesocket.sendString("strobeflash on 1000 30") # on for 1000 ms at 30% intensity, indicates command was received #time.sleep(10) # while status != "reached": # wait until oculus has reached destination status = rospy.wait_for_message("platform_state", String) serial_push.publish(apptoxbee[cmd]+"ed") print status # time.sleep(5) blue_push.publish(str(int(cmd) + 1)) # send 3 if parked, 5 if returned
def move(linear, angular): global lastlinear, lastangular, lastmove lastmove = rospy.Time.now() # don't send repeat stop commands if linear == lastlinear and angular == lastangular and linear == 0 and angular == 0: return lastlinear = linear lastangular = angular cmd = None d = "0.3" # .3 a = "25" # 40 arcmult = 3 # 3 if linear == 0 and angular == 0: cmd = "move stop" elif linear > 0 and angular == 0: cmd = "forward "+d elif linear < 0 and angular == 0: cmd = "backward "+d elif linear == 0 and angular > 0: cmd = "left "+a elif linear == 0 and angular < 0: cmd = "right "+a elif linear > 0 and not angular == 0: # forward arc angle = str(int(math.degrees(angular))/arcmult) cmd = "arcmove "+d+" "+angle elif linear < 0 and not angular == 0: # backwards arc angle = str(int(math.degrees(angular))/arcmult) cmd = "arcmove -"+d+" "+angle if not cmd == None: oculusprimesocket.sendString(cmd)
def jointCallback(data): global currentJointPositions, startupok joint_names = [] positions = [] i = 0 while i < len(data.name): joint_names.append(data.name[i]) positions.append(data.position[i]) # print("joint: "+data.name[i]+", angle: "+str(data.position[i])) i += 1 currentJointPositions = JointPosition(joint_names, positions, 0.0, 0.0) if not startupok: startupok = True oculusprimesocket.sendString("state arm ready") oculusprimesocket.sendString("messageclients openmanipulator ready")
def sendScan(): global scanpoints s = "state rosscan " size = len(scanpoints) step = 8 # vga depth cam, size typically 640 if (size < 600): # xaxxon lidar # step = size/100 step = 2 i = 0 while i < size - step: s += str(round(scanpoints[i], 3)) + "," i += step try: s += str(round(scanpoints[size - 1], 3)) except IndexError: # rare lidar scan size error oculusprimesocket.sendString("messageclients remote_nav.py IndexError") oculusprimesocket.sendString(s)
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 goalcancel(): global goalseek, recoveryrotate move_base.cancel_goal() goalseek = False oculusprimesocket.sendString("messageclients cancel navigation goal") oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosgoalcancel") globalpath = [] recoveryrotate = False
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 transformCallback(data): global dockfound, dockcamready if not xbasecam: return if not dockcamready: dockcamready = True oculusprimesocket.sendString("state dockcamready true") if len(data.transforms) == 0: #no target found if dockfound == True: # avoid unnecessary socket chatter oculusprimesocket.sendString("state dockfound false") dockfound = False return xc = -(data.transforms[0].transform.translation.x + 0.018 ) # add bot/dock contacts offset actual is 0.024 zc = data.transforms[0].transform.translation.z # camangle = math.atan(xc/zc) baselinkangle = math.atan( (xc + ybasecam) / (zc + xbasecam)) # target left, should be positive baselinkdistance = math.sqrt((xc + ybasecam)**2 + (zc + xbasecam)**2) quaternion = (data.transforms[0].transform.rotation.x, data.transforms[0].transform.rotation.y, data.transforms[0].transform.rotation.z, data.transforms[0].transform.rotation.w) euler = tf.transformations.euler_from_quaternion(quaternion) targetpitch = euler[1] # TODO: this is to dockcam, should be to base_link # baselinkdistance baselinkangle camangle targetpitch # dockmetrics = str(baselinkdistance)+" "+str(math.degrees(baselinkangle))+" "+str(math.degrees(camangle))+" "+str(math.degrees(targetpitch)) dockmetrics = "{0:.4f}".format(baselinkdistance) + " " dockmetrics += "{0:.3f}".format(math.degrees(baselinkangle)) + " " # dockmetrics += "{0:.3f}".format(math.degrees(camangle)) + " " dockmetrics += "{0:.3f}".format(math.degrees(targetpitch)) oculusprimesocket.sendString("state dockmetrics " + dockmetrics) oculusprimesocket.sendString("state dockfound true") dockfound = True
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
def cleanup(): oculusprimesocket.sendString("move stop")
def callback(data): ## determine controller input th = 0.6 #threshold direction = "S" horiz = data.axes[0] vert = data.axes[1] if abs(horiz) < th and abs(vert) < th: direction = "S" elif abs(horiz) > abs(vert): #horiz if horiz < 0: direction = "R" else: direction = "L" else: #vert if vert > 0: direction = "F" else: direction = "B" cam = "S" cvert = data.axes[3] if abs(cvert) > th: if cvert < 0: cam = "U" else: cam = "D" strobebutton = data.buttons[14] strobe = "off" if strobebutton == 1: strobe = "on" dockbutton = data.buttons[12] dock = "nil" if dockbutton == 1: dock = "go" ## send orders to bot # move global lastdirection if direction == "S" and lastdirection != "S": oculusprimesocket.sendString("move stop") lastdirection = direction if direction == "F" and lastdirection != "F": oculusprimesocket.sendString("move forward") lastdirection = direction elif direction == "B" and lastdirection != "B": oculusprimesocket.sendString("move backward") lastdirection = direction elif direction == "R" and lastdirection != "R": oculusprimesocket.sendString("move right") lastdirection = direction elif direction == "L" and lastdirection != "L": oculusprimesocket.sendString("move left") lastdirection = direction # camera tilt global lastcam if cam == "S" and lastcam != "S": oculusprimesocket.sendString("cameracommand stop") lastcam = cam elif cam == "U" and lastcam != "U": oculusprimesocket.sendString("cameracommand up") lastcam = cam elif cam == "D" and lastcam != "D": oculusprimesocket.sendString("cameracommand down") lastcam = cam # strobe global laststrobe if strobe == "on" and laststrobe != "on": oculusprimesocket.sendString("strobeflash on") laststrobe = strobe elif strobe == "off" and laststrobe != "off": oculusprimesocket.sendString("strobeflash off") laststrobe = strobe # dock global lastdock if dock=="go" and lastdock != "go": oculusprimesocket.sendString("streamsettingsset high"); oculusprimesocket.sendString("publish camera") rospy.sleep(2) oculusprimesocket.sendString("autodock go") lastdock = dock elif dock=="nil": lastdock = "nil"
def cleanup(): oculusprimesocket.sendString("state delete odometrybroadcast") oculusprimesocket.sendString("odometrystop") oculusprimesocket.sendString("state delete navigationenabled") # MAIN rospy.init_node('odom_tf', anonymous=False) before = rospy.Time.now() br = tf.TransformBroadcaster() odom_pub = rospy.Publisher('odom', Odometry, queue_size=10) rospy.on_shutdown(cleanup) oculusprimesocket.connect() oculusprimesocket.sendString("state odometrybroadcast 250") # ms oculusprimesocket.sendString("odometrystart") broadcast("* * 0 0".split()) # broadcast zero odometry baseline while not rospy.is_shutdown(): # t = rospy.get_time() # if t-lastupdate > updateinterval: # request odometry update # oculusprimesocket.sendString("odometryreport") s = oculusprimesocket.replyBufferSearch("<state> distanceangle ") if not s == "": broadcast(s.split()) # lastupdate = now.to_sec() rospy.sleep(0.005) # was 0.01
oculusprimesocket.sendString("messageclients cancel navigation goal") oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosgoalcancel") globalpath = [] recoveryrotate = False # main oculusprimesocket.connect() rospy.init_node('remote_nav', anonymous=False) if os.path.exists(lockfilepath): os.remove(lockfilepath) oculusprimesocket.sendString("log remote_nav.py connected") oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosamcl") oculusprimesocket.sendString("state delete rosglobalpath") oculusprimesocket.sendString("state delete rosmapinfo") oculusprimesocket.sendString("state delete rosscan") oculusprimesocket.sendString("state delete rosgoalstatus") rospy.Subscriber("map", OccupancyGrid, mapcallBack) rospy.Subscriber("odom", Odometry, odomCallback) rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, amclPoseCallback) rospy.Subscriber("move_base/goal", MoveBaseActionGoal, goalCallback) rospy.Subscriber("move_base/DWAPlannerROS/global_plan", Path, globalPathCallback) rospy.Subscriber("scan", LaserScan, scanCallback) rospy.Subscriber("move_base/feedback", MoveBaseActionFeedback, feedbackCallback) initpose_pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10)
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
while not rospy.is_shutdown(): t = rospy.get_time() if t >= nextmove: nextmove = t + listentime if goalseek and not initturn: move(odomx, odomy, odomth, targetx, targety, targetth, goalth) followpath = False if t - lastpath > 3: goalpose = True if int(t- lastpath) > 10 and goalseek: # recovery behavior, rotate oculusprimesocket.sendString("speed "+str(turnspeed) ) oculusprimesocket.sendString("move right") rospy.sleep(1) # determine direction (angle) on map try: (trans,rot) = listener.lookupTransform('/map', '/odom', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue quaternion = (rot[0], rot[1], rot[2], rot[3]) tfth = tf.transformations.euler_from_quaternion(quaternion)[2] rospy.sleep(0.01) cleanup()
elif linear < 0 and not angular == 0: # backwards arc angle = str(int(math.degrees(angular))/arcmult) cmd = "arcmove -"+d+" "+angle if not cmd == None: oculusprimesocket.sendString(cmd) # print (str(linear)+", "+str(angular)+", "+cmd) def cleanup(): oculusprimesocket.sendString("odometrystop") oculusprimesocket.sendString("log cmd_vel_listener.py disconnecting") # goodbye # Main # initialize node rospy.init_node('cmd_vel_listener', anonymous=False) rospy.on_shutdown(cleanup) lastmove = rospy.Time.now() # connect to oculusprime java server oculusprimesocket.connect() oculusprimesocket.sendString("log cmd_vel_listener.py connected") oculusprimesocket.sendString("odometrystart") # twist message event listener rospy.Subscriber("cmd_vel", Twist, twistCallback) rospy.spin()
def cleanup(): oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosamcl") oculusprimesocket.sendString("state delete rosglobalpath") oculusprimesocket.sendString("state delete rosscan") oculusprimesocket.sendString("log remote_nav.py disconnecting")
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 cleanup(): oculusprimesocket.sendString("log arcmove_globalpath_follower.py disconnecting")
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
def cleanup(): oculusprimesocket.sendString("odometrystop") oculusprimesocket.sendString("log cmd_vel_listener.py disconnecting") # goodbye
def cleanup(): oculusprimesocket.sendString("move stop") oculusprimesocket.sendString("state delete navigationenabled")
def cleanup(): oculusprimesocket.sendString("state delete odometrybroadcast") oculusprimesocket.sendString("odometrystop") # oculusprimesocket.sendString("state delete navigationenabled") oculusprimesocket.sendString("log odom_tf.py disconnecting") # goodbye # MAIN rospy.init_node('odom_tf', anonymous=False) before = rospy.Time.now() br = tf.TransformBroadcaster() odom_pub = rospy.Publisher('odom', Odometry, queue_size=10) rospy.on_shutdown(cleanup) oculusprimesocket.connect() oculusprimesocket.sendString("log odom_tf.py connected") oculusprimesocket.sendString("state odometrybroadcast 250" ) # ms, needs to be set prior to odometrystart oculusprimesocket.sendString("odometrystart") broadcast("* * 0 0".split()) # broadcast zero odometry baseline while not rospy.is_shutdown(): # t = rospy.get_time() # if t-lastupdate > updateinterval: # request odometry update # oculusprimesocket.sendString("odometryreport") s = oculusprimesocket.replyBufferSearch("<state> distanceangle ") if not s == "": broadcast(s.split()) # lastupdate = now.to_sec()
def cleanup(): oculusprimesocket.sendString("state delete odometrybroadcast") oculusprimesocket.sendString("odometrystop") # oculusprimesocket.sendString("state delete navigationenabled") oculusprimesocket.sendString("log odom_tf.py disconnecting") # goodbye
def loco(data): global motion, wp, final, prev print "The waypoint is ", data.data wp = data.data if wp == "25": final = 0 motion = 1 oculusprimesocket.sendString("gotowaypoint " + str(prev) + "e") while motion == 1: time.sleep(1) motion = 1 final = 1 oculusprimesocket.sendString("gotowaypoint " + wp) motion = 1 final = 1 else: prev = int(wp) transit = (int(wp) - 1)/6 + 1 final = 0 motion = 1 oculusprimesocket.sendString("gotowaypoint transit" + str(transit)) while motion == 1: time.sleep(1) motion = 1 if (int(wp) - 1)%6 < 3: oculusprimesocket.sendString("gotowaypoint " + str((transit - 1)*6 + 3) + "e") while motion == 1: time.sleep(1) motion = 1 oculusprimesocket.sendString("gotowaypoint " + wp + "e") while motion == 1: time.sleep(1) final = 1 oculusprimesocket.sendString("gotowaypoint " + wp)
def cleanup(): oculusprimesocket.sendString("state delete navigationenabled") oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosamcl") oculusprimesocket.sendString("state delete rosglobalpath") oculusprimesocket.sendString("state delete rosscan") oculusprimesocket.sendString("messageclients navigation disabled")
def cleanup(): oculusprimesocket.sendString("state delete odometrybroadcast") oculusprimesocket.sendString("odometrystop") oculusprimesocket.sendString("state delete navigationenabled")
def callback(msg): if (msg.data == "disconnected"): oculusprimesocket.sendString("driverexit")
listener = tf.TransformListener() while not rospy.is_shutdown(): t = rospy.get_time() if t >= nextmove: nextmove = t + listentime if goalseek and not initturn: move(odomx, odomy, odomth, targetx, targety, targetth, goalth) followpath = False if t - lastpath > 3: goalpose = True if int(t - lastpath) > 10 and goalseek: # recovery behavior, rotate oculusprimesocket.sendString("speed " + str(turnspeed)) oculusprimesocket.sendString("move right") rospy.sleep(1) # determine direction (angle) on map try: (trans, rot) = listener.lookupTransform('/map', '/odom', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue quaternion = (rot[0], rot[1], rot[2], rot[3]) tfth = tf.transformations.euler_from_quaternion(quaternion)[2] rospy.sleep(0.01) cleanup()
oculusprimesocket.sendString("state delete odometrybroadcast") oculusprimesocket.sendString("odometrystop") # oculusprimesocket.sendString("state delete navigationenabled") oculusprimesocket.sendString("log odom_tf.py disconnecting") # goodbye # MAIN rospy.init_node('odom_tf', anonymous=False) before = rospy.Time.now() br = tf.TransformBroadcaster() odom_pub = rospy.Publisher('odom', Odometry, queue_size=10) rospy.on_shutdown(cleanup) oculusprimesocket.connect() oculusprimesocket.sendString("log odom_tf.py connected") oculusprimesocket.sendString("state odometrybroadcast 250") # ms, needs to be set prior to odometrystart oculusprimesocket.sendString("odometrystart") broadcast("* * 0 0".split()) # broadcast zero odometry baseline while not rospy.is_shutdown(): # t = rospy.get_time() # if t-lastupdate > updateinterval: # request odometry update # oculusprimesocket.sendString("odometryreport") s = oculusprimesocket.replyBufferSearch("<state> distanceangle ") if not s=="": broadcast(s.split()) # lastupdate = now.to_sec()
# MAIN # rospy.init_node('dwa_base_controller', anonymous=False) rospy.init_node('global_path_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 global_path_follower.py connected") # oculusprimesocket.sendString("state odomturndpms "+str(degperms)) # degrees per ms # oculusprimesocket.sendString("state odomturnpwm 100") # approx starting point smooth floor # oculusprimesocket.sendString("state odomlinearmpms "+str(meterspersec/1000)) # oculusprimesocket.sendString("state odomlinearpwm 150") # approx starting point # oculusprimesocket.sendString("speed "+str(linearspeed) ) while not rospy.is_shutdown(): t = rospy.get_time() if t >= nextmove: # nextmove = t + listentime if goalseek and (followpath or goalpose): move(odomx, odomy, odomth, targetx, targety, targetth, goalth) # blocking nextmove = rospy.get_time() + listentime
def cleanup(): # oculusprimesocket.sendString("move stop") # oculusprimesocket.sendString("state delete navigationenabled") oculusprimesocket.sendString("log global_path_follower.py disconnecting")
oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosamcl") oculusprimesocket.sendString("state delete rosglobalpath") oculusprimesocket.sendString("state delete rosscan") oculusprimesocket.sendString("messageclients navigation disabled") # main oculusprimesocket.connect() rospy.init_node('remote_nav', anonymous=False) if os.path.exists(lockfilepath): os.remove(lockfilepath) oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosamcl") oculusprimesocket.sendString("state delete rosglobalpath") oculusprimesocket.sendString("state delete rosmapinfo") oculusprimesocket.sendString("state delete rosscan") rospy.Subscriber("map", OccupancyGrid, mapcallBack) rospy.Subscriber("odom", Odometry, odomCallback) rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, amclPoseCallback) rospy.Subscriber("move_base/goal", MoveBaseActionGoal, goalCallback) rospy.Subscriber("move_base/DWAPlannerROS/global_plan", Path, globalPathCallback) rospy.Subscriber("scan", LaserScan, scanCallback) rospy.Subscriber("move_base/feedback", MoveBaseActionFeedback, feedbackCallback) initpose_pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10) # goal_pub = rospy.Publisher('move_base/goal', MoveBaseActionGoal, queue_size=10)
# rospy.init_node('dwa_base_controller', anonymous=False) rospy.init_node('global_path_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 global_path_follower.py connected") # oculusprimesocket.sendString("state odomturndpms "+str(degperms)) # degrees per ms # oculusprimesocket.sendString("state odomturnpwm 100") # approx starting point smooth floor # oculusprimesocket.sendString("state odomlinearmpms "+str(meterspersec/1000)) # oculusprimesocket.sendString("state odomlinearpwm 150") # approx starting point # oculusprimesocket.sendString("speed "+str(linearspeed) ) while not rospy.is_shutdown(): t = rospy.get_time() if t >= nextmove: # nextmove = t + listentime if goalseek and (followpath or goalpose): move(odomx, odomy, odomth, targetx, targety, targetth, goalth) # blocking
def cleanup(): oculusprimesocket.sendString("odometrystop") oculusprimesocket.sendString("state stopbetweenmoves false") oculusprimesocket.sendString("move stop")
oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosgoalcancel") globalpath = [] recoveryrotate = False # main oculusprimesocket.connect() rospy.init_node('remote_nav', anonymous=False) if os.path.exists(lockfilepath): os.remove(lockfilepath) oculusprimesocket.sendString("log remote_nav.py connected") oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosamcl") oculusprimesocket.sendString("state delete rosglobalpath") oculusprimesocket.sendString("state delete rosmapinfo") oculusprimesocket.sendString("state delete rosscan") oculusprimesocket.sendString("state delete rosgoalstatus") rospy.Subscriber("map", OccupancyGrid, mapcallBack) rospy.Subscriber("odom", Odometry, odomCallback) rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, amclPoseCallback) rospy.Subscriber("move_base/goal", MoveBaseActionGoal, goalCallback) rospy.Subscriber("move_base/DWAPlannerROS/global_plan", Path, globalPathCallback) rospy.Subscriber("scan", LaserScan, scanCallback) rospy.Subscriber("move_base/feedback", MoveBaseActionFeedback,
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)
s = "state rosscan " step = 8 size = len(scanpoints) i = 0 while i < size-step: s += str(round(scanpoints[i],3))+"," i += step s += str(round(scanpoints[size-1],3)) oculusprimesocket.sendString(s) # main oculusprimesocket.connect() oculusprimesocket.sendString("state delete roscurrentgoal") oculusprimesocket.sendString("state delete rosamcl") oculusprimesocket.sendString("state delete rosglobalpath") oculusprimesocket.sendString("state delete rosmapinfo") oculusprimesocket.sendString("state delete rosscan") oculusprimesocket.sendString("state delete rosmapupdated") rospy.init_node('map_remote', anonymous=False) if os.path.exists(lockfilepath): os.remove(lockfilepath) rospy.Subscriber("map", OccupancyGrid, mapcallBack) # rospy.Subscriber("scan", LaserScan, scanCallback)
#!/usr/bin/env python import rospy import oculusprimesocket from std_msgs.msg import String def callback(msg): if (msg.data == "disconnected"): oculusprimesocket.sendString("driverexit") """main""" oculusprimesocket.connect() rospy.init_node('webrtc_status_listener', anonymous=True) oculusprimesocket.sendString("log webrtc_status_listener.py connected") rospy.Subscriber("webrtcstatus", String, callback) rospy.spin()