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