00001 """autogenerated by genmsg_py from Odometry.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import geometry_msgs.msg
00006 import std_msgs.msg
00007
00008 class Odometry(roslib.message.Message):
00009 _md5sum = "cd5e73d190d741a2f92e81eda573aca7"
00010 _type = "nav_msgs/Odometry"
00011 _has_header = True
00012 _full_text = """# This represents an estimate of a position and velocity in free space.
00013 # The pose in this message should be specified in the coordinate frame given by header.frame_id.
00014 # The twist in this message should be specified in the coordinate frame given by the child_frame_id
00015 Header header
00016 string child_frame_id
00017 geometry_msgs/PoseWithCovariance pose
00018 geometry_msgs/TwistWithCovariance twist
00019
00020 ================================================================================
00021 MSG: std_msgs/Header
00022 # Standard metadata for higher-level stamped data types.
00023 # This is generally used to communicate timestamped data
00024 # in a particular coordinate frame.
00025 #
00026 # sequence ID: consecutively increasing ID
00027 uint32 seq
00028 #Two-integer timestamp that is expressed as:
00029 # * stamp.secs: seconds (stamp_secs) since epoch
00030 # * stamp.nsecs: nanoseconds since stamp_secs
00031 # time-handling sugar is provided by the client library
00032 time stamp
00033 #Frame this data is associated with
00034 # 0: no frame
00035 # 1: global frame
00036 string frame_id
00037
00038 ================================================================================
00039 MSG: geometry_msgs/PoseWithCovariance
00040 # This represents a pose in free space with uncertainty.
00041
00042 Pose pose
00043
00044 # Row-major representation of the 6x6 covariance matrix
00045 # The orientation parameters use a fixed-axis representation.
00046 # In order, the parameters are:
00047 # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
00048 float64[36] covariance
00049
00050 ================================================================================
00051 MSG: geometry_msgs/Pose
00052 # A representation of pose in free space, composed of postion and orientation.
00053 Point position
00054 Quaternion orientation
00055
00056 ================================================================================
00057 MSG: geometry_msgs/Point
00058 # This contains the position of a point in free space
00059 float64 x
00060 float64 y
00061 float64 z
00062
00063 ================================================================================
00064 MSG: geometry_msgs/Quaternion
00065 # This represents an orientation in free space in quaternion form.
00066
00067 float64 x
00068 float64 y
00069 float64 z
00070 float64 w
00071
00072 ================================================================================
00073 MSG: geometry_msgs/TwistWithCovariance
00074 # This expresses velocity in free space with uncertianty.
00075
00076 Twist twist
00077
00078 # Row-major representation of the 6x6 covariance matrix
00079 # The orientation parameters use a fixed-axis representation.
00080 # In order, the parameters are:
00081 # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
00082 float64[36] covariance
00083
00084 ================================================================================
00085 MSG: geometry_msgs/Twist
00086 # This expresses velocity in free space broken into it's linear and angular parts.
00087 Vector3 linear
00088 Vector3 angular
00089
00090 ================================================================================
00091 MSG: geometry_msgs/Vector3
00092 # This represents a vector in free space.
00093
00094 float64 x
00095 float64 y
00096 float64 z
00097 """
00098 __slots__ = ['header','child_frame_id','pose','twist']
00099 _slot_types = ['Header','string','geometry_msgs/PoseWithCovariance','geometry_msgs/TwistWithCovariance']
00100
00101 def __init__(self, *args, **kwds):
00102 """
00103 Constructor. Any message fields that are implicitly/explicitly
00104 set to None will be assigned a default value. The recommend
00105 use is keyword arguments as this is more robust to future message
00106 changes. You cannot mix in-order arguments and keyword arguments.
00107
00108 The available fields are:
00109 header,child_frame_id,pose,twist
00110
00111 @param args: complete set of field values, in .msg order
00112 @param kwds: use keyword arguments corresponding to message field names
00113 to set specific fields.
00114 """
00115 if args or kwds:
00116 super(Odometry, self).__init__(*args, **kwds)
00117
00118 if self.header is None:
00119 self.header = std_msgs.msg._Header.Header()
00120 if self.child_frame_id is None:
00121 self.child_frame_id = ''
00122 if self.pose is None:
00123 self.pose = geometry_msgs.msg.PoseWithCovariance()
00124 if self.twist is None:
00125 self.twist = geometry_msgs.msg.TwistWithCovariance()
00126 else:
00127 self.header = std_msgs.msg._Header.Header()
00128 self.child_frame_id = ''
00129 self.pose = geometry_msgs.msg.PoseWithCovariance()
00130 self.twist = geometry_msgs.msg.TwistWithCovariance()
00131
00132 def _get_types(self):
00133 """
00134 internal API method
00135 """
00136 return self._slot_types
00137
00138 def serialize(self, buff):
00139 """
00140 serialize message into buffer
00141 @param buff: buffer
00142 @type buff: StringIO
00143 """
00144 try:
00145 _x = self
00146 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00147 _x = self.header.frame_id
00148 length = len(_x)
00149 buff.write(struct.pack('<I%ss'%length, length, _x))
00150 _x = self.child_frame_id
00151 length = len(_x)
00152 buff.write(struct.pack('<I%ss'%length, length, _x))
00153 _x = self
00154 buff.write(_struct_7d.pack(_x.pose.pose.position.x, _x.pose.pose.position.y, _x.pose.pose.position.z, _x.pose.pose.orientation.x, _x.pose.pose.orientation.y, _x.pose.pose.orientation.z, _x.pose.pose.orientation.w))
00155 buff.write(_struct_36d.pack(*self.pose.covariance))
00156 _x = self
00157 buff.write(_struct_6d.pack(_x.twist.twist.linear.x, _x.twist.twist.linear.y, _x.twist.twist.linear.z, _x.twist.twist.angular.x, _x.twist.twist.angular.y, _x.twist.twist.angular.z))
00158 buff.write(_struct_36d.pack(*self.twist.covariance))
00159 except struct.error, se: self._check_types(se)
00160 except TypeError, te: self._check_types(te)
00161
00162 def deserialize(self, str):
00163 """
00164 unpack serialized message in str into this message instance
00165 @param str: byte array of serialized message
00166 @type str: str
00167 """
00168 try:
00169 if self.header is None:
00170 self.header = std_msgs.msg._Header.Header()
00171 if self.pose is None:
00172 self.pose = geometry_msgs.msg.PoseWithCovariance()
00173 if self.twist is None:
00174 self.twist = geometry_msgs.msg.TwistWithCovariance()
00175 end = 0
00176 _x = self
00177 start = end
00178 end += 12
00179 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00180 start = end
00181 end += 4
00182 (length,) = _struct_I.unpack(str[start:end])
00183 start = end
00184 end += length
00185 self.header.frame_id = str[start:end]
00186 start = end
00187 end += 4
00188 (length,) = _struct_I.unpack(str[start:end])
00189 start = end
00190 end += length
00191 self.child_frame_id = str[start:end]
00192 _x = self
00193 start = end
00194 end += 56
00195 (_x.pose.pose.position.x, _x.pose.pose.position.y, _x.pose.pose.position.z, _x.pose.pose.orientation.x, _x.pose.pose.orientation.y, _x.pose.pose.orientation.z, _x.pose.pose.orientation.w,) = _struct_7d.unpack(str[start:end])
00196 start = end
00197 end += 288
00198 self.pose.covariance = _struct_36d.unpack(str[start:end])
00199 _x = self
00200 start = end
00201 end += 48
00202 (_x.twist.twist.linear.x, _x.twist.twist.linear.y, _x.twist.twist.linear.z, _x.twist.twist.angular.x, _x.twist.twist.angular.y, _x.twist.twist.angular.z,) = _struct_6d.unpack(str[start:end])
00203 start = end
00204 end += 288
00205 self.twist.covariance = _struct_36d.unpack(str[start:end])
00206 return self
00207 except struct.error, e:
00208 raise roslib.message.DeserializationError(e)
00209
00210
00211 def serialize_numpy(self, buff, numpy):
00212 """
00213 serialize message with numpy array types into buffer
00214 @param buff: buffer
00215 @type buff: StringIO
00216 @param numpy: numpy python module
00217 @type numpy module
00218 """
00219 try:
00220 _x = self
00221 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00222 _x = self.header.frame_id
00223 length = len(_x)
00224 buff.write(struct.pack('<I%ss'%length, length, _x))
00225 _x = self.child_frame_id
00226 length = len(_x)
00227 buff.write(struct.pack('<I%ss'%length, length, _x))
00228 _x = self
00229 buff.write(_struct_7d.pack(_x.pose.pose.position.x, _x.pose.pose.position.y, _x.pose.pose.position.z, _x.pose.pose.orientation.x, _x.pose.pose.orientation.y, _x.pose.pose.orientation.z, _x.pose.pose.orientation.w))
00230 buff.write(self.pose.covariance.tostring())
00231 _x = self
00232 buff.write(_struct_6d.pack(_x.twist.twist.linear.x, _x.twist.twist.linear.y, _x.twist.twist.linear.z, _x.twist.twist.angular.x, _x.twist.twist.angular.y, _x.twist.twist.angular.z))
00233 buff.write(self.twist.covariance.tostring())
00234 except struct.error, se: self._check_types(se)
00235 except TypeError, te: self._check_types(te)
00236
00237 def deserialize_numpy(self, str, numpy):
00238 """
00239 unpack serialized message in str into this message instance using numpy for array types
00240 @param str: byte array of serialized message
00241 @type str: str
00242 @param numpy: numpy python module
00243 @type numpy: module
00244 """
00245 try:
00246 if self.header is None:
00247 self.header = std_msgs.msg._Header.Header()
00248 if self.pose is None:
00249 self.pose = geometry_msgs.msg.PoseWithCovariance()
00250 if self.twist is None:
00251 self.twist = geometry_msgs.msg.TwistWithCovariance()
00252 end = 0
00253 _x = self
00254 start = end
00255 end += 12
00256 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00257 start = end
00258 end += 4
00259 (length,) = _struct_I.unpack(str[start:end])
00260 start = end
00261 end += length
00262 self.header.frame_id = str[start:end]
00263 start = end
00264 end += 4
00265 (length,) = _struct_I.unpack(str[start:end])
00266 start = end
00267 end += length
00268 self.child_frame_id = str[start:end]
00269 _x = self
00270 start = end
00271 end += 56
00272 (_x.pose.pose.position.x, _x.pose.pose.position.y, _x.pose.pose.position.z, _x.pose.pose.orientation.x, _x.pose.pose.orientation.y, _x.pose.pose.orientation.z, _x.pose.pose.orientation.w,) = _struct_7d.unpack(str[start:end])
00273 start = end
00274 end += 288
00275 self.pose.covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=36)
00276 _x = self
00277 start = end
00278 end += 48
00279 (_x.twist.twist.linear.x, _x.twist.twist.linear.y, _x.twist.twist.linear.z, _x.twist.twist.angular.x, _x.twist.twist.angular.y, _x.twist.twist.angular.z,) = _struct_6d.unpack(str[start:end])
00280 start = end
00281 end += 288
00282 self.twist.covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=36)
00283 return self
00284 except struct.error, e:
00285 raise roslib.message.DeserializationError(e)
00286
00287 _struct_I = roslib.message.struct_I
00288 _struct_3I = struct.Struct("<3I")
00289 _struct_7d = struct.Struct("<7d")
00290 _struct_6d = struct.Struct("<6d")
00291 _struct_36d = struct.Struct("<36d")