def callback(msg):
global tf_listener, frame_id, pub
try:
if strict_tf:
tf_listener.waitForTransform(frame_id, msg.header.frame_id,
msg.header.stamp, rospy.Duration(1.0))
if strict_tf:
stamp = msg.header.stamp
else:
stamp = rospy.Time(0.0)
# frame_id -> header
(pos, rot) = tf_listener.lookupTransform(frame_id, msg.header.frame_id,
stamp)
trans = concatenate_matrices(translation_matrix(pos),
quaternion_matrix(rot))
new_msg = FootstepArray()
new_msg.header.frame_id = frame_id
new_msg.header.stamp = msg.header.stamp
for footstep in msg.footsteps:
new_footstep = Footstep()
new_footstep.leg = footstep.leg
new_footstep.duration = footstep.duration
new_footstep.footstep_group = footstep.footstep_group
new_footstep.swing_height = footstep.swing_height
new_footstep.dimensions = footstep.dimensions
old_pose = concatenate_matrices(
translation_matrix([
footstep.pose.position.x, footstep.pose.position.y,
footstep.pose.position.z
]),
quaternion_matrix([
footstep.pose.orientation.x, footstep.pose.orientation.y,
footstep.pose.orientation.z, footstep.pose.orientation.w
]),
)
new_pose = concatenate_matrices(trans, old_pose)
translation = translation_from_matrix(new_pose)
rotation = quaternion_from_matrix(new_pose)
new_footstep.pose.position.x = translation[0]
new_footstep.pose.position.y = translation[1]
new_footstep.pose.position.z = translation[2]
new_footstep.pose.orientation.x = rotation[0]
new_footstep.pose.orientation.y = rotation[1]
new_footstep.pose.orientation.z = rotation[2]
new_footstep.pose.orientation.w = rotation[3]
new_msg.footsteps.append(new_footstep)
pub.publish(new_msg)
except tf.Exception as e:
rospy.logerr(
"[transform_footstep_array] Failed to lookup transform: %s" %
(e.message))
def callback(msg):
global tf_listener, frame_id, pub
try:
if strict_tf:
tf_listener.waitForTransform(frame_id, msg.header.frame_id,
msg.header.stamp,
rospy.Duration(1.0))
if strict_tf:
stamp = msg.header.stamp
else:
stamp = rospy.Time(0.0)
# frame_id -> header
(pos, rot) = tf_listener.lookupTransform(frame_id, msg.header.frame_id,
stamp)
trans = concatenate_matrices(translation_matrix(pos), quaternion_matrix(rot))
new_msg = FootstepArray()
new_msg.header.frame_id = frame_id
new_msg.header.stamp = msg.header.stamp
for footstep in msg.footsteps:
new_footstep = Footstep()
new_footstep.leg = footstep.leg
new_footstep.duration = footstep.duration
new_footstep.footstep_group = footstep.footstep_group
new_footstep.swing_height = footstep.swing_height
new_footstep.dimensions = footstep.dimensions
old_pose = concatenate_matrices(translation_matrix([
footstep.pose.position.x,
footstep.pose.position.y,
footstep.pose.position.z]),
quaternion_matrix([
footstep.pose.orientation.x,
footstep.pose.orientation.y,
footstep.pose.orientation.z,
footstep.pose.orientation.w]),
)
new_pose = concatenate_matrices(trans, old_pose)
translation = translation_from_matrix(new_pose)
rotation = quaternion_from_matrix(new_pose)
new_footstep.pose.position.x = translation[0]
new_footstep.pose.position.y = translation[1]
new_footstep.pose.position.z = translation[2]
new_footstep.pose.orientation.x = rotation[0]
new_footstep.pose.orientation.y = rotation[1]
new_footstep.pose.orientation.z = rotation[2]
new_footstep.pose.orientation.w = rotation[3]
new_msg.footsteps.append(new_footstep)
pub.publish(new_msg)
except tf.Exception, e:
rospy.logerr("[transform_footstep_array] Failed to lookup transform: %s" % (e.message))
def main():
pub = rospy.Publisher("/footsteps", FootstepArray)
r = rospy.Rate(3)
ysize = 0
zpos = 0
while not rospy.is_shutdown():
msg = FootstepArray()
now = rospy.Time.now()
msg.header.frame_id = FRAME_ID
msg.header.stamp = now
xpos = 0
for i in range(20):
footstep = Footstep()
if i % 2 == 0:
footstep.leg = Footstep.LEFT
footstep.pose.position.y = 0.21
else:
footstep.leg = Footstep.RIGHT
footstep.pose.position.y = -0.21
footstep.pose.orientation.w = 1.0
footstep.pose.position.x = xpos
footstep.pose.position.z = zpos
footstep.dimensions.x = 0.25
footstep.dimensions.y = 0.15
footstep.dimensions.z = 0.01
footstep.footstep_group = i / 5
msg.footsteps.append(footstep)
xpos = xpos + 0.25
zpos = zpos + 0.1
ysize = ysize + 0.01
if ysize > 0.15:
ysize = 0.0
if zpos > 0.5:
zpos = 0.0
pub.publish(msg)
r.sleep()
def main():
pub = rospy.Publisher("/footsteps", FootstepArray)
r = rospy.Rate(3)
ysize = 0
zpos = 0
while not rospy.is_shutdown():
msg = FootstepArray()
now = rospy.Time.now()
msg.header.frame_id = FRAME_ID
msg.header.stamp = now
xpos = 0
for i in range(20):
footstep = Footstep()
if i % 2 == 0:
footstep.leg = Footstep.LEFT
footstep.pose.position.y = 0.21
else:
footstep.leg = Footstep.RIGHT
footstep.pose.position.y = -0.21
footstep.pose.orientation.w = 1.0
footstep.pose.position.x = xpos
footstep.pose.position.z = zpos
footstep.dimensions.x = 0.25
footstep.dimensions.y = 0.15
footstep.dimensions.z = 0.01
footstep.footstep_group = i / 5
msg.footsteps.append(footstep)
xpos = xpos + 0.25
zpos = zpos + 0.1
ysize = ysize + 0.01
if ysize > 0.15:
ysize = 0.0
if zpos > 0.5:
zpos = 0.0
pub.publish(msg)
r.sleep()
