def approach():
# Description: Drive within a certian distace of the tag 'targetTagID'
# Return
# null
# Argument
# targetTagID - The ID of the tag that we wish to point the robot's camera towards
global tagPose
#relX=tagPose.pose.pose.position.x
#relZ=tagPose.pose.pose.position.z
approached = False
jcv = JoyCmd()
jcv.axis3 = 0.0
jcv.btn1 = 0.0
jcv.btn2 = 0.0
jcv.btn3 = 0.0
while approached == False:
if rospy.is_shutdown():
break
if tagPose != None:
relX = tagPose.pose.pose.position.x
relZ = tagPose.pose.pose.position.z
#print('Approaching x=', relX)
#print('Approaching z=', relZ)
vRel = np.array([relZ, relX])
relPosNorm = np.linalg.norm(vRel)
relPosUnitVec = vRel / relPosNorm
thetaDot = 0
print relPosNorm
if relPosNorm > .5:
zDot = relPosUnitVec[0]
xDot = relPosUnitVec[1]
else:
zDot = 0
xDot = 0
approached = True
else:
zDot = 0
xDot = 0
print 'lost tag'
jcv.axis1 = xDot
jcv.axis2 = zDot
virtualJoy_pub.publish(jcv)
r.sleep()
print('Arrived at tag')
def __init__(self):
rospy.init_node('keyboard', anonymous=True)
#Publishers
self.joyconnPub= rospy.Publisher('/joy/connected', Bool, queue_size = 1)
self.joycmdPub = rospy.Publisher('/joy/cmd', JoyCmd, queue_size = 10)
self.setTagPub = rospy.Publisher('set_current_tag', Int32, queue_size = 1)
self.setReadyArmPub = rospy.Publisher('ready_for_arm_mov', Bool, queue_size = 1)
self.setReadyVehPub = rospy.Publisher('ready_for_veh_mov', Bool, queue_size = 1)
self.setApproachRetreatPub = rospy.Publisher('set_approach_retreat', Bool, queue_size = 1)
self.setStopAutomodePub = rospy.Publisher('stop_automode', Bool, queue_size = 1)
r = rospy.Rate(60)
self.cmd_msg = JoyCmd()
self.conn_msg = Bool()
self.conn_msg.data = True
self.mult = 1.0/3.0
#Initialize state variables
self.set_current_tag_msg = int()
self.setReadyArm_msg = Bool()
self.setReadyArm_msg.data = False
self.setReadyVeh_msg = Bool()
self.setReadyVeh_msg.data = False
self.setApproachRetreatPub_msg = Bool()
self.setApproachRetreatPub_msg.data = False
self.setStopAutomodePub_msg = Bool()
self.setStopAutomodePub_msg.data = False
print(msg_text)
while not rospy.is_shutdown():
self.cmd_msg = self.detectCmds()
# Publish at a frequency of 60 Hz
self.joyconnPub.publish(self.conn_msg)
self.joycmdPub.publish(self.cmd_msg)
#Detect other inputs from the keyboard
#Detect a commanded tag number
r.sleep()
def __init__(self):
rospy.init_node('keyboard', anonymous=True)
self.joyconnPub = rospy.Publisher('/joy/connected', Bool, queue_size=1)
self.joycmdPub = rospy.Publisher('/joy/cmd', JoyCmd, queue_size=10)
r = rospy.Rate(60)
self.cmd_msg = JoyCmd()
self.conn_msg = Bool()
self.conn_msg.data = True
while not rospy.is_shutdown():
self.cmd_msg = self.detectCmds()
# Publish at a frequency of 60 Hz
self.joyconnPub.publish(self.conn_msg)
self.joycmdPub.publish(self.cmd_msg)
r.sleep()
def pointAtTag(targetTagID):
# Description: Search for the specified tag
# Return
# void - Prints a message when we are pointing at a tag
# Argument
# targetTagID - The ID of the tag that we wish to point the robot's camera towards
zDot = 0
xDot = 0
rate = .25
pointed = False
global tagPose
global tagID
jcv = JoyCmd()
jcv.axis1 = 0.0
jcv.axis2 = 0.0
jcv.btn1 = 0.0
jcv.btn2 = 0.0
jcv.btn3 = 0.0
while pointed == False:
if rospy.is_shutdown():
break
# Spin around and look for the tag 'locatedTag.label.' Stop once we are pointing at it within a small window for error
if targetTagID == tagID and tagPose != None: # We have found the tag that we were looking for.
relX = tagPose.pose.pose.position.x
#print('tag position = ', relX)
if relX < .05 and relX > -.05: # If we are pointing at the tag
thetaDot = 0
pointed = True
else:
pointed = False
else:
thetaDot = rate
pointed = False
jcv.axis3 = thetaDot
virtualJoy_pub.publish(jcv)
print('Pointed at tag')
def pointAtTag(targetTagID):
# Description: Search for the specified tag
# Return
# void - Prints a message when we are pointing at a tag
# Argument
# targetTagID - The ID of the tag that we wish to point the robot's camera towards
print("starting to point at target")
zDot=0
xDot=0
rate=.3 # Maybe make rate dependent upon angle
pointed = False
jcv = JoyCmd()
jcv.axis1 = 0.0
jcv.axis2 = 0.0
jcv.btn1 = 0.0
jcv.btn2 = 0.0
jcv.btn3 = 0.0
while pointed == False:
if rospy.is_shutdown():
break
# Spin around and look for the tag 'locatedTag.label.' Stop once we are pointing at it within a small window for error
if targetTagID == tagID and tagPose != None: # We have found the tag that we were looking for.
relX=tagPose.pose.pose.position.x
print('tag position = ', relX)
if relX<.1 and relX>-.1: # If we are pointing at the tag -> original values were .05
thetaDot=0
pointed = True
else:
pointed = False
#thetaDot=rate
else:
thetaDot=rate
pointed = False
if not stopAuto:
jcv.axis3 = thetaDot
virtualJoy_pub.publish(jcv)
print('Pointed at tag = ', tagID)
def approachRetreat():
# Description: Drive within a certain distace of the tag 'targetTagID'
# Return
# null
# Argument
# targetTagID - The ID of the tag that we wish to point the robot's camera towards
#relX=tagPose.pose.pose.position.x
#relZ=tagPose.pose.pose.position.z
approached=False
jcv = JoyCmd()
jcv.axis3 = 0.0
jcv.btn1 = 0.0
jcv.btn2 = 0.0
jcv.btn3 = 0.0
while approached==False:
if rospy.is_shutdown():
break
if tagPose != None:
relX=tagPose.pose.pose.position.x
relZ=tagPose.pose.pose.position.z
q_x = tagPose.pose.pose.orientation.x
q_y = tagPose.pose.pose.orientation.y
q_z = tagPose.pose.pose.orientation.z
q_w = tagPose.pose.pose.orientation.w
relRollX, relPitchY, relYawZ = euler_from_quaternion(q_x, q_y, q_z, q_w)
xDot = 0.0
zDot = 0.0
thetaDot = 0.0
print('Approaching x=', relX)
print('Approaching z=', relZ)
print('Approaching relYawZ =', relYawZ)
print('Approaching relRollX =', relRollX)
print('Approaching relPitchY =', relPitchY)
# Error criteria for z distance away is 'tagAppDist'
# Error criteria for x distance away is hard coded below
xMov = 0.0
zMov = 0.0
yawMov = 0.0
# Set the x, z, and theta velocities
if abs(relX) > .01:
# keep moving in the x (sideways) until error is small
xMov = relX
# Adjust the zMoz depending on if we are approaching or retreating
if ( (abs(relZ) > tagAppDist) and appret ):
# keep moving in the z (forwards) until within range
zMov = relZ
print("Approaching tag ", tagID)
if ( (abs(relZ) < tagRetDist) and (not appret ) ):
# keep moving in the negative z direction until within range
error = -1.0 *( tagRetDist - relZ) -.3 # This should always be a negative number
zMov = error
print("Retreating from tag ", tagID)
# if abs(relPitchY) > .05:
# keep rotating about towards the center of the tag
# yawMov = relPitchY
# normalize the commands
vRel=np.array([xMov,zMov])
relPosNorm=np.linalg.norm(vRel) # This relative position vector only involves X,Z,
# not orientation (assumes X within range based on line 59
if relPosNorm > 0:
# we need to move
# Calculate speed and cap it
speedX = 6
speedZ = 0.2
xDot = -xMov * speedX
zDot = zMov * speedZ
if xDot > .3:
xDot = .3
if zDot > .3:
zDot = .3
# if yawMov > 0:
# we need to rotate
# thetaDot = -yawMov * 3 # hard code for now
if (relPosNorm == 0.0) and (yawMov == 0.0): # yawMov should always be 0
# We have arrived - both translation and rotation errors are within limits
xDot=0
zDot=0
thetaDot = 0.0
approached=True
at_target_pub.publish(tagID)
else:
zDot=0
xDot=0
print 'Obstacle in the way!!'
if not stopAuto:
jcv.axis1 = xDot
jcv.axis2 = zDot
#jcv.axis3 = thetaDot
#print( "Sending motor commands: Axis1 = ", xDot, " Axis2 = ", zDot, " Axis3 = ", thetaDot)
print("Sending motor commands: Axis1 = ", xDot, " Axis2 = ", zDot)
virtualJoy_pub.publish(jcv)
r.sleep()
print('Arrived at Position')
#stopAuto = False # set for testing
# This is the main loop
first_time = True
while not rospy.is_shutdown():
#target = 1 #Hardcode to whatever tag is in video feed for testing
if not stopAuto:
print("Start aprilTagController")
pointAtTag(target)
approachRetreat()
print 'Done'
first_time = True
elif (stopAuto and first_time):
jcv = JoyCmd()
jcv.axis1 = 0.0
jcv.axis2 = 0.0
jcv.axis3 = 0.0
jcv.btn1 = 0.0
jcv.btn2 = 0.0
jcv.btn3 = 0.0
virtualJoy_pub.publish(jcv)
first_time = False
r.sleep()
# Finally, stop the robot when shutting down
jcv = JoyCmd()
jcv.axis1 = 0.0
jcv.axis2 = 0.0
def detectCmds(self):
msg = JoyCmd()
mult = 1.0 / 3.0
#Detect forward and backward keys assigned to 'w' and 's'
if keyboard.is_pressed('w'):
msg.axis1 = 1.0
if keyboard.is_pressed('s'):
msg.axis1 = msg.axis1 - 1.0
#Detect left and right keys 'a' and 'd'
if keyboard.is_pressed('a'):
msg.axis2 = 1.0
if keyboard.is_pressed('d'):
msg.axis2 = msg.axis2 - 1.0
#Detect rotation keys 'q' for counterclockwise, 'e' for clockwise
if keyboard.is_pressed('q'):
msg.axis3 = 1.0
if keyboard.is_pressed('e'):
msg.axis3 = msg.axis3 - 1.0
#Detect keys for multipliers
if keyboard.is_pressed('alt'):
mult = 2.0 / 3.0
if keyboard.is_pressed('shift'):
mult = 1.0
msg.axis1 = msg.axis1 * mult
msg.axis2 = msg.axis2 * mult
msg.axis3 = msg.axis3 * mult
return msg
def detectCmds(self): char = self.getch() #print(char) msg = JoyCmd() msg.axis1 = 0 msg.axis2 = 0 msg.axis3 = 0 #Non-Holonomic Steering #Detect forward and backward keys assigned to 'w' and 's' #-> changed to 'i' and ',', (might need to the axis number was changed from 1 to 2) if 'i' in char: msg.axis2 = 1.0 elif ',' in char: msg.axis2 = msg.axis2 - 1.0 #Detect rotation keys 'q' for counterclockwise, 'e' for clockwise #-> changed to 'j' and 'l', (might need to be flipped -but then flipped after first test) elif 'l' in char: msg.axis3 = 1.0 elif 'j' in char: msg.axis3 = msg.axis3 - 1.0 # Holonomic Steering #Detect forward and backward keys assigned to 'w' and 's' #-> changed to 'I' and '<', (might need to the axis number was changed from 1 to 2) if 'I' in char: msg.axis2 = 1.0 elif '<' in char: msg.axis2 = msg.axis2 - 1.0 #Detect left and right keys 'a' and 'd' #-> changed to 'J' and 'L' elif 'J' in char: msg.axis1 = 1.0 elif 'L' in char: msg.axis1 = msg.axis1 - 1.0 #Detect corner keys to move diagonally elif 'U' in char: msg.axis2 = 0.71 msg.axis1 = 0.71 elif 'O' in char: msg.axis2 = 0.71 msg.axis1 = msg.axis1 - 0.71 elif '>' in char: msg.axis2 = msg.axis2 - 0.71 msg.axis1 = msg.axis1 - 0.71 elif 'M' in char: msg.axis2 = msg.axis2 - 0.71 msg.axis1 = 0.71 #Detect keys for multipliers elif 'z' in char: self.mult = 0.8*self.mult self.printStatus( "currently:\tspeed %s " % (self.mult)) elif 'x' in char: self.mult = 1.2*self.mult self.printStatus( "currently:\tspeed %s " % (self.mult)) #Dummy character that also works to stop the vehicle elif 'k' in char: msg.axis1 = 0 msg.axis2 = 0 msg.axis3 = 0 self.printStatus( "currently:\t Vehicle Stopped!") elif 'K' in char: msg.axis1 = 0 msg.axis2 = 0 msg.axis3 = 0 #pass self.printStatus( "currently:\t Vehicle Stopped!") #Detect other inputs from the keyboard #Detect keyboard press to override the booleans elif 't' in char: self.setReadyArm_msg.data = True self.setReadyArmPub.publish(self.setReadyArm_msg) self.printStatus( "currently:\t Sent ready_for_arm_mov = True") elif 'f' in char: self.setReadyArm_msg.data = False self.setReadyArmPub.publish(self.setReadyArm_msg) self.printStatus( "currently:\t Sent ready_for_arm_mov = False") elif 'r' in char: self.setReadyVeh_msg.data = True self.setReadyVehPub.publish(self.setReadyVeh_msg) self.printStatus( "currently:\t Sent ready_for_veh_mov = True") elif 'd' in char: self.setReadyVeh_msg.data = False self.setReadyVehPub.publish(self.setReadyVeh_msg) self.printStatus( "currently:\t Sent ready_for_veh_mov = False") elif 'e' in char: self.setApproachRetreatPub_msg.data = True self.setApproachRetreatPub.publish(self.setApproachRetreatPub_msg) self.printStatus( "currently:\t Sent set_approach_retreat = True") elif 's' in char: self.setApproachRetreatPub_msg.data = False self.setApproachRetreatPub.publish(self.setApproachRetreatPub_msg) self.printStatus( "currently:\t Sent set_approach_retreat = False") elif 'w' in char: self.setStopAutomodePub_msg.data = True self.setStopAutomodePub.publish(self.setStopAutomodePub_msg) self.printStatus( "currently:\t Sent stop_automode = True") elif 'a' in char: self.setStopAutomodePub_msg.data = False self.setStopAutomodePub.publish(self.setStopAutomodePub_msg) self.printStatus( "currently:\t Sent stop_automode = False") else: # Check if one of the tag numbers was pressed for number in range(1,6): if str(number) in char: self.set_current_tag_msg = int(number) self.setTagPub.publish(self.set_current_tag_msg) self.printStatus( "currently:\t Sent set_current_tag = %s " % (number)) msg.axis1 = msg.axis1*self.mult msg.axis2 = msg.axis2*self.mult msg.axis3 = msg.axis3*self.mult return msg
def __init__(self):
rospy.init_node('keyboard', anonymous=True)
#Publishers
self.joyconnPub = rospy.Publisher('/joy/connected', Bool, queue_size=1)
self.joycmdPub = rospy.Publisher('/joy/cmd', JoyCmd, queue_size=10)
self.setTagPub = rospy.Publisher('set_current_tag',
Int32,
queue_size=1)
self.setReadyArmPub = rospy.Publisher('ready_for_arm_mov',
Bool,
queue_size=1)
self.setReadyVehPub = rospy.Publisher('ready_for_veh_mov',
Bool,
queue_size=1)
r = rospy.Rate(60)
self.cmd_msg = JoyCmd()
self.conn_msg = Bool()
self.conn_msg.data = True
self.set_current_tag_msg = int()
self.setReadyArm_msg = Bool()
self.setReadyArm_msg.data = False
self.setReadyVeh_msg = Bool()
self.setReadyVeh_msg.data = False
print(msg)
while not rospy.is_shutdown():
self.cmd_msg = self.detectCmds()
# Publish at a frequency of 60 Hz
self.joyconnPub.publish(self.conn_msg)
self.joycmdPub.publish(self.cmd_msg)
#Detect other inputs from the keyboard
#Detect a commanded tag number
for number in range(1, 6):
if keyboard.is_pressed(str(number)):
self.set_current_tag_msg = int(number)
self.setTagPub.publish(self.set_current_tag_msg)
if keyboard.is_pressed('t'):
self.setReadyArm_msg.data = True
self.setReadyArmPub.publish(self.setReadyArm_msg)
if keyboard.is_pressed('f'):
self.setReadyArm_msg.data = False
self.setReadyArmPub.publish(self.setReadyArm_msg)
if keyboard.is_pressed('r'):
self.setReadyVeh_msg.data = True
self.setReadyVehPub.publish(self.setReadyVeh_msg)
if keyboard.is_pressed('d'):
self.setReadyVeh_msg.data = False
self.setReadyVehPub.publish(self.setReadyVeh_msg)
r.sleep()
def detectCmds(self):
msg = JoyCmd()
mult = 1.0 / 3.0
#Holonomic Steering
if keyboard.is_pressed('shift'):
#Detect forward and backward keys
if keyboard.is_pressed('I'):
msg.axis1 = 1.0
if keyboard.is_pressed('<'):
msg.axis1 = msg.axis1 - 1.0
#Detect left and right keys 'J' and 'L'
if keyboard.is_pressed('J'):
msg.axis2 = 1.0
if keyboard.is_pressed('L'):
msg.axis2 = msg.axis2 - 1.0
#Detect corner keys to move diagonally
if keyboard.is_pressed('U'):
msg.axis1 = 0.71
msg.axis2 = 0.71
if keyboard.is_pressed('O'):
msg.axis1 = 0.71
msg.axis2 = msg.axis2 - 0.71
if keyboard.is_pressed('>'):
msg.axis1 = msg.axis1 - 0.71
msg.axis2 = msg.axis2 - 0.71
if keyboard.is_pressed('M'):
msg.axis1 = 0.71
msg.axis2 = msg.axis2 - 0.71
else:
#Non-Holonomic Steering
#Detect forward and backward keys
if keyboard.is_pressed('i'):
msg.axis1 = 1.0
if keyboard.is_pressed(','):
msg.axis1 = msg.axis1 - 1.0
#Detect rotation keys 'j' for counterclockwise, 'l' for clockwise
if keyboard.is_pressed('j'):
msg.axis3 = 1.0
if keyboard.is_pressed('l'):
msg.axis3 = msg.axis3 - 1.0
#Detect keys for multipliers
if keyboard.is_pressed('alt'):
mult = 0.25 * mult
#if keyboard.is_pressed('shift'):
# mult = 1.0
msg.axis1 = msg.axis1 * mult
msg.axis2 = msg.axis2 * mult
msg.axis3 = msg.axis3 * mult
return msg