def checkGoToGoal(self, x0, y0, th0, x1, y1, th1):
dTol = 0.05 # 5cm
thTol = 0.04 # Approx 2.5 degrees
self.controller.setLinearTolerance(dTol)
self.controller.setAngularTolerance(thTol)
cur = Pose()
cur.x = x0
cur.y = y0
cur.theta = th0
goal = Pose()
goal.x = x1
goal.y = y1
goal.theta = th1
lastDistance = self.controller.getGoalDistance(cur, goal)
dT = 0.05
for i in range(1000):
if self.controller.atGoal(cur, goal):
return
desired = self.controller.getVelocity(cur, goal, dT)
newTheta = cur.theta + dT * desired.thetaVel
midTheta = (cur.theta + newTheta) / 2.0
cur.x += dT * desired.xVel * cos(midTheta)
cur.y += dT * desired.xVel * sin(midTheta)
cur.theta = newTheta
# If we get here, we didn't reach the goal.
self.assertFalse('Did not reach the goal: p0=' + str((x0, y0, th0)) +
' p1=' + str((x1, y1, th1)))
def testRotateRight(self):
cur = Pose()
goal = Pose()
goal.theta = -pi / 2
desired = self.controller.getVelocity(cur, goal, 0.1)
self.assertEquals(desired.xVel, 0)
self.assertLess(desired.thetaVel, 0)
def testButtonHookRight(self):
cur = Pose()
goal = Pose()
goal.x = 1
goal.theta = -pi
desired = self.controller.getVelocity(cur, goal, 0.1)
self.assertGreater(desired.xVel, 0)
self.assertGreater(desired.thetaVel, 0)
def testCurveBackLeft(self):
cur = Pose()
goal = Pose()
cur.x = 1
cur.y = 1
goal.theta = pi / 2
desired = self.controller.getVelocity(cur, goal, 0.1)
self.assertLess(desired.xVel, 0)
self.assertGreater(desired.thetaVel, 0)
def on_initial_pose(self, msg): #(self, msg):
q = [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.x,
msg.pose.pose.orientation.x, msg.pose.pose.orientation.w
]
roll, pitch, yaw = euler_from_quaternion(q)
pose = Pose()
pose.x = msg.pose.pose.position.x
pose.y = msg.pose.pose.position.y
pose.theta = yaw
rospy.loginfo('Setting initial pose to ')
self.odometry.setPose(pose)