def test_set_rostime(self):
from rospy.rostime import _set_rostime
from rospy import Time
self.assertAlmostEqual(time.time(), rospy.get_time(), 1)
for t in [Time.from_sec(1.0), Time.from_sec(4.0)]:
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
def test_set_rostime(self):
from rospy.rostime import _set_rostime
from rospy import Time
self.assertAlmostEqual(time.time(), rospy.get_time(), 1)
for t in [Time.from_sec(1.0), Time.from_sec(4.0)]:
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
def test_sleep(self):
# test wallclock sleep
rospy.rostime.switch_to_wallclock()
rospy.sleep(0.1)
rospy.sleep(rospy.Duration.from_sec(0.1))
from rospy.rostime import _set_rostime
from rospy import Time
t = Time.from_sec(1.0)
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
import threading
#start sleeper
self.failIf(test_sleep_done)
sleepthread = threading.Thread(target=sleeper, args=())
sleepthread.daemon = True
sleepthread.start()
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_sleep_done)
t = Time.from_sec(1000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start duration sleeper
self.failIf(test_duration_sleep_done)
dursleepthread = threading.Thread(target=duration_sleeper, args=())
dursleepthread.daemon = True
dursleepthread.start()
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_duration_sleep_done)
t = Time.from_sec(2000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start backwards sleeper
self.failIf(test_backwards_sleep_done)
backsleepthread = threading.Thread(target=backwards_sleeper, args=())
backsleepthread.daemon = True
backsleepthread.start()
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_backwards_sleep_done)
t = Time.from_sec(1.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_backwards_sleep_done,
"backwards sleeper was not given an exception")
def test_sleep(self):
# test wallclock sleep
rospy.rostime.switch_to_wallclock()
rospy.sleep(0.1)
rospy.sleep(rospy.Duration.from_sec(0.1))
rospy.sleep(rospy.Duration.from_seconds(0.1))
from rospy.rostime import _set_rostime
from rospy import Time
t = Time.from_sec(1.0)
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
import thread
#start sleeper
self.failIf(test_sleep_done)
thread.start_new_thread(sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_sleep_done)
t = Time.from_sec(1000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start duration sleeper
self.failIf(test_duration_sleep_done)
thread.start_new_thread(duration_sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_duration_sleep_done)
t = Time.from_sec(2000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start backwards sleeper
self.failIf(test_backwards_sleep_done)
thread.start_new_thread(backwards_sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_backwards_sleep_done)
t = Time.from_sec(1.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_backwards_sleep_done,
"backwards sleeper was not given an exception")
def test_sleep(self):
# test wallclock sleep
rospy.rostime.switch_to_wallclock()
rospy.sleep(0.1)
rospy.sleep(rospy.Duration.from_sec(0.1))
from rospy.rostime import _set_rostime
from rospy import Time
t = Time.from_sec(1.0)
_set_rostime(t)
self.assertEquals(t, rospy.get_rostime())
self.assertEquals(t.to_time(), rospy.get_time())
import thread
#start sleeper
self.failIf(test_sleep_done)
thread.start_new_thread(sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_sleep_done)
t = Time.from_sec(1000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start duration sleeper
self.failIf(test_duration_sleep_done)
thread.start_new_thread(duration_sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_duration_sleep_done)
t = Time.from_sec(2000000.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_sleep_done, "sleeper did not wake up")
#start backwards sleeper
self.failIf(test_backwards_sleep_done)
thread.start_new_thread(backwards_sleeper, ())
time.sleep(1.0) #make sure thread is spun up
self.failIf(test_backwards_sleep_done)
t = Time.from_sec(1.0)
_set_rostime(t)
time.sleep(0.5) #give sleeper time to wakeup
self.assert_(test_backwards_sleep_done, "backwards sleeper was not given an exception")
def to_msg(self):
return LocationTag(self.gid, self.bid, self.rssi,
Time.from_sec(self.timestamp))
def test_Time(self):
# This is a copy of test_roslib_rostime
from rospy import Time, Duration
# #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")
try: # neg time fails
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)
bridge = CvBridge()
ctime = Time.now()
ctime_sec = ctime.to_sec()
bag = rosbag.Bag('/home/hoangqc/Datasets/test.bag', 'w')
for i in range(200):
m_Imu = Imu()
m_Odom = Odometry()
m_Imu.header.stamp = ctime
m_Imu.header.seq = i
m_Imu.orientation = Quaternion(0, 0, 0, 1)
m_Imu.angular_velocity = Vector3(0, 0, 0)
m_Imu.linear_acceleration = Vector3(0, 0, 0, 10)
newTime = Time.from_sec(ctime_sec + i * delta_imu)
bag.write(topic='/imu0', msg=m_Imu, t=newTime)
bag.write(topic='/odometry', msg=m_Odom, t=newTime)
...
for i in range(20):
newTime = Time.from_sec(ctime_sec + i * delta_img)
img_L = np.zeros((400, 640, 3), np.uint8)
img_L_depth = np.zeros((400, 640), np.uint16)
img_R = np.zeros((400, 640, 3), np.uint8)
img_L.fill(128)
img_R.fill(200)
img_L_depth.fill(128)
return msg_depth, msg_seg
if __name__ == "__main__":
waitForTakeOff()
is_flying = client.getMultirotorState().landed_state
while is_flying:
if count_IMU == 0:
ctime = Time.now()
if client.simIsPause() == False:
client.simPause(True)
if count_IMU % 5 == 0:
img_Time = Time.from_sec(ctime.to_sec() + count_img * interval_img)
msg_L, msg_R = getBinocularImg(count=count_img, time=img_Time, need_compress=False)
bag.write(topic='/cam0/image_raw', msg=msg_L, t=img_Time)
bag.write(topic='/cam1/image_raw', msg=msg_R, t=img_Time)
print('- Get Image: ', count_img)
count_img += 1
# if count_IMU % 100 == 0:
# msg_depth, msg_seg = getGroundTruthImg(count=count_img, time=img_Time, need_compress=False)
# bag.write(topic='/cam0/depth', msg=msg_depth, t=img_Time)
# bag.write(topic='/cam0/segmentation', msg=msg_seg, t=img_Time)
# print('- - - Get GroundTruth: ', count_img)
# ...
for i in range(10):
def test_Time(self):
# This is a copy of test_roslib_rostime
from rospy import Time, Duration
# #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")
try: # neg time fails
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)
def to_msg(self):
return LocationTag(self.gid, self.bid, self.rssi, Time.from_sec(self.timestamp))
def reversed_time(t):
return Time.from_sec(end_limit - t.to_sec() + start_limit)
def reverse_bag(bagfile,
out_filename=None,
topics=None,
compression=rosbag.Compression.NONE,
start_limit=float('-inf'),
end_limit=float('inf'),
debug=False):
# get min and max time in bagfile
if start_limit == float('-inf') or end_limit == float('inf'):
limits = get_limits(bagfile)
start_limit = limits[0] if start_limit == float(
'-inf') else start_limit
end_limit = limits[1] if end_limit == float('inf') else end_limit
assert end_limit > start_limit
# check output file
if out_filename is None:
out_filename = bagfile.rstrip('.bag') + "_reversed.bag"
# output some information
print("topics filter: ", topics)
# split bagfile
inbag = rosbag.Bag(bagfile,
'r',
skip_index=False,
chunk_threshold=8 * 1024 * 1024)
if debug:
print("writing to %s." % out_filename)
outbag = rosbag.Bag(out_filename,
'w',
compression=compression,
chunk_threshold=32 * 1024 * 1024)
progress = tqdm(total=end_limit - start_limit,
unit='bag-s',
unit_scale=True,
desc="Forward pass")
def reversed_time(t):
return Time.from_sec(end_limit - t.to_sec() + start_limit)
try:
messages = []
last_t = start_limit
for next in inbag.read_messages(topics, Time.from_sec(start_limit),
Time.from_sec(end_limit), None, False,
False):
messages.append(next)
progress.update(next[2].to_sec() - last_t)
last_t = next[2].to_sec()
last_t = end_limit
progress = tqdm(total=end_limit - start_limit,
unit='bag-s',
unit_scale=True,
desc="Backward pass")
while len(messages) > 0:
next = messages.pop()
progress.update(last_t - next[2].to_sec())
new_msg = next[1]
if hasattr(new_msg, 'header'):
new_msg.header.stamp = reversed_time(new_msg.header.stamp)
elif hasattr(new_msg, 'transforms'):
for i in range(len(new_msg.transforms)):
assert hasattr(new_msg.transforms[i], 'header')
new_msg.transforms[i].header.stamp = reversed_time(
new_msg.transforms[i].header.stamp)
else:
print("Unrecognized message type: " + new_msg)
outbag.write(next[0], new_msg, reversed_time(next[2]), raw=False)
last_t = next[2].to_sec()
finally:
outbag.close()
def merge_bag(main_bagfile,
bagfiles,
outfile=None,
topics=None,
compression=rosbag.Compression.NONE,
reindex=True,
start_time=float('-inf'),
end_time=float('inf')):
# get min and max time in bagfile
bagfiles = [main_bagfile] + bagfiles
limits = [get_limits(bagfile) for bagfile in bagfiles]
start_limit = max(start_time, min(limits, key=lambda x: x[0])[0])
end_limit = min(end_time, max(limits, key=lambda x: x[1])[1])
# check output file
if outfile is None:
pattern = main_bagfile + "_merged_%i.bag"
outfile = main_bagfile + "_merged.bag"
index = 0
while (os.path.exists(outfile)):
outfile = pattern % index
index += 1
# output some information
# print "merge bag %s in %s" % (bagfile, main_bagfile)
print("topics filter: ", topics)
print("writing to %s." % outfile)
# merge bagfile
outbag = rosbag.Bag(outfile,
'w',
compression=compression,
chunk_threshold=32 * 1024 * 1024)
bags = []
for i in range(len(bagfiles)):
if i > 0 and (limits[i][0] > end_limit or limits[i][1] < start_limit):
print('Skipping ' + bagfiles[i])
bags.append(None)
continue
bags.append(
rosbag.Bag(bagfiles[i],
'r',
skip_index=False,
chunk_threshold=8 * 1024 * 1024))
bags[-1] = bags[-1].read_messages(topics, Time.from_sec(start_limit),
Time.from_sec(end_limit), None, True,
False)
next = [None] * len(bagfiles)
def find_next(next):
i = 0
next_time = float('inf')
for n in range(len(next)):
if next[n] is None:
continue
test_time = next[n][2].to_sec()
if test_time < next_time:
i = n
next_time = test_time
return i, next_time
def update_next(next, i):
if i == 0:
next[0] = get_next(bags[i])
else:
next[i] = get_next(bags[i], reindex, limits[0], limits[i][0],
topics)
for i in range(len(bagfiles)):
update_next(next, i)
try:
print('Beginning write.')
progress = tqdm(total=end_limit - start_limit,
unit='s',
unit_scale=True)
prev_time = start_limit
next_i, next_time = find_next(
next) # type: (int, Union[float, rostime.Time])
while next_time <= end_limit:
if next_time >= start_limit:
outbag.write(next[next_i][0],
next[next_i][1],
next[next_i][2],
raw=True)
progress.update(max(0, next_time - prev_time))
if prev_time > next_time:
print("Prev time:", str(prev_time), "Next time:",
str(next_time))
prev_time = next_time
update_next(next, next_i)
next_i, next_time = find_next(next)
progress.close()
finally:
outbag.close()
