4 from jsk_network_tools.msg
import FC2OCSLargeData, SilverhammerInternalBuffer
6 from threading
import Lock
7 from StringIO
import StringIO
8 from std_msgs.msg
import Time
11 from struct
import pack
12 import diagnostic_updater
13 from diagnostic_msgs.msg
import DiagnosticStatus
15 from roslib.message
import get_message_class
21 from multiprocessing
import Process, Queue
26 message_class_str = rospy.get_param(
"~message",
27 "jsk_network_tools/FC2OCSLargeData")
31 raise Exception(
"invalid topic type: %s"%message_class_str)
34 self.diagnostic_updater.setHardwareID(
"none")
36 self.
latch = rospy.get_param(
"~latch",
True)
43 self.
receive_ip = rospy.get_param(
"~receive_ip",
"localhost")
45 if not self.topic_prefix.startswith(
"/"):
62 stat.summary(DiagnosticStatus.OK,
"OK")
64 now = rospy.Time.now()
65 stat.add(
"Uptime [sec]",
67 stat.add(
"Time from last input [sec]",
73 self.diagnostic_updater.update()
77 sub = rospy.Subscriber(
"~input", SilverhammerInternalBuffer, self.
callback)
82 deserialized_data = []
83 rospy.loginfo(
"data size: %d" % len(msg.data))
84 rospy.msg.deserialize_messages(b, deserialized_data,
86 rospy.loginfo(
"received %d message" % len(deserialized_data))
88 if len(deserialized_data) > 0:
90 msg = deserialized_data[0]
92 now = rospy.Time.now()
94 if pub.name
in messages:
96 rospy.loginfo(
"publishing %s" % pub.name)
98 if (hasattr(messages[pub.name],
"header")
and 99 hasattr(messages[pub.name].header,
"stamp")):
100 messages[pub.name].header.stamp = now
101 pub.publish(messages[pub.name])
104 rospy.logwarn(
"""cannot find '%s' in deserialized messages %s""" % (pub.name, messages.keys()))
105 synchronized_publishers = []
106 at_lest_one_topic =
False 109 if pub.name
in messages:
111 synchronized_publishers.append(pub)
112 if (hasattr(messages[pub.name],
"header")
and 113 hasattr(messages[pub.name].header,
"stamp")):
114 messages[pub.name].header.stamp = now
115 if (hasattr(messages[pub.name],
"header")
and 116 hasattr(messages[pub.name].header,
"seq")):
118 if (pub.name
in self.prev_seq_ids.keys()
and 119 messages[pub.name].header.seq == self.
prev_seq_ids[pub.name]):
120 rospy.loginfo(
"skip publishing %s " % (pub.name))
123 rospy.loginfo(
"messages[%s].header.seq: %d"% (pub.name,
124 messages[pub.name].header.seq))
125 rospy.loginfo(
"self.prev_seq_ids[%s]: %d" % (pub.name,
126 messages[pub.name].header.seq))
127 self.
prev_seq_ids[pub.name] = messages[pub.name].header.seq
128 at_lest_one_topic =
True 129 if at_lest_one_topic:
130 for pub
in synchronized_publishers:
131 pub.publish(messages[pub.name])
133 rospy.logerr(
"missed some packets")
136 if __name__ ==
"__main__":
137 rospy.init_node(
"silverhammer_highspeed_receiver")
timestamp_overwrite_topics
def diagnosticCallback(self, stat)
def diagnosticTimerCallback(self, event)
publish_only_if_updated_topics
fragment_packets_torelance