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