def oneFootstep(origin, leg, x=0, y=0, z=0, roll=0, pitch=0, yaw=0):
msg = Footstep()
msg.leg = leg
translation = [x, y, z]
q = quaternion_from_euler(roll, pitch, yaw)
offset = transRotToMat(translation, q)
target = concatenate_matrices(origin, offset)
print(origin)
msg.pose = matToMsg(target)
msg.dimensions.x = 0.23
msg.dimensions.y = 0.13
msg.dimensions.z = 0.001
return msg
def oneFootstep(origin, leg, x=0, y=0, z=0, roll=0, pitch=0, yaw=0):
msg = Footstep()
msg.leg = leg
translation = [x, y, z]
q = quaternion_from_euler(roll, pitch, yaw)
offset = transRotToMat(translation, q)
target = concatenate_matrices(origin, offset)
print origin
msg.pose = matToMsg(target)
msg.dimensions.x = 0.23
msg.dimensions.y = 0.13
msg.dimensions.z = 0.001
return msg
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 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()
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 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 toROSMsg(self): footstep = Footstep() footstep.leg = self.leg footstep.pose = self.toROSPose() return footstep
def toROSMsg(self): footstep = Footstep() footstep.leg = self.leg footstep.pose = self.toROSPose() return footstep