def cb_intpose(self, int_pose):
'''
cb_intpose Function, that subscribes to the incoming PoseStamped (position and orientation of duckiebot) message and
publishes the LanePose message with the entries d and phi, that can be used for the controller
:param int_pose: PoseStamped ros message
:return: None
'''
# TODO Change to debug level
if self.verbose:
self.log('Received intersection pose.')
if self.parametersChanged:
self.log('Parameters changed.', 'info')
self.refresh_parameters()
self.parametersChanged = False
trajectory = self.trajectories[self.trajectory]
track = trajectory['track']
tangent_angle = trajectory['tangent_angle']
curvature = trajectory['curvature']
# compute distance d and angle phi, same as in lane following
d, phi, curv = self.relative_pose(int_pose, track, tangent_angle,
curvature)
# convert to LanePose() message
lane_pose = LanePose()
lane_pose.header.stamp = int_pose.header.stamp
lane_pose.d = d
lane_pose.phi = phi
lane_pose.curvature = curv
# publish lane pose msg
self.pub_lanepose.publish(lane_pose)
if self.verbose:
self.log("distance: " + str(d))
self.log("angle: " + str(phi))
if self.track_publishing_skipped_iterations % 10 == 0:
self.track_publishing_skipped_iterations = 0
self.publish_track(track, tangent_angle)
self.track_publishing_skipped_iterations += 1
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
# 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)
# Step 3: build messages and publish things
[d_max, phi_max] = self.filter.getEstimate()
# print "d_max = ", d_max
# print "phi_max = ", phi_max
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:])
# publish the belief image
belief_img = self.getDistributionImage(self.filter.belief,
segment_list_msg.header.stamp)
self.pub_lane_pose.publish(lanePose)
self.pub_belief_img.publish(belief_img)
def getDistributionImage(self, mat, stamp):
bridge = CvBridge()
img = bridge.cv2_to_imgmsg((255 * mat).astype('uint8'), "mono8")
img.header.stamp = stamp
return 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))
# TODO (1): see above, method does not exist
#self.pub_belief_img.publish(belief_img)
# 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 change with in_lane
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):
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.getEstimateList()
#print "d_max = ", d_max
#print "phi_max = ", phi_max
# sum_phi_l = np.sum(phi_max[1:self.filter.num_belief])
# sum_d_l = np.sum(d_max[1:self.filter.num_belief])
# av_phi_l = np.average(phi_max[1:self.filter.num_belief])
# av_d_l = np.average(d_max[1:self.filter.num_belief])
max_val = self.filter.getMax()
in_lane = max_val > self.filter.min_max
#if (sum_phi_l<-1.6 and av_d_l>0.05):
# print "I see a left curve"
#elif (sum_phi_l>1.6 and av_d_l <-0.05):
# print "I see a right curve"
#else:
# print "I am on a straight line"
delta_dmax = np.median(d_max[1:]) # - d_max[0]
delta_phimax = np.median(phi_max[1:]) #- phi_max[0]
if len(self.d_median) >= 5:
self.d_median.pop(0)
self.phi_median.pop(0)
self.d_median.append(delta_dmax)
self.phi_median.append(delta_phimax)
# 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
#print "Delta dmax", delta_dmax
#print "Delta phimax", delta_phimax
CURVATURE_LEFT = 0.025
CURVATURE_RIGHT = -0.054
CURVATURE_STRAIGHT = 0
if np.median(self.phi_median) < -0.3 and np.median(
self.d_median) > 0.05:
print "left curve"
lanePose.curvature = CURVATURE_LEFT
elif np.median(self.phi_median) > 0.2 and np.median(
self.d_median) < -0.02:
print "right curve"
lanePose.curvature = CURVATURE_RIGHT
else:
print "straight line"
lanePose.curvature = CURVATURE_STRAIGHT
# publish the belief image
belief_img = self.getDistributionImage(self.filter.belief,
segment_list_msg.header.stamp)
self.pub_lane_pose.publish(lanePose)
self.pub_belief_img.publish(belief_img)
# 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 = in_lane
self.pub_in_lane.publish(in_lane_msg)
