def take_pic_thread(self):
"""
Takes one hd picture and saves it to polled_image_resp.
:return: None
"""
self.polled_image_resp = None
try:
self.cam.setShootMode(param=['still'])
# set timestamp for picture
now = time.time()
self.hdpic_tstamp = Time(now)
# get status snapshot of cam
event = self.cam.getEvent(param=[False])
if "error" in event:
self.polled_image_resp = self.polled_image_error(
str(event['error']))
return
# check if is available to take pic
if event['result'][1]['cameraStatus'] != 'IDLE':
rospy.loginfo("Camera is busy")
self.polled_image_resp = self.polled_image_error(
"Camera is busy")
return
# take pic
self.hdpic_resp = self.cam.actTakePicture()
if 'error' in self.hdpic_resp:
rospy.logerr(self.hdpic_resp['error'])
self.polled_image_resp = self.polled_image_error(
str(self.hdpic_resp['error']))
return
# download pic
url = self.hdpic_resp['result'][0][0].replace('\\', '')
self.hdpicture = urllib2.urlopen(url).read()
self.hdpic_seq += 1 # increment sequence counter
rospy.loginfo("Picture taken")
# publish one pic
self.pub_hdpic()
# service response
self.polled_image_resp = GetPolledImageResponse(
success=True,
status_message="Picture taken",
stamp=self.hdpic_tstamp)
return
except Exception as err:
rospy.logerr(str(err))
self.polled_image_resp = self.polled_image_error(
"Couldn't take picture")
return
def polled_image_error(self, msg):
"""
Returns a failed GetPolledImageResponse.
:param msg: Error message to return with GetPolledImageResponse.
:return: GetPolledImageResponse.
"""
rospy.logerr(msg)
return GetPolledImageResponse(success=False,
status_message=msg,
stamp=Time(0))
async def manage_time(
self, *, task_status: TaskStatus = anyio.TASK_STATUS_IGNORED
) -> None:
use_sim_time = await self.get_param_default("/use_sim_time", False)
if not use_sim_time:
task_status.started()
else:
self._time = Time(0, 0)
task_status.started()
async with self.create_subscription("/clock", Clock) as subscription:
async for clock in subscription:
self._time = clock.clock
def __init__(self):
self.find_camera()
self.hdpic_seq = 0
self.hdpicture = None
self.hdpic_tstamp = Time(0, 0)
self.hdpic_msg = CompressedImage() # message to publish
self.cameras = None
self.cam = None
self.mode = None
self.hdpic_resp = None
self.polled_image_resp = None
self.hdpic_pub = None
self.incoming = None
self.img_msg = CompressedImage()
self.pub = None
self.success_liveview = False
def pub_thermalview(self):
"""
publishes thermal images at thermalview
"""
# publisher setup
pub = rospy.Publisher('thermalview/compressed',
CompressedImage,
queue_size=10)
rospy.loginfo("Beginning thermalview")
img_msg = CompressedImage() # message to publish
while not rospy.is_shutdown():
# get image
tmpimg = self.cam_api.get_image()
tmpstr = cStringIO.StringIO()
tmpimg.save(tmpstr, "PNG")
self.thermal_pic = tmpstr.getvalue()
if True: # is empty?
# set timestamp for picture
now = time.time()
self.thermal_tstamp = Time(now)
# fill message fields
img_msg.header.seq = self.thermal_pic_seq
img_msg.header.stamp = self.thermal_pic_tstamp
img_msg.header.frame_id = "thermal_cam"
img_msg.format = 'png'
img_msg.data = self.thermal_pic
# end fill
self.thermal_pic_seq += 1
pub.publish(img_msg)
def itercursors(self, topics=[], reverse=False):
if self.index is not None:
raise NotImplementedError('Cannot provide items indexed')
if reverse:
raise NotImplementedError(
'Cannot provide items in reverse when file is not indexed')
# Decode only messages that are supposed to be decoded
# print self._log.get_message_count(topic_filters=self.decoder_keys())
st, end = self.log.get_start_time(), self.log.get_end_time()
start_t = Time(st + (end - st) * self.start_idx / 100.0)
print('{} :: Reading ROSBag from {:3.2f}% onwards'.format(
self.__class__.__name__, self.start_idx))
for self.idx, (channel, msg, t) in \
enumerate(self.log.read_messages(
topics=self.decoder.keys() if not len(topics) else topics,
start_time=start_t)):
# try:
# yield (msg.header.stamp, channel, msg)
# except:
yield (t, channel, msg)
def test_Duration(self):
from genpy.rostime import Time, Duration
self.test_TVal(TVal=Duration, test_neg=True)
# test from_sec
v = Duration(1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
v = Duration(0, 1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
# test neg
v = -Duration(1, -1)
self.assertEquals(-1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, -1)
self.assertEquals(1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(999999999, v.nsecs)
# test addition
self.assertEquals(Duration(1, 0) + Time(1, 0), Time(2, 0))
failed = False
try:
v = Duration(1, 0) + 1
failed = True
except:
pass
self.failIf(failed, "Duration + int must fail")
v = Duration(1, 0) + Duration(1, 0)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2, 0), v)
v = Duration(-1, -1) + Duration(1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(), v)
v = Duration(1) + Duration(0, 1000000000)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2), v)
v = Duration(100, 100) + Duration(300)
self.assertEquals(Duration(400, 100), v)
v = Duration(100, 100) + Duration(300, 300)
self.assertEquals(Duration(400, 400), v)
v = Duration(100, 100) + Duration(300, -101)
self.assertEquals(Duration(399, 999999999), v)
# test subtraction
try:
v = Duration(1, 0) - 1
failed = True
except:
pass
self.failIf(failed, "Duration - non duration must fail")
try:
v = Duration(1, 0) - Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Duration - Time must fail")
v = Duration(1, 0) - Duration(1, 0)
self.assertEquals(Duration(), v)
v = Duration(-1, -1) - Duration(1, 1)
self.assertEquals(Duration(-3, 999999998), v)
v = Duration(1) - Duration(0, 1000000000)
self.assertEquals(Duration(), v)
v = Duration(2) - Duration(0, 1000000000)
self.assertEquals(Duration(1), v)
v = Duration(100, 100) - Duration(300)
self.assertEquals(Duration(-200, 100), v)
v = Duration(100, 100) - Duration(300, 101)
self.assertEquals(Duration(-201, 999999999), v)
# test abs
self.assertEquals(abs(Duration()), Duration())
self.assertEquals(abs(Duration(1)), Duration(1))
self.assertEquals(abs(Duration(-1)), Duration(1))
self.assertEquals(abs(Duration(0, -1)), Duration(0, 1))
self.assertEquals(abs(Duration(-1, -1)), Duration(1, 1))
self.assertEquals(abs(Duration(0, 1)), Duration(0, 1))
# Duration (float secs) vs. Duration(int, int)
self.assertEquals(Duration.from_sec(0.5), Duration(0.5))
t = Duration(0.5)
self.assert_(type(t.secs) == int)
self.assertEquals(0, t.secs)
self.assertEquals(500000000, t.nsecs)
try:
Duration(0.5, 0.5)
self.fail("should have thrown value error")
except ValueError:
pass
# Test mul
self.assertEquals(Duration(4), Duration(2) * 2)
self.assertEquals(Duration(4), Duration(2) * 2.)
self.assertEquals(Duration(10), Duration(4) * 2.5)
self.assertEquals(Duration(4, 8), Duration(2, 4) * 2)
v = Duration(4, 8) - (Duration(2, 4) * 2.)
self.assert_(abs(v.to_nsec()) < 100)
v = Duration(5, 10) - (Duration(2, 4) * 2.5)
self.assert_(abs(v.to_nsec()) < 100)
# Test div
self.assertEquals(Duration(4), Duration(8) / 2)
self.assertEquals(Duration(4), Duration(8) / 2.)
self.assertEquals(Duration(4), Duration(8) // 2)
self.assertEquals(Duration(4), Duration(8) // 2.)
self.assertEquals(Duration(4), Duration(9) // 2)
self.assertEquals(Duration(4), Duration(9) // 2.)
self.assertEquals(Duration(4, 2), Duration(8, 4) / 2)
v = Duration(4, 2) - (Duration(8, 4) / 2.)
self.assert_(abs(v.to_nsec()) < 100)
self.assertEquals(Duration(4, 2), Duration(8, 4) // 2)
self.assertEquals(Duration(4, 2), Duration(9, 5) // 2)
v = Duration(4, 2) - (Duration(9, 5) // 2.)
self.assert_(abs(v.to_nsec()) < 100)
def test_Time(self):
from genpy.rostime import Time, Duration
self.test_TVal(TVal=Time, test_neg=False)
# #1600 Duration > Time should fail
failed = False
try:
v = Duration.from_sec(0.1) > Time.from_sec(0.5)
failed = True
except:
pass
self.failIf(failed, "should have failed to compare")
try:
v = Time.from_sec(0.4) > Duration.from_sec(0.1)
failed = True
except:
pass
self.failIf(failed, "should have failed to compare")
# TODO: sub
# neg time fails
try:
Time(-1)
failed = True
except:
pass
self.failIf(failed, "negative time not allowed")
try:
Time(1, -1000000001)
failed = True
except:
pass
self.failIf(failed, "negative time not allowed")
# test Time.now() is within 10 seconds of actual time (really generous)
import time
t = time.time()
v = Time.from_sec(t)
self.assertEquals(v.to_sec(), t)
# test from_sec()
self.assertEquals(Time.from_sec(0), Time())
self.assertEquals(Time.from_sec(1.), Time(1))
self.assertEquals(Time.from_sec(v.to_sec()), v)
self.assertEquals(v.from_sec(v.to_sec()), v)
# test to_time()
self.assertEquals(v.to_sec(), v.to_time())
# test addition
# - time + time fails
try:
v = Time(1, 0) + Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Time + Time must fail")
# - time + duration
v = Time(1, 0) + Duration(1, 0)
self.assertEquals(Time(2, 0), v)
v = Duration(1, 0) + Time(1, 0)
self.assertEquals(Time(2, 0), v)
v = Time(1, 1) + Duration(1, 1)
self.assertEquals(Time(2, 2), v)
v = Duration(1, 1) + Time(1, 1)
self.assertEquals(Time(2, 2), v)
v = Time(1) + Duration(0, 1000000000)
self.assertEquals(Time(2), v)
v = Duration(1) + Time(0, 1000000000)
self.assertEquals(Time(2), v)
v = Time(100, 100) + Duration(300)
self.assertEquals(Time(400, 100), v)
v = Duration(300) + Time(100, 100)
self.assertEquals(Time(400, 100), v)
v = Time(100, 100) + Duration(300, 300)
self.assertEquals(Time(400, 400), v)
v = Duration(300, 300) + Time(100, 100)
self.assertEquals(Time(400, 400), v)
v = Time(100, 100) + Duration(300, -101)
self.assertEquals(Time(399, 999999999), v)
v = Duration(300, -101) + Time(100, 100)
self.assertEquals(Time(399, 999999999), v)
# test subtraction
try:
v = Time(1, 0) - 1
failed = True
except:
pass
self.failIf(failed, "Time - non Duration must fail")
class Foob(object):
pass
try:
v = Time(1, 0) - Foob()
failed = True
except:
pass
self.failIf(failed, "Time - non TVal must fail")
# - Time - Duration
v = Time(1, 0) - Duration(1, 0)
self.assertEquals(Time(), v)
v = Time(1, 1) - Duration(-1, -1)
self.assertEquals(Time(2, 2), v)
v = Time(1) - Duration(0, 1000000000)
self.assertEquals(Time(), v)
v = Time(2) - Duration(0, 1000000000)
self.assertEquals(Time(1), v)
v = Time(400, 100) - Duration(300)
self.assertEquals(Time(100, 100), v)
v = Time(100, 100) - Duration(0, 101)
self.assertEquals(Time(99, 999999999), v)
# - Time - Time = Duration
v = Time(100, 100) - Time(100, 100)
self.assertEquals(Duration(), v)
v = Time(100, 100) - Time(100)
self.assertEquals(Duration(0, 100), v)
v = Time(100) - Time(200)
self.assertEquals(Duration(-100), v)
# Time (float secs) vs. Time(int, int)
self.assertEquals(Time.from_sec(0.5), Time(0.5))
t = Time(0.5)
self.assert_(type(t.secs) == int)
self.assertEquals(0, t.secs)
self.assertEquals(500000000, t.nsecs)
try:
Time(0.5, 0.5)
self.fail("should have thrown value error")
except ValueError:
pass
def get_time(self) -> Time:
return self._time or Time.from_sec(time.time())
def test_Time(self):
from genpy.rostime import Time, Duration
self.test_TVal(TVal=Time, test_neg=False)
# #1600 Duration > Time should fail
failed = False
try:
v = Duration.from_sec(0.1) > Time.from_sec(0.5)
failed = True
except: pass
self.failIf(failed, "should have failed to compare")
try:
v = Time.from_sec(0.4) > Duration.from_sec(0.1)
failed = True
except: pass
self.failIf(failed, "should have failed to compare")
# TODO: sub
# neg time fails
try:
Time(-1)
failed = True
except: pass
self.failIf(failed, "negative time not allowed")
try:
Time(1, -1000000001)
failed = True
except: pass
self.failIf(failed, "negative time not allowed")
# test Time.now() is within 10 seconds of actual time (really generous)
import time
t = time.time()
v = Time.from_sec(t)
self.assertEquals(v.to_sec(), t)
# test from_sec()
self.assertEquals(Time.from_sec(0), Time())
self.assertEquals(Time.from_sec(1.), Time(1))
self.assertEquals(Time.from_sec(v.to_sec()), v)
self.assertEquals(v.from_sec(v.to_sec()), v)
# test to_time()
self.assertEquals(v.to_sec(), v.to_time())
# test addition
# - time + time fails
try:
v = Time(1,0) + Time(1, 0)
failed = True
except: pass
self.failIf(failed, "Time + Time must fail")
# - time + duration
v = Time(1,0) + Duration(1, 0)
self.assertEquals(Time(2, 0), v)
v = Duration(1, 0) + Time(1,0)
self.assertEquals(Time(2, 0), v)
v = Time(1,1) + Duration(1, 1)
self.assertEquals(Time(2, 2), v)
v = Duration(1, 1) + Time(1,1)
self.assertEquals(Time(2, 2), v)
v = Time(1) + Duration(0, 1000000000)
self.assertEquals(Time(2), v)
v = Duration(1) + Time(0, 1000000000)
self.assertEquals(Time(2), v)
v = Time(100, 100) + Duration(300)
self.assertEquals(Time(400, 100), v)
v = Duration(300) + Time(100, 100)
self.assertEquals(Time(400, 100), v)
v = Time(100, 100) + Duration(300, 300)
self.assertEquals(Time(400, 400), v)
v = Duration(300, 300) + Time(100, 100)
self.assertEquals(Time(400, 400), v)
v = Time(100, 100) + Duration(300, -101)
self.assertEquals(Time(399, 999999999), v)
v = Duration(300, -101) + Time(100, 100)
self.assertEquals(Time(399, 999999999), v)
# test subtraction
try:
v = Time(1,0) - 1
failed = True
except: pass
self.failIf(failed, "Time - non Duration must fail")
class Foob(object): pass
try:
v = Time(1,0) - Foob()
failed = True
except: pass
self.failIf(failed, "Time - non TVal must fail")
# - Time - Duration
v = Time(1,0) - Duration(1, 0)
self.assertEquals(Time(), v)
v = Time(1,1) - Duration(-1, -1)
self.assertEquals(Time(2, 2), v)
v = Time(1) - Duration(0, 1000000000)
self.assertEquals(Time(), v)
v = Time(2) - Duration(0, 1000000000)
self.assertEquals(Time(1), v)
v = Time(400, 100) - Duration(300)
self.assertEquals(Time(100, 100), v)
v = Time(100, 100) - Duration(0, 101)
self.assertEquals(Time(99, 999999999), v)
# - Time - Time = Duration
v = Time(100, 100) - Time(100, 100)
self.assertEquals(Duration(), v)
v = Time(100, 100) - Time(100)
self.assertEquals(Duration(0,100), v)
v = Time(100) - Time(200)
self.assertEquals(Duration(-100), v)
# Time (float secs) vs. Time(int, int)
self.assertEquals(Time.from_sec(0.5), Time(0.5))
t = Time(0.5)
self.assert_(type(t.secs) == int)
self.assertEquals(0, t.secs)
self.assertEquals(500000000, t.nsecs)
try:
Time(0.5, 0.5)
self.fail("should have thrown value error")
except ValueError: pass
def __init__(self):
self.thermal_pic = None
self.thermal_pic_seq = 0
self.thermal_pic_tstamp = Time(0, 0)
self.thermal_pic_msg = CompressedImage() # message to publish
self.cam_api = seek_api.SeekAPI()
print("Writing IMU data...")
with open(args.imu, 'r') as imu_file:
imu_reader = csv.reader(imu_file)
next(imu_reader) # skip header
seq_counter = 1
for row in imu_reader:
ts = float(row[0])
gyr = [float(row[i]) for i in range(1, 4)]
acc = [float(row[i]) for i in range(4, 7)]
imu_msg = Imu()
imu_msg.header.frame_id = 'imu'
imu_msg.header.seq = seq_counter
seq_counter += 1
imu_msg.header.stamp = Time.from_sec(ts)
imu_msg.angular_velocity.x = gyr[0]
imu_msg.angular_velocity.y = gyr[1]
imu_msg.angular_velocity.z = gyr[2]
imu_msg.linear_acceleration.x = acc[0]
imu_msg.linear_acceleration.y = acc[1]
imu_msg.linear_acceleration.z = acc[2]
bag_file.write('/imu', imu_msg, t=imu_msg.header.stamp)
print("Writing video data...")
bridge = cv_bridge.CvBridge()
if args.use_video:
cap = cv2.VideoCapture(args.video)
