def pose_cb(self, msg):
self.have_pose = True
dx = self.true_position[0] - msg.pose.position.x
dy = self.true_position[1] - msg.pose.position.y
dz = self.true_position[2] - msg.pose.position.z
dist = math.sqrt(dx * dx + dy * dy + dz * dz)
print 'New position: (%9.6f, %9.6f, %9.6f). Difference: %9.6f.' % (
msg.pose.position.x, msg.pose.position.y, msg.pose.position.z,
dist)
if dist > self.position_tolerance:
self.failed = True
orientation = bullet.Quaternion(msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w)
angular_dist = orientation.angle(self.true_orientation)
print 'New orientation: (%9.6f, %9.6f, %9.6f, %9.6f). Difference: %9.6f' % (
orientation.x(), orientation.y(), orientation.z(), orientation.w(),
angular_dist)
if angular_dist > self.orientation_tolerance:
self.failed = True
def setUp(self):
print sys.argv
self.assertTrue(len(sys.argv) >= 11)
self.true_position = (float(sys.argv[1]), float(sys.argv[2]),
float(sys.argv[3]))
self.position_tolerance = float(sys.argv[4])
self.true_orientation = bullet.Quaternion(float(sys.argv[5]),
float(sys.argv[6]),
float(sys.argv[7]),
float(sys.argv[8]))
self.orientation_tolerance = float(sys.argv[9])
self.time_limit = float(sys.argv[10])
self.have_pose = False
self.failed = False
print 'True position: (%9.6f, %9.6f, %9.6f). Tolerance: %9.6f' % (
self.true_position[0], self.true_position[1],
self.true_position[2], self.position_tolerance)
print 'True orientation: (%9.6f, %9.6f, %9.6f, %9.6f). Tolerance: %9.6f' % (
self.true_orientation.x(), self.true_orientation.y(),
self.true_orientation.z(), self.true_orientation.w(),
self.orientation_tolerance)
def test_quaternion_euler(self):
ground_truth = [
((0, 0, 0, 1), (0, 0, 0)),
((0, 0, 1, 0), (math.pi, 0, 0)),
((0.000000, 0.000000, math.sqrt(2) / 2, math.sqrt(2) / 2),
(math.pi / 2, 0, 0)),
(
(0.000000, 0.000000, 0.382683, 0.923880), (math.pi / 4, 0, 0)
), #\todo had to decrease accuracy due to cut and paste number, should use analytic like above
]
for quattuple, eulers in ground_truth:
print quattuple
quat = bullet.Quaternion(quattuple[0], quattuple[1], quattuple[2],
quattuple[3])
# check yaw
if math.fabs(bullet.Matrix3x3(quat).getEulerZYXYaw(0) -
eulers[0]) < math.fabs(
bullet.Matrix3x3(quat).getEulerZYXYaw(1) -
eulers[0]):
yaw = bullet.Matrix3x3(quat).getEulerZYXYaw(0)
else:
yaw = bullet.Matrix3x3(quat).getEulerZYXYaw(1)
self.assertAlmostEqual(
yaw, eulers[0], 5,
"yaw %f quaternion to euler %f correctness" % (yaw, eulers[0]))
#check pitch
if math.fabs(
bullet.Matrix3x3(quat).getEulerZYXPitch(0) -
eulers[1]) < math.fabs(
bullet.Matrix3x3(quat).getEulerZYXPitch(1) -
eulers[1]):
pitch = bullet.Matrix3x3(quat).getEulerZYXPitch(0)
else:
pitch = bullet.Matrix3x3(quat).getEulerZYXPitch(1)
self.assertAlmostEqual(
pitch, eulers[1], 5,
"pitch %f quaternion to euler %f correctness" %
(pitch, eulers[1]))
#check roll
if math.fabs(
bullet.Matrix3x3(quat).getEulerZYXRoll(0) -
eulers[2]) < math.fabs(
bullet.Matrix3x3(quat).getEulerZYXRoll(1) - eulers[2]):
roll = bullet.Matrix3x3(quat).getEulerZYXRoll(0)
else:
roll = bullet.Matrix3x3(quat).getEulerZYXRoll(1)
self.assertAlmostEqual(
roll, eulers[2], 5,
"roll %f quaternion to euler %f correctness" %
(roll, eulers[2]))
def test_basic_localization(self):
target_x = float(sys.argv[1])
target_y = float(sys.argv[2])
target_a = float(sys.argv[3])
self.offset = bullet.Transform(bullet.Quaternion(target_a, 0, 0),
bullet.Vector3(target_x, target_y, 0))
self.tolerance_d = float(sys.argv[4])
self.tolerance_a = float(sys.argv[5])
target_time = float(sys.argv[6])
self.frame = sys.argv[7]
while rospy.rostime.get_time() == 0.0:
print 'Waiting for initial time publication'
time.sleep(0.1)
# Construct goal a robot-centric frame; let move_base_local do the work
goal = PoseStamped()
goal.header.stamp = rospy.get_rostime()
goal.header.frame_id = self.frame
goal.pose.position.x = target_x
goal.pose.position.y = target_y
goal.pose.position.z = 0
# Use bullet and tf to build a quaternion from the user-specified yaw
goal.pose.orientation = tf.quaternion_bt_to_msg(
bullet.Quaternion(target_a, 0, 0))
self.pub.publish(goal)
start_time = rospy.rostime.get_time()
while (not self.action_succeeded or not self.ontarget
) and (rospy.rostime.get_time() - start_time) < target_time:
print 'Waiting for end time %.6f (current: %.6f)' % (target_time, (
rospy.rostime.get_time() - start_time))
time.sleep(0.1)
print 'Waited for end time %.6f (current: %.6f)' % (target_time, (
rospy.rostime.get_time() - start_time))
self.assertTrue(self.action_succeeded)
self.assertTrue(self.ontarget)
def tf_cb(self, msg):
if self.active:
for t in msg.transforms:
if t.header.frame_id == 'odom_combined':
tx = t.transform
# Convert Transform.msg to bullet
current_pose = bullet.Transform(
bullet.Quaternion(tx.rotation.x, tx.rotation.y,
tx.rotation.z, tx.rotation.w),
bullet.Vector3(tx.translation.x, tx.translation.y,
tx.translation.z))
print 'pose: %.3f %.3f %.3f' % (
current_pose.getOrigin().x(),
current_pose.getOrigin().y(),
current_pose.getBasis().getEulerZYXYaw())
if not self.have_goal:
#self.goal = current_pose * self.offset
self.goal = self.offset
self.have_goal = True
print 'offs: %.3f %.3f' % (self.offset.getOrigin().x(),
self.offset.getOrigin().y())
print 'goal: %.3f %.3f %.3f' % (
self.goal.getOrigin().x(),
self.goal.getOrigin().y(),
self.goal.getBasis().getEulerZYXYaw())
# Did the robot go where we told it?
dx = abs(current_pose.getOrigin().x() -
self.goal.getOrigin().x())
dy = abs(current_pose.getOrigin().y() -
self.goal.getOrigin().y())
da = abs(
angle_diff(current_pose.getBasis().getEulerZYXYaw(),
self.goal.getBasis().getEulerZYXYaw()))
print 'dx: %.3f dy: %.3f (%.3f) da: %.3f (%.3f)' % (
dx, dy, self.tolerance_d, da, self.tolerance_a)
# TODO: compare orientation
if dx < self.tolerance_d and dy < self.tolerance_d and da < self.tolerance_a:
self.ontarget = True
print 'On target'
else:
self.ontarget = False
print 'Off target'
def approachTable(self, standoff, near):
if not ts.tiltScan(8.0):
print '[ApproachTable] Failed to change tilt scan'
return False
resp = dt.detectTable()
if not resp:
print '[ApproachTable] Failed to detect table'
return False
self.vm = Marker()
self.vm.header.frame_id = resp.table.header.frame_id
self.vm.ns = "approachtable"
self.vm.pose.position.z = resp.table.table.points[0].z
approach_pose = self.computeApproachPose(self.robot_position,
resp.table.table, standoff,
True)
if approach_pose == None:
return False
self.vm.id = 1000
self.vm.type = Marker.ARROW
self.vm.action = Marker.ADD
self.vm.pose.position.x = approach_pose[0]
self.vm.pose.position.y = approach_pose[1]
#self.vm.z = 0.0
self.vm.pose.orientation = tf.quaternion_bt_to_msg(
bullet.Quaternion(approach_pose[2], 0.0, 0.0))
self.vm.scale.x = 0.6
self.vm.scale.y = 0.25
self.vm.scale.z = 0.1
self.vm.color.a = 0.5
self.vm.color.r = 1.0
self.vm.points = []
self.pub_vis.publish(self.vm)
if not ts.tiltScan(2.0):
print '[ApproachTable] Failed to change tilt scan'
return False
# Call out to blocking MoveBase
return self.mb.moveBase(resp.table.header.frame_id, approach_pose[0],
approach_pose[1], approach_pose[2])
def test_euler_quaternion_euler(self):
yprs = [
(0, 0, 0),
#\todo normalize on +-pi (math.pi*3/2, 0, 0),
(math.pi, 0, 0),
(math.pi / 2, 0, 0),
(math.pi / 4, 0, 0),
(math.pi / 8, 0, 0),
(math.pi / 2, math.pi / 8, 0),
(math.pi / 4, math.pi / 8, 0),
(math.pi / 8, math.pi / 8, 0),
(math.pi / 2, 0, math.pi / 8),
(math.pi / 4, 0, math.pi / 8),
(math.pi / 8, 0, math.pi / 8),
(math.pi / 2, math.pi / 8, math.pi / 8),
(math.pi / 4, math.pi / 8, math.pi / 8),
(math.pi / 8, math.pi / 8, math.pi / 8),
(math.pi / 8, 0, 0),
(math.pi / 4, 0, 0),
(math.pi / 2, 0, 0)
]
for eulers in yprs:
print eulers
quat = bullet.Quaternion(eulers[0], eulers[1], eulers[2])
# check yaw
if math.fabs(bullet.Matrix3x3(quat).getEulerZYXYaw(0) -
eulers[0]) < math.fabs(
bullet.Matrix3x3(quat).getEulerZYXYaw(1) -
eulers[0]):
yaw = bullet.Matrix3x3(quat).getEulerZYXYaw(0)
else:
yaw = bullet.Matrix3x3(quat).getEulerZYXYaw(1)
self.assertAlmostEqual(
yaw, eulers[0], 7,
"yaw %f euler to quaternion to euler %f correctness" %
(yaw, eulers[0]))
#check pitch
if math.fabs(
bullet.Matrix3x3(quat).getEulerZYXPitch(0) -
eulers[1]) < math.fabs(
bullet.Matrix3x3(quat).getEulerZYXPitch(1) -
eulers[1]):
pitch = bullet.Matrix3x3(quat).getEulerZYXPitch(0)
else:
pitch = bullet.Matrix3x3(quat).getEulerZYXPitch(1)
self.assertAlmostEqual(
pitch, eulers[1], 7,
"pitch %f euler to quaternion to euler %f correctness" %
(pitch, eulers[1]))
#check roll
if math.fabs(
bullet.Matrix3x3(quat).getEulerZYXRoll(0) -
eulers[2]) < math.fabs(
bullet.Matrix3x3(quat).getEulerZYXRoll(1) - eulers[2]):
roll = bullet.Matrix3x3(quat).getEulerZYXRoll(0)
else:
roll = bullet.Matrix3x3(quat).getEulerZYXRoll(1)
self.assertAlmostEqual(
roll, eulers[2], 7,
"roll %f euler to quaternion to euler %f correctness" %
(roll, eulers[2]))
try:
transform_stamped = tf.TransformStamped()
print "getting stamp"
print transform_stamped.stamp
# mytime = rospy.Time().now()
mytime = rospy.Time(10,20)
transform_stamped.stamp = mytime
print mytime
print "getting stamp", transform_stamped.stamp
print "transform:", transform_stamped.transform
transform_stamped.transform.setIdentity()
print "after setIdentity()", transform_stamped.transform
# transform_stamped.transform.basis.setEulerZYX(0,0,0)
quat = bullet.Quaternion(math.pi/2,0,0)
print "quaternion ", quat
transform_stamped.transform.setRotation(quat)
print "setting rotation to PI/2\n After setRotation"
print transform_stamped.transform
other_transform = bullet.Transform()
other_transform.setIdentity()
transform_stamped.transform = other_transform
print "After assignment of Identity"
print transform_stamped.transform
other_transform = bullet.Transform()
other_transform.setIdentity()
other_transform.setRotation(quat)
transform_stamped.transform = other_transform
if qdict.has_key('x'):
msg.pose.position.x = float(qdict['x'][0])
else:
msg.pose.position.x = 0.0
if qdict.has_key('y'):
msg.pose.position.y = float(qdict['y'][0])
else:
msg.pose.position.y = 0.0
if qdict.has_key('z'):
msg.pose.position.z = float(qdict['z'][0])
else:
msg.pose.position.z = 0.0
if qdict.has_key('t'):
q = bullet.Quaternion(float(qdict['t'][0]), 0, 0)
msg.pose.orientation.x = float(q.x())
msg.pose.orientation.y = float(q.y())
msg.pose.orientation.z = float(q.z())
msg.pose.orientation.w = float(q.w())
else:
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.0
msg.pose.orientation.z = 0.0
msg.pose.orientation.w = 1.0
rwt.publish(msg)
elif cmd == "service":
print "Service[%s]" % topic
