def testCoulombForce(self):
RC = -1
OC = -1
GC = 1
r = [3,4]
f = Util.coulombForce(r, OC, RC)
self.assertEquals(True, f[0] < 0 and f[1] < 0)
f = Util.coulombForce(r, GC, RC)
self.assertEquals(True, f[0] > 0 and f[1] > 0)
def callback(scan,odom):
# the odometry parameter should be global
global globalOdom
global vx, spinning, moving, theta, Pd, Ld
globalOdom = odom
# make a new twist message
command = Twist()
# Fill in the fields. Field values are unspecified
# until they are actually assigned. The Twist message
# holds linear and angular velocities.
command.linear.x = 0.0
command.linear.y = 0.0
command.linear.z = 0.0
command.angular.x = 0.0
command.angular.y = 0.0
command.angular.z = 0.0
#get goal x and y locations from the launch file
goalX = rospy.get_param('lab2/goalX',0.0)
goalY = rospy.get_param('lab2/goalY',0.0)
# find current (x,y) position of robot based on odometry
currentX = globalOdom.pose.pose.position.x
currentY = globalOdom.pose.pose.position.y
if goalX != currentX and goalY != currentY:
# find current orientation of robot based on odometry (quaternion coordinates)
xOr = globalOdom.pose.pose.orientation.x
yOr = globalOdom.pose.pose.orientation.y
zOr = globalOdom.pose.pose.orientation.z
wOr = globalOdom.pose.pose.orientation.w
# find orientation of robot (Euler coordinates)
(roll, pitch, yaw) = euler_from_quaternion([xOr, yOr, zOr, wOr])
# find currentAngle of robot (equivalent to yaw)
# now that you have yaw, the robot's pose is completely defined by (currentX, currentY, currentAngle)
currentAngle = yaw
# find laser scanner properties (min scan angle, max scan angle, scan angle increment)
maxAngle = scan.angle_max
minAngle = scan.angle_min
angleIncrement = scan.angle_increment
# find current laser angle, max scan length, distance array for all scans, and number of laser scans
currentLaserTheta = minAngle
maxScanLength = scan.range_max
distanceArray = scan.ranges
numScans = len(distanceArray)
# the code below (currently commented) shows how
# you can print variables to the terminal (may
# be useful for debugging)
#print 'x: {0}'.format(currentX)
#print 'y: {0}'.format(currentY)
#print 'theta: {0}'.format(currentAngle)
if not spinning and not moving:
# for each laser scan
fResult = [0,0]
for curScan in range(0, numScans):
d = distanceArray[curScan]
if d != 30:
r = Util.cartFromPolar(d, currentLaserTheta)
globR = Util.rotZTheta(r, -currentAngle)
f = Util.coulombForce(globR,
OBSTACLE_CHARGE * Util.scanAngleMod(currentLaserTheta),
ROBOT_CHARGE)
fResult[0] += f[0]
fResult[1] += f[1]
currentLaserTheta += angleIncrement
goal = [goalX-currentX , goalY-currentY]
mGoal = math.pow(math.pow(goal[0],2) + math.pow(goal[1],2),0.5)
unitGoal = [goal[0]/mGoal, goal[1]/mGoal]
goalForce = Util.coulombForce(goal, GOAL_CHARGE, ROBOT_CHARGE)
fResult[0] += goalForce[0]
fResult[1] += goalForce[1]
localF = Util.rotZTheta(fResult, currentAngle)
m, theta = Util.polarFromCart(localF)
print 'Force {}'.format(fResult)
print 'U {}'.format(command.linear.x)
global vx, vy, u, omega
vx += fResult[0]*DT
vy += fResult[1]*DT
print "location: ({}, {})".format(currentX, currentY)
print "Angle: {}".format(currentAngle)
print "Goal: ({}, {})".format(goalX, goalY)
print "GoalVector: {}".format(goal)
print "ax: {}, ay: {}".format(fResult[0], fResult[1])
print "vx: {}, vy: {}".format(vx, vy)
Pd = [vx*DT, vy*DT]
Ld = [currentX+Pd[0], currentY+Pd[1]]
print "Desired Point: {} ".format(Pd)
u, omega = Util.unicycleTracking(Pd, u, omega, currentAngle)
if abs(currentAngle - theta) > .1:
command.angular.z = omega
spinning = True
else:
command.linear.x = u
else:
u, omega = Util.unicycleTracking(Pd, u, omega, currentAngle)
if abs(currentAngle - theta) > .1:
command.angular.z = omega/abs(omega)
spinning = True
elif abs(currentX - Ld[0]) < 0.1 and abs(currentY - Ld[1])< 0.1:
print 'u: {}'.format(u)
print 'location: ({}, {})'.format(currentX, currentY)
command.linear.x = u/abs(u)
spinning = False
moving = True
else:
spinning = False
moving = False
else:
pass
pub.publish(command)
