def callback_tf(data):
"""
Uses the tf package for the same operation as callback_tf_manual:
Append a newly acquired transform message to the list of stored transforms, and plot a c2x message if one was
recorded already.
TODO currently is just a test for a single object (since it just takes data.transforms[0]. requires generalization
can probably achieve this by simply adding the setTransform line in the tracks object? (since its 1 tf per car)
"""
global transforms, steps, c2x, transformer, time_obj
c2x_selection = c2x[-1]
transforms.append(data)
tf_object = data.transforms[0]
# Force time == 0 or you will run into issues TODO time==0 force here
tf_object.header.stamp = rospy.Time(0)
transformer.setTransform(tf_object) # set the data as the transform
if c2x_selection is None:
return
lock.acquire()
tracks = c2x_selection.tracks
seq = c2x_selection.header.seq
# transformer: tf.TransformerROS
for track in tracks:
time_obj = rospy.Time(0)
x_pos = track.box.center_x
y_pos = track.box.center_y
try:
head = SimulatedVehicle.create_def_header(frame_id=src_id)
# Don't use the current timestamp, use 0 to use the latest available tf data
head.stamp = rospy.Time(0)
# Try to transform the point
# Create a point with z=0 for the source frame (out of c2x tracks x/y) coordinate
point_pos = PointStamped(header=head,
point=Point(x=x_pos, y=y_pos, z=0))
# Now transform the point using the data
tf_point_pos = transformer.transformPoint(target_frame=dest_id,
ps=point_pos)
# Print/Visualize the point
# print("("+str(x_pos)+" | "+str(y_pos)+") --> ("+str(tf_point.point.x)+" | "+str(tf_point.point.y)+")")
next_point = (tf_point_pos.point.x, tf_point_pos.point.y,
track.object_id, "y")
visuals.plot_points_tuple([next_point], append=True)
except tf.ExtrapolationException as e:
# Extrapolation error, print but keep going (possible just because only one frame was received so far)
print(e)
# End of for going over tracks
steps += 1 # c2x was used in another step, increase the counter
lock.release()
def callback_c2x_tf(data):
"""
Stores the last c2x message, but transforms it beforehand
"""
tracks = data.tracks
# transformer: tf.TransformerROS
for track in tracks:
time_obj = rospy.Time(0)
x_pos = track.box.center_x
y_pos = track.box.center_y
try:
# Try to transform the point
# Create a point with z=0 for the source frame (out of c2x tracks x/y) coordinate
point = PointStamped(
header=SimulatedVehicle.create_def_header(frame_id=src_id),
point=Point(x=x_pos, y=y_pos, z=0))
# Don't use the current timestamp, use 0 to use the latest available tf data
point.header.stamp = rospy.Time(0)
# Now transform the point using the data
tf_point = transformer.transformPoint(target_frame=dest_id,
ps=point)
track.box.center_x = tf_point.point.x
track.box.center_y = tf_point.point.y
except tf.ExtrapolationException as e:
# Extrapolation error, print but keep going (possible just because only one frame was received so far)
print(e)
except tf.ConnectivityException as e:
# print("Connectivity Exception during transform")
print(e)
except tf.LookupException as e:
print("Lookup exception")
pass
# Now change the frame_id to dest_id, since the tf was already performed
data.header.frame_id = dest_id
c2x.append(data)
steps = 0
def callback_fascare(data):
"""
Callback function for the fascare car.
In addition to the received data, data from the viewcar2 data buffer is used
"""
global do_velo_cut
if do_velo_cut:
data = velocity_cut(data)
global viewcar2_odom_data, visuals, transformer_fascare, do_ego_plot
try:
viewcar2_odom_data
except NameError:
return # No viewcar2 data was received so far, simply skip this step
if len(data.boxes) == 0:
# no boxes included in current message (possibly due to velocity cutting)
return
transparency = 0.2 # Used across all plots that should be transparant
focus_association = True # If True: No annotation for the rest of the plot, extra plot for associated objects
global do_visual_plot
if do_visual_plot:
visuals.scatter_box_array(data.boxes,
append=False,
color="b",
alpha=transparency,
annotate=(not focus_association))
# Currently the most recently stored entry is used as viewcar data.
# In a more general case, where varying sensor transmission frequency needs to be taken into account, this should
# instead use a function that determines the closest match w.r.t. time stamps.
# For this, the difference in time stamps is minimal (only up to 0.02 seconds difference)
vc_data = viewcar2_odom_data[-1]
# Transform the last set of viewcar2_data into this frames ibeo_front_center
src_id = "odom"
dest_id = "ibeo_front_center"
tracks = vc_data.boxes
head = SimulatedVehicle.create_def_header(frame_id=src_id)
for track in tracks:
x_pos = track.box.center_x
y_pos = track.box.center_y
try:
point = PointStamped(header=head,
point=Point(x=x_pos, y=y_pos, z=0))
point.header.stamp = rospy.Time(0)
tf_point = transformer_fascare.transformPoint(target_frame=dest_id,
ps=point)
track.box.center_x = tf_point.point.x
track.box.center_y = tf_point.point.y
# if transforms of more than x/y pos. are necessary, insert them here
track.box.header.frame_id = dest_id # Changed the frame that the point is in
# For all exceptions: print them, but keep going instead of stopping everything
except tf.ExtrapolationException as e:
# Extrapolation error, print but keep going (possible just because only one frame was received so far)
print(e)
except tf.ConnectivityException as e:
# print("Connectivity Exception during transform")
print(e)
except tf.LookupException as e:
# print("Lookup Exception during transform")
print(e)
vc_data.header.frame_id = dest_id
global do_visual_plot
if do_visual_plot:
visuals.scatter_box_array(vc_data.boxes,
append=True,
color="y",
alpha=transparency,
annotate=(not focus_association))
if do_ego_plot:
# Plot the ego objects with alpha=1
ego_fascare = copy.deepcopy(data.boxes[0])
ego_fascare.box.center_x = 0
ego_fascare.box.center_y = 0
ego_fascare.object_id = -1
ego_viewcar2 = copy.deepcopy(data.boxes[0])
# Acquire the center of the point cloud
try:
if do_ego_plot:
ego_point = PointStamped(header=head,
point=Point(x=0, y=0, z=0))
ego_point.header.stamp = rospy.Time(0)
ego_point.header.frame_id = dest_id # Start in frame dest_id=ibeo_front_center of viewcar2
global transformer_viewcar2
# Transform into src_id = "odom" first (from the viewcar2 ibeo_front_center)
ego_point = transformer_viewcar2.transformPoint(
target_frame=src_id, ps=ego_point)
# and then use the other transformer to move it back into "ibeo_front_center", but for fascare
ego_point = transformer_fascare.transformPoint(
target_frame=dest_id, ps=ego_point)
# For all exceptions: print them, but keep going instead of stopping everything
except tf.ExtrapolationException as e:
# Extrapolation error, print but keep going (possible just because only one frame was received so far)
print(e)
except tf.ConnectivityException as e:
# print("Connectivity Exception during transform")
print(e)
except tf.LookupException as e:
# print("Lookup Exception during transform")
print(e)
ego_viewcar2.box.center_x = ego_point.point.x
ego_viewcar2.box.center_y = ego_point.point.y
ego_viewcar2.object_id = -2
# acquired 2 objects that can now be plotted
global do_visual_plot
if do_visual_plot:
visuals.scatter_box_array([ego_fascare],
append=True,
color="b",
alpha=1,
annotate=False)
visuals.scatter_box_array([ego_viewcar2],
append=True,
color="y",
alpha=1,
annotate=False)
# ---
# now do the association:
# first, add the two tracks to the history
global do_assoc
if do_assoc:
global history
history.add("fascare", data.boxes)
history.add("viewcar2", vc_data.boxes)
assoc = t2tassoc(data.boxes, vc_data.boxes)
global do_visual_plot # check if visual plot is wanted
if do_visual_plot and focus_association: # Extra plot for associated objects
assoc_boxes = non_singletons(assoc)
visuals.scatter_box_array(assoc_boxes,
append=True,
color="r",
alpha=1,
annotate=True)
# ---
# Visualize in MarkerArrays for rviz:
global vis_publisher
marker_color = (0, 0, 1, 0.5) # blue boxes for the fascar boxes
fascar_markers = vis_pub.boxes_to_marker_array(data.boxes, marker_color)
marker_color = (0, 0.4, 0, 0.5) # green boxes for the viewcar boxes
viewcar2_markers = vis_pub.boxes_to_marker_array(vc_data.boxes,
marker_color)
marker_color = (1, 0, 0, 1) # red, non-opaque boxes for the visualization
assoc_boxes = avg_fusion(
assoc) # use this to average between assoc results and display that
# assoc_markers = non_singletons(assoc) # use this two display 2 boxes (only association overlay)
assoc_markers = vis_pub.boxes_to_marker_array(assoc_boxes, marker_color)
global publish_assoc_markers
if publish_assoc_markers:
all_markers = vis_pub.merge_marker_array(
[fascar_markers, viewcar2_markers, assoc_markers])
else:
all_markers = vis_pub.merge_marker_array(
[fascar_markers, viewcar2_markers]) # skip assoc markers
vis_publisher.publish(vis_pub.delete_with_first(all_markers))
def callback_viewcar2(data):
"""
Callback function for the viewcar2 object.
Received information is transformed to UTM and then saved to a buffer, so that the callback_fascare function
can process the given information
"""
# data: TrackedLaserScan
global do_velo_cut
if do_velo_cut:
data = velocity_cut(data)
if len(data.boxes) == 0:
# empty, return
return
global append_ego
if append_ego:
ego_box = copy.deepcopy(data.boxes[0]) # copy any box
ego_box.object_id = -2
ego_box.box.center_x = 0
ego_box.box.center_y = 0
data.boxes.append(ego_box)
global transformer_viewcar2
src_id = "ibeo_front_center" # Transform data from this frame...
dest_id = "odom" # ... to this. # Using odom, but might need to use some other global frame?
tracks = data.boxes
head = SimulatedVehicle.create_def_header(frame_id=src_id)
for track in tracks:
x_pos = track.box.center_x
y_pos = track.box.center_y
try:
point = PointStamped(header=head,
point=Point(x=x_pos, y=y_pos, z=0))
point.header.stamp = rospy.Time(0)
tf_point = transformer_viewcar2.transformPoint(
target_frame=dest_id, ps=point)
track.box.center_x = tf_point.point.x
track.box.center_y = tf_point.point.y
# if transforms of more than x/y pos. are necessary, insert them here
track.box.header.frame_id = dest_id # Changed the frame that the point is in
# For all exceptions: print them, but keep going instead of stopping everything
except tf.ExtrapolationException as e:
# Extrapolation error, print but keep going (possible just because only one frame was received so far)
print(e)
except tf.ConnectivityException as e:
# print("Connectivity Exception during transform")
print(e)
except tf.LookupException as e:
# print("Lookup Exception during transform")
print(e)
data.header.frame_id = dest_id
global viewcar2_odom_data
try:
viewcar2_odom_data
except NameError:
viewcar2_odom_data = []
viewcar2_odom_data.append(data)
def callback_c2x_tf(data):
"""
Stores the last c2x message, but transforms it beforehand.
Transformation is only for position and velocity data, since this is the only data that is actively used by the
program. The only exception is the plotting of bounding boxes. These will therefore not be rotated correctly, but
their size will be correct anyway.
"""
global history, steps, constant_velo, c2x_offset_x, c2x_offset_y
tracks = data.tracks
head = SimulatedVehicle.create_def_header(frame_id=src_id)
# transformer: tf.TransformerROS
for track in tracks:
time_obj = rospy.Time(0)
x_pos = track.box.center_x
y_pos = track.box.center_y
x_vel = track.box.velocity_x
y_vel = track.box.velocity_y
try:
# Try to transform the point
# Create a point with z=0 for the source frame (out of c2x tracks x/y) coordinate
point = PointStamped(header=head,
point=Point(x=x_pos, y=y_pos, z=0))
# Don't use the current timestamp, use 0 to use the latest available tf data
point.header.stamp = rospy.Time(0)
# Now transform the point using the data
tf_point = transformer.transformPoint(target_frame=dest_id,
ps=point)
# Acquire the transformation matrix for this
tf_mat = tf_c.toMatrix(
tf_c.fromTf(
transformer.lookupTransform(target_frame=dest_id,
source_frame=src_id,
time=rospy.Time(0))))
# print(tf_mat)
# tf_vel stores the transformed velocity
tf_vel = np.dot(tf_mat[0:2, 0:2], [x_vel, y_vel])
# Update the track with the transformed data
track.box.center_x = tf_point.point.x + c2x_offset_x
track.box.center_y = tf_point.point.y + c2x_offset_y
track.box.velocity_x = tf_vel[0]
track.box.velocity_y = tf_vel[1]
# DEBUG
# print(str(track.box.center_x)+"\t"+str(track.box.center_y))
# print("steps: "+str(steps)+"\tvelx: "+str(point_vel.point.x)+" vely: "+str(point_vel.point.y))
except tf.ExtrapolationException as e:
# Extrapolation error, print but keep going (possible just because only one frame was received so far)
print(e)
except tf.ConnectivityException as e:
# print("Connectivity Exception during transform")
print(e)
except tf.LookupException as e:
print(e)
pass
# Now change the frame_id to dest_id, since the tf was already performed
data.header.frame_id = dest_id
c2x.append(data)
history.add(
"c2x_0",
data.tracks) # Add the data to the history object under the name c2x_0
# The following block adds several fake measurements to the c2x tracking that are time delayed to the original
# measurement from the current timestep
# They also have changed x/y data based on velocity, so that the track doesn't need to be reused later on with the
# outdated coordinates and instead can work with "updated" coordinates for time steps that are not included in the
# real c2x messages.
add_points = 4 # how many "fake" points should be added between this and the next measuremen
# Currently the number and timing of "fake" points are fixed. They are selected to fit the data sets 1&2, but
# in general it can be better to use previous data to estimate the time difference and number of points
if len(data.tracks
) > 0: # only do the following if the track contained something
for i in range(add_points):
# add 4 more measurements, each based on the data, but with manipulated time and position based on velocity
fake_data = copy.deepcopy(data) # Create a copy of the data
# now change the timestamp of this new data
# c2x data arrives every 0.2 seconds, so to fit an additional 4 measurements
time_shift = rospy.Duration(0,
40000000) # increase by 1/20 of a sec
# this would need to change if it should automatically fit to data of other data sets.
fake_data.header.stamp = fake_data.header.stamp + time_shift
for track in fake_data.tracks:
track.box.header.stamp = track.box.header.stamp + time_shift
track.box.center_x += constant_velo * track.box.velocity_x * (
i + 1)
track.box.center_y += constant_velo * track.box.velocity_y * (
i + 1)
c2x.append(fake_data)
history.add("c2x_0", fake_data.tracks)
steps = 0
def callback_tracking(data):
"""
Plot data from the original laser scans
"""
# Set inc_c2x to False if you want this function to just display data from the lidar tracking
# Set inc_c2x to True if you want this function to display c2x data on top of that, and perform t2ta between the
# two tracks
global visuals, lock, transforms, steps, c2x, inc_c2x
lock.acquire()
# c2x_selection = c2x_list[len(c2x_list)-1] # Select the last c2x data
# Don't take the last received c2x, but instead take the c2x data that is closest to the current data (wrt time)
c2x_selection = closest_match(c2x, data.header.stamp)
visuals.plot_box_array(data.boxes, append=False)
if inc_c2x and c2x_selection is not None:
# Also include c2x data in the plot
# ---
# The following is the c2x data transformation, where the c2x pos/vel etc are transformed into the same coord.
# frame as the tracking data that was received in this time step.
# For this, all tracks in the c2x data are transformed using the transformer object.
# Afterwards, T2TA is attempted on the resulting data. Results of this are printed to console, but currently
# not passed on in any way. A future implementation should probably include a publisher, that publishes the
# resulting data to a new topic, that a t2tf client can subscribe to.
# ---
tracks = c2x_selection.tracks
# transformer: tf.TransformerROS
for track in tracks:
time_obj = rospy.Time(0)
x_pos = track.box.center_x
y_pos = track.box.center_y
# Experimental transform of box l/w, velocity
x_vel = track.box.velocity_x
y_vel = track.box.velocity_y
length = track.box.length
width = track.box.width
try:
# Try to transform the point TODO shouldn't be necessary if done in the c2x_callback
# Create a point_pos with z=0 for the source frame (out of c2x tracks x/y) coordinate
src_f_id = c2x_selection.header.frame_id # Don't use the global var, instead use the set value
head = SimulatedVehicle.create_def_header(frame_id=src_f_id)
# Don't use the current timestamp, use 0 to use the latest available tf data
head.stamp = rospy.Time(0)
# The above line allows for tf outside this function and prevents performing multiple tf steps
point_pos = PointStamped(header=head,
point=Point(x=x_pos, y=y_pos, z=0))
point_vel = PointStamped(header=head,
point=Point(x=x_vel, y=y_vel, z=0))
# check x/y for the length/width point? might depend on angle and not be this easy
point_lw = PointStamped(header=head,
point=Point(x=width, y=length, z=0))
# Now transform the point using the data
tf_point_pos = transformer.transformPoint(target_frame=dest_id,
ps=point_pos)
tf_point_vel = transformer.transformPoint(target_frame=dest_id,
ps=point_vel)
tf_point_lw = transformer.transformPoint(target_frame=dest_id,
ps=point_lw)
# Store the results back in the track for further use
track.box.center_x = tf_point_pos.point.x
track.box.center_y = tf_point_pos.point.y
track.box.velocity_x = tf_point_vel.point.x
track.box.velocity_y = tf_point_vel.point.y
# For the following 2, check x/y and if tf them like this makes sense at all
track.box.length = tf_point_lw.point.y
track.box.width = tf_point_lw.point.x
# Plotting of the newly transformed point
next_point = (tf_point_pos.point.x, tf_point_pos.point.y,
track.object_id, "y")
visuals.plot_points_tuple([next_point], append=True)
except tf.ExtrapolationException as e:
# Extrapolation error, print but keep going (possible just because only one frame was received so far)
print(e)
except tf.ConnectivityException as e:
# print("Connectivity Exception during transform")
print(e)
# End of for going over tracks
steps += 1 # c2x was used in another step, increase the counter
# Attempt to perform T2TA here
sensor_tracks = []
sensor_tracks.append(
data.boxes
) # one sensor track is the data for this callback (=lidar tracking data)
sensor_tracks.append(tracks) # The other one is the c2x tracking data
try:
assoc = t2ta.t2ta_collected(sensor_tracks,
threshold=t2ta_thresh,
distance=sim_fct)
# analyse the results
ids = [
] # this list will hold lists where each entry is an object id in a cluster
for a in assoc: # get a list of all associations
temp = [] # stores ids for one association
for box in a: # all tracking boxes in the current association
temp.append(box.object_id)
ids.append(
temp) # ids is a list made up of lists of object ids
if len(a) > 1:
# If a non-singleton cluster was found, print all ids that belong to it
print("<< Non-singleton Cluster: " + str(temp) + " >>")
"""
# The following is a block that just looks for id==100 in the clusters (which is the c2x id in maven-1/2.bag
for cl_ids in ids:
if 100 in cl_ids:
if len(cl_ids) > 1:
print("<< Non-singleton Cluster containing 100: "+str(cl_ids)+" >>")
else:
print("<< Singleton Cluster containing 100 found >>")
"""
except ValueError:
print("ValueError during t2ta")
# --- debug:
# print(str(data.header.stamp.secs)+" vs. "+str(c2x_selection.header.stamp.secs)+
# " --diff: "+str(np.abs(data.header.stamp.secs-c2x_selection.header.stamp.secs))+" !!!")
# Finished all plotting, release the lock
lock.release()