def toLanePose(self,vicon_pose_msg):
quat = vicon_pose_msg.pose.orientation
euler_angles = tf.transformations.euler_from_quaternion([quat.x, quat.y,quat.z,quat.w])
lane_pose = LanePose()
lane_pose.d = vicon_pose_msg.pose.position.x - self.x_goal
lane_pose.phi = euler_angles[2] - self.phi_goal
lane_pose.header.stamp = vicon_pose_msg.header.stamp
return lane_pose
def toLanePose(self, vicon_pose_msg):
quat = vicon_pose_msg.pose.orientation
euler_angles = tf.transformations.euler_from_quaternion(
[quat.x, quat.y, quat.z, quat.w])
lane_pose = LanePose()
lane_pose.d = vicon_pose_msg.pose.position.x - self.x_goal
lane_pose.phi = euler_angles[2] - self.phi_goal
lane_pose.header.stamp = vicon_pose_msg.header.stamp
return lane_pose
def test_v_dot_simple(self):
# publish wheel_cmd_executed
wheels_cmd = WheelsCmdStamped()
rate = rospy.Rate(10)
for i in range(10):
wheels_cmd.header.stamp = rospy.Time.now()
wheels_cmd.vel_left = 0.5
wheels_cmd.vel_right = 0.5
self.pub_wheels_cmd_executed.publish(wheels_cmd)
rate.sleep()
# publish LanePose
lane_pose = LanePose()
lane_pose.d = 0
lane_pose.sigma_d = 0
lane_pose.phi = 0
lane_pose.sigma_phi = 0
lane_pose.status = 0
lane_pose.in_lane = True
for i in [1, 2]:
lane_pose.header.stamp = rospy.Time.now()
self.pub_lane_pose.publish(lane_pose)
rate.sleep()
# publish StopLineReading
stop_line_reading = StopLineReading()
stop_line_reading.stop_line_detected = True
stop_line_reading.at_stop_line = False
stop_line_reading.stop_line_point = Point()
for x in [0.51, 0.5]:
stop_line_reading.header.stamp = rospy.Time.now()
stop_line_reading.stop_line_point.x = x
self.pub_stop_line_reading.publish(stop_line_reading)
rate.sleep()
# Wait for the odometry_training_pairs_node to finish starting up
timeout = time.time() + 2.0
while not self.sub_v_sample.get_num_connections() and \
not rospy.is_shutdown() and not time.time() > timeout:
rospy.sleep(0.1)
msg = self.v_received
# self.assertEqual(hasattr(msg,'d_L'),True)
self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L)
self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R)
self.assertAlmostEqual(msg.dt, 0.1, 2, 'dt = %s' % msg.dt)
self.assertEqual(
msg.theta_angle_pose_delta, 0,
'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta)
self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5,
'x = %s' % msg.x_axis_pose_delta)
self.assertEqual(msg.y_axis_pose_delta, 0,
'y = %s' % msg.y_axis_pose_delta)
def test_v_dot_simple(self):
# publish wheel_cmd_executed
wheels_cmd = WheelsCmdStamped()
rate = rospy.Rate(10)
for i in range(10):
wheels_cmd.header.stamp = rospy.Time.now()
wheels_cmd.vel_left = 0.5
wheels_cmd.vel_right = 0.5
self.pub_wheels_cmd_executed.publish(wheels_cmd)
rate.sleep()
# publish LanePose
lane_pose = LanePose()
lane_pose.d = 0
lane_pose.sigma_d = 0
lane_pose.phi = 0
lane_pose.sigma_phi = 0
lane_pose.status = 0
lane_pose.in_lane = True
for i in [1, 2]:
lane_pose.header.stamp = rospy.Time.now()
self.pub_lane_pose.publish(lane_pose)
rate.sleep()
# publish StopLineReading
stop_line_reading = StopLineReading()
stop_line_reading.stop_line_detected = True
stop_line_reading.at_stop_line = False
stop_line_reading.stop_line_point = Point()
for x in [0.51, 0.5]:
stop_line_reading.header.stamp = rospy.Time.now()
stop_line_reading.stop_line_point.x = x
self.pub_stop_line_reading.publish(stop_line_reading)
rate.sleep()
# Wait for the odometry_training_pairs_node to finish starting up
timeout = time.time()+2.0
while not self.sub_v_sample.get_num_connections() and \
not rospy.is_shutdown() and not time.time() > timeout:
rospy.sleep(0.1)
msg = self.v_received
# self.assertEqual(hasattr(msg,'d_L'),True)
self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L)
self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R)
self.assertAlmostEqual(msg.dt, 0.1, 2,'dt = %s' % msg.dt)
self.assertEqual(msg.theta_angle_pose_delta, 0, 'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta)
self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5, 'x = %s' % msg.x_axis_pose_delta)
self.assertEqual(msg.y_axis_pose_delta, 0, 'y = %s' % msg.y_axis_pose_delta)
def processSegments(self, segment_list_msg):
if not self.active:
return
current_time = time.time()
self.filter.predict(dt=current_time - self.t_last_update,
v=self.velocity.v,
w=self.velocity.omega)
self.t_last_update = current_time
ml = self.filter.update(segment_list_msg.segments)
if ml is not None:
ml_img = self.getDistributionImage(ml,
segment_list_msg.header.stamp)
self.pub_ml_img.publish(ml_img)
[d_max, phi_max] = self.filter.getEstimate()
max_val = self.filter.getMax()
in_lane = bool(max_val > self.filter.min_max)
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
# publish the belief image
belief_img = self.getDistributionImage(self.filter.belief,
segment_list_msg.header.stamp)
current_time_print = time.time()
message_time = segment_list_msg.header.stamp.sec + segment_list_msg.header.stamp.nanosec * 1e-9
delay = current_time_print - message_time
#print("wheels_cmd timestamp: " + str(segment_list_msg.header.stamp))
#print("current time: " + str(current_time_print))
#print("delay: " + str(delay))
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)
def cbProcessSegments(self, segment_list_msg):
"""Callback to process the segments
Args:
segment_list_msg (:obj:`SegmentList`): message containing list of processed segments
"""
# Get actual timestamp for latency measurement
timestamp_before_processing = rospy.Time.now()
# Step 1: predict
current_time = rospy.get_time()
if self.currentVelocity:
dt = current_time - self.t_last_update
self.filter.predict(dt=dt,
v=self.currentVelocity.v,
w=self.currentVelocity.omega)
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
# Getting the highest belief value from the belief matrix
max_val = self.filter.getMax()
# Comparing it to a minimum belief threshold to make sure we are certain enough of our estimate
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
lanePose.phi = phi_max
lanePose.in_lane = in_lane
# XXX: is it always NORMAL?
lanePose.status = lanePose.NORMAL
self.pub_lane_pose.publish(lanePose)
self.debugOutput(segment_list_msg, d_max, phi_max,
timestamp_before_processing)
def publishEstimate(self, segment_list_msg=None):
belief = self.filter.getEstimate()
# build lane pose message to send
lanePose = LanePose()
lanePose.header.stamp = self.last_update_stamp
lanePose.d = belief['mean'][0]
lanePose.phi = belief['mean'][1]
lanePose.d_phi_covariance = [
belief['covariance'][0][0], belief['covariance'][0][1],
belief['covariance'][1][0], belief['covariance'][1][1]
]
lanePose.in_lane = True
lanePose.status = lanePose.NORMAL
self.pub_lane_pose.publish(lanePose)
if segment_list_msg is not None:
self.debugOutput(segment_list_msg, lanePose.d, lanePose.phi)
def processSegments(self, segment_list_msg):
if not self.active:
return
# Step 1: predict
current_time = rospy.get_time()
self.filter.predict(dt=current_time - self.t_last_update,
v=self.velocity.v,
w=self.velocity.omega)
self.t_last_update = current_time
# Step 2: update
ml = self.filter.update(segment_list_msg.segments)
if ml is not None:
ml_img = self.getDistributionImage(ml,
segment_list_msg.header.stamp)
self.pub_ml_img.publish(ml_img)
# Step 3: build messages and publish things
[d_max, phi_max] = self.filter.getEstimate()
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
lanePose.phi = phi_max
lanePose.in_lane = in_lane
lanePose.status = lanePose.NORMAL
# 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)
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
# Step 1: predict
current_time = rospy.get_time()
self.filter.predict(dt=current_time - self.t_last_update,
v=self.velocity.v,
w=self.velocity.omega)
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()
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
lanePose.phi = phi_max
lanePose.in_lane = in_lane
lanePose.status = lanePose.NORMAL
# publish the belief image
bridge = CvBridge()
belief_img = bridge.cv2_to_imgmsg(
(255 * self.filter.belief).astype('uint8'), "mono8")
belief_img.header.stamp = 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)
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):
# 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)
def updateEstimate(self, segListMsg):
self.testCount = self.testCount + 1 # Just some counter, gives an idea of the speed.
# ######## EXTRACT ######### data from segments back into separated arrays.
whitePointsArray, yellowPointsArray = self.segList2Array(segListMsg)
nWhite = whitePointsArray.shape[0]
nYellow = yellowPointsArray.shape[0]
# ###### REMOVE WHITE POINTS OF LEFT LANE ######
if nYellow >= 2: # Need minimum of two points to define a line.
self.ransacY.fit(
np.reshape(yellowPointsArray[:, 0], (-1, 1)),
np.array(np.reshape(yellowPointsArray[:, 1], (-1, 1))))
zYellowOnly = np.array([
self.ransacY.estimator_.coef_,
self.ransacY.estimator_.intercept_
])
whitePointsArray = self.removeLeftLane(zYellowOnly,
whitePointsArray)
nWhite = whitePointsArray.shape[0]
nYellow = yellowPointsArray.shape[0]
# ###### FIT TWO LANES ########
if nWhite >= 2 and nYellow >= 2:
z = self.fitTwoLanes(whitePointsArray, yellowPointsArray)
#self.zTwoLane = z
m = z[0]
c = z[1]
zWhite = np.array([m, c])
zYellow = np.array([m, c + self.laneWidth * np.sqrt(m**2 + 1)])
elif nYellow >= 2:
zWhite = zYellowOnly
elif nWhite >= 2:
self.ransacW.fit(np.reshape(whitePointsArray[:, 0], (-1, 1)),
np.reshape(whitePointsArray[:, 1], (-1, 1)))
zWhite = np.array([
self.ransacW.estimator_.coef_,
self.ransacW.estimator_.intercept_
])
if nWhite >= 2 or nYellow >= 2:
# ######### Calculate d and phi from geometry ##########
m = np.asscalar(zWhite[0])
c = np.asscalar(zWhite[1])
phiWhite = np.arcsin(-m / (np.sqrt(1 + m**2)))
r_wz_b = np.array([
[-c * m / (1 + m**2)], [c - (c * m**2 / (1 + m**2))]
]) # Position vector to nearest point on the line (normal to line)
C_wb = np.array([[np.cos(phiWhite), -np.sin(phiWhite)],
[np.sin(phiWhite),
np.cos(phiWhite)]])
r_wz_w = np.matmul(
C_wb, r_wz_b
) # Distance TO white lane from duckiebot. This should be negative
if nWhite < 2 and nYellow >= 2: # Then the line we have is actually yellow
r_dw_w = np.array([[0], [-self.laneWidth / 2]])
else:
if r_wz_w[1] >= 0: # Then white line is on our left.
# This can occur in two situations
# 1) We are in the other lane
# 2) We are turning a right corner.
# In either case, it is actually the OTHER white line
r_dw_w = np.array([[0], [-1.5 * self.laneWidth]])
else:
r_dw_w = np.array([[0], [self.laneWidth / 2]])
r_zd_w = -r_wz_w - r_dw_w
distWhite = r_zd_w[1] # Distance TO duckiebot from desired point.
phi = phiWhite
d = distWhite
else:
d = 0
phi = 0
# ######### VISUALIZE ###########
if nWhite >= 2 and nYellow >= 2:
pa1 = self.line2PointArray(zWhite, 0, 1)
pa2 = self.line2PointArray(zYellow, 0, 1)
self.visualizeCurves(pa1, pa2)
elif nWhite >= 2 or nYellow >= 2:
pa1 = self.line2PointArray(zWhite, 0, 1)
self.visualizeCurves(pa1)
# ###### PUBLISH LANE POSE #######
lanePose = LanePose()
lanePose.header.stamp = segListMsg.header.stamp
# Complimentary filter lol
lanePose.d = 0.8 * float(d) + 0.2 * float(self.d_baseline)
lanePose.phi = 0.8 * float(phi) + 0.2 * float(self.phi_baseline)
lanePose.in_lane = True
lanePose.status = lanePose.NORMAL
self.correct(lanePose) # Kalman Filter Modifications
lanePose.d = np.asscalar(self.x_k[0][0])
lanePose.phi = np.asscalar(self.x_k[1][0])
self.pub_lane_pose.publish(lanePose)
def MainLoop(self):
if not self.active:
return
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
# run state machine
if self.state == self.state_dict['IDLE']:
# waiting for FSMState to tell us that Duckiebot is at an intersection (see FSMCallback)
pass
elif self.state == self.state_dict['INITIALIZING_LOCALIZATION']:
if self.InitializeLocalization():
self.state = self.state_dict['INITIALIZING_PATH']
rospy.loginfo(
"[%s] Initialized intersection localization, initializing path."
% (self.node_name))
elif self.state == self.state_dict['INITIALIZING_PATH']:
if self.InitializePath():
self.state = self.state_dict['TRAVERSING']
self.s = 0.0
self.traversing_start = rospy.Time.now()
rospy.loginfo(
"[%s] Initialized path, waiting for go signal." %
(self.node_name))
else:
rospy.loginfo(
"[%s] Could not initialize path. Trying again." %
(self.node_name))
elif self.state == self.state_dict['TRAVERSING']:
'''open-loop'''
if self.open_loop:
msg_cmds = Twist2DStamped()
msg_cmds.header.stamp = rospy.Time.now()
if 1.0 < (rospy.Time.now() -
self.traversing_start).to_sec() and self.go:
pos, vel = self.pathPlanner.EvaluatePath(self.s)
dt = 0.01
_, vel2 = self.pathPlanner.EvaluatePath(self.s + dt)
self.alpha = self.v / np.linalg.norm(vel)
dir = vel / np.linalg.norm(vel)
dir2 = vel2 / np.linalg.norm(vel2)
theta = np.arctan2(dir[1], dir[0])
theta2 = np.arctan2(dir2[1], dir2[0])
omega = (theta2 - theta) / dt
msg_cmds.v = self.v
msg_cmds.omega = self.alpha * omega
if (msg_cmds.v -
0.5 * math.fabs(msg_cmds.omega) * 0.1) < 0.065:
msg_cmds.v = 0.065 + 0.5 * math.fabs(
msg_cmds.omega) * 0.1
self.alpha = self.alpha * msg_cmds.v / self.v
msg_cmds.v = msg_cmds.v * self.v_scale
msg_cmds.omega = msg_cmds.omega * self.omega_scale
self.s = self.s + self.alpha * (rospy.Time.now(
) - self.traversing_last_time).to_sec()
if (self.s > 0.99):
msg_cmds.v = self.v
msg_cmds.omega = 0.0
self.state = self.state_dict['DONE']
self.done_time = rospy.Time.now()
else:
msg_cmds.v = 0.0
msg_cmds.omega = 0.0
self.traversing_last_time = rospy.Time.now()
self.pub_cmds.publish(msg_cmds)
else:
'''closed-loop'''
msg_lane_pose = LanePose()
msg_lane_pose.header.stamp = rospy.Time.now()
if 1.0 < (rospy.Time.now() -
self.traversing_start).to_sec() and self.go:
pose, _ = self.poseEstimator.PredictState(
msg_lane_pose.header.stamp)
dist, theta, curvature, self.s = self.pathPlanner.ComputeLaneError(
pose, self.s)
#rospy.loginfo("the s is: "+str(self.s))
if (self.s > self.s_max):
msg_lane_pose.v_ref = self.v
msg_lane_pose.d = 0.0
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = 0.0
msg_lane_pose.curvature_ref = 0.0
self.state = self.state_dict['DONE']
self.done_time = rospy.Time.now()
else:
msg_lane_pose.v_ref = self.v
msg_lane_pose.d = dist
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = theta
msg_lane_pose.curvature_ref = curvature
else:
msg_lane_pose.v_ref = 0.0
msg_lane_pose.d = 0.0
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = 0.0
msg_lane_pose.curvature_ref = 0.0
self.pub_lane_pose.publish(msg_lane_pose)
elif self.state == self.state_dict['DONE']:
# switch back to lane following if in lane
if self.in_lane and (rospy.Time.now() - self.in_lane_time
).to_sec() < self.in_lane_timeout:
msg_done = BoolStamped()
msg_done.header.stamp = rospy.Time.now()
msg_done.data = True
self.pub_done.publish(msg_done)
self.state = self.state_dict['IDLE']
# go straight for 2.0 secs if not in lane yet. After 2 secs stop.
else:
'''open-loop'''
if self.open_loop:
if (rospy.Time.now() - self.done_time
).to_sec() < self.in_lane_wait_time:
msg_cmds = Twist2DStamped()
msg_cmds.header.stamp = rospy.Time.now()
msg_cmds.v = self.v * self.v_scale
msg_cmds.omega = 0.0 * self.omega_scale
self.pub_cmds.publish(msg_cmds)
else:
msg_cmds = Twist2DStamped()
msg_cmds.header.stamp = rospy.Time.now()
msg_cmds.v = 0.0 * self.v_scale
msg_cmds.omega = 0.0 * self.omega_scale
self.pub_cmds.publish(msg_cmds)
rospy.loginfo(
"[%s] Could not find lane. Stopping now." %
(self.node_name))
self.state = self.state_dict['ERROR']
else:
'''closed-loop'''
if (rospy.Time.now() - self.done_time
).to_sec() < self.in_lane_wait_time:
msg_lane_pose = LanePose()
msg_lane_pose.header.stamp = rospy.Time.now()
msg_lane_pose.v_ref = self.v
msg_lane_pose.d = 0.0
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = 0.0
msg_lane_pose.curvature_ref = 0.0
self.pub_lane_pose.publish(msg_lane_pose)
else:
msg_lane_pose = LanePose()
msg_lane_pose.header.stamp = rospy.Time.now()
msg_lane_pose.v_ref = 0.0
msg_lane_pose.d = 0.0
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = 0.0
msg_lane_pose.curvature_ref = 0.0
self.pub_lane_pose.publish(msg_lane_pose)
rospy.loginfo(
"[%s] Could not find lane. Stopping now." %
(self.node_name))
self.state = self.state_dict['ERROR']
else:
rospy.loginfo("[%s] Something went wrong." % (self.node_name))
rate.sleep()
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)
