def find_ground_coordinates(
gpg: GroundProjectionGeometry, sl: SegmentList, skip_not_on_ground: bool = True
) -> SegmentList:
"""
Creates a new segment list with the ground coordinates set.
"""
cutoff = 0.01
sl2 = SegmentList()
sl2.header = sl.header
# Get ground truth of segmentList
for s1 in sl.segments:
g0 = gpg.vector2ground(s1.pixels_normalized[0])
g1 = gpg.vector2ground(s1.pixels_normalized[1])
if skip_not_on_ground:
if g0.x < cutoff or g1.x < cutoff:
continue
points = [g0, g1]
pixels_normalized = [s1.pixels_normalized[0], s1.pixels_normalized[1]]
color = s1.color
s2 = Segment(points=points, pixels_normalized=pixels_normalized, color=color)
# TODO what about normal and points
sl2.segments.append(s2)
return sl2
def processSegmentList(self, segmentList):
filteredSegments = SegmentList()
filteredSegments.header = segmentList.header
for segment in segmentList.segments:
# Filter off segments behind us
if segment.points[0].x < 0 or segment.points[1].x < 0:
continue
# Discard RED lines, for now
if segment.color != segment.WHITE and segment.color != segment.YELLOW:
print('#########################CODE RED!!')
continue
# Apply a few more fancy filters to the segment
d_i, phi_i, l_i, state = self.fancyFilters(segment)
# Ignore if the line segment is UNKNOWN (i.e., = 0)
if state == 0:
continue
if d_i > self.d_max or d_i < self.d_min or phi_i < self.phi_min or phi_i > self.phi_max:
continue
# filteredSegments.append(segment)
# print(filteredSegments)
filteredSegments.segments.append(segment)
return filteredSegments
def debugOutput(self, segment_list_msg, d_max, phi_max):
"""Creates and publishes debug messages
Args:
segment_list_msg (:obj:`SegmentList`): message containing list of filtered segments
d_max (:obj:`float`): best estimate for d
phi_max (:obj:``float): best estimate for phi
"""
if self._debug:
# Get the segments that agree with the best estimate and publish them
inlier_segments = self.filter.get_inlier_segments(
segment_list_msg.segments, d_max, phi_max)
inlier_segments_msg = SegmentList()
inlier_segments_msg.header = segment_list_msg.header
inlier_segments_msg.segments = inlier_segments
self.pub_seglist_filtered.publish(inlier_segments_msg)
# Create belief image and publish it
ml = self.filter.generate_measurement_likelihood(
segment_list_msg.segments)
if ml is not None:
ml_img = self.bridge.cv2_to_imgmsg(
np.array(255 * ml).astype("uint8"), "mono8")
ml_img.header.stamp = segment_list_msg.header.stamp
self.pub_ml_img.publish(ml_img)
def lineseglist_cb(self, seglist_msg):
"""
Projects a list of line segments on the ground reference frame point by point by
calling :py:meth:`pixel_msg_to_ground_msg`. Then publishes the projected list of segments.
Args:
seglist_msg (:obj:`duckietown_msgs.msg.SegmentList`): Line segments in pixel space from
unrectified images
"""
if self.camera_info_received:
seglist_out = SegmentList()
seglist_out.header = seglist_msg.header
for received_segment in seglist_msg.segments:
new_segment = Segment()
new_segment.points[0] = self.pixel_msg_to_ground_msg(
received_segment.pixels_normalized[0])
new_segment.points[1] = self.pixel_msg_to_ground_msg(
received_segment.pixels_normalized[1])
new_segment.color = received_segment.color
# TODO what about normal and points
seglist_out.segments.append(new_segment)
self.pub_lineseglist.publish(seglist_out)
if not self.first_processing_done:
self.log("First projected segments published.")
self.first_processing_done = True
if self.pub_debug_img.get_num_connections() > 0:
debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(
self.debug_image(seglist_out))
debug_image_msg.header = seglist_out.header
self.pub_debug_img.publish(debug_image_msg)
else:
self.log("Waiting for a CameraInfo message", "warn")
def debugOutput(self, segment_list_msg, d_max, phi_max, timestamp_before_processing):
"""Creates and publishes debug messages
Args:
segment_list_msg (:obj:`SegmentList`): message containing list of filtered segments
d_max (:obj:`float`): best estimate for d
phi_max (:obj:``float): best estimate for phi
timestamp_before_processing (:obj:`float`): timestamp dating from before the processing
"""
if self._debug:
# Latency of Estimation including curvature estimation
estimation_latency_stamp = rospy.Time.now() - timestamp_before_processing
estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9
self.latencyArray.append(estimation_latency)
if len(self.latencyArray) >= 20:
self.latencyArray.pop(0)
# print "Latency of segment list: ", segment_latency
self.loginfo(f"Mean latency of Estimation:................. {np.mean(self.latencyArray)}")
# Get the segments that agree with the best estimate and publish them
inlier_segments = self.filter.get_inlier_segments(segment_list_msg.segments, d_max, phi_max)
inlier_segments_msg = SegmentList()
inlier_segments_msg.header = segment_list_msg.header
inlier_segments_msg.segments = inlier_segments
self.pub_seglist_filtered.publish(inlier_segments_msg)
# Create belief image and publish it
belief_img = self.bridge.cv2_to_imgmsg(
np.array(255 * self.filter.belief).astype("uint8"), "mono8"
)
belief_img.header.stamp = segment_list_msg.header.stamp
self.pub_belief_img.publish(belief_img)
def lineseglist_cb(self, seglist_msg):
seglist_out = SegmentList()
seglist_out.header = seglist_msg.header
for received_segment in seglist_msg.segments:
new_segment = Segment()
new_segment.points[0] = self.gpg.vector2ground(received_segment.pixels_normalized[0])
new_segment.points[1] = self.gpg.vector2ground(received_segment.pixels_normalized[1])
new_segment.color = received_segment.color
# TODO what about normal and points
seglist_out.segments.append(new_segment)
self.pub_lineseglist_.publish(seglist_out)
def storeSegments(self,in_segs):
rec_seg_list = SegmentList()
rec_seg_list.header = in_segs.header
for in_seg in in_segs.segments:
new_seg = Segment()
new_seg.points[0].x = in_seg.points[0].x
new_seg.points[0].y = in_seg.points[0].y
new_seg.points[0].z = 0.0
new_seg.points[1].x = in_seg.points[1].x
new_seg.points[1].y = in_seg.points[1].y
new_seg.points[1].z = 0.0
new_seg.color = in_seg.color
rec_seg_list.segments.append(new_seg)
self.seg_list.append(rec_seg_list)
self.pubMap()
def lineseglist_cb(self, seglist_msg):
message_time = seglist_msg.header.stamp.sec + seglist_msg.header.stamp.nanosec*1e-9
current_time = time.time()
delay = current_time - message_time
#fname = "/dev/shm/segment" + str(message_time)
#seglist_msg.segments = pickle.load(open(fname, "rb"))
#os.unlink(fname)
#print("message time: " + str(message_time))
#print("current time: " + str(current_time))
#print("delay: " + str(delay), "# segments: ", len(seglist_msg.segments))
seglist_out = SegmentList()
seglist_out.header = seglist_msg.header
for received_segment in seglist_msg.segments:
new_segment = Segment()
new_segment.points[0] = self.gp.vector2ground(received_segment.pixels_normalized[0])
new_segment.points[1] = self.gp.vector2ground(received_segment.pixels_normalized[1])
new_segment.color = received_segment.color
# TODO what about normal and points
seglist_out.segments.append(new_segment)
self.pub_lineseglist_.publish(seglist_out)
def processSegments(self, segment_list_msg):
if not self.active:
return
# Get actual timestamp for latency measurement
timestamp_now = rospy.Time.now()
# TODO-TAL double call to param server ... --> see TODO in the readme, not only is it never updated, but it is alwas 0
# Step 0: get values from server
if (rospy.get_param('~curvature_res') is not self.curvature_res):
self.curvature_res = rospy.get_param('~curvature_res')
self.filter.updateRangeArray(self.curvature_res)
# Step 1: predict
current_time = rospy.get_time()
dt = current_time - self.t_last_update
v = self.velocity.v
w = self.velocity.omega
self.filter.predict(dt=dt, v=v, w=w)
self.t_last_update = current_time
# Step 2: update
self.filter.update(segment_list_msg.segments)
parking_detected = BoolStamped()
parking_detected.header.stamp = segment_list_msg.header.stamp
parking_detected.data = self.filter.parking_detected
self.pub_parking_detection.publish(parking_detected)
parking_on = BoolStamped()
parking_on.header.stamp = segment_list_msg.header.stamp
parking_on.data = True
self.pub_parking_on.publish(parking_on)
if self.active_mode:
# Step 3: build messages and publish things
[d_max, phi_max] = self.filter.getEstimate()
# print "d_max = ", d_max
# print "phi_max = ", phi_max
inlier_segments = self.filter.get_inlier_segments(
segment_list_msg.segments, d_max, phi_max)
inlier_segments_msg = SegmentList()
inlier_segments_msg.header = segment_list_msg.header
inlier_segments_msg.segments = inlier_segments
self.pub_seglist_filtered.publish(inlier_segments_msg)
#max_val = self.filter.getMax()
#in_lane = max_val > self.filter.min_max
# build lane pose message to send
lanePose = LanePose()
lanePose.header.stamp = segment_list_msg.header.stamp
lanePose.d = d_max[0]
lanePose.phi = phi_max[0]
#lanePose.in_lane = in_lane
# XXX: is it always NORMAL?
lanePose.status = lanePose.NORMAL
if self.curvature_res > 0:
lanePose.curvature = self.filter.getCurvature(
d_max[1:], phi_max[1:])
self.pub_lane_pose.publish(lanePose)
# TODO-TAL add a debug param to not publish the image !!
# TODO-TAL also, the CvBridge is re instantiated every time...
# publish the belief image
bridge = CvBridge()
belief_img = bridge.cv2_to_imgmsg(
np.array(255 * self.filter.beliefArray[0]).astype("uint8"),
"mono8")
belief_img.header.stamp = segment_list_msg.header.stamp
self.pub_belief_img.publish(belief_img)
# Latency of Estimation including curvature estimation
estimation_latency_stamp = rospy.Time.now() - timestamp_now
estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9
self.latencyArray.append(estimation_latency)
if (len(self.latencyArray) >= 20):
self.latencyArray.pop(0)
# print "Latency of segment list: ", segment_latency
# print("Mean latency of Estimation:................. %s" % np.mean(self.latencyArray))
# also publishing a separate Bool for the FSM
in_lane_msg = BoolStamped()
in_lane_msg.header.stamp = segment_list_msg.header.stamp
in_lane_msg.data = self.filter.inLane
if (self.filter.inLane):
self.pub_in_lane.publish(in_lane_msg)
def processSegments(self, segment_list_msg):
# Get actual timestamp for latency measurement
timestamp_now = rospy.Time.now()
if not self.active:
return
# TODO-TAL double call to param server ... --> see TODO in the readme, not only is it never updated, but it is alwas 0
# Step 0: get values from server
if (rospy.get_param('~curvature_res') is not self.curvature_res):
self.curvature_res = rospy.get_param('~curvature_res')
self.filter.updateRangeArray(self.curvature_res)
## #######################################################
# ########################################################
# Parameters
# ########################################################
lane_width = 0.23 # [m]
valid_rad = 0.5 # [m]
lookahead = 0.1 # [m]
n_test_points = 50
max_buffer_size = 25
# ########################################################
# Process inputs
# ########################################################
# Generate an xy list from the data
white_xy = []
yellow_xy = []
for segment in segment_list_msg.segments:
# White points
if segment.color == 0:
white_xy.append([segment.points[0].x, segment.points[0].y])
# Yellow points
elif segment.color == 1:
yellow_xy.append([segment.points[0].x, segment.points[0].y])
else:
print("Unrecognized segment color")
# Turn into numpy arrays
white_xy = np.array(white_xy)
yellow_xy = np.array(yellow_xy)
# ########################################################
# Process buffer
# ########################################################
# Integrate the odometry
current_time = rospy.get_time()
dt = current_time - self.t_last_update
self.t_last_update = current_time
delta_x = self.velocity.v * dt
delta_theta = self.velocity.omega * dt
# Form the displacement
delta_r = delta_x * np.array(
[-np.sin(delta_theta), np.cos(delta_theta)])
# If buffers contains data, propogate it BACKWARD
if len(self.buffer_white) > 0:
self.buffer_white = self.buffer_white - delta_r
if len(self.buffer_yellow) > 0:
self.buffer_yellow = self.buffer_yellow - delta_r
# If new observations were received, then append them to the top of the list
if len(white_xy) > 0:
if len(self.buffer_white) > 0:
self.buffer_white = np.vstack((white_xy, self.buffer_white))
else:
self.buffer_white = white_xy
if len(yellow_xy) > 0:
if len(self.buffer_yellow) > 0:
self.buffer_yellow = np.vstack((yellow_xy, self.buffer_yellow))
else:
self.buffer_yellow = yellow_xy
# ########################################################
# Trim training points to within a valid radius
# ########################################################
valid_white_xy = []
valid_yellow_xy = []
# Select points within rad
if len(self.buffer_white) > 0:
tf_valid_white = self.computeRad(self.buffer_white) <= valid_rad
valid_white_xy = self.buffer_white[tf_valid_white, :]
if len(self.buffer_yellow) > 0:
tf_valid_yellow = self.computeRad(self.buffer_yellow) <= valid_rad
valid_yellow_xy = self.buffer_yellow[tf_valid_yellow, :]
# ########################################################
# Fit GPs
# ########################################################
# Set up a linspace for prediction
if len(valid_white_xy) > 0:
x_pred_white = np.linspace(0, np.minimum(valid_rad, np.amax(valid_white_xy[:, 0])), \
num=n_test_points+1).reshape(-1, 1)
else:
x_pred_white = np.linspace(0, valid_rad,
num=n_test_points + 1).reshape(-1, 1)
if len(valid_yellow_xy) > 0:
x_pred_yellow = np.linspace(0, np.minimum(valid_rad, np.amax(valid_yellow_xy[:, 0])), \
num=n_test_points+1).reshape(-1, 1)
else:
x_pred_yellow = np.linspace(0, valid_rad,
num=n_test_points + 1).reshape(-1, 1)
# Start with the prior
y_pred_white = -lane_width / 2.0 * np.ones((len(x_pred_white), 1))
y_pred_yellow = lane_width / 2.0 * np.ones((len(x_pred_yellow), 1))
t_start = time.time()
# White GP. Note that we're fitting y = f(x)
gpWhite = []
if len(valid_white_xy) > 2:
x_train_white = valid_white_xy[:, 0].reshape(-1, 1)
y_train_white = valid_white_xy[:, 1]
whiteKernel = C(0.01, (-lane_width, 0)) * RBF(5, (0.3, 0.4))
# whiteKernel = C(1.0, (1e-3, 1e3)) * RBF(5, (1e-2, 1e2))
gpWhite = GaussianProcessRegressor(kernel=whiteKernel,
optimizer=None)
# x = f(y)
gpWhite.fit(x_train_white, y_train_white)
# Predict
y_pred_white = gpWhite.predict(x_pred_white)
# Yellow GP
gpYellow = []
if len(valid_yellow_xy) > 2:
x_train_yellow = valid_yellow_xy[:, 0].reshape(-1, 1)
y_train_yellow = valid_yellow_xy[:, 1]
# yellowKernel = C(lane_width / 2.0, (0, lane_width)) * RBF(5, (0.3,
# 0.4))
yellowKernel = C(0.01, (0, lane_width)) * RBF(5, (0.3, 0.4))
gpYellow = GaussianProcessRegressor(kernel=yellowKernel,
optimizer=None)
gpYellow.fit(x_train_yellow, y_train_yellow)
# Predict
y_pred_yellow = gpYellow.predict(x_pred_yellow)
t_end = time.time()
# print("MODEL BUILDING TIME: ", t_end - t_start)
# Make xy point arrays
xy_gp_white = np.hstack((x_pred_white, y_pred_white.reshape(-1, 1)))
xy_gp_yellow = np.hstack((x_pred_yellow, y_pred_yellow.reshape(-1, 1)))
# Trim predicted values to valid radius
# Logical conditions
tf_valid_white = self.computeRad(xy_gp_white) <= valid_rad
tf_valid_yellow = self.computeRad(xy_gp_yellow) <= valid_rad
# Select points within rad
valid_xy_gp_white = xy_gp_white[tf_valid_white, :]
valid_xy_gp_yellow = xy_gp_yellow[tf_valid_yellow, :]
# ########################################################
# Display
# ########################################################
self.visualizeCurves(valid_xy_gp_white, valid_xy_gp_yellow)
# ########################################################
# Compute d and \phi
# ########################################################
# Evaluate each GP at the lookahead distance and average to get d
# Start with prior
y_white = -lane_width / 2
y_yellow = lane_width / 2
if gpWhite:
y_white = gpWhite.predict(np.array([lookahead]).reshape(-1, 1))
if gpYellow:
y_yellow = gpYellow.predict(np.array([lookahead]).reshape(-1, 1))
# Take the average, and negate
disp = np.mean((y_white, y_yellow))
d = -disp
# Compute phi from the lookahead distance and d
L = np.sqrt(d**2 + lookahead**2)
phi = disp / L
print("D is: ", d)
print("phi is: ", phi)
# ########################################################
# Publish the lanePose message
# ########################################################
lanePose = LanePose()
lanePose.header.stamp = segment_list_msg.header.stamp
lanePose.d = d
lanePose.phi = phi
lanePose.in_lane = True
lanePose.status = lanePose.NORMAL
lanePose.curvature = 0
self.pub_lane_pose.publish(lanePose)
# ########################################################
# Save valid training points to buffer
# ########################################################
# Cap the buffer at max_buffer_size
white_max = np.minimum(len(self.buffer_white), max_buffer_size)
yellow_max = np.minimum(len(self.buffer_yellow), max_buffer_size)
self.buffer_white = self.buffer_white[0:white_max - 1]
self.buffer_yellow = self.buffer_yellow[0:yellow_max - 1]
# print ("SIZE of white buffer: ", len(self.buffer_white))
# print ("YELLOW of yellow buffer: ", len(self.buffer_yellow))
## #######################################################
# Step 1: predict
current_time = rospy.get_time()
dt = current_time - self.t_last_update
v = self.velocity.v
w = self.velocity.omega
self.filter.predict(dt=dt, v=v, w=w)
# self.t_last_update = current_time
# Step 2: update
self.filter.update(segment_list_msg.segments)
# Step 3: build messages and publish things
[d_max, phi_max] = self.filter.getEstimate()
# print "d_max = ", d_max
# print "phi_max = ", phi_max
inlier_segments = self.filter.get_inlier_segments(
segment_list_msg.segments, d_max, phi_max)
inlier_segments_msg = SegmentList()
inlier_segments_msg.header = segment_list_msg.header
inlier_segments_msg.segments = inlier_segments
self.pub_seglist_filtered.publish(inlier_segments_msg)
max_val = self.filter.getMax()
in_lane = max_val > self.filter.min_max
# build lane pose message to send
# lanePose = LanePose()
# lanePose.header.stamp = segment_list_msg.header.stamp
# lanePose.d = d_max[0]
# lanePose.phi = phi_max[0]
# lanePose.in_lane = in_lane
# # XXX: is it always NORMAL?
# lanePose.status = lanePose.NORMAL
# if self.curvature_res > 0:
# lanePose.curvature = self.filter.getCurvature(d_max[1:], phi_max[1:])
# self.pub_lane_pose.publish(lanePose)
# TODO-TAL add a debug param to not publish the image !!
# TODO-TAL also, the CvBridge is re instantiated every time...
# publish the belief image
bridge = CvBridge()
belief_img = bridge.cv2_to_imgmsg(
np.array(255 * self.filter.beliefArray[0]).astype("uint8"),
"mono8")
belief_img.header.stamp = segment_list_msg.header.stamp
self.pub_belief_img.publish(belief_img)
# Latency of Estimation including curvature estimation
estimation_latency_stamp = rospy.Time.now() - timestamp_now
estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9
self.latencyArray.append(estimation_latency)
if (len(self.latencyArray) >= 20):
self.latencyArray.pop(0)
# print "Latency of segment list: ", segment_latency
# print("Mean latency of Estimation:................. %s" % np.mean(self.latencyArray))
# also publishing a separate Bool for the FSM
in_lane_msg = BoolStamped()
in_lane_msg.header.stamp = segment_list_msg.header.stamp
in_lane_msg.data = True #TODO-TAL change with in_lane. Is this messqge useful since it is alwas true ?
self.pub_in_lane.publish(in_lane_msg)
def processSegments(self, segment_list_msg):
# Get actual timestamp for latency measurement
timestamp_now = rospy.Time.now()
if not self.active:
return
# TODO-TAL double call to param server ... --> see TODO in the readme, not only is it never updated, but it is alwas 0
# Step 0: get values from server
if (rospy.get_param('~curvature_res') is not self.curvature_res):
self.curvature_res = rospy.get_param('~curvature_res')
self.filter.updateRangeArray(self.curvature_res)
# Step 1: predict
current_time = rospy.get_time()
dt = current_time - self.t_last_update
v = self.velocity.v
w = self.velocity.omega
# ==== HERE THE UPPER BOUNDS ========
ub_d = dt * self.omega_max * self.gain * 1000
ub_phi = dt * self.v_ref * self.gain * 1000
self.filter.predict(dt=dt, v=v, w=w)
self.t_last_update = current_time
# Step 2: update
self.filter.update(segment_list_msg.segments, self.lane_offset)
# Step 3: build messages and publish things
[d_max, phi_max] = self.filter.getEstimate()
inlier_segments = self.filter.get_inlier_segments(
segment_list_msg.segments, d_max, phi_max)
inlier_segments_msg = SegmentList()
inlier_segments_msg.header = segment_list_msg.header
inlier_segments_msg.segments = inlier_segments
self.pub_seglist_filtered.publish(inlier_segments_msg)
max_val = self.filter.getMax()
in_lane = max_val > self.filter.min_max
# build lane pose message to send
lanePose = LanePose()
lanePose.header.stamp = segment_list_msg.header.stamp
lanePose.d = d_max[0]
if not self.last_d:
self.last_d = lanePose.d
if lanePose.d - self.last_d > ub_d:
lanePose.d = self.last_d + ub_d
rospy.loginfo("d changed too much")
if lanePose.d - self.last_d < -ub_d:
lanePose.d = self.last_d - ub_d
rospy.loginfo("d changed too much")
lanePose.phi = phi_max[0]
if not self.last_phi:
self.last_phi = lanePose.phi
if lanePose.phi - self.last_phi > ub_phi:
lanePose.phi = self.last_phi + ub_phi
rospy.loginfo("phi changed too much")
if lanePose.phi - self.last_phi < -ub_phi:
lanePose.phi = self.last_phi - ub_phi
rospy.loginfo("phi changed too much")
lanePose.in_lane = in_lane
# XXX: is it always NORMAL?
lanePose.status = lanePose.NORMAL
if self.curvature_res > 0:
lanePose.curvature = self.filter.getCurvature(
d_max[1:], phi_max[1:])
self.pub_lane_pose.publish(lanePose)
# Save for next iteration <=========
self.last_d = lanePose.d
self.last_phi = lanePose.phi
# publish the belief image
bridge = CvBridge()
belief_img = bridge.cv2_to_imgmsg(
np.array(255 * self.filter.beliefArray[0]).astype("uint8"),
"mono8")
belief_img.header.stamp = segment_list_msg.header.stamp
self.pub_belief_img.publish(belief_img)
# Latency of Estimation including curvature estimation
estimation_latency_stamp = rospy.Time.now() - timestamp_now
estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs / 1e9
self.latencyArray.append(estimation_latency)
if (len(self.latencyArray) >= 20):
self.latencyArray.pop(0)
# also publishing a separate Bool for the FSM
in_lane_msg = BoolStamped()
in_lane_msg.header.stamp = segment_list_msg.header.stamp
in_lane_msg.data = True #TODO-TAL change with in_lane. Is this messqge useful since it is alwas true ?
self.pub_in_lane.publish(in_lane_msg)
def processSegments(self,segment_list_msg):
# Get actual timestamp for latency measurement
timestamp_now = rospy.Time.now()
if not self.active:
return
# Step 1: predict
current_time = rospy.get_time()
dt = current_time - self.t_last_update
v = self.velocity.v
w = self.velocity.omega
self.filter.predict(dt=dt, v=v, w=w)
self.t_last_update = current_time
# Step 2: update
self.filter.update(segment_list_msg.segments)
# Step 3: build messages and publish things
[d_max, phi_max] = self.filter.getEstimate()
## Inlier segments
inlier_segments = self.filter.get_inlier_segments(segment_list_msg.segments, d_max, phi_max)
inlier_segments_msg = SegmentList()
inlier_segments_msg.header = segment_list_msg.header
inlier_segments_msg.segments = inlier_segments
self.pub_seglist_filtered.publish(inlier_segments_msg)
## Lane pose
in_lane = self.filter.isInLane()
lanePose = LanePose()
lanePose.header.stamp = segment_list_msg.header.stamp
lanePose.d = d_max
lanePose.phi = phi_max
lanePose.in_lane = in_lane
lanePose.status = lanePose.NORMAL
self.pub_lane_pose.publish(lanePose)
## Belief image
bridge = CvBridge()
if self.mode == 'histogram':
belief_img = bridge.cv2_to_imgmsg(np.array(255 * self.filter.beliefArray).astype("uint8"), "mono8")
elif self.mode == 'particle':
belief_img = bridge.cv2_to_imgmsg(np.array(255 * self.filter.getBeliefArray()).astype("uint8"), "mono8")
belief_img.header.stamp = segment_list_msg.header.stamp
self.pub_belief_img.publish(belief_img)
## Latency of Estimation
estimation_latency_stamp = rospy.Time.now() - timestamp_now
estimation_latency = estimation_latency_stamp.secs + estimation_latency_stamp.nsecs/1e9
self.latencyArray.append(estimation_latency)
if (len(self.latencyArray) >= 20):
self.latencyArray.pop(0)
# Separate Bool for the FSM
in_lane_msg = BoolStamped()
in_lane_msg.header.stamp = segment_list_msg.header.stamp
in_lane_msg.data = True #TODO-TAL change with in_lane. Is this messqge useful since it is alwas true ?
self.pub_in_lane.publish(in_lane_msg)