Exemplo n.º 1
0
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")
Exemplo n.º 2
0
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")
Exemplo n.º 5
0
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")
Exemplo n.º 7
0
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")
Exemplo n.º 8
0
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)
Exemplo n.º 9
0
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)
Exemplo n.º 10
0
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)
Exemplo n.º 11
0
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)
Exemplo n.º 12
0
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")  
Exemplo n.º 15
0
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)
Exemplo n.º 16
0
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")
Exemplo n.º 17
0
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
Exemplo n.º 18
0
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
Exemplo n.º 19
0
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
Exemplo n.º 20
0
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
Exemplo n.º 22
0
def cleanup():
    oculusprimesocket.sendString("move stop")
Exemplo n.º 23
0
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"
Exemplo n.º 24
0
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
Exemplo n.º 25
0
	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
Exemplo n.º 27
0

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()
Exemplo n.º 29
0
def cleanup():
	oculusprimesocket.sendString("move stop")
Exemplo n.º 30
0
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 
Exemplo n.º 35
0
def cleanup():
    oculusprimesocket.sendString("move stop")
    oculusprimesocket.sendString("state delete navigationenabled")
Exemplo n.º 36
0
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()
Exemplo n.º 37
0
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")
Exemplo n.º 38
0
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)
Exemplo n.º 40
0
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")	
Exemplo n.º 41
0
def cleanup():
    oculusprimesocket.sendString("state delete odometrybroadcast")
    oculusprimesocket.sendString("odometrystop")
    oculusprimesocket.sendString("state delete navigationenabled")
Exemplo n.º 42
0
def callback(msg):
	if (msg.data == "disconnected"):
		oculusprimesocket.sendString("driverexit")  
Exemplo n.º 43
0
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()
Exemplo n.º 44
0
	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
Exemplo n.º 46
0
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")   
Exemplo n.º 48
0
	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")
Exemplo n.º 52
0
    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,
Exemplo n.º 53
0
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)
Exemplo n.º 54
0
	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)

Exemplo n.º 55
0
#!/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")