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