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 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 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("move stop")
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 roscurrentgoal")
oculusprimesocket.sendString("state delete rosamcl")
oculusprimesocket.sendString("state delete rosglobalpath")
oculusprimesocket.sendString("state delete rosscan")
oculusprimesocket.sendString("log remote_nav.py disconnecting")
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("state delete odometrybroadcast")
oculusprimesocket.sendString("odometrystop")
# oculusprimesocket.sendString("state delete navigationenabled")
oculusprimesocket.sendString("log odom_tf.py disconnecting") # goodbye
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")
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 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()
def cleanup():
oculusprimesocket.sendString("odometrystop")
oculusprimesocket.sendString("state stopbetweenmoves false")
oculusprimesocket.sendString("move stop")
