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))
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
#!/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
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)
