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
Пример #3
0
 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)
Пример #4
0
 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)
Пример #5
0
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))
Пример #9
0
 def toROSMsg(self):
   footstep = Footstep()
   footstep.leg = self.leg
   footstep.pose = self.toROSPose()
   return footstep
Пример #10
0
 def toROSMsg(self):
   footstep = Footstep()
   footstep.leg = self.leg
   footstep.pose = self.toROSPose()
   return footstep