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 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()