def getSpheroTwist(self, name, move_id):
twist = SpheroTwist()
twist.name = name
if move_id == 0:
twist.linear.x = -STEP_LENGTH; twist.linear.y = STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif move_id == 1:
twist.linear.x = 0; twist.linear.y = STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif move_id == 2:
twist.linear.x = STEP_LENGTH; twist.linear.y = STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif move_id == 3:
twist.linear.x = -STEP_LENGTH; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif move_id == 4:
twist.linear.x = STEP_LENGTH; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif move_id == 5:
twist.linear.x = -STEP_LENGTH; twist.linear.y = -STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif move_id == 6:
twist.linear.x = 0; twist.linear.y = -STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif move_id == 7:
twist.linear.x = STEP_LENGTH; twist.linear.y = -STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
return twist
def aprtCallback(self, msg):
# print msg.id
if not self.initialized: # still initializing
return
for i in msg.id:
if self.order[0] == i:
firstInd = msg.id.index(self.order[0])
self.location[self.order[0]] = (msg.pose[firstInd].x,
msg.pose[firstInd].y)
continue
twist = SpheroTwist()
twist.name = self.numToSphero[i]
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
msgIndex = msg.id.index(i)
self.location[i] = (msg.pose[msgIndex].x, msg.pose[msgIndex].y)
me = i
e_x = self.destination[me][0] - self.location[me][0]
e_y = self.destination[me][1] - self.location[me][1]
# print self.numToSphero[i]
# print "Location (%d,%d)" % self.location[me]
# print "Destination (%d,%d)" % self.destination[me]
distance = math.sqrt((e_x * e_x) + (e_y * e_y))
#print "Distance: %d" % distance
#print "(ex,ey): (%d,%d)" %(e_x, e_y)
if distance <= RADIUS or self.destination[me] == (-1, -1):
print "Radius"
leader = self.order[self.order.index(me) - 1]
self.destination[me] = self.location[leader]
self.error[me] = 0
continue
elif self.count % 8 == 0:
print "reset"
leader = self.order[self.order.index(me) - 1]
self.destination[me] = self.location[leader]
self.error[me] = (0, 0)
e_x = self.destination[me][0] - self.location[me][0]
e_y = self.destination[me][1] - self.location[me][1]
distance = math.sqrt((e_x * e_x) + (e_y * e_y))
distance -= RADIUS
omega = math.atan2(e_y, e_x)
de = distance - self.serror
self.error[me] = distance
v = (KP * distance) + (KD * de)
max(v, MAXSPEED)
twist.linear.x = v * math.cos(omega)
twist.linear.y = -v * math.sin(omega)
self.cmdVelPub.publish(twist)
self.count += 1
print "count: %d" % self.count
def keyPressEvent(self, e):
twist = None
print "key pressed"
selected_items = self.spheroListWidget.selectedItems()
if len(selected_items) == 0:
return
print "selected"
if e.key() == QtCore.Qt.Key_U:
twist = SpheroTwist()
twist.linear.x = -STEP_LENGTH; twist.linear.y = STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif e.key() == QtCore.Qt.Key_I:
twist = SpheroTwist()
twist.linear.x = 0; twist.linear.y = STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif e.key() == QtCore.Qt.Key_O:
twist = SpheroTwist()
twist.linear.x = STEP_LENGTH; twist.linear.y = STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif e.key() == QtCore.Qt.Key_J:
twist = SpheroTwist()
twist.linear.x = -STEP_LENGTH; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif e.key() == QtCore.Qt.Key_K:
twist = SpheroTwist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif e.key() == QtCore.Qt.Key_L:
twist = SpheroTwist()
twist.linear.x = STEP_LENGTH; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif e.key() == QtCore.Qt.Key_M:
twist = SpheroTwist()
twist.linear.x = -STEP_LENGTH; twist.linear.y = -STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif e.key() == QtCore.Qt.Key_Comma:
twist = SpheroTwist()
twist.linear.x = 0; twist.linear.y = -STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
elif e.key() == QtCore.Qt.Key_Period:
twist = SpheroTwist()
twist.linear.x = STEP_LENGTH; twist.linear.y = -STEP_LENGTH; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
if twist != None:
twist.name = str(selected_items[0].text())
self.cmdVelPub.publish(twist)
def aprtCallback(self, msg):
if not self.initialized: #still initializing
return
for key in self.location:
self.location[key] = (-1, -1)
for i in range(0, len(msg.id)):
self.location[msg.id[i]] = (msg.pose[i].x, msg.pose[i].y)
print str(msg.id[i]) + " " + str(self.location[msg.id[i]])
for key in msg.id:
twist = SpheroTwist()
twist.name = self.numToSphero[key]
(twist.linear.x, twist.linear.y) = self.getVelocityForSphero(key)
self.cmdVelPub.publish(
twist
) # how to tell sphero to move. all fields in twist must be explicitly set.
def keyPressEvent(self, e):
print "key pressed"
selected_items = self.spheroListWidget.selectedItems()
if len(selected_items) == 0:
return
print "selected"
twist = SpheroTwist()
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
if e.key() == QtCore.Qt.Key_U:
twist.linear.x = -STEP_LENGTH
twist.linear.y = STEP_LENGTH
elif e.key() == QtCore.Qt.Key_I:
twist.linear.y = STEP_LENGTH
elif e.key() == QtCore.Qt.Key_O:
twist.linear.x = STEP_LENGTH
twist.linear.y = STEP_LENGTH
elif e.key() == QtCore.Qt.Key_J:
twist.linear.x = -STEP_LENGTH
# elif e.key() == QtCore.Qt.Key_K:
elif e.key() == QtCore.Qt.Key_L:
twist.linear.x = STEP_LENGTH
elif e.key() == QtCore.Qt.Key_M:
twist.linear.x = -STEP_LENGTH
twist.linear.y = -STEP_LENGTH
elif e.key() == QtCore.Qt.Key_Comma:
twist.linear.y = -STEP_LENGTH
elif e.key() == QtCore.Qt.Key_Period:
twist.linear.x = STEP_LENGTH
twist.linear.y = -STEP_LENGTH
if twist.linear.x != 0 or twist.linear.y != 0:
twist.name = str(selected_items[0].text())
self.cmdVelPub.publish(twist)
def aprtCallback(self, msg):
if not self.initialized: #still initializing
return
for key in self.location:
self.location[key] = (400,300)
for i in range(0,len(msg.id)):
self.location[msg.id[i]] = (msg.pose[i].x, msg.pose[i].y)
#print str(msg.id[i]) + " " + str(self.location[msg.id[i]])
x = self.location[self.PREY_KEY][0] - self.location[self.PRED_KEY][0]
y = self.location[self.PREY_KEY][1] - self.location[self.PRED_KEY][1]
if x * x + y * y < 3025 and self.delay == 0:
TEMP_KEY = self.PRED_KEY
self.PRED_KEY = self.PREY_KEY
self.PREY_KEY = TEMP_KEY
self.delay = 8
for key in msg.id:
twist = SpheroTwist()
twist.name = self.numToSphero[key]
(twist.linear.x, twist.linear.y) = self.getVelocityForSphero(key)
self.cmdVelPub.publish(twist) # how to tell sphero to move. all fields in twist must be explicitly set.
def getTwistFromDirection(self, direction):
twist = SpheroTwist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
if direction == Directions.NORTH:
twist.linear.y = FOLLOW_SPEED
elif direction == Directions.EAST:
twist.linear.x = FOLLOW_SPEED
elif direction == Directions.SOUTH:
twist.linear.y = -FOLLOW_SPEED
elif direction == Directions.WEST:
twist.linear.x = -FOLLOW_SPEED
return twist
def aprtCallback(self, msg):
if not self.initialized: #still initializing
return
# if self.phase == 'initial':
# self.changePhase()
# return;
# if self.phase == 'setup':
# return;
# if self.phase == 'swing':
# if self.ballMoving:
# self.changePhase()
# return;
# if self.phase == 'rolling':
# if not self.ballMoving:
# self.changePhase()
# return;
spheroID = SPHERO_ID
spheroIndex = 0
ballID = BALL_ID
ballIndex = 0
goalIndex = 0
for i in range(len(msg.id)):
if msg.id[i] == ballID:
ballIndex = i
elif msg.id[i] == spheroID:
spheroIndex = i
elif msg.id[i] == GOAL_ID:
goalIndex = i
else:
self.obstacles.append(msg.pose[i])
if self.phase == 'initial':
self.changePhase()
# detect ball moving
if self.location[ballID][0] - msg.pose[
ballIndex].x < 3 and self.location[ballID][1] - msg.pose[
ballIndex].y < 3:
self.ballMoving = False
print "ball not moving"
# set the ballfield location
self.ballField.setX(msg.pose[ballIndex].x)
self.ballField.setY(msg.pose[ballIndex].y)
if self.phase == 'rolling':
self.changePhase()
else:
self.ballMoving = True
print "ball moving"
print self.location[ballID]
self.ballField.setAlpha(0)
if self.phase == 'swing':
self.changePhase()
# setup or swing when ball not moving
if not self.ballMoving:
if self.phase == 'setup':
print "Setup phase"
goalPose = msg.pose[goalIndex]
ballPose = msg.pose[ballIndex]
spheroPose = msg.pose[spheroIndex]
self.nextPoint = self.tree.create(goalPose, ballPose)
#print "Sphero Position: " + str((spheroPose.x, spheroPose.y))
distance = self.ballField.calculateDistance(
self.nextPoint[0], ballPose.x, self.nextPoint[1],
goalPose.y)
theta = self.ballField.calcTheta(self.nextPoint)
#print "Theta: " + str(theta)
deltaX = math.cos(theta)
deltaY = -math.sin(theta)
multiplier = 160
#print "OffsetX & OffsetY: " + str((multiplier * deltaX, multiplier * deltaY))
swingPos = [
msg.pose[ballIndex].x + (multiplier * deltaX),
msg.pose[ballIndex].y + (multiplier * deltaY)
]
print "Swing Position: " + str(swingPos)
SetupField = AttractiveField(-1, swingPos[0], swingPos[1], 35,
0, 8)
setupFieldDist = SetupField.calculateDistance(
SetupField.xpos, spheroPose.x, SetupField.ypos,
spheroPose.y)
if (setupFieldDist < 8):
self.changePhase()
return
print "Setup Field Dist: " + str(
SetupField.calculateDistance(SetupField.xpos, spheroPose.x,
SetupField.ypos,
spheroPose.y))
deltaX = SetupField.calcVelocity(msg.pose[spheroIndex])[0]
deltaY = SetupField.calcVelocity(msg.pose[spheroIndex])[1]
print "DeltaX & DeltaY: " + str((deltaX, deltaY))
twist = SpheroTwist()
twist.linear.x = deltaX
twist.linear.y = deltaY
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
selected_items = self.spheroListWidget.selectedItems()
twist.name = str(selected_items[0].text())
self.cmdVelPub.publish(twist)
elif self.phase == 'swing':
print "Swing phase"
self.ballField.setX(msg.pose[ballIndex].x)
self.ballField.setY(msg.pose[ballIndex].y)
self.ballField.setAlpha(100)
deltaX = self.ballField.calcVelocity(msg.pose[spheroIndex])[0]
print "Delta X: " + str(deltaX)
deltaY = self.ballField.calcVelocity(msg.pose[spheroIndex])[1]
print "Delta Y: " + str(deltaY)
# Normalize vector to only rotate the sphero for 40 frames
if self.rotateFrames < 40:
deltaX /= math.sqrt(
math.pow(deltaX, 2) + math.pow(deltaY, 2))
deltaY /= math.sqrt(
math.pow(deltaX, 2) + math.pow(deltaY, 2))
self.rotateFrames += 1
twist = SpheroTwist()
twist.linear.x = deltaX
twist.linear.y = deltaY
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
selected_items = self.spheroListWidget.selectedItems()
twist.name = str(selected_items[0].text())
self.cmdVelPub.publish(twist)
else:
#Checks to stop the sphero from follow through when it strikes the ball
if self.phase == 'rolling':
print 'Rolling phase'
twist = SpheroTwist()
if self.reverseFrames < 10:
if self.reverseFrames < 1:
deltaX = -twist.linear.x
deltaY = -twist.linear.y
else:
deltaX = twist.linear.x
deltaY = twist.linear.y
self.reverseFrames += 1
else:
deltaX = 0
deltaY = 0
#twist = SpheroTwist()
print 'Delta X: ' + str(deltaX)
print 'Delta Y: ' + str(deltaY)
twist.linear.x = deltaX
twist.linear.y = deltaY
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
selected_items = self.spheroListWidget.selectedItems()
twist.name = str(selected_items[0].text())
self.cmdVelPub.publish(twist)
'''twist = SpheroTwist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
selected_items = self.spheroListWidget.selectedItems();
twist.name = str(selected_items[0].text())
self.cmdVelPub.publish(twist)'''
self.location[ballID] = (msg.pose[ballIndex].x, msg.pose[ballIndex].y)
self.location[spheroID] = (msg.pose[spheroIndex].x,
msg.pose[spheroIndex].y)
def getSpheroTwist(self, name, move_id):
twist = SpheroTwist()
twist.name = name
if move_id == 0:
twist.linear.x = -STEP_LENGTH
twist.linear.y = STEP_LENGTH
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
elif move_id == 1:
twist.linear.x = 0
twist.linear.y = STEP_LENGTH
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
elif move_id == 2:
twist.linear.x = STEP_LENGTH
twist.linear.y = STEP_LENGTH
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
elif move_id == 3:
twist.linear.x = -STEP_LENGTH
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
elif move_id == 4:
twist.linear.x = STEP_LENGTH
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
elif move_id == 5:
twist.linear.x = -STEP_LENGTH
twist.linear.y = -STEP_LENGTH
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
elif move_id == 6:
twist.linear.x = 0
twist.linear.y = -STEP_LENGTH
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
elif move_id == 7:
twist.linear.x = STEP_LENGTH
twist.linear.y = -STEP_LENGTH
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
return twist