00001
00002 import roslib; roslib.load_manifest('walk_msgs')
00003 import rospy
00004
00005 from geometry_msgs.msg import Pose, Point
00006 import walk_msgs.msg
00007 import walk_msgs.srv
00008
00009 class Client(object):
00010 """
00011 Generic GetPath client.
00012
00013 We make here the assumption that all GetPath services are
00014 compatible in the sense that they provide /at least/ the fields of
00015 the walk_msgs.GetPath service message.
00016
00017 This class can then be inherited to add algorithm specific
00018 behaviors.
00019 """
00020
00021 client = None
00022 """ROS GetPath service client."""
00023
00024 serviceType = None
00025 """ROS service type."""
00026
00027 serviceName = None
00028 """Service name."""
00029
00030
00031 initial_left_foot_position = None
00032 """Initial left foot position (Pose)."""
00033
00034 initial_right_foot_position = None
00035 """Initial right foot position (Pose)."""
00036
00037 initial_center_of_mass_position = None
00038 """Initial center of mass position (3d point)."""
00039
00040
00041 final_left_foot_position = None
00042 """Final left foot position (Pose)."""
00043 final_right_foot_position = None
00044 """Final right foot position (Pose)."""
00045 final_center_of_mass_position = None
00046 """Final center of mass position (3d point)."""
00047
00048 start_with_left_foot = None
00049 """Which foot is the first flying foot?"""
00050 footprints = None
00051 """
00052 List of footprints.
00053
00054 The footprint type can vary depending on the used pattern
00055 generator. You have to make sure that the footprint type you use
00056 is the one that the used GetPath service expects.
00057 """
00058
00059 @staticmethod
00060 def identityPose():
00061 """
00062 Generate an identity pose.
00063 """
00064 pose = Pose()
00065 pose.position.x = 0.
00066 pose.position.y = 0.
00067 pose.position.z = 0.
00068
00069 pose.orientation.x = 0.
00070 pose.orientation.y = 0.
00071 pose.orientation.z = 0.
00072 pose.orientation.w = 1.
00073 return pose
00074
00075
00076 def __init__(self,
00077 serviceType = walk_msgs.srv.GetPath,
00078 serviceName = 'getPath'):
00079 """
00080 Initialize the class and create the proxy to call the service.
00081
00082 This will hang as long as the service is not available.
00083 """
00084 self.serviceType = serviceType
00085 self.serviceName = serviceName
00086 rospy.wait_for_service(serviceName)
00087 self.client = rospy.ServiceProxy(serviceName, serviceType)
00088
00089 self.initial_left_foot_position = self.identityPose()
00090 self.initial_right_foot_position = self.identityPose()
00091 self.initial_center_of_mass_position = Point()
00092
00093 self.final_left_foot_position = self.identityPose()
00094 self.final_right_foot_position = self.identityPose()
00095 self.final_center_of_mass_position = Point()
00096
00097
00098 def __call__(self):
00099 """
00100 Call the service and return the computed paths.
00101
00102 This will trigger the trajectory generation in the generator
00103 node.
00104 """
00105 response = self.client(
00106 self.initial_left_foot_position,
00107 self.initial_right_foot_position,
00108 self.initial_center_of_mass_position,
00109 self.final_left_foot_position,
00110 self.final_right_foot_position,
00111 self.final_center_of_mass_position,
00112 self.start_with_left_foot,
00113 self.footprints)
00114 return response.path
