def __init__(self):
self.buffer = tf2_py.BufferCore(
rospy.Duration.from_sec(SETTINGS.tf_cache_max_time))
self.topics = []
self.bags = []
y.transform.translation.x = x.transform.translation.x
y.transform.translation.y = x.transform.translation.y
y.transform.translation.z = x.transform.translation.z
y.transform.rotation.x = x.transform.rotation.x
y.transform.rotation.y = x.transform.rotation.y
y.transform.rotation.z = x.transform.rotation.z
y.transform.rotation.w = x.transform.rotation.w
return y
#path = '/home/jsteeen/Documents/rosbags/cool_2019-04-21-02-27-42_0.bag'
#path = '/home/jsteeen/Documents/rosbags/cool_2019-04-21-02-29-36_0.bag'
#path = '/home/jsteeen/Documents/rosbags/grasp_2019-04-21-00-31-48_0.bag'
path = '/home/jsteeen/Documents/rosbags/grasp_2019-04-21-00-33-18_0.bag'
cv_bridge = cv_bridge.CvBridge()
tf_buffer = tf2.BufferCore()
psm1_msgs = []
cam_info = [None, None]
img_msg = [None, None]
with rosbag.Bag(path) as bag:
for topic, msg, stamp in bag.read_messages(topics=['/tf']):
for tf in msg.transforms:
tf_buffer.set_transform(fix_tf_msg(tf), 'default_authority')
# camera info msgs do not change during the recording, so we save only the first one
cam_info[0] = next(
bag.read_messages(topics=['/basler_stereo/left/camera_info']))[1]
cam_info[1] = next(
bag.read_messages(topics=['/basler_stereo/right/camera_info']))[1]
def initializeReplay(global_frame_id, robot_list):
"""!
@brief Load the TF tree from the provided bag file.
This initialization takes a bag file, as provided by the user on the parameter
server and reads the entire TF tree into a buffer for use in replaying.
@param global_frame_id The global refernce frame used by Gazebo, often 'map'
@param robot_list The list of robots in the simulation. Used to determine the start
and end times that have valid TF transforms for all robots.
@return A tuple containing:
1. The tf buffer
2. A rospy.Time of the start time of the bag file
3. A rospy.Time of the end time of the bag file
4. A rospy.Duration of the period used during processing
@throw rospy.ROSInitException Thrown if the bag file can't be opened or found.
"""
rospy.loginfo('Loading bag file...')
# Determine where the bag file is located via parameter server.
bag_file = ParameterLookup.lookup(parameter='~bag_file')
if 'example_replay.bag' in bag_file:
rospy.logwarn(
msg='You might be using the example bag file. Did you mean to provide your own using the bag_file parameter?')
# Open the bag file. This might throw an error, so catch it and send a warning
# to the user.
try:
bag = rosbag.Bag(f=bag_file)
except Exception as ex:
error_string = 'Unable to open {bag}: {error}'.format(
bag=bag_file, error=ex)
rospy.logfatal(msg=error_string)
raise rospy.ROSInitException(error_string)
# If this point is reached, the bag file is loaded and ready.
# Now look up the simulated rate with the same method as the bag parameter.
rate = ParameterLookup.lookupWithDefault(
parameter='~simulated_rate', default=30.0)
period = rospy.Duration(secs=(1.0 / rate))
# Now, parse the bag file into a tf buffer
start_time = rospy.Time(bag.get_start_time())
end_time = rospy.Time(bag.get_end_time())
tf_buffer = tf2_py.BufferCore((end_time - start_time))
for topic, msg, _ in bag.read_messages(topics=['/tf', '/tf_static']):
for msg_tf in msg.transforms:
if topic == '/tf_static':
tf_buffer.set_transform_static(msg_tf, 'default_authority')
else:
tf_buffer.set_transform(msg_tf, 'default_authority')
# Now that everything is loaded, we don't need the bag file.
bag.close()
# Find the first start_time that has a valid transform for every single robot.
good_start_time = False
# This only finishes if every robot has a viable transform at this time step
while not good_start_time and start_time < end_time:
good_start_time = True
for robot in robot_list:
(is_good, _) = tf_buffer.can_transform_core(
global_frame_id, robot.getFullFrame(), start_time)
good_start_time = good_start_time and is_good
if not good_start_time:
start_time += period
# If the start time is now greater than the end time, then there were no
# time frames that had valid transforms for every robot. This is a problem.
if not start_time < end_time:
error_string = 'Unable to find valid transforms for all robots in bag file.'
rospy.logfatal(error_string)
raise rospy.exceptions.ROSInitException(error_string)
# Now do the same thing for the end time, but decrement instead. The end time
# is only a gate on when the whole simulation should end, so it does not have to
# be a whole number of increments from start time. Therefore, just start at the
# end of the bag file.
good_end_time = False
# You technically need to watch for the end > start since there may be a specific
# setup that only has frame interpolation in a very narrow time range and if period
# is larger than that range, end_time is liable to never exist within that range.
while not good_end_time and end_time > start_time:
good_end_time = True
for robot in robot_list:
(is_good, _) = tf_buffer.can_transform_core(
global_frame_id, robot.getFullFrame(), end_time)
good_end_time = good_end_time and is_good
if not good_end_time:
end_time -= period
if not end_time > start_time:
error_string = 'Unable to find valid transforms for all robots in bag file.'
rospy.logfatal(error_string)
raise rospy.exceptions.ROSInitException(error_string)
return (tf_buffer, start_time, end_time, period)
def __init__(self, cache_time):
'''
Constructor
'''
self.__logger = get_logger_by_class(self.__class__)
self.__tf_core = tf2_py.BufferCore(rospy.Duration(cache_time))