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()