footstep_sample.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 FRAME_ID = "/map"
00004 
00005 from jsk_footstep_msgs.msg import Footstep, FootstepArray
00006 
00007 import rospy
00008 
00009 def main():
00010     pub = rospy.Publisher("/footsteps", FootstepArray)
00011     r = rospy.Rate(3)
00012     ysize = 0
00013     zpos = 0
00014     while not rospy.is_shutdown():
00015         msg = FootstepArray()
00016         now = rospy.Time.now()
00017         msg.header.frame_id = FRAME_ID
00018         msg.header.stamp = now
00019         xpos = 0
00020         
00021         for i in range(20):
00022             footstep = Footstep()
00023             if i % 2 == 0:
00024                 footstep.leg = Footstep.LEFT
00025                 footstep.pose.position.y = 0.21
00026             else:
00027                 footstep.leg = Footstep.RIGHT
00028                 footstep.pose.position.y = -0.21
00029             footstep.pose.orientation.w = 1.0
00030             footstep.pose.position.x = xpos
00031             footstep.pose.position.z = zpos
00032             footstep.dimensions.x = 0.25
00033             footstep.dimensions.y = 0.15
00034             footstep.dimensions.z = 0.01
00035             footstep.footstep_group = i / 5
00036             msg.footsteps.append(footstep)
00037             xpos = xpos + 0.25
00038             zpos = zpos + 0.1
00039             ysize = ysize + 0.01
00040             if ysize > 0.15:
00041                 ysize = 0.0
00042             if zpos > 0.5:
00043                 zpos = 0.0
00044         pub.publish(msg)
00045         r.sleep()
00046 
00047 if __name__ == "__main__":
00048     rospy.init_node("footstep_sample")
00049     main()
00050     


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03