def main():
n = xrange(2 * 10 ** 3)
for type in [
(bullet.Vector3, 1, 2, 3),
(bullet.Box2dShape, bullet.Vector3(1, 2, 3)),
(bullet.BoxShape, bullet.Vector3(1, 2, 3)),
(bullet.SphereShape, 3.0,),
(bullet.CylinderShape, bullet.Vector3(1, 2, 3)),
(bullet.CylinderShapeX, bullet.Vector3(1, 2, 3)),
(bullet.CylinderShapeZ, bullet.Vector3(1, 2, 3)),
(bullet.StaticPlaneShape, bullet.Vector3(1, 2, 3), 1),
(bullet.CapsuleShape, 3.0, 5.0),
(bullet.IndexedMesh,),
(bullet.TriangleIndexVertexArray,),
(bullet.BvhTriangleMeshShape, bullet.TriangleIndexVertexArray()),
(bullet.CollisionObject,),
(bullet.Transform,),
(bullet.DefaultMotionState,),
(bullet.RigidBody,),
(bullet.KinematicCharacterController,
bullet.BoxShape(bullet.Vector3(1, 2, 3)), 5, 1),
(bullet.CollisionDispatcher,),
(bullet.OverlappingPairCache,),
(bullet.AxisSweep3, bullet.Vector3(0, 0, 0), bullet.Vector3(2, 2, 2)),
(bullet.SequentialImpulseConstraintSolver,),
(bullet.CollisionWorld,),
(bullet.DynamicsWorld,),
(bullet.DiscreteDynamicsWorld,),
]:
measure(n, type)
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 cloud_cb(self, msg):
print 'Received message with %d points' % (len(msg.points))
newmsg = PointCloud(msg.header, [], [])
for c in msg.chan:
newmsg.channels.append(ChannelFloat32(c.name, []))
for i in range(0, len(msg.points)):
if self.frame is None:
p = msg.points[i]
else:
tfp = tf.PointStamped(
bullet.Vector3(msg.points[i].x, msg.points[i].y,
msg.points[i].z), 0, 0, msg.header.frame_id)
tfpprime = self.tf.transform_point(self.frame, tfp)
p = Point32(tfpprime.point.x(), tfpprime.point.y(),
tfpprime.point.z())
if p.z >= self.zmin and p.z <= self.zmax:
newmsg.points.append(p)
for j in range(0, len(msg.chan)):
newmsg.channels[j].values.append(msg.channels[j].values[i])
print 'Publishing message with %d points' % (len(newmsg.points))
self.pub.publish(newmsg)
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)
print "po5", po5.this
tr = tf.TransformStamped()
lps = tf.PoseStamped()
lps.pose.setIdentity()
print "setting stamp"
mytime = rospy.Time(10, 20)
lps.stamp = mytime
print mytime
print "getting stamp"
output = lps.stamp
print output
print lps.pose
print "setting pose.positon to 1,2,3"
lps.pose.setOrigin(bullet.Vector3(1, 2, 3))
print lps.pose.getOrigin()
print lps.pose
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)