Exemplo n.º 1
0
 def drive_robot(self):
     import oculusprimesocket as oc
     
     # connect to robot server
     result = oc.connect()
     
     # initialize
     for i in range(7):
         evbuf = self.jsdev.read(8)
         
     # Main event loop to poll for joystick state changes
     while result:
         evbuf = self.jsdev.read(8)
         if evbuf:
             time, value, type, number = struct.unpack('IhBB', evbuf)
     
             if type & 0x01:
                 button = self.button_map[number]
                 if button:
                     self.button_states[button] = value
                     if(button == 'a'):
                         oc.sendString('move forward')
                     if(button == 'b'):
                         oc.sendString('move stop')
                     if(button == 'minus'):
                         oc.sendString('nudge left')
                     if(button == 'plus'):
                         oc.sendString('nudge right')
                         
                     if(self.debug_mode):
                         if value:
                             print("%s pressed" % (button))
                             if(button == 'a'):
                                 oc.sendString('speech moving forward')
                             if(button == 'b'):
                                 oc.sendString('speech stopping')
                             if(button == 'minus'):
                                 oc.sendString('speech left')
                             if(button == 'plus'):
                                 oc.sendString('speech right')
                         else:
                             print("%s released" % (button))
Exemplo n.º 2
0
        oculusprimesocket.waitForReplySearch("<state> direction stop")

    if goalrotate:
        rospy.sleep(1)


def cleanup():
    oculusprimesocket.sendString("move stop")
    oculusprimesocket.sendString("state delete navigationenabled")


# MAIN

rospy.init_node('dwa_base_controller', anonymous=False)
listener = tf.TransformListener()
oculusprimesocket.connect()

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)
rospy.on_shutdown(cleanup)

while not rospy.is_shutdown():
    t = rospy.get_time()

    if t >= nextmove:
        # nextmove = t + listentime
	# if goalrotate:
		# rospy.sleep(1) 
		
			
def cleanup():
	# oculusprimesocket.sendString("move stop")
	# oculusprimesocket.sendString("state delete navigationenabled")
	oculusprimesocket.sendString("log global_path_follower.py disconnecting")   


# 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
Exemplo n.º 4
0
#!/usr/bin/python

import rospy
from std_msgs.msg import String, Int64
from collections import deque
import time
import oculusprimesocket

rospy.init_node("master_autopark")
blue_push = rospy.Publisher("blue_push", String, queue_size=5)
serial_push = rospy.Publisher("xbee_update", String, queue_size=2)
last_command = deque(maxlen=2) # only remember the last command and the current one
last_command.append("Idle") # initial state
oculusprimesocket.connect() # initiate connection to oculus prime
flag = 0 # 1 = obstacle, don't send commands
apptoxbee = {"2": "park", "4": "return"}

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
Exemplo n.º 5
0
 def __init__(self):
     #connect to the oculus server
     result = oc.connect()
     return result
def main(args=None):
	global rest
	
	rospy.init_node('autocrawler_openmanipulator', anonymous=False)
	rospy.Subscriber("open_manipulator/joint_states", JointState, jointCallback) 
	
	oculusprimesocket.connect()	
	oculusprimesocket.sendString("log autocrawler_openmanipulator.py connected")  

	# jointpos = JointPosition(["joint1","joint2","joint3","joint4","gripper"],[0,0,0,0],1,1)

	while not rospy.is_shutdown():
		s = oculusprimesocket.replyBufferSearch("<state> arm")
		
		if re.search("arm on", s): # enable
			oculusprimesocket.sendString("malgcommand W")
			# TODO: send open_manipulator_controller roslaunch command 
			
		elif re.search("arm off", s):
			if not rest:
				moveToHome()
				rospy.sleep(1.5)
				armrest()
				rospy.sleep(2)
			os.system('rosnode kill /open_manipulator')
			rospy.sleep(1)
			oculusprimesocket.sendString("malgcommand Q")
			oculusprimesocket.sendString("messageclients openmanipulator off")  
			oculusprimesocket.sendString("state delete arm")  
			rospy.signal_shutdown("exit")
			# TODO: kill open_manipulator_controller roslaunch 
		
		elif re.search("arm enable", s): # enable
			rospy.wait_for_service('open_manipulator/set_actuator_state')
			set_actuator_state = rospy.ServiceProxy('open_manipulator/set_actuator_state', SetActuatorState)
			set_actuator_state(True)
			
		elif re.search("arm disable", s): # disable
			rospy.wait_for_service('open_manipulator/set_actuator_state')
			set_actuator_state = rospy.ServiceProxy('open_manipulator/set_actuator_state', SetActuatorState)
			if not rest:
				moveToHome()
				rospy.sleep(1.5)
				armrest()
				rospy.sleep(2)
			set_actuator_state(False)
		
		elif re.search("arm open", s): # open TODO: BROKE
			print("open")
			gripperopen()
			
		elif re.search("arm close", s): # open TODO: BROKE
			print("close")
			gripperclose()
			
		elif re.search("arm home", s): # home
			print("home")
			moveToHome()
			
		elif re.search("arm rest", s): # rest
			print("rest")
			armrest()
		
		elif re.search("arm floor", s): # rest
			print("floor")
			moveToFloor()
		
		elif re.search("arm forward", s): # forward
			print("forward")
			armForward()
		
		elif re.search("arm record", s): # record1
			print("record"+s[-1])
			num = int(s[-1])-1
			recordedJointPositions[num] = getJointPosition()
			printJointPos(recordedJointPositions[num])
			
		elif re.search("arm goto", s): # rest
			print("goto"+s[-1])
			num = int(s[-1])-1
			if not recordedJointPositions[num] == None:
				rospy.wait_for_service('open_manipulator/goal_joint_space_path')
				set_joint_position = rospy.ServiceProxy('open_manipulator/goal_joint_space_path', SetJointPosition)
				newjointpositions = getJointPosition()
				newjointpositions.position[0] = recordedJointPositions[num].position[0]
				newjointpositions.position[1] = recordedJointPositions[num].position[1]
				newjointpositions.position[2] = recordedJointPositions[num].position[2]
				newjointpositions.position[3] = recordedJointPositions[num].position[3]
				set_joint_position('', newjointpositions , 2)
				rest = False
			
		elif re.search("arm pickup", s):
			gripperopen()
			moveToHome()
			rospy.sleep(1.5)
			moveToFloor()
			rospy.sleep(2)
			gripperclose()
			rospy.sleep(2)
			moveToHome()
			
		elif re.search("arm grab", s):
			gripperopen()
			moveToHome()
			rospy.sleep(1)
			armForward()
			rospy.sleep(2)
			gripperclose()
			rospy.sleep(2.5)
			moveToHome()
			rospy.sleep(1)
			gripperopen()
			rospy.sleep(3)
			armrest()
			gripperclose()
		
		rospy.sleep(0.01)