def handleTimeRequest(self, data):
""" Respond to device with system time. """
t = Time()
t.data = rospy.Time.now()
data_buffer = StringIO.StringIO()
t.serialize(data_buffer)
self.send( TopicInfo.ID_TIME, data_buffer.getvalue() )
self.lastsync = rospy.Time.now()
def handleTimeRequest(self, data):
""" Respond to device with system time. """
t = Time()
t.data = rospy.Time.now()
data_buffer = StringIO.StringIO()
t.serialize(data_buffer)
self.send(TopicInfo.ID_TIME, data_buffer.getvalue())
self.lastsync = rospy.Time.now()
def handleTimeRequest(self, data):
""" Respond to device with system time. """
t = Time()
t.deserialize(data)
if t.data.secs == 0 and t.data.nsecs == 0:
t = Time()
t.data = rospy.Time.now()
data_buffer = StringIO.StringIO()
t.serialize(data_buffer)
self.send( TopicInfo.ID_TIME, data_buffer.getvalue() )
self.lastsync = rospy.Time.now()
else:
# TODO sync the time somehow
self.lastsync = rospy.Time.now()
pass
def run(self):
""" Forward recieved messages to appropriate publisher. """
data = ''
while not rospy.is_shutdown():
if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3):
rospy.logerr("Lost sync with device, restarting...")
self.requestTopics()
self.lastsync = rospy.Time.now()
flag = [0,0]
flag[0] = self.port.read(1)
if (flag[0] != '\xff'):
continue
flag[1] = self.port.read(1)
if ( flag[1] != '\xff'):
rospy.loginfo("Failed Packet Flags ")
continue
# topic id (2 bytes)
header = self.port.read(4)
if (len(header) != 4):
#self.port.flushInput()
continue
topic_id, msg_length = struct.unpack("<hh", header)
msg = self.port.read(msg_length)
if (len(msg) != msg_length):
rospy.loginfo("Packet Failed : Failed to read msg data")
#self.port.flushInput()
continue
chk = self.port.read(1)
checksum = sum(map(ord,header) ) + sum(map(ord, msg)) + ord(chk)
if checksum%256 == 255:
if topic_id == TopicInfo.ID_PUBLISHER:
try:
m = TopicInfo()
m.deserialize(msg)
self.senders[m.topic_id] = Publisher(m.topic_name, m.message_type)
rospy.loginfo("Setup Publisher on %s [%s]" % (m.topic_name, m.message_type) )
except Exception as e:
rospy.logerr("Failed to parse publisher: %s", e)
elif topic_id == TopicInfo.ID_SUBSCRIBER:
try:
m = TopicInfo()
m.deserialize(msg)
self.receivers[m.topic_name] = [m.topic_id, Subscriber(m.topic_name, m.message_type, self)]
rospy.loginfo("Setup Subscriber on %s [%s]" % (m.topic_name, m.message_type))
except Exception as e:
rospy.logerr("Failed to parse subscriber. %s"%e)
elif topic_id == TopicInfo.ID_SERVICE_SERVER:
try:
m = TopicInfo()
m.deserialize(msg)
self.senders[m.topic_id]=ServiceServer(m.topic_name, m.message_type, self)
rospy.loginfo("Setup ServiceServer on %s [%s]"%(m.topic_name, m.message_type) )
except:
rospy.logerr("Failed to parse service server")
elif topic_id == TopicInfo.ID_SERVICE_CLIENT:
pass
elif topic_id == TopicInfo.ID_PARAMETER_REQUEST:
self.handleParameterRequest(msg)
elif topic_id == TopicInfo.ID_LOG:
self.handleLogging(msg)
elif topic_id == TopicInfo.ID_TIME:
t = Time()
t.data = rospy.Time.now()
data_buffer = StringIO.StringIO()
t.serialize(data_buffer)
self.send( TopicInfo.ID_TIME, data_buffer.getvalue() )
self.lastsync = rospy.Time.now()
elif topic_id >= 100: # TOPIC
try:
self.senders[topic_id].handlePacket(msg)
except KeyError:
rospy.logerr("Tried to publish before configured, topic id %d" % topic_id)
else:
rospy.logerr("Unrecognized command topic!")
rospy.sleep(0.001)
def requestSyncTime(self):
t = Time()
data_buffer = StringIO.StringIO()
t.serialize(data_buffer)
self.send(TopicInfo.ID_TIME, data_buffer.getvalue())
def run(self):
""" Forward recieved messages to appropriate publisher. """
data = ''
while not rospy.is_shutdown():
if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3):
rospy.logerr("Lost sync with device, restarting...")
self.requestTopics()
self.lastsync = rospy.Time.now()
flag = [0,0]
flag[0] = self.port.read(1)
if (flag[0] != '\xff'):
continue
flag[1] = self.port.read(1)
if ( flag[1] != '\xff'):
rospy.loginfo("Failed Packet Flags ")
continue
# topic id (2 bytes)
header = self.port.read(4)
if (len(header) != 4):
#self.port.flushInput()
continue
topic_id, msg_length = struct.unpack("<hh", header)
msg = self.port.read(msg_length)
if (len(msg) != msg_length):
rospy.loginfo("Packet Failed : Failed to read msg data")
#self.port.flushInput()
continue
chk = self.port.read(1)
checksum = sum(map(ord,header) ) + sum(map(ord, msg)) + ord(chk)
if checksum%256 == 255:
if topic_id == TopicInfo.ID_PUBLISHER:
try:
m = TopicInfo()
m.deserialize(msg)
self.senders[m.topic_id] = Publisher(m.topic_name, m.message_type)
rospy.loginfo("Setup Publisher on %s [%s]" % (m.topic_name, m.message_type) )
except Exception as e:
rospy.logerr("Failed to parse publisher: %s", e)
elif topic_id == TopicInfo.ID_SUBSCRIBER:
try:
m = TopicInfo()
m.deserialize(msg)
self.receivers[m.topic_name] = [m.topic_id, Subscriber(m.topic_name, m.message_type, self)]
rospy.loginfo("Setup Subscriber on %s [%s]" % (m.topic_name, m.message_type))
except Exception as e:
rospy.logerr("Failed to parse subscriber. %s"%e)
elif topic_id == TopicInfo.ID_SERVICE_SERVER:
try:
m = TopicInfo()
m.deserialize(msg)
service = ServiceServer(m.topic_name, m.message_type, self)
self.receivers[m.topic_name] = [m.topic_id, service]
self.senders[m.topic_id] = service
rospy.loginfo("Setup ServiceServer on %s [%s]"%(m.topic_name, m.message_type) )
except:
rospy.logerr("Failed to parse service server")
elif topic_id == TopicInfo.ID_SERVICE_CLIENT:
pass
elif topic_id == TopicInfo.ID_PARAMETER_REQUEST:
self.handleParameterRequest(msg)
elif topic_id == TopicInfo.ID_LOG:
self.handleLogging(msg)
elif topic_id == TopicInfo.ID_TIME:
t = Time()
t.data = rospy.Time.now()
data_buffer = StringIO.StringIO()
t.serialize(data_buffer)
self.send( TopicInfo.ID_TIME, data_buffer.getvalue() )
self.lastsync = rospy.Time.now()
elif topic_id >= 100: # TOPIC
try:
self.senders[topic_id].handlePacket(msg)
except KeyError:
rospy.logerr("Tried to publish before configured, topic id %d" % topic_id)
else:
rospy.logerr("Unrecognized command topic!")
rospy.sleep(0.001)
