def parsePacketData(self, packet_data): """ Parse the recieved packet's data and its msg object containing that data. @param packet_data : raw binary string of packet data @param type packe_data : string @return : ros msg @rtype : ros smg """ msg = self.msgConstructor() msg.deserialize(packet_data) self.last_recv = msg self.last_recv_time = time.time() return msg
def parsePacketData(self, packet_data):
"""
Parse the recieved packet's data and
its msg object containing that data.
@param packet_data : raw binary string of packet data
@param type packe_data : string
@return : ros msg
@rtype : ros smg
"""
msg = self.msgConstructor()
msg.deserialize(packet_data)
self.last_recv = msg
self.last_recv_time = time.time()
return msg
def process_entry(self, r):
blob = self.fs.get(r['blob_id'])
msg = self.msg_class()
msg.deserialize(blob.read())
return msg, r
def process_entry(self, r):
blob = self.fs.get(r["blob_id"])
msg = self.msg_class()
msg.deserialize(blob.read())
return msg, r
def spin(self):
mode = self.MODE_FIRST_FF
bytes = 0
topic = 0
need_len = 1
checksum = 0
message_in = b'' if python3 else ''
struct_B = struct.Struct("<B")
self.mutex_.acquire()
for i in self.publishers_:
self.publishers_[i].negotiated_ = False
for i in self.subscribers_:
self.subscribers_[i].negotiated_ = False
self.mutex_.release()
if not self.hardware_.connected():
return -1
else:
self.negotiateTopicsAll()
while self.spin_ and self.hardware_.connected():
if need_len > NodeHandleBase.INPUT_SIZE:
need_len = 1
mode = NodeHandle.MODE_FIRST_FF
continue
data, rv = self.hardware_.read(need_len)
if rv < 0:
return -1
for i in range(0, rv):
(sum, ) = struct_B.unpack(data[i:i + 1])
checksum += sum
if mode == NodeHandle.MODE_MESSAGE:
for i in range(0, rv):
message_in = message_in + data[i:i + 1]
bytes -= 1
if bytes == 0:
need_len = 1
mode = NodeHandle.MODE_MSG_CHECKSUM
else:
need_len = bytes
elif mode == NodeHandle.MODE_FIRST_FF:
message_in = b'' if python3 else ''
(sum, ) = struct_B.unpack(data[0:1])
if sum == 0xff:
mode += 1
elif mode == NodeHandle.MODE_PROTOCOL_VER:
(sum, ) = struct_B.unpack(data[0:1])
if sum == NodeHandleBase.PROTOCOL_VER:
mode += 1
else:
mode = NodeHandle.MODE_FIRST_FF
elif mode == self.MODE_SIZE_L:
(checksum, ) = struct_B.unpack(data[0:1])
bytes = checksum
mode += 1
elif mode == NodeHandle.MODE_SIZE_L1:
(sum, ) = struct_B.unpack(data[0:1])
bytes += sum << 8
mode += 1
elif mode == NodeHandle.MODE_SIZE_H:
(sum, ) = struct_B.unpack(data[0:1])
bytes += sum << 16
mode += 1
elif mode == NodeHandle.MODE_SIZE_H1:
(sum, ) = struct_B.unpack(data[0:1])
bytes += sum << 24
mode += 1
elif mode == NodeHandle.MODE_SIZE_CHECKSUM:
if (checksum % 256) == 255:
mode += 1
else:
mode = NodeHandle.MODE_FIRST_FF
elif mode == NodeHandle.MODE_TOPIC_L:
(topic, ) = struct_B.unpack(data[0:1])
mode += 1
checksum = topic
elif mode == NodeHandle.MODE_TOPIC_L1:
(sum, ) = struct_B.unpack(data[0:1])
topic += sum << 8
mode += 1
elif mode == NodeHandle.MODE_TOPIC_H:
(sum, ) = struct_B.unpack(data[0:1])
topic += sum << 16
mode += 1
elif mode == NodeHandle.MODE_TOPIC_H1:
(sum, ) = struct_B.unpack(data[0:1])
topic += sum << 24
mode = NodeHandle.MODE_MESSAGE
if bytes == 0:
mode = NodeHandle.MODE_MSG_CHECKSUM
else:
need_len = bytes
elif mode == NodeHandle.MODE_MSG_CHECKSUM:
mode = NodeHandle.MODE_FIRST_FF
if (checksum % 256) == 255:
if topic == tinyros_msgs.msg.TopicInfo.ID_PUBLISHER:
self.negotiateTopicsAll()
elif topic == tinyros_msgs.msg.TopicInfo.ID_ROSTOPIC_REQUEST:
msg = std_msgs.msg.String()
msg.deserialize(message_in)
self.topic_list_ = msg.data
self.topic_list_recieved_ = True
elif topic == tinyros_msgs.msg.TopicInfo.ID_ROSSERVICE_REQUEST:
msg = std_msgs.msg.String()
msg.deserialize(message_in)
self.service_list_ = msg.data
self.service_list_recieved_ = True
elif topic == tinyros_msgs.msg.TopicInfo.ID_NEGOTIATED:
ti = tinyros_msgs.msg.TopicInfo()
ti.deserialize(message_in)
try:
self.publishers_[
ti.topic_id].negotiated_ = ti.negotiated
except KeyError as ex:
pass
try:
self.subscribers_[
ti.topic_id].negotiated_ = ti.negotiated
except KeyError as ex:
pass
elif topic == tinyros_msgs.msg.TopicInfo.ID_TIME:
self.syncTime(message_in)
else:
try:
obj = tinyros.SpinObject()
obj.sub = self.subscribers_[topic]
obj.message_in = message_in
if obj.sub.topic_ == NodeHandleBase.TINYROS_LOG_TOPIC:
self.spin_log_thread_pool_.schedule(obj)
else:
if obj.sub.srv_flag_ == True:
self.spin_srv_thread_pool_.schedule(obj)
else:
self.spin_thread_pool_.schedule(obj)
except:
pass
