def straightSteps():
origin = getCurrentLeftLeg()
footsteps = FootstepArray()
footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2),
oneFootstep(origin, Footstep.LEFT, 0.4, 0),
oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2),
oneFootstep(origin, Footstep.LEFT, 0.8, 0),
oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2),
oneFootstep(origin, Footstep.LEFT, 1.2, 0),
oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2)]
footsteps.header.frame_id = "odom"
footsteps.header.stamp = rospy.Time.now()
sendFootstep(footsteps)
def currentSpot():
origin = getCurrentLeftLeg()
footsteps = FootstepArray()
footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0, -0.2)]
footsteps.header.frame_id = "odom"
footsteps.header.stamp = rospy.Time.now()
sendFootstep(footsteps)
def straightSteps():
origin = getCurrentLeftLeg()
footsteps = FootstepArray()
footsteps.footsteps = [
oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2),
oneFootstep(origin, Footstep.LEFT, 0.4, 0),
oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2),
oneFootstep(origin, Footstep.LEFT, 0.8, 0),
oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2),
oneFootstep(origin, Footstep.LEFT, 1.2, 0),
oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2)
]
footsteps.header.frame_id = "odom"
footsteps.header.stamp = rospy.Time.now()
sendFootstep(footsteps)
def currentSpot():
origin = getCurrentLeftLeg()
footsteps = FootstepArray()
footsteps.footsteps = [
oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
oneFootstep(origin, Footstep.LEFT, 0, 0),
oneFootstep(origin, Footstep.RIGHT, 0, -0.2)
]
footsteps.header.frame_id = "odom"
footsteps.header.stamp = rospy.Time.now()
sendFootstep(footsteps)
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 joyCB(self, status, history):
JoyPose6D.joyCB(self, status, history)
footsteps = FootstepArray()
footsteps.header.frame_id = self.frame_id
footsteps.header.stamp = rospy.Time(0.0)
if status.triangle and not history.latest().triangle:
# remove the latest one
if len(self.footsteps) >= 2:
self.footsteps = self.footsteps[:-1]
self.pre_pose.pose = self.footsteps[-1].pose
elif len(self.footsteps) == 1:
self.footsteps = []
if len(self.footsteps) == 0:
self.pre_pose.pose.position.x = 0
self.pre_pose.pose.position.y = 0
self.pre_pose.pose.position.z = 0
self.pre_pose.pose.orientation.x = 0
self.pre_pose.pose.orientation.y = 0
self.pre_pose.pose.orientation.z = 0
self.pre_pose.pose.orientation.w = 1
# pre_pose -> Footstep
current_step = Footstep()
current_step.pose = self.pre_pose.pose
if status.cross and not history.latest().cross:
# left
current_step.leg = Footstep.LEFT
self.footsteps.append(current_step)
elif status.circle and not history.latest().circle:
# right
current_step.leg = Footstep.RIGHT
self.footsteps.append(current_step)
footsteps.footsteps.extend(self.footsteps)
footsteps.footsteps.append(current_step)
self.footstep_pub.publish(footsteps)
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()