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