def publish_time_ref(secs, nsecs, source):
"""Publish a time reference."""
# Doesn't follow the standard publishing pattern since several time
# refs could be published simultaneously
if self.time_ref_pub is None:
self.time_ref_pub = rospy.Publisher(
'~time_reference', TimeReference, queue_size=10)
time_ref_msg = TimeReference()
time_ref_msg.header = self.h
time_ref_msg.time_ref.secs = secs
time_ref_msg.time_ref.nsecs = nsecs
time_ref_msg.source = source
self.time_ref_pub.publish(time_ref_msg)
def publish_time_ref(secs, nsecs, source):
"""Publish a time reference."""
# Doesn't follow the standard publishing pattern since several time
# refs could be published simultaneously
if self.time_ref_pub is None:
self.time_ref_pub = rospy.Publisher(
'time_reference', TimeReference, queue_size=10)
time_ref_msg = TimeReference()
time_ref_msg.header = self.h
time_ref_msg.time_ref.secs = secs
time_ref_msg.time_ref.nsecs = nsecs
time_ref_msg.source = source
self.time_ref_pub.publish(time_ref_msg)
def ImageCallbackResize(Imagedata):
ResizedImage = TimeReference()
img = np.array(list(map(ord, list(Imagedata.data)))).astype("uint8")
img = np.reshape(img[::-1], (Imagedata.height, Imagedata.width))
img = resize(img, (128, 128),
preserve_range=True,
mode='constant',
anti_aliasing=True)
img = np.reshape(img, (1, 16384)).astype("uint8")
img_byte = img.tobytes()
ResizedImage.source = base64.b64encode(img_byte)
ResizedImage.header = Imagedata.header
pub.publish(ResizedImage)
def timepulse_callback(self, channel):
self.get_logger().info(f"{time.time()} Timepulse trigger")
gps_msg = NavSatFix()
timeref_msg = TimeReference()
msg_hdr = Header()
system_time = self.get_clock().now().to_msg()
msg_hdr.frame_id = 'base_link' # center of the plane
try:
ubx = self.ubp.read()
except IOError:
self.get_logger().warning("GPS disconnected. Attempting to reconnect.")
self.ubp = GPSReader(self.port, self.baud,
self.TIMEOUT, self.UBXONLY)
return
while ubx:
if (ubx.msg_cls + ubx.msg_id) == b"\x01\x07": # NAV_PVT
# <UBX(NAV-PVT, iTOW=16:50:32, year=2015, month=10, day=25, hour=16, min=50, second=48, valid=b'\xf0', tAcc=4294967295, nano=0, fixType=0, flags=b'\x00', flags2=b'$', numSV=0, lon=0, lat=0, height=0, hMSL=-17000, hAcc=4294967295, vAcc=4294967295, velN=0, velE=0, velD=0, gSpeed=0, headMot=0, sAcc=20000, headAcc=18000000, pDOP=9999, reserved1=65034815406080, headVeh=0, magDec=0, magAcc=0)>
msg_hdr.stamp = self._gen_timestamp_from_utc(ubx)
fix_stat = NavSatStatus()
if ubx.fixType == 0:
self.get_logger().warning(f"No fix yet.")
break
fix_stat.service = SERVICE_GPS
gps_msg.status = fix_stat
gps_msg.header = msg_hdr
gps_msg.latitude = float(ubx.lat)/10000000
gps_msg.longitude = float(ubx.lon)/10000000
gps_msg.altitude = float(ubx.height)/1000
timeref_msg.header = msg_hdr
timeref_msg.time_ref = system_time
timeref_msg.source = "GPS"
self.fix_pub.publish(gps_msg)
self.time_pub.publish(timeref_msg)
self.get_logger().info(f"Publishing gps message: ({timeref_msg.header.stamp.sec}.{timeref_msg.header.stamp.nanosec}): ({gps_msg.latitude}, {gps_msg.longitude}, {gps_msg.altitude})")
return
else:
self.get_logger().info(f"Other GPS MSG: {(ubx.msg_cls + ubx.msg_id)}")
ubx = self.ubp.read()
def main(args):
model = YOLO(**vars(args))
node.create_subscription(Image, 'usb_cam/image_raw', callback)
pub = node.create_publisher(TimeReference, 'people_info')
msg = TimeReference()
while rclpy.ok():
# for _ in range(5):
rclpy.spin_once(node)
try:
result = detect_image(model, args.classes_path)
print(result)
except:
result = {'objects': []}
print('no people')
msg.header = header
msg.source = json.dumps(result)
pub.publish(msg)
model.close_session()
node.destroy_node()
rclpy.shutdown()
