00001 """autogenerated by genpy from head_monitor_msgs/HeadMonitorActionFeedback.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import arm_navigation_msgs.msg
00008 import actionlib_msgs.msg
00009 import geometry_msgs.msg
00010 import head_monitor_msgs.msg
00011 import sensor_msgs.msg
00012 import genpy
00013 import std_msgs.msg
00014
00015 class HeadMonitorActionFeedback(genpy.Message):
00016 _md5sum = "1145699f06665025be6a400f94f4440d"
00017 _type = "head_monitor_msgs/HeadMonitorActionFeedback"
00018 _has_header = True
00019 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00020
00021 Header header
00022 actionlib_msgs/GoalStatus status
00023 HeadMonitorFeedback feedback
00024
00025 ================================================================================
00026 MSG: std_msgs/Header
00027 # Standard metadata for higher-level stamped data types.
00028 # This is generally used to communicate timestamped data
00029 # in a particular coordinate frame.
00030 #
00031 # sequence ID: consecutively increasing ID
00032 uint32 seq
00033 #Two-integer timestamp that is expressed as:
00034 # * stamp.secs: seconds (stamp_secs) since epoch
00035 # * stamp.nsecs: nanoseconds since stamp_secs
00036 # time-handling sugar is provided by the client library
00037 time stamp
00038 #Frame this data is associated with
00039 # 0: no frame
00040 # 1: global frame
00041 string frame_id
00042
00043 ================================================================================
00044 MSG: actionlib_msgs/GoalStatus
00045 GoalID goal_id
00046 uint8 status
00047 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00048 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00049 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00050 # and has since completed its execution (Terminal State)
00051 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00052 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00053 # to some failure (Terminal State)
00054 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00055 # because the goal was unattainable or invalid (Terminal State)
00056 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00057 # and has not yet completed execution
00058 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00059 # but the action server has not yet confirmed that the goal is canceled
00060 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00061 # and was successfully cancelled (Terminal State)
00062 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00063 # sent over the wire by an action server
00064
00065 #Allow for the user to associate a string with GoalStatus for debugging
00066 string text
00067
00068
00069 ================================================================================
00070 MSG: actionlib_msgs/GoalID
00071 # The stamp should store the time at which this goal was requested.
00072 # It is used by an action server when it tries to preempt all
00073 # goals that were requested before a certain time
00074 time stamp
00075
00076 # The id provides a way to associate feedback and
00077 # result message with specific goal requests. The id
00078 # specified must be unique.
00079 string id
00080
00081
00082 ================================================================================
00083 MSG: head_monitor_msgs/HeadMonitorFeedback
00084 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00085 arm_navigation_msgs/RobotState current_state
00086 arm_navigation_msgs/RobotState paused_trajectory_state
00087 arm_navigation_msgs/CollisionObject paused_collision_map
00088
00089
00090
00091 ================================================================================
00092 MSG: arm_navigation_msgs/RobotState
00093 # This message contains information about the robot state, i.e. the positions of its joints and links
00094 sensor_msgs/JointState joint_state
00095 arm_navigation_msgs/MultiDOFJointState multi_dof_joint_state
00096
00097 ================================================================================
00098 MSG: sensor_msgs/JointState
00099 # This is a message that holds data to describe the state of a set of torque controlled joints.
00100 #
00101 # The state of each joint (revolute or prismatic) is defined by:
00102 # * the position of the joint (rad or m),
00103 # * the velocity of the joint (rad/s or m/s) and
00104 # * the effort that is applied in the joint (Nm or N).
00105 #
00106 # Each joint is uniquely identified by its name
00107 # The header specifies the time at which the joint states were recorded. All the joint states
00108 # in one message have to be recorded at the same time.
00109 #
00110 # This message consists of a multiple arrays, one for each part of the joint state.
00111 # The goal is to make each of the fields optional. When e.g. your joints have no
00112 # effort associated with them, you can leave the effort array empty.
00113 #
00114 # All arrays in this message should have the same size, or be empty.
00115 # This is the only way to uniquely associate the joint name with the correct
00116 # states.
00117
00118
00119 Header header
00120
00121 string[] name
00122 float64[] position
00123 float64[] velocity
00124 float64[] effort
00125
00126 ================================================================================
00127 MSG: arm_navigation_msgs/MultiDOFJointState
00128 #A representation of a multi-dof joint state
00129 time stamp
00130 string[] joint_names
00131 string[] frame_ids
00132 string[] child_frame_ids
00133 geometry_msgs/Pose[] poses
00134
00135 ================================================================================
00136 MSG: geometry_msgs/Pose
00137 # A representation of pose in free space, composed of postion and orientation.
00138 Point position
00139 Quaternion orientation
00140
00141 ================================================================================
00142 MSG: geometry_msgs/Point
00143 # This contains the position of a point in free space
00144 float64 x
00145 float64 y
00146 float64 z
00147
00148 ================================================================================
00149 MSG: geometry_msgs/Quaternion
00150 # This represents an orientation in free space in quaternion form.
00151
00152 float64 x
00153 float64 y
00154 float64 z
00155 float64 w
00156
00157 ================================================================================
00158 MSG: arm_navigation_msgs/CollisionObject
00159 # a header, used for interpreting the poses
00160 Header header
00161
00162 # the id of the object
00163 string id
00164
00165 # The padding used for filtering points near the object.
00166 # This does not affect collision checking for the object.
00167 # Set to negative to get zero padding.
00168 float32 padding
00169
00170 #This contains what is to be done with the object
00171 CollisionObjectOperation operation
00172
00173 #the shapes associated with the object
00174 arm_navigation_msgs/Shape[] shapes
00175
00176 #the poses associated with the shapes - will be transformed using the header
00177 geometry_msgs/Pose[] poses
00178
00179 ================================================================================
00180 MSG: arm_navigation_msgs/CollisionObjectOperation
00181 #Puts the object into the environment
00182 #or updates the object if already added
00183 byte ADD=0
00184
00185 #Removes the object from the environment entirely
00186 byte REMOVE=1
00187
00188 #Only valid within the context of a CollisionAttachedObject message
00189 #Will be ignored if sent with an CollisionObject message
00190 #Takes an attached object, detaches from the attached link
00191 #But adds back in as regular object
00192 byte DETACH_AND_ADD_AS_OBJECT=2
00193
00194 #Only valid within the context of a CollisionAttachedObject message
00195 #Will be ignored if sent with an CollisionObject message
00196 #Takes current object in the environment and removes it as
00197 #a regular object
00198 byte ATTACH_AND_REMOVE_AS_OBJECT=3
00199
00200 # Byte code for operation
00201 byte operation
00202
00203 ================================================================================
00204 MSG: arm_navigation_msgs/Shape
00205 byte SPHERE=0
00206 byte BOX=1
00207 byte CYLINDER=2
00208 byte MESH=3
00209
00210 byte type
00211
00212
00213 #### define sphere, box, cylinder ####
00214 # the origin of each shape is considered at the shape's center
00215
00216 # for sphere
00217 # radius := dimensions[0]
00218
00219 # for cylinder
00220 # radius := dimensions[0]
00221 # length := dimensions[1]
00222 # the length is along the Z axis
00223
00224 # for box
00225 # size_x := dimensions[0]
00226 # size_y := dimensions[1]
00227 # size_z := dimensions[2]
00228 float64[] dimensions
00229
00230
00231 #### define mesh ####
00232
00233 # list of triangles; triangle k is defined by tre vertices located
00234 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00235 int32[] triangles
00236 geometry_msgs/Point[] vertices
00237
00238 """
00239 __slots__ = ['header','status','feedback']
00240 _slot_types = ['std_msgs/Header','actionlib_msgs/GoalStatus','head_monitor_msgs/HeadMonitorFeedback']
00241
00242 def __init__(self, *args, **kwds):
00243 """
00244 Constructor. Any message fields that are implicitly/explicitly
00245 set to None will be assigned a default value. The recommend
00246 use is keyword arguments as this is more robust to future message
00247 changes. You cannot mix in-order arguments and keyword arguments.
00248
00249 The available fields are:
00250 header,status,feedback
00251
00252 :param args: complete set of field values, in .msg order
00253 :param kwds: use keyword arguments corresponding to message field names
00254 to set specific fields.
00255 """
00256 if args or kwds:
00257 super(HeadMonitorActionFeedback, self).__init__(*args, **kwds)
00258
00259 if self.header is None:
00260 self.header = std_msgs.msg.Header()
00261 if self.status is None:
00262 self.status = actionlib_msgs.msg.GoalStatus()
00263 if self.feedback is None:
00264 self.feedback = head_monitor_msgs.msg.HeadMonitorFeedback()
00265 else:
00266 self.header = std_msgs.msg.Header()
00267 self.status = actionlib_msgs.msg.GoalStatus()
00268 self.feedback = head_monitor_msgs.msg.HeadMonitorFeedback()
00269
00270 def _get_types(self):
00271 """
00272 internal API method
00273 """
00274 return self._slot_types
00275
00276 def serialize(self, buff):
00277 """
00278 serialize message into buffer
00279 :param buff: buffer, ``StringIO``
00280 """
00281 try:
00282 _x = self
00283 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00284 _x = self.header.frame_id
00285 length = len(_x)
00286 if python3 or type(_x) == unicode:
00287 _x = _x.encode('utf-8')
00288 length = len(_x)
00289 buff.write(struct.pack('<I%ss'%length, length, _x))
00290 _x = self
00291 buff.write(_struct_2I.pack(_x.status.goal_id.stamp.secs, _x.status.goal_id.stamp.nsecs))
00292 _x = self.status.goal_id.id
00293 length = len(_x)
00294 if python3 or type(_x) == unicode:
00295 _x = _x.encode('utf-8')
00296 length = len(_x)
00297 buff.write(struct.pack('<I%ss'%length, length, _x))
00298 buff.write(_struct_B.pack(self.status.status))
00299 _x = self.status.text
00300 length = len(_x)
00301 if python3 or type(_x) == unicode:
00302 _x = _x.encode('utf-8')
00303 length = len(_x)
00304 buff.write(struct.pack('<I%ss'%length, length, _x))
00305 _x = self
00306 buff.write(_struct_3I.pack(_x.feedback.current_state.joint_state.header.seq, _x.feedback.current_state.joint_state.header.stamp.secs, _x.feedback.current_state.joint_state.header.stamp.nsecs))
00307 _x = self.feedback.current_state.joint_state.header.frame_id
00308 length = len(_x)
00309 if python3 or type(_x) == unicode:
00310 _x = _x.encode('utf-8')
00311 length = len(_x)
00312 buff.write(struct.pack('<I%ss'%length, length, _x))
00313 length = len(self.feedback.current_state.joint_state.name)
00314 buff.write(_struct_I.pack(length))
00315 for val1 in self.feedback.current_state.joint_state.name:
00316 length = len(val1)
00317 if python3 or type(val1) == unicode:
00318 val1 = val1.encode('utf-8')
00319 length = len(val1)
00320 buff.write(struct.pack('<I%ss'%length, length, val1))
00321 length = len(self.feedback.current_state.joint_state.position)
00322 buff.write(_struct_I.pack(length))
00323 pattern = '<%sd'%length
00324 buff.write(struct.pack(pattern, *self.feedback.current_state.joint_state.position))
00325 length = len(self.feedback.current_state.joint_state.velocity)
00326 buff.write(_struct_I.pack(length))
00327 pattern = '<%sd'%length
00328 buff.write(struct.pack(pattern, *self.feedback.current_state.joint_state.velocity))
00329 length = len(self.feedback.current_state.joint_state.effort)
00330 buff.write(_struct_I.pack(length))
00331 pattern = '<%sd'%length
00332 buff.write(struct.pack(pattern, *self.feedback.current_state.joint_state.effort))
00333 _x = self
00334 buff.write(_struct_2I.pack(_x.feedback.current_state.multi_dof_joint_state.stamp.secs, _x.feedback.current_state.multi_dof_joint_state.stamp.nsecs))
00335 length = len(self.feedback.current_state.multi_dof_joint_state.joint_names)
00336 buff.write(_struct_I.pack(length))
00337 for val1 in self.feedback.current_state.multi_dof_joint_state.joint_names:
00338 length = len(val1)
00339 if python3 or type(val1) == unicode:
00340 val1 = val1.encode('utf-8')
00341 length = len(val1)
00342 buff.write(struct.pack('<I%ss'%length, length, val1))
00343 length = len(self.feedback.current_state.multi_dof_joint_state.frame_ids)
00344 buff.write(_struct_I.pack(length))
00345 for val1 in self.feedback.current_state.multi_dof_joint_state.frame_ids:
00346 length = len(val1)
00347 if python3 or type(val1) == unicode:
00348 val1 = val1.encode('utf-8')
00349 length = len(val1)
00350 buff.write(struct.pack('<I%ss'%length, length, val1))
00351 length = len(self.feedback.current_state.multi_dof_joint_state.child_frame_ids)
00352 buff.write(_struct_I.pack(length))
00353 for val1 in self.feedback.current_state.multi_dof_joint_state.child_frame_ids:
00354 length = len(val1)
00355 if python3 or type(val1) == unicode:
00356 val1 = val1.encode('utf-8')
00357 length = len(val1)
00358 buff.write(struct.pack('<I%ss'%length, length, val1))
00359 length = len(self.feedback.current_state.multi_dof_joint_state.poses)
00360 buff.write(_struct_I.pack(length))
00361 for val1 in self.feedback.current_state.multi_dof_joint_state.poses:
00362 _v1 = val1.position
00363 _x = _v1
00364 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00365 _v2 = val1.orientation
00366 _x = _v2
00367 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00368 _x = self
00369 buff.write(_struct_3I.pack(_x.feedback.paused_trajectory_state.joint_state.header.seq, _x.feedback.paused_trajectory_state.joint_state.header.stamp.secs, _x.feedback.paused_trajectory_state.joint_state.header.stamp.nsecs))
00370 _x = self.feedback.paused_trajectory_state.joint_state.header.frame_id
00371 length = len(_x)
00372 if python3 or type(_x) == unicode:
00373 _x = _x.encode('utf-8')
00374 length = len(_x)
00375 buff.write(struct.pack('<I%ss'%length, length, _x))
00376 length = len(self.feedback.paused_trajectory_state.joint_state.name)
00377 buff.write(_struct_I.pack(length))
00378 for val1 in self.feedback.paused_trajectory_state.joint_state.name:
00379 length = len(val1)
00380 if python3 or type(val1) == unicode:
00381 val1 = val1.encode('utf-8')
00382 length = len(val1)
00383 buff.write(struct.pack('<I%ss'%length, length, val1))
00384 length = len(self.feedback.paused_trajectory_state.joint_state.position)
00385 buff.write(_struct_I.pack(length))
00386 pattern = '<%sd'%length
00387 buff.write(struct.pack(pattern, *self.feedback.paused_trajectory_state.joint_state.position))
00388 length = len(self.feedback.paused_trajectory_state.joint_state.velocity)
00389 buff.write(_struct_I.pack(length))
00390 pattern = '<%sd'%length
00391 buff.write(struct.pack(pattern, *self.feedback.paused_trajectory_state.joint_state.velocity))
00392 length = len(self.feedback.paused_trajectory_state.joint_state.effort)
00393 buff.write(_struct_I.pack(length))
00394 pattern = '<%sd'%length
00395 buff.write(struct.pack(pattern, *self.feedback.paused_trajectory_state.joint_state.effort))
00396 _x = self
00397 buff.write(_struct_2I.pack(_x.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.secs, _x.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.nsecs))
00398 length = len(self.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names)
00399 buff.write(_struct_I.pack(length))
00400 for val1 in self.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names:
00401 length = len(val1)
00402 if python3 or type(val1) == unicode:
00403 val1 = val1.encode('utf-8')
00404 length = len(val1)
00405 buff.write(struct.pack('<I%ss'%length, length, val1))
00406 length = len(self.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids)
00407 buff.write(_struct_I.pack(length))
00408 for val1 in self.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids:
00409 length = len(val1)
00410 if python3 or type(val1) == unicode:
00411 val1 = val1.encode('utf-8')
00412 length = len(val1)
00413 buff.write(struct.pack('<I%ss'%length, length, val1))
00414 length = len(self.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids)
00415 buff.write(_struct_I.pack(length))
00416 for val1 in self.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids:
00417 length = len(val1)
00418 if python3 or type(val1) == unicode:
00419 val1 = val1.encode('utf-8')
00420 length = len(val1)
00421 buff.write(struct.pack('<I%ss'%length, length, val1))
00422 length = len(self.feedback.paused_trajectory_state.multi_dof_joint_state.poses)
00423 buff.write(_struct_I.pack(length))
00424 for val1 in self.feedback.paused_trajectory_state.multi_dof_joint_state.poses:
00425 _v3 = val1.position
00426 _x = _v3
00427 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00428 _v4 = val1.orientation
00429 _x = _v4
00430 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00431 _x = self
00432 buff.write(_struct_3I.pack(_x.feedback.paused_collision_map.header.seq, _x.feedback.paused_collision_map.header.stamp.secs, _x.feedback.paused_collision_map.header.stamp.nsecs))
00433 _x = self.feedback.paused_collision_map.header.frame_id
00434 length = len(_x)
00435 if python3 or type(_x) == unicode:
00436 _x = _x.encode('utf-8')
00437 length = len(_x)
00438 buff.write(struct.pack('<I%ss'%length, length, _x))
00439 _x = self.feedback.paused_collision_map.id
00440 length = len(_x)
00441 if python3 or type(_x) == unicode:
00442 _x = _x.encode('utf-8')
00443 length = len(_x)
00444 buff.write(struct.pack('<I%ss'%length, length, _x))
00445 _x = self
00446 buff.write(_struct_fb.pack(_x.feedback.paused_collision_map.padding, _x.feedback.paused_collision_map.operation.operation))
00447 length = len(self.feedback.paused_collision_map.shapes)
00448 buff.write(_struct_I.pack(length))
00449 for val1 in self.feedback.paused_collision_map.shapes:
00450 buff.write(_struct_b.pack(val1.type))
00451 length = len(val1.dimensions)
00452 buff.write(_struct_I.pack(length))
00453 pattern = '<%sd'%length
00454 buff.write(struct.pack(pattern, *val1.dimensions))
00455 length = len(val1.triangles)
00456 buff.write(_struct_I.pack(length))
00457 pattern = '<%si'%length
00458 buff.write(struct.pack(pattern, *val1.triangles))
00459 length = len(val1.vertices)
00460 buff.write(_struct_I.pack(length))
00461 for val2 in val1.vertices:
00462 _x = val2
00463 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00464 length = len(self.feedback.paused_collision_map.poses)
00465 buff.write(_struct_I.pack(length))
00466 for val1 in self.feedback.paused_collision_map.poses:
00467 _v5 = val1.position
00468 _x = _v5
00469 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00470 _v6 = val1.orientation
00471 _x = _v6
00472 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00473 except struct.error as se: self._check_types(se)
00474 except TypeError as te: self._check_types(te)
00475
00476 def deserialize(self, str):
00477 """
00478 unpack serialized message in str into this message instance
00479 :param str: byte array of serialized message, ``str``
00480 """
00481 try:
00482 if self.header is None:
00483 self.header = std_msgs.msg.Header()
00484 if self.status is None:
00485 self.status = actionlib_msgs.msg.GoalStatus()
00486 if self.feedback is None:
00487 self.feedback = head_monitor_msgs.msg.HeadMonitorFeedback()
00488 end = 0
00489 _x = self
00490 start = end
00491 end += 12
00492 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00493 start = end
00494 end += 4
00495 (length,) = _struct_I.unpack(str[start:end])
00496 start = end
00497 end += length
00498 if python3:
00499 self.header.frame_id = str[start:end].decode('utf-8')
00500 else:
00501 self.header.frame_id = str[start:end]
00502 _x = self
00503 start = end
00504 end += 8
00505 (_x.status.goal_id.stamp.secs, _x.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00506 start = end
00507 end += 4
00508 (length,) = _struct_I.unpack(str[start:end])
00509 start = end
00510 end += length
00511 if python3:
00512 self.status.goal_id.id = str[start:end].decode('utf-8')
00513 else:
00514 self.status.goal_id.id = str[start:end]
00515 start = end
00516 end += 1
00517 (self.status.status,) = _struct_B.unpack(str[start:end])
00518 start = end
00519 end += 4
00520 (length,) = _struct_I.unpack(str[start:end])
00521 start = end
00522 end += length
00523 if python3:
00524 self.status.text = str[start:end].decode('utf-8')
00525 else:
00526 self.status.text = str[start:end]
00527 _x = self
00528 start = end
00529 end += 12
00530 (_x.feedback.current_state.joint_state.header.seq, _x.feedback.current_state.joint_state.header.stamp.secs, _x.feedback.current_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00531 start = end
00532 end += 4
00533 (length,) = _struct_I.unpack(str[start:end])
00534 start = end
00535 end += length
00536 if python3:
00537 self.feedback.current_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
00538 else:
00539 self.feedback.current_state.joint_state.header.frame_id = str[start:end]
00540 start = end
00541 end += 4
00542 (length,) = _struct_I.unpack(str[start:end])
00543 self.feedback.current_state.joint_state.name = []
00544 for i in range(0, length):
00545 start = end
00546 end += 4
00547 (length,) = _struct_I.unpack(str[start:end])
00548 start = end
00549 end += length
00550 if python3:
00551 val1 = str[start:end].decode('utf-8')
00552 else:
00553 val1 = str[start:end]
00554 self.feedback.current_state.joint_state.name.append(val1)
00555 start = end
00556 end += 4
00557 (length,) = _struct_I.unpack(str[start:end])
00558 pattern = '<%sd'%length
00559 start = end
00560 end += struct.calcsize(pattern)
00561 self.feedback.current_state.joint_state.position = struct.unpack(pattern, str[start:end])
00562 start = end
00563 end += 4
00564 (length,) = _struct_I.unpack(str[start:end])
00565 pattern = '<%sd'%length
00566 start = end
00567 end += struct.calcsize(pattern)
00568 self.feedback.current_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
00569 start = end
00570 end += 4
00571 (length,) = _struct_I.unpack(str[start:end])
00572 pattern = '<%sd'%length
00573 start = end
00574 end += struct.calcsize(pattern)
00575 self.feedback.current_state.joint_state.effort = struct.unpack(pattern, str[start:end])
00576 _x = self
00577 start = end
00578 end += 8
00579 (_x.feedback.current_state.multi_dof_joint_state.stamp.secs, _x.feedback.current_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00580 start = end
00581 end += 4
00582 (length,) = _struct_I.unpack(str[start:end])
00583 self.feedback.current_state.multi_dof_joint_state.joint_names = []
00584 for i in range(0, length):
00585 start = end
00586 end += 4
00587 (length,) = _struct_I.unpack(str[start:end])
00588 start = end
00589 end += length
00590 if python3:
00591 val1 = str[start:end].decode('utf-8')
00592 else:
00593 val1 = str[start:end]
00594 self.feedback.current_state.multi_dof_joint_state.joint_names.append(val1)
00595 start = end
00596 end += 4
00597 (length,) = _struct_I.unpack(str[start:end])
00598 self.feedback.current_state.multi_dof_joint_state.frame_ids = []
00599 for i in range(0, length):
00600 start = end
00601 end += 4
00602 (length,) = _struct_I.unpack(str[start:end])
00603 start = end
00604 end += length
00605 if python3:
00606 val1 = str[start:end].decode('utf-8')
00607 else:
00608 val1 = str[start:end]
00609 self.feedback.current_state.multi_dof_joint_state.frame_ids.append(val1)
00610 start = end
00611 end += 4
00612 (length,) = _struct_I.unpack(str[start:end])
00613 self.feedback.current_state.multi_dof_joint_state.child_frame_ids = []
00614 for i in range(0, length):
00615 start = end
00616 end += 4
00617 (length,) = _struct_I.unpack(str[start:end])
00618 start = end
00619 end += length
00620 if python3:
00621 val1 = str[start:end].decode('utf-8')
00622 else:
00623 val1 = str[start:end]
00624 self.feedback.current_state.multi_dof_joint_state.child_frame_ids.append(val1)
00625 start = end
00626 end += 4
00627 (length,) = _struct_I.unpack(str[start:end])
00628 self.feedback.current_state.multi_dof_joint_state.poses = []
00629 for i in range(0, length):
00630 val1 = geometry_msgs.msg.Pose()
00631 _v7 = val1.position
00632 _x = _v7
00633 start = end
00634 end += 24
00635 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00636 _v8 = val1.orientation
00637 _x = _v8
00638 start = end
00639 end += 32
00640 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00641 self.feedback.current_state.multi_dof_joint_state.poses.append(val1)
00642 _x = self
00643 start = end
00644 end += 12
00645 (_x.feedback.paused_trajectory_state.joint_state.header.seq, _x.feedback.paused_trajectory_state.joint_state.header.stamp.secs, _x.feedback.paused_trajectory_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00646 start = end
00647 end += 4
00648 (length,) = _struct_I.unpack(str[start:end])
00649 start = end
00650 end += length
00651 if python3:
00652 self.feedback.paused_trajectory_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
00653 else:
00654 self.feedback.paused_trajectory_state.joint_state.header.frame_id = str[start:end]
00655 start = end
00656 end += 4
00657 (length,) = _struct_I.unpack(str[start:end])
00658 self.feedback.paused_trajectory_state.joint_state.name = []
00659 for i in range(0, length):
00660 start = end
00661 end += 4
00662 (length,) = _struct_I.unpack(str[start:end])
00663 start = end
00664 end += length
00665 if python3:
00666 val1 = str[start:end].decode('utf-8')
00667 else:
00668 val1 = str[start:end]
00669 self.feedback.paused_trajectory_state.joint_state.name.append(val1)
00670 start = end
00671 end += 4
00672 (length,) = _struct_I.unpack(str[start:end])
00673 pattern = '<%sd'%length
00674 start = end
00675 end += struct.calcsize(pattern)
00676 self.feedback.paused_trajectory_state.joint_state.position = struct.unpack(pattern, str[start:end])
00677 start = end
00678 end += 4
00679 (length,) = _struct_I.unpack(str[start:end])
00680 pattern = '<%sd'%length
00681 start = end
00682 end += struct.calcsize(pattern)
00683 self.feedback.paused_trajectory_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
00684 start = end
00685 end += 4
00686 (length,) = _struct_I.unpack(str[start:end])
00687 pattern = '<%sd'%length
00688 start = end
00689 end += struct.calcsize(pattern)
00690 self.feedback.paused_trajectory_state.joint_state.effort = struct.unpack(pattern, str[start:end])
00691 _x = self
00692 start = end
00693 end += 8
00694 (_x.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.secs, _x.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00695 start = end
00696 end += 4
00697 (length,) = _struct_I.unpack(str[start:end])
00698 self.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names = []
00699 for i in range(0, length):
00700 start = end
00701 end += 4
00702 (length,) = _struct_I.unpack(str[start:end])
00703 start = end
00704 end += length
00705 if python3:
00706 val1 = str[start:end].decode('utf-8')
00707 else:
00708 val1 = str[start:end]
00709 self.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names.append(val1)
00710 start = end
00711 end += 4
00712 (length,) = _struct_I.unpack(str[start:end])
00713 self.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids = []
00714 for i in range(0, length):
00715 start = end
00716 end += 4
00717 (length,) = _struct_I.unpack(str[start:end])
00718 start = end
00719 end += length
00720 if python3:
00721 val1 = str[start:end].decode('utf-8')
00722 else:
00723 val1 = str[start:end]
00724 self.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids.append(val1)
00725 start = end
00726 end += 4
00727 (length,) = _struct_I.unpack(str[start:end])
00728 self.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids = []
00729 for i in range(0, length):
00730 start = end
00731 end += 4
00732 (length,) = _struct_I.unpack(str[start:end])
00733 start = end
00734 end += length
00735 if python3:
00736 val1 = str[start:end].decode('utf-8')
00737 else:
00738 val1 = str[start:end]
00739 self.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids.append(val1)
00740 start = end
00741 end += 4
00742 (length,) = _struct_I.unpack(str[start:end])
00743 self.feedback.paused_trajectory_state.multi_dof_joint_state.poses = []
00744 for i in range(0, length):
00745 val1 = geometry_msgs.msg.Pose()
00746 _v9 = val1.position
00747 _x = _v9
00748 start = end
00749 end += 24
00750 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00751 _v10 = val1.orientation
00752 _x = _v10
00753 start = end
00754 end += 32
00755 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00756 self.feedback.paused_trajectory_state.multi_dof_joint_state.poses.append(val1)
00757 _x = self
00758 start = end
00759 end += 12
00760 (_x.feedback.paused_collision_map.header.seq, _x.feedback.paused_collision_map.header.stamp.secs, _x.feedback.paused_collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00761 start = end
00762 end += 4
00763 (length,) = _struct_I.unpack(str[start:end])
00764 start = end
00765 end += length
00766 if python3:
00767 self.feedback.paused_collision_map.header.frame_id = str[start:end].decode('utf-8')
00768 else:
00769 self.feedback.paused_collision_map.header.frame_id = str[start:end]
00770 start = end
00771 end += 4
00772 (length,) = _struct_I.unpack(str[start:end])
00773 start = end
00774 end += length
00775 if python3:
00776 self.feedback.paused_collision_map.id = str[start:end].decode('utf-8')
00777 else:
00778 self.feedback.paused_collision_map.id = str[start:end]
00779 _x = self
00780 start = end
00781 end += 5
00782 (_x.feedback.paused_collision_map.padding, _x.feedback.paused_collision_map.operation.operation,) = _struct_fb.unpack(str[start:end])
00783 start = end
00784 end += 4
00785 (length,) = _struct_I.unpack(str[start:end])
00786 self.feedback.paused_collision_map.shapes = []
00787 for i in range(0, length):
00788 val1 = arm_navigation_msgs.msg.Shape()
00789 start = end
00790 end += 1
00791 (val1.type,) = _struct_b.unpack(str[start:end])
00792 start = end
00793 end += 4
00794 (length,) = _struct_I.unpack(str[start:end])
00795 pattern = '<%sd'%length
00796 start = end
00797 end += struct.calcsize(pattern)
00798 val1.dimensions = struct.unpack(pattern, str[start:end])
00799 start = end
00800 end += 4
00801 (length,) = _struct_I.unpack(str[start:end])
00802 pattern = '<%si'%length
00803 start = end
00804 end += struct.calcsize(pattern)
00805 val1.triangles = struct.unpack(pattern, str[start:end])
00806 start = end
00807 end += 4
00808 (length,) = _struct_I.unpack(str[start:end])
00809 val1.vertices = []
00810 for i in range(0, length):
00811 val2 = geometry_msgs.msg.Point()
00812 _x = val2
00813 start = end
00814 end += 24
00815 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00816 val1.vertices.append(val2)
00817 self.feedback.paused_collision_map.shapes.append(val1)
00818 start = end
00819 end += 4
00820 (length,) = _struct_I.unpack(str[start:end])
00821 self.feedback.paused_collision_map.poses = []
00822 for i in range(0, length):
00823 val1 = geometry_msgs.msg.Pose()
00824 _v11 = val1.position
00825 _x = _v11
00826 start = end
00827 end += 24
00828 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00829 _v12 = val1.orientation
00830 _x = _v12
00831 start = end
00832 end += 32
00833 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00834 self.feedback.paused_collision_map.poses.append(val1)
00835 return self
00836 except struct.error as e:
00837 raise genpy.DeserializationError(e)
00838
00839
00840 def serialize_numpy(self, buff, numpy):
00841 """
00842 serialize message with numpy array types into buffer
00843 :param buff: buffer, ``StringIO``
00844 :param numpy: numpy python module
00845 """
00846 try:
00847 _x = self
00848 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00849 _x = self.header.frame_id
00850 length = len(_x)
00851 if python3 or type(_x) == unicode:
00852 _x = _x.encode('utf-8')
00853 length = len(_x)
00854 buff.write(struct.pack('<I%ss'%length, length, _x))
00855 _x = self
00856 buff.write(_struct_2I.pack(_x.status.goal_id.stamp.secs, _x.status.goal_id.stamp.nsecs))
00857 _x = self.status.goal_id.id
00858 length = len(_x)
00859 if python3 or type(_x) == unicode:
00860 _x = _x.encode('utf-8')
00861 length = len(_x)
00862 buff.write(struct.pack('<I%ss'%length, length, _x))
00863 buff.write(_struct_B.pack(self.status.status))
00864 _x = self.status.text
00865 length = len(_x)
00866 if python3 or type(_x) == unicode:
00867 _x = _x.encode('utf-8')
00868 length = len(_x)
00869 buff.write(struct.pack('<I%ss'%length, length, _x))
00870 _x = self
00871 buff.write(_struct_3I.pack(_x.feedback.current_state.joint_state.header.seq, _x.feedback.current_state.joint_state.header.stamp.secs, _x.feedback.current_state.joint_state.header.stamp.nsecs))
00872 _x = self.feedback.current_state.joint_state.header.frame_id
00873 length = len(_x)
00874 if python3 or type(_x) == unicode:
00875 _x = _x.encode('utf-8')
00876 length = len(_x)
00877 buff.write(struct.pack('<I%ss'%length, length, _x))
00878 length = len(self.feedback.current_state.joint_state.name)
00879 buff.write(_struct_I.pack(length))
00880 for val1 in self.feedback.current_state.joint_state.name:
00881 length = len(val1)
00882 if python3 or type(val1) == unicode:
00883 val1 = val1.encode('utf-8')
00884 length = len(val1)
00885 buff.write(struct.pack('<I%ss'%length, length, val1))
00886 length = len(self.feedback.current_state.joint_state.position)
00887 buff.write(_struct_I.pack(length))
00888 pattern = '<%sd'%length
00889 buff.write(self.feedback.current_state.joint_state.position.tostring())
00890 length = len(self.feedback.current_state.joint_state.velocity)
00891 buff.write(_struct_I.pack(length))
00892 pattern = '<%sd'%length
00893 buff.write(self.feedback.current_state.joint_state.velocity.tostring())
00894 length = len(self.feedback.current_state.joint_state.effort)
00895 buff.write(_struct_I.pack(length))
00896 pattern = '<%sd'%length
00897 buff.write(self.feedback.current_state.joint_state.effort.tostring())
00898 _x = self
00899 buff.write(_struct_2I.pack(_x.feedback.current_state.multi_dof_joint_state.stamp.secs, _x.feedback.current_state.multi_dof_joint_state.stamp.nsecs))
00900 length = len(self.feedback.current_state.multi_dof_joint_state.joint_names)
00901 buff.write(_struct_I.pack(length))
00902 for val1 in self.feedback.current_state.multi_dof_joint_state.joint_names:
00903 length = len(val1)
00904 if python3 or type(val1) == unicode:
00905 val1 = val1.encode('utf-8')
00906 length = len(val1)
00907 buff.write(struct.pack('<I%ss'%length, length, val1))
00908 length = len(self.feedback.current_state.multi_dof_joint_state.frame_ids)
00909 buff.write(_struct_I.pack(length))
00910 for val1 in self.feedback.current_state.multi_dof_joint_state.frame_ids:
00911 length = len(val1)
00912 if python3 or type(val1) == unicode:
00913 val1 = val1.encode('utf-8')
00914 length = len(val1)
00915 buff.write(struct.pack('<I%ss'%length, length, val1))
00916 length = len(self.feedback.current_state.multi_dof_joint_state.child_frame_ids)
00917 buff.write(_struct_I.pack(length))
00918 for val1 in self.feedback.current_state.multi_dof_joint_state.child_frame_ids:
00919 length = len(val1)
00920 if python3 or type(val1) == unicode:
00921 val1 = val1.encode('utf-8')
00922 length = len(val1)
00923 buff.write(struct.pack('<I%ss'%length, length, val1))
00924 length = len(self.feedback.current_state.multi_dof_joint_state.poses)
00925 buff.write(_struct_I.pack(length))
00926 for val1 in self.feedback.current_state.multi_dof_joint_state.poses:
00927 _v13 = val1.position
00928 _x = _v13
00929 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00930 _v14 = val1.orientation
00931 _x = _v14
00932 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00933 _x = self
00934 buff.write(_struct_3I.pack(_x.feedback.paused_trajectory_state.joint_state.header.seq, _x.feedback.paused_trajectory_state.joint_state.header.stamp.secs, _x.feedback.paused_trajectory_state.joint_state.header.stamp.nsecs))
00935 _x = self.feedback.paused_trajectory_state.joint_state.header.frame_id
00936 length = len(_x)
00937 if python3 or type(_x) == unicode:
00938 _x = _x.encode('utf-8')
00939 length = len(_x)
00940 buff.write(struct.pack('<I%ss'%length, length, _x))
00941 length = len(self.feedback.paused_trajectory_state.joint_state.name)
00942 buff.write(_struct_I.pack(length))
00943 for val1 in self.feedback.paused_trajectory_state.joint_state.name:
00944 length = len(val1)
00945 if python3 or type(val1) == unicode:
00946 val1 = val1.encode('utf-8')
00947 length = len(val1)
00948 buff.write(struct.pack('<I%ss'%length, length, val1))
00949 length = len(self.feedback.paused_trajectory_state.joint_state.position)
00950 buff.write(_struct_I.pack(length))
00951 pattern = '<%sd'%length
00952 buff.write(self.feedback.paused_trajectory_state.joint_state.position.tostring())
00953 length = len(self.feedback.paused_trajectory_state.joint_state.velocity)
00954 buff.write(_struct_I.pack(length))
00955 pattern = '<%sd'%length
00956 buff.write(self.feedback.paused_trajectory_state.joint_state.velocity.tostring())
00957 length = len(self.feedback.paused_trajectory_state.joint_state.effort)
00958 buff.write(_struct_I.pack(length))
00959 pattern = '<%sd'%length
00960 buff.write(self.feedback.paused_trajectory_state.joint_state.effort.tostring())
00961 _x = self
00962 buff.write(_struct_2I.pack(_x.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.secs, _x.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.nsecs))
00963 length = len(self.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names)
00964 buff.write(_struct_I.pack(length))
00965 for val1 in self.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names:
00966 length = len(val1)
00967 if python3 or type(val1) == unicode:
00968 val1 = val1.encode('utf-8')
00969 length = len(val1)
00970 buff.write(struct.pack('<I%ss'%length, length, val1))
00971 length = len(self.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids)
00972 buff.write(_struct_I.pack(length))
00973 for val1 in self.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids:
00974 length = len(val1)
00975 if python3 or type(val1) == unicode:
00976 val1 = val1.encode('utf-8')
00977 length = len(val1)
00978 buff.write(struct.pack('<I%ss'%length, length, val1))
00979 length = len(self.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids)
00980 buff.write(_struct_I.pack(length))
00981 for val1 in self.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids:
00982 length = len(val1)
00983 if python3 or type(val1) == unicode:
00984 val1 = val1.encode('utf-8')
00985 length = len(val1)
00986 buff.write(struct.pack('<I%ss'%length, length, val1))
00987 length = len(self.feedback.paused_trajectory_state.multi_dof_joint_state.poses)
00988 buff.write(_struct_I.pack(length))
00989 for val1 in self.feedback.paused_trajectory_state.multi_dof_joint_state.poses:
00990 _v15 = val1.position
00991 _x = _v15
00992 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00993 _v16 = val1.orientation
00994 _x = _v16
00995 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00996 _x = self
00997 buff.write(_struct_3I.pack(_x.feedback.paused_collision_map.header.seq, _x.feedback.paused_collision_map.header.stamp.secs, _x.feedback.paused_collision_map.header.stamp.nsecs))
00998 _x = self.feedback.paused_collision_map.header.frame_id
00999 length = len(_x)
01000 if python3 or type(_x) == unicode:
01001 _x = _x.encode('utf-8')
01002 length = len(_x)
01003 buff.write(struct.pack('<I%ss'%length, length, _x))
01004 _x = self.feedback.paused_collision_map.id
01005 length = len(_x)
01006 if python3 or type(_x) == unicode:
01007 _x = _x.encode('utf-8')
01008 length = len(_x)
01009 buff.write(struct.pack('<I%ss'%length, length, _x))
01010 _x = self
01011 buff.write(_struct_fb.pack(_x.feedback.paused_collision_map.padding, _x.feedback.paused_collision_map.operation.operation))
01012 length = len(self.feedback.paused_collision_map.shapes)
01013 buff.write(_struct_I.pack(length))
01014 for val1 in self.feedback.paused_collision_map.shapes:
01015 buff.write(_struct_b.pack(val1.type))
01016 length = len(val1.dimensions)
01017 buff.write(_struct_I.pack(length))
01018 pattern = '<%sd'%length
01019 buff.write(val1.dimensions.tostring())
01020 length = len(val1.triangles)
01021 buff.write(_struct_I.pack(length))
01022 pattern = '<%si'%length
01023 buff.write(val1.triangles.tostring())
01024 length = len(val1.vertices)
01025 buff.write(_struct_I.pack(length))
01026 for val2 in val1.vertices:
01027 _x = val2
01028 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01029 length = len(self.feedback.paused_collision_map.poses)
01030 buff.write(_struct_I.pack(length))
01031 for val1 in self.feedback.paused_collision_map.poses:
01032 _v17 = val1.position
01033 _x = _v17
01034 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01035 _v18 = val1.orientation
01036 _x = _v18
01037 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01038 except struct.error as se: self._check_types(se)
01039 except TypeError as te: self._check_types(te)
01040
01041 def deserialize_numpy(self, str, numpy):
01042 """
01043 unpack serialized message in str into this message instance using numpy for array types
01044 :param str: byte array of serialized message, ``str``
01045 :param numpy: numpy python module
01046 """
01047 try:
01048 if self.header is None:
01049 self.header = std_msgs.msg.Header()
01050 if self.status is None:
01051 self.status = actionlib_msgs.msg.GoalStatus()
01052 if self.feedback is None:
01053 self.feedback = head_monitor_msgs.msg.HeadMonitorFeedback()
01054 end = 0
01055 _x = self
01056 start = end
01057 end += 12
01058 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01059 start = end
01060 end += 4
01061 (length,) = _struct_I.unpack(str[start:end])
01062 start = end
01063 end += length
01064 if python3:
01065 self.header.frame_id = str[start:end].decode('utf-8')
01066 else:
01067 self.header.frame_id = str[start:end]
01068 _x = self
01069 start = end
01070 end += 8
01071 (_x.status.goal_id.stamp.secs, _x.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01072 start = end
01073 end += 4
01074 (length,) = _struct_I.unpack(str[start:end])
01075 start = end
01076 end += length
01077 if python3:
01078 self.status.goal_id.id = str[start:end].decode('utf-8')
01079 else:
01080 self.status.goal_id.id = str[start:end]
01081 start = end
01082 end += 1
01083 (self.status.status,) = _struct_B.unpack(str[start:end])
01084 start = end
01085 end += 4
01086 (length,) = _struct_I.unpack(str[start:end])
01087 start = end
01088 end += length
01089 if python3:
01090 self.status.text = str[start:end].decode('utf-8')
01091 else:
01092 self.status.text = str[start:end]
01093 _x = self
01094 start = end
01095 end += 12
01096 (_x.feedback.current_state.joint_state.header.seq, _x.feedback.current_state.joint_state.header.stamp.secs, _x.feedback.current_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01097 start = end
01098 end += 4
01099 (length,) = _struct_I.unpack(str[start:end])
01100 start = end
01101 end += length
01102 if python3:
01103 self.feedback.current_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
01104 else:
01105 self.feedback.current_state.joint_state.header.frame_id = str[start:end]
01106 start = end
01107 end += 4
01108 (length,) = _struct_I.unpack(str[start:end])
01109 self.feedback.current_state.joint_state.name = []
01110 for i in range(0, length):
01111 start = end
01112 end += 4
01113 (length,) = _struct_I.unpack(str[start:end])
01114 start = end
01115 end += length
01116 if python3:
01117 val1 = str[start:end].decode('utf-8')
01118 else:
01119 val1 = str[start:end]
01120 self.feedback.current_state.joint_state.name.append(val1)
01121 start = end
01122 end += 4
01123 (length,) = _struct_I.unpack(str[start:end])
01124 pattern = '<%sd'%length
01125 start = end
01126 end += struct.calcsize(pattern)
01127 self.feedback.current_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01128 start = end
01129 end += 4
01130 (length,) = _struct_I.unpack(str[start:end])
01131 pattern = '<%sd'%length
01132 start = end
01133 end += struct.calcsize(pattern)
01134 self.feedback.current_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01135 start = end
01136 end += 4
01137 (length,) = _struct_I.unpack(str[start:end])
01138 pattern = '<%sd'%length
01139 start = end
01140 end += struct.calcsize(pattern)
01141 self.feedback.current_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01142 _x = self
01143 start = end
01144 end += 8
01145 (_x.feedback.current_state.multi_dof_joint_state.stamp.secs, _x.feedback.current_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01146 start = end
01147 end += 4
01148 (length,) = _struct_I.unpack(str[start:end])
01149 self.feedback.current_state.multi_dof_joint_state.joint_names = []
01150 for i in range(0, length):
01151 start = end
01152 end += 4
01153 (length,) = _struct_I.unpack(str[start:end])
01154 start = end
01155 end += length
01156 if python3:
01157 val1 = str[start:end].decode('utf-8')
01158 else:
01159 val1 = str[start:end]
01160 self.feedback.current_state.multi_dof_joint_state.joint_names.append(val1)
01161 start = end
01162 end += 4
01163 (length,) = _struct_I.unpack(str[start:end])
01164 self.feedback.current_state.multi_dof_joint_state.frame_ids = []
01165 for i in range(0, length):
01166 start = end
01167 end += 4
01168 (length,) = _struct_I.unpack(str[start:end])
01169 start = end
01170 end += length
01171 if python3:
01172 val1 = str[start:end].decode('utf-8')
01173 else:
01174 val1 = str[start:end]
01175 self.feedback.current_state.multi_dof_joint_state.frame_ids.append(val1)
01176 start = end
01177 end += 4
01178 (length,) = _struct_I.unpack(str[start:end])
01179 self.feedback.current_state.multi_dof_joint_state.child_frame_ids = []
01180 for i in range(0, length):
01181 start = end
01182 end += 4
01183 (length,) = _struct_I.unpack(str[start:end])
01184 start = end
01185 end += length
01186 if python3:
01187 val1 = str[start:end].decode('utf-8')
01188 else:
01189 val1 = str[start:end]
01190 self.feedback.current_state.multi_dof_joint_state.child_frame_ids.append(val1)
01191 start = end
01192 end += 4
01193 (length,) = _struct_I.unpack(str[start:end])
01194 self.feedback.current_state.multi_dof_joint_state.poses = []
01195 for i in range(0, length):
01196 val1 = geometry_msgs.msg.Pose()
01197 _v19 = val1.position
01198 _x = _v19
01199 start = end
01200 end += 24
01201 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01202 _v20 = val1.orientation
01203 _x = _v20
01204 start = end
01205 end += 32
01206 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01207 self.feedback.current_state.multi_dof_joint_state.poses.append(val1)
01208 _x = self
01209 start = end
01210 end += 12
01211 (_x.feedback.paused_trajectory_state.joint_state.header.seq, _x.feedback.paused_trajectory_state.joint_state.header.stamp.secs, _x.feedback.paused_trajectory_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01212 start = end
01213 end += 4
01214 (length,) = _struct_I.unpack(str[start:end])
01215 start = end
01216 end += length
01217 if python3:
01218 self.feedback.paused_trajectory_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
01219 else:
01220 self.feedback.paused_trajectory_state.joint_state.header.frame_id = str[start:end]
01221 start = end
01222 end += 4
01223 (length,) = _struct_I.unpack(str[start:end])
01224 self.feedback.paused_trajectory_state.joint_state.name = []
01225 for i in range(0, length):
01226 start = end
01227 end += 4
01228 (length,) = _struct_I.unpack(str[start:end])
01229 start = end
01230 end += length
01231 if python3:
01232 val1 = str[start:end].decode('utf-8')
01233 else:
01234 val1 = str[start:end]
01235 self.feedback.paused_trajectory_state.joint_state.name.append(val1)
01236 start = end
01237 end += 4
01238 (length,) = _struct_I.unpack(str[start:end])
01239 pattern = '<%sd'%length
01240 start = end
01241 end += struct.calcsize(pattern)
01242 self.feedback.paused_trajectory_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01243 start = end
01244 end += 4
01245 (length,) = _struct_I.unpack(str[start:end])
01246 pattern = '<%sd'%length
01247 start = end
01248 end += struct.calcsize(pattern)
01249 self.feedback.paused_trajectory_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01250 start = end
01251 end += 4
01252 (length,) = _struct_I.unpack(str[start:end])
01253 pattern = '<%sd'%length
01254 start = end
01255 end += struct.calcsize(pattern)
01256 self.feedback.paused_trajectory_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01257 _x = self
01258 start = end
01259 end += 8
01260 (_x.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.secs, _x.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01261 start = end
01262 end += 4
01263 (length,) = _struct_I.unpack(str[start:end])
01264 self.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names = []
01265 for i in range(0, length):
01266 start = end
01267 end += 4
01268 (length,) = _struct_I.unpack(str[start:end])
01269 start = end
01270 end += length
01271 if python3:
01272 val1 = str[start:end].decode('utf-8')
01273 else:
01274 val1 = str[start:end]
01275 self.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names.append(val1)
01276 start = end
01277 end += 4
01278 (length,) = _struct_I.unpack(str[start:end])
01279 self.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids = []
01280 for i in range(0, length):
01281 start = end
01282 end += 4
01283 (length,) = _struct_I.unpack(str[start:end])
01284 start = end
01285 end += length
01286 if python3:
01287 val1 = str[start:end].decode('utf-8')
01288 else:
01289 val1 = str[start:end]
01290 self.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids.append(val1)
01291 start = end
01292 end += 4
01293 (length,) = _struct_I.unpack(str[start:end])
01294 self.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids = []
01295 for i in range(0, length):
01296 start = end
01297 end += 4
01298 (length,) = _struct_I.unpack(str[start:end])
01299 start = end
01300 end += length
01301 if python3:
01302 val1 = str[start:end].decode('utf-8')
01303 else:
01304 val1 = str[start:end]
01305 self.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids.append(val1)
01306 start = end
01307 end += 4
01308 (length,) = _struct_I.unpack(str[start:end])
01309 self.feedback.paused_trajectory_state.multi_dof_joint_state.poses = []
01310 for i in range(0, length):
01311 val1 = geometry_msgs.msg.Pose()
01312 _v21 = val1.position
01313 _x = _v21
01314 start = end
01315 end += 24
01316 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01317 _v22 = val1.orientation
01318 _x = _v22
01319 start = end
01320 end += 32
01321 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01322 self.feedback.paused_trajectory_state.multi_dof_joint_state.poses.append(val1)
01323 _x = self
01324 start = end
01325 end += 12
01326 (_x.feedback.paused_collision_map.header.seq, _x.feedback.paused_collision_map.header.stamp.secs, _x.feedback.paused_collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01327 start = end
01328 end += 4
01329 (length,) = _struct_I.unpack(str[start:end])
01330 start = end
01331 end += length
01332 if python3:
01333 self.feedback.paused_collision_map.header.frame_id = str[start:end].decode('utf-8')
01334 else:
01335 self.feedback.paused_collision_map.header.frame_id = str[start:end]
01336 start = end
01337 end += 4
01338 (length,) = _struct_I.unpack(str[start:end])
01339 start = end
01340 end += length
01341 if python3:
01342 self.feedback.paused_collision_map.id = str[start:end].decode('utf-8')
01343 else:
01344 self.feedback.paused_collision_map.id = str[start:end]
01345 _x = self
01346 start = end
01347 end += 5
01348 (_x.feedback.paused_collision_map.padding, _x.feedback.paused_collision_map.operation.operation,) = _struct_fb.unpack(str[start:end])
01349 start = end
01350 end += 4
01351 (length,) = _struct_I.unpack(str[start:end])
01352 self.feedback.paused_collision_map.shapes = []
01353 for i in range(0, length):
01354 val1 = arm_navigation_msgs.msg.Shape()
01355 start = end
01356 end += 1
01357 (val1.type,) = _struct_b.unpack(str[start:end])
01358 start = end
01359 end += 4
01360 (length,) = _struct_I.unpack(str[start:end])
01361 pattern = '<%sd'%length
01362 start = end
01363 end += struct.calcsize(pattern)
01364 val1.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01365 start = end
01366 end += 4
01367 (length,) = _struct_I.unpack(str[start:end])
01368 pattern = '<%si'%length
01369 start = end
01370 end += struct.calcsize(pattern)
01371 val1.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01372 start = end
01373 end += 4
01374 (length,) = _struct_I.unpack(str[start:end])
01375 val1.vertices = []
01376 for i in range(0, length):
01377 val2 = geometry_msgs.msg.Point()
01378 _x = val2
01379 start = end
01380 end += 24
01381 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01382 val1.vertices.append(val2)
01383 self.feedback.paused_collision_map.shapes.append(val1)
01384 start = end
01385 end += 4
01386 (length,) = _struct_I.unpack(str[start:end])
01387 self.feedback.paused_collision_map.poses = []
01388 for i in range(0, length):
01389 val1 = geometry_msgs.msg.Pose()
01390 _v23 = val1.position
01391 _x = _v23
01392 start = end
01393 end += 24
01394 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01395 _v24 = val1.orientation
01396 _x = _v24
01397 start = end
01398 end += 32
01399 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01400 self.feedback.paused_collision_map.poses.append(val1)
01401 return self
01402 except struct.error as e:
01403 raise genpy.DeserializationError(e)
01404
01405 _struct_I = genpy.struct_I
01406 _struct_B = struct.Struct("<B")
01407 _struct_3I = struct.Struct("<3I")
01408 _struct_fb = struct.Struct("<fb")
01409 _struct_b = struct.Struct("<b")
01410 _struct_4d = struct.Struct("<4d")
01411 _struct_2I = struct.Struct("<2I")
01412 _struct_3d = struct.Struct("<3d")