I don't know what parameter I should enter here.
Trying to use this function to get (x,y,z) out of a bag file. However, I don't know what I should enter as "joint_name" input. Function won't work unless i supply this input.
I try to use: get_joint_data(somefile.bag, ?). I don't know what to put there. base_link, base_footprint, start_of_device don't work.
def get_joint_data(bagFile, joint_name, convert_to_sec=True):
"""
Function that filters bag files to obtain data from a joint that is
published to the /tf topic.
Only x-, y- and z-coordinates are returned
if convert_to_sec is set to True (default), then the first timestamp
will be taken as zero seconds, and the following timesteps that are
retured will be relative to this timestamp.
"""
# INITIALIZATION
x = np.array([])
y = np.array([])
z = np.array([])
all_t = np.array([])
bag = rosbag.Bag(bagFile) # Initialize rosbag object
first = True # True on first iteration to take first timestamp
# Add message values to collections
for topic, msg, t in bag.read_messages(topics=['/tf']):
joint = msg.transforms[0].child_frame_id
translation = msg.transforms[0].transform.translation
if joint == joint_name:
# Get timestamp in seconds
t = msg.transforms[0].header.stamp
t_sec = t.to_sec()
if (first and convert_to_sec):
t_0 = t_sec
first = False
all_t = np.append(all_t, t_sec-t_0)
# Get x, y and z coordinates
pose = [translation.x , translation.y, translation.z]
x = np.append(x, pose[0])
y = np.append(y, pose[1])
z = np.append(z, pose[2])
pose = [x,y,z]
return pose, all_t
what do you mean with
As long as
base_link
is in the bag, and does not contain a leading/
, it seems to produce what you want (I'm assuming the messed up indentation is only here and not in the original file...) Maybe have a look at ..the topic manually and check what you have in the
child_frame_id
field of some of the messages.Also, you should probably rename one of the two
pose
variables to make sure you don't run into any issues there...