corrected_initialpose.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Maps the initialpose pointing to Nao's torso to a corrected pose with correct
00004 # height.
00005 #
00006 # Author: Johannes Garimort
00007 # License: BSD
00008 
00009 import roslib
00010 roslib.load_manifest('footstep_planner')
00011 import rospy
00012 import geometry_msgs.msg as geo_msgs
00013 
00014 
00015 def callback(pose, pub):
00016     newpose = pose
00017     newpose.pose.pose.position.z += 0.315
00018     pub.publish(newpose)
00019 
00020 
00021 def listener():
00022     rospy.init_node('corrected_initialpose')
00023     pub = rospy.Publisher('nao_corrected_initialpose',
00024                           geo_msgs.PoseWithCovarianceStamped)
00025     rospy.Subscriber("initialpose", geo_msgs.PoseWithCovarianceStamped,
00026                      callback, pub)
00027     rospy.spin()
00028 
00029 
00030 if __name__ == '__main__':
00031     try:
00032         listener()
00033     except rospy.ROSInterruptException:
00034         pass


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Wed Aug 26 2015 11:54:31