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 __init__(self):
# Save the name of the node
self.node_name = rospy.get_name()
self.mode = None
self.turn_type = -1
self.in_lane = False
self.lane_pose = LanePose()
self.stop_line_reading = StopLineReading()
self.trajectory_reparam = rospy.get_param("~trajectory_reparam",1)
self.pub_cmd = rospy.Publisher("~car_cmd",Twist2DStamped,queue_size=1)
self.pub_done = rospy.Publisher("~intersection_done",BoolStamped,queue_size=1)
# Construct maneuvers
self.maneuvers = dict()
self.maneuvers[0] = self.getManeuver("turn_left")
self.maneuvers[1] = self.getManeuver("turn_forward")
self.maneuvers[2] = self.getManeuver("turn_right")
self.maneuvers[3] = self.getManeuver("turn_back")
# self.maneuvers[-1] = self.getManeuver("turn_stop")
self.srv_turn_left = rospy.Service("~turn_left", Empty, self.cbSrvLeft)
self.srv_turn_right = rospy.Service("~turn_right", Empty, self.cbSrvRight)
self.srv_turn_forward = rospy.Service("~turn_forward", Empty, self.cbSrvForward)
self.srv_turn_back = rospy.Service("~turn_back", Empty, self.cbSrvBack)
self.rate = rospy.Rate(30)
# Subscribers
self.sub_in_lane = rospy.Subscriber("~in_lane", BoolStamped, self.cbInLane, queue_size=1)
self.sub_turn_type = rospy.Subscriber("~turn_type", Int16, self.cbTurnType, queue_size=1)
self.sub_mode = rospy.Subscriber("~mode", FSMState, self.cbFSMState, queue_size=1)
self.sub_lane_pose = rospy.Subscriber("~lane_pose", LanePose, self.cbLanePose, queue_size=1)
self.sub_stop_line = rospy.Subscriber("~stop_line_reading", StopLineReading, self.cbStopLine, queue_size=1)
def __init__(self):
self.node_name = rospy.get_name()
self.prev_time = None
# Publication
self.pub_car_cmd = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1)
# Subscriptions
self.sub_lane_reading = rospy.Subscriber("~lane_pose", LanePose, self.updatePose, queue_size=1)
self.pose_msg = LanePose()
# Setup PID gains
self.k_P_phi = -3.5
self.k_P_d = -2.5
self.k_I_d = 0.5
self.k_I_phi = 0.1
self.k_D_d = 0.7
self.k_D_phi = 0.4
# Initialization
self.prev_d_err = 0
self.prev_phi_err = 0
self.v_ref = 0.5
self.v_max = 1
self.d_offset = 0
self.d_integral_top_cutoff = 0.3
self.d_integral_bottom_cutoff = -0.3
self.phi_integral_top_cutoff = 1.0
self.phi_integral_bottom_cutoff = -1.0
def __init__(self, node_name):
# Initialize the DTROS parent class
super(StopLineFilterNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION)
# Initialize the parameters
self.stop_distance = DTParam("~stop_distance", param_type=ParamType.FLOAT)
self.min_segs = DTParam("~min_segs", param_type=ParamType.INT)
self.off_time = DTParam("~off_time", param_type=ParamType.FLOAT)
self.max_y = DTParam("~max_y", param_type=ParamType.FLOAT)
## params
# self.stop_distance = self.setupParam("~stop_distance", 0.22) # distance from the stop line that we should stop
# self.min_segs = self.setupParam("~min_segs", 2) # minimum number of red segments that we should detect to estimate a stop
# self.off_time = self.setupParam("~off_time", 2)
# self.max_y = self.setupParam("~max_y ", 0.2) # If y value of detected red line is smaller than max_y we will not set at_stop_line true.
## state vars
self.lane_pose = LanePose()
self.state = "JOYSTICK_CONTROL"
self.sleep = False
## publishers and subscribers
self.sub_segs = rospy.Subscriber("~segment_list", SegmentList, self.cb_segments)
self.sub_lane = rospy.Subscriber("~lane_pose", LanePose, self.cb_lane_pose)
self.sub_mode = rospy.Subscriber("fsm_node/mode", FSMState, self.cb_state_change)
self.pub_stop_line_reading = rospy.Publisher("~stop_line_reading", StopLineReading, queue_size=1)
self.pub_at_stop_line = rospy.Publisher("~at_stop_line", BoolStamped, queue_size=1)
def __init__(self):
self.node_name = "Stop Line Filter"
self.active = True
## state vars
self.lane_pose = LanePose()
## params
self.stop_distance = self.setupParam(
"~stop_distance",
0.2) # distance from the stop line that we should stop
self.min_segs = self.setupParam(
"~min_segs", 2
) # minimum number of red segments that we should detect to estimate a stop
self.off_time = self.setupParam("~off_time", 2)
self.lanewidth = 0 # updated continuously below
self.state = "JOYSTICK_CONTROL"
self.sleep = False
## publishers and subscribers
self.sub_segs = rospy.Subscriber("~segment_list", SegmentList,
self.processSegments)
self.sub_lane = rospy.Subscriber("~lane_pose", LanePose,
self.processLanePose)
self.sub_mode = rospy.Subscriber("fsm_node/mode", FSMState,
self.processStateChange)
self.pub_stop_line_reading = rospy.Publisher("~stop_line_reading",
StopLineReading,
queue_size=1)
self.pub_at_stop_line = rospy.Publisher("~at_stop_line",
BoolStamped,
queue_size=1)
self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0),
self.updateParams)
def __init__(self, outputFile):
f = open(outputFile, 'wt')
self.writer = csv.writer(f)
self.oldLP = LanePose()
self.z = 0.0
self.tiles_crossed = 0
rospy.Subscriber('/teamgrapes/lane_filter_node/lane_pose', LanePose,
self.estimatePose)
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 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 obstacleCallback(self, obstacle_poses):
amount_obstacles = len(obstacle_poses.poses)
# rospy.loginfo('Number of obstacles: %d', len(obstacle_poses.poses))
amount_obstacles_on_track = 0
obstacle_poses_on_track = PoseArray()
obstacle_poses_on_track.header = obstacle_poses.header
avoidance_active = BoolStamped()
avoidance_active.data = False
target = LanePose()
# target.header.frame_id = self.robot_name
target.v_ref = 10 # max speed high, current top 0.38
for x in range(amount_obstacles):
# rospy.loginfo(obstacle_poses.poses[x].position.z)
if obstacle_poses.poses[x].position.z > 0:
# Bounding window
# get relative coordinates
x_obstacle = obstacle_poses.poses[x].position.x * 1000 # mm
y_obstacle = obstacle_poses.poses[x].position.y * 1000 # mm
# get global coordinates
global_pos_vec = self.avoider.coordinatetransform(
x_obstacle, y_obstacle, self.theta_current, self.d_current)
# rospy.loginfo('x_obstacle = %f', x_obstacle)
# rospy.loginfo('y_obstacle = %f', y_obstacle)
# rospy.loginfo('theta_current = %f', self.theta_current)
# rospy.loginfo('d_current = %f', self.d_current)
x_global = global_pos_vec[0] # mm
y_global = global_pos_vec[1] # mm
# rospy.loginfo(global_pos_vec)
# check if obstacle is within boundaries
if x_global < self.x_bounding_width and abs(
y_global) < self.y_bounding_width:
# rospy.loginfo('Obstacle in range - Beware')
obstacle_poses_on_track.poses.append(
obstacle_poses.poses[x])
amount_obstacles_on_track += 1
if amount_obstacles_on_track == 0: # rospy.loginfo('0 obstacles on track')
v = 0
elif amount_obstacles_on_track == 1:
#ToDo: check if self.d_current can be accessed through forwarding of self
# targets = self.avoider.avoid(obstacle_poses_on_track, self.d_current, self.theta_current)
# target.d_ref = targets[0]
target.v_ref = 0 # due to inaccuracies in theta, stop in any case
# if targets[1]: # emergency stop
# target.v_ref = 0
# self.theta_target_pub.publish(targets[2]) # theta not calculated in current version
# rospy.loginfo('1 obstacles on track')
# rospy.loginfo('d_target= %f', targets[0])
# rospy.loginfo('emergency_stop = %f', targets[1])
avoidance_active.data = True
else:
target.v_ref = 0
# rospy.loginfo('%d obstacles on track', amount_obstacles_on_track)
avoidance_active.data = True
target.v_ref = 0
# rospy.loginfo('emergency_stop = 1')
self.obstavoidpose_topic.publish(target)
self.avoid_pub.publish(avoidance_active)
# rospy.loginfo('Avoidance flag set: %s', avoidance_active.data)
return
def __init__(self):
self.velocity_to_m_per_s = 0.67
self.omega_to_rad_per_s = 0.45 * 2 * math.pi
self.omega_max = 50.0
self.omega_min = -50.0
#PID for CTE
self.Kp_d = 6
self.Ki_d = 0
self.Kd_d = 0
#PID for phi
self.Kp_phi = 0.8
self.Ki_phi = 0.1
self.Kd_phi = 0
#Useful Info
self.last_dt = 0
self.t_start = rospy.get_time()
self.prev_pose_msg = LanePose()
self.desired_d = 0
self.desired_phi = 0
self.in_lane_bool = False
self.Lane_Control = False
self.ct_integral_top_cutoff = 0.3
self.ct_integral_bottom_cutoff = -0.3
# Publication
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1)
# Subscriptions
self.sub_fsm_mode = rospy.Subscriber("/tyscrduckie/fsm_node/mode",
FSMState,
self.cbMode,
queue_size=1)
self.sub_lane_reading = rospy.Subscriber(
"/tyscrduckie/lane_filter_node/lane_pose", LanePose,
self.PoseHandling)
# The PID
self.cte_control = Controller(self.Kp_d, self.Ki_d, self.Kd_d,
self.ct_integral_top_cutoff,
self.ct_integral_bottom_cutoff)
self.phi_control = Controller(self.Kp_phi, self.Ki_phi, self.Kd_phi,
self.ct_integral_top_cutoff,
self.ct_integral_bottom_cutoff)
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 __init__(self):
self.node_name = rospy.get_name()
self.lane_reading = LanePose()
self.car_control_lane = Twist2DStamped()
self.car_control_joy = Twist2DStamped()
self.safe = True
self.in_lane = True
self.at_stop_line = False
self.stop = False
# Params:
self.max_cross_track_error = self.setupParameter(
"~max_cross_track_error", 0.1)
self.max_heading_error = self.setupParameter("~max_heading_error",
math.pi / 4)
self.min_speed = self.setupParameter("~min_speed", 0.1)
self.max_speed = self.setupParameter("~max_speed", 0.3)
self.max_steer = self.setupParameter("~max_steer", 0.2)
# Publicaiton
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1)
self.pub_safe = rospy.Publisher("~safe", Bool, queue_size=1)
# Subscriptions
self.sub_lane_pose = rospy.Subscriber("~lane_pose",
LanePose,
self.cbLanePose,
queue_size=1)
self.sub_lane_control = rospy.Subscriber("~car_cmd_lane",
Twist2DStamped,
self.cbLaneControl,
queue_size=1)
self.sub_joy_control = rospy.Subscriber("~car_cmd_joy",
Twist2DStamped,
self.cbJoyControl,
queue_size=1)
self.sub_at_stop_line = rospy.Subscriber("~stop_line_reading",
StopLineReading,
self.cbStopLine,
queue_size=1)
self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0),
self.updateParams)
def __init__(self):
self.node_name = rospy.get_name()
self.lane_pose = LanePose()
# Parameters
self.stop_distance = 0.2 # distance from the stop line that we should stop
self.min_segs = 1 # minimum number of red segments that we should detect to estimate a stop
self.lane_width = 0.2
# Topics
self.pub_stop_line_reading = rospy.Publisher("~stop_line_reading",
StopLineReading,
queue_size=1)
self.pub_at_stop_line = rospy.Publisher("~at_stop_line",
BoolStamped,
queue_size=1)
self.sub_segs = rospy.Subscriber("~segment_list", SegmentList,
self.on_segment_list)
self.sub_lane = rospy.Subscriber("~lane_pose", LanePose,
self.on_lane_pose)
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 __init__(self):
self.node_name = "Lane Filter"
self.mean_0 = [
self.setupParam("~mean_d_0", 0),
self.setupParam("~mean_phi_0", 0)
]
self.cov_0 = [[self.setupParam("~sigma_d_0", 0.1), 0],
[0, self.setupParam("~sigma_phi_0", 0.01)]]
self.delta_d = self.setupParam("~delta_d", 0.02) # in meters
self.delta_phi = self.setupParam("~delta_phi", 0.02) # in radians
self.d_max = self.setupParam("~d_max", 0.5)
self.d_min = self.setupParam("~d_min", -0.7)
self.phi_min = self.setupParam("~phi_min", -pi / 2)
self.phi_max = self.setupParam("~phi_max", pi / 2)
self.cov_v = self.setupParam("~cov_v", 0.5) # linear velocity "input"
self.cov_omega = self.setupParam("~cov_omega",
0.01) # angular velocity "input"
self.linewidth_white = self.setupParam("~linewidth_white", 0.04)
self.linewidth_yellow = self.setupParam("~linewidth_yellow", 0.02)
self.lanewidth = self.setupParam("~lanewidth", 0.4)
self.min_max = self.setupParam("~min_max", 0.3) # nats
self.d, self.phi = np.mgrid[self.d_min:self.d_max:self.delta_d,
self.phi_min:self.phi_max:self.delta_phi]
self.beliefRV = np.empty(self.d.shape)
self.initializeBelief()
self.lanePose = LanePose()
self.lanePose.d = self.mean_0[0]
self.lanePose.phi = self.mean_0[1]
self.sub = rospy.Subscriber("~segment_list", SegmentList,
self.processSegments)
# self.sub = rospy.Subscriber("~velocity",
self.pub_lane_pose = rospy.Publisher("~lane_pose",
LanePose,
queue_size=1)
self.pub_belief_img = rospy.Publisher("~belief_img",
Image,
queue_size=1)
self.pub_entropy = rospy.Publisher("~entropy", Float32, queue_size=1)
self.pub_in_lane = rospy.Publisher("~in_lane", Bool, queue_size=1)
def __init__(self):
self.node_name = "Lane Filter"
self.active = True
self.updateParams(None)
self.d,self.phi = np.mgrid[self.d_min:self.d_max:self.delta_d,self.phi_min:self.phi_max:self.delta_phi]
self.beliefRV=np.empty(self.d.shape)
self.initializeBelief()
self.lanePose = LanePose()
self.lanePose.d=self.mean_0[0]
self.lanePose.phi=self.mean_0[1]
self.dwa = -(self.zero_val*self.l_peak**2 + self.zero_val*self.l_max**2 - self.l_max**2*self.peak_val - 2*self.zero_val*self.l_peak*self.l_max + 2*self.l_peak*self.l_max*self.peak_val)/(self.l_peak**2*self.l_max*(self.l_peak - self.l_max)**2)
self.dwb = (2*self.zero_val*self.l_peak**3 + self.zero_val*self.l_max**3 - self.l_max**3*self.peak_val - 3*self.zero_val*self.l_peak**2*self.l_max + 3*self.l_peak**2*self.l_max*self.peak_val)/(self.l_peak**2*self.l_max*(self.l_peak - self.l_max)**2)
self.dwc = -(self.zero_val*self.l_peak**3 + 2*self.zero_val*self.l_max**3 - 2*self.l_max**3*self.peak_val - 3*self.zero_val*self.l_peak*self.l_max**2 + 3*self.l_peak*self.l_max**2*self.peak_val)/(self.l_peak*self.l_max*(self.l_peak - self.l_max)**2)
self.t_last_update = rospy.get_time()
self.v_current = 0
self.w_current = 0
self.v_last = 0
self.w_last = 0
self.v_avg = 0
self.w_avg = 0
# Subscribers
if self.use_propagation:
self.sub_velocity = rospy.Subscriber("/lane_filter_node/velocity", Twist2DStamped, self.updateVelocity)
self.sub = rospy.Subscriber("~segment_list", SegmentList, self.processSegments, queue_size=1)
# Publishers
self.pub_lane_pose = rospy.Publisher("~lane_pose", LanePose, queue_size=1)
self.pub_belief_img = rospy.Publisher("~belief_img", Image, queue_size=1)
self.pub_entropy = rospy.Publisher("~entropy",Float32, queue_size=1)
#self.pub_prop_img = rospy.Publisher("~prop_img", Image, queue_size=1)
self.pub_in_lane = rospy.Publisher("~in_lane",BoolStamped, queue_size=1)
self.sub_switch = rospy.Subscriber("~switch", BoolStamped, self.cbSwitch, queue_size=1)
self.timer = rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams)
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 __init__(self):
self.node_name = rospy.get_name()
## Parameters
self.stop_line_max_age = self.setupParameter(
"~stop_line_max_age", 1.0
) # [sec] don't use stop_line_reading msg pairs more than this far apart
self.lane_pose_max_age = self.setupParameter(
"~lane_pose_max_age", 1.0
) # [sec] don't use lane_pose msg pairs more than this far apart
self.wheels_cmd_max_age = self.setupParameter(
"~wheels_cmd_max_age",
2.0) # [sec] throw away wheels_cmd's older than this
## state vars
self.in_lane = False
self.current_wheels_cmd = WheelsCmdStamped()
self.old_stop_line_msg = StopLineReading()
self.old_lane_pose_msg = LanePose()
self.cmd_buffer = deque() # buffer of wheel commands
self.v_sample_msg = Vsample(
) # global variable because different parts of this are set in different callbacks
## publishers and subscribers
self.sub_stop_line_reading = rospy.Subscriber("~stop_line_reading",
StopLineReading,
self.stopLineCB)
self.sub_lane_pose = rospy.Subscriber("~lane_pose", LanePose,
self.lanePoseCB)
self.sub_wheels_cmd_executed = rospy.Subscriber(
"~wheels_cmd_executed", WheelsCmdStamped, self.wheelsCmdCB)
self.pub_v_sample = rospy.Publisher("~v_sample", Vsample, queue_size=1)
self.pub_theta_dot_sample = rospy.Publisher("~theta_dot_sample",
ThetaDotSample,
queue_size=1)
rospy.loginfo('[%s] Initialized' % self.node_name)
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 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 __init__(self, node_name):
# Initialize the DTROS parent class
super(LaneControllerNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Add the node parameters to the parameters dictionary
# TODO: MAKE TO WORK WITH NEW DTROS PARAMETERS
self.params = dict()
self.params['~v_bar'] = DTParam('~v_bar',
param_type=ParamType.FLOAT,
min_value=0.0,
max_value=5.0)
self.params['~k_d'] = DTParam('~k_d',
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params['~k_theta'] = DTParam('~k_theta',
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params['~k_Id'] = DTParam('~k_Id',
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params['~k_Iphi'] = DTParam('~k_Iphi',
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params['~theta_thres'] = rospy.get_param('~theta_thres', None)
self.params['~d_thres'] = rospy.get_param('~d_thres', None)
self.params['~d_offset'] = rospy.get_param('~d_offset', None)
self.params['~integral_bounds'] = rospy.get_param(
'~integral_bounds', None)
self.params['~d_resolution'] = rospy.get_param('~d_resolution', None)
self.params['~phi_resolution'] = rospy.get_param(
'~phi_resolution', None)
self.params['~omega_ff'] = rospy.get_param('~omega_ff', None)
self.params['~verbose'] = rospy.get_param('~verbose', None)
self.params['~stop_line_slowdown'] = rospy.get_param(
'~stop_line_slowdown', None)
# Need to create controller object before updating parameters, otherwise it will fail
self.controller = LaneController(self.params)
# self.updateParameters() # TODO: This needs be replaced by the new DTROS callback when it is implemented
# Initialize variables
self.fsm_state = None
self.wheels_cmd_executed = WheelsCmdStamped()
self.pose_msg = LanePose()
self.pose_initialized = False
self.pose_msg_dict = dict()
self.last_s = None
self.stop_line_distance = None
self.stop_line_detected = False
self.at_stop_line = False
self.obstacle_stop_line_distance = None
self.obstacle_stop_line_detected = False
self.at_obstacle_stop_line = False
self.current_pose_source = 'lane_filter'
# Construct publishers
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1,
dt_topic_type=TopicType.CONTROL)
# Construct subscribers
self.sub_lane_reading = rospy.Subscriber("~lane_pose",
LanePose,
self.cbAllPoses,
"lane_filter",
queue_size=1)
self.sub_intersection_navigation_pose = rospy.Subscriber(
"~intersection_navigation_pose",
LanePose,
self.cbAllPoses,
"intersection_navigation",
queue_size=1)
self.sub_wheels_cmd_executed = rospy.Subscriber(
"~wheels_cmd",
WheelsCmdStamped,
self.cbWheelsCmdExecuted,
queue_size=1)
self.sub_stop_line = rospy.Subscriber("~stop_line_reading",
StopLineReading,
self.cbStopLineReading,
queue_size=1)
self.sub_obstacle_stop_line = rospy.Subscriber(
"~obstacle_distance_reading",
StopLineReading,
self.cbObstacleStopLineReading,
queue_size=1)
self.log("Initialized!")
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)
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 __init__(self, node_name):
# initialize the DTROS parent class
super(Sensor_Fusion, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# covariance noise matrices
self.R = np.zeros((2, 3))
self.R[0][0] = 0.01 #!
self.R[1][1] = 0.01
self.R[2][2] = 0.04
self.Q = 0.005 * np.eye(2)
# higher publishing frequency
self.higher_pub_rate = 0 # 1 = on; 0 = off
self.ticks_for_pub = 14 # amount of ticks between published poses
# parameter
self.tick_to_meter = np.pi * 0.067 / 135. # 6.7cm diameter, 135 = #ticks per revolution
self.wheeltrack = 0.101
# msg
self.msg_fusion = LanePose()
self.msg_enc_pose = LanePose()
# values which need to be available over multiple calls
self.z_m = np.zeros((3, 1)) # (d_enc, phi_enc, d_cam, phi_cam)^T
self.old_z_m0 = 0
self.last_left_ticks, self.last_right_ticks = 0, 0
self.time_last_image = 0
self.first_encoder_meas_l, self.first_encoder_meas_r = 0, 0
self.i_first_calibration = 0
self.distance, self.old_distance = 0, 0
self.direction_l, self.direction_r = 1, 1
self.only_cam = 0
self.sum_alpha, self.sum_ticks = 0, 0
self.recalib_counter, self.recalib_old_phi = 0, 0
self.ticks_encoder_left, self.ticks_encoder_left_old = 0, 0
self.ticks_encoder_right, self.ticks_encoder_right_old = 0, 0
self.x = np.zeros((2, 1))
self.P = np.eye(2)
self.K = np.zeros((2, 3))
self.I = np.eye(2)
self.H = np.array([[0, 1], [1, 0], [0, 1]])
self.save_alpha = np.zeros((80, 1))
self.save_timestamp = np.zeros((80, 1))
self.save_distance = np.zeros((80, 1))
# Publisher
self.pub_pose_est = rospy.Publisher(
"~fusion_lane_pose", LanePose,
queue_size=10) # for lane_controller_node
#self.pub_enc_est = rospy.Publisher("~encoder_pose", LanePose, queue_size=10) ## just for test
# Subscriber
self.sub_camera_pose = rospy.Subscriber("lane_filter_node/lane_pose",
LanePose,
self.SF,
queue_size=1)
self.sub_encoder_ticks = rospy.Subscriber(
"left_wheel_encoder_node/tick",
WheelEncoderStamped,
self.update_encoder_measurement_l,
queue_size=1)
self.sub_encoder_ticks = rospy.Subscriber(
"right_wheel_encoder_node/tick",
WheelEncoderStamped,
self.update_encoder_measurement_r,
queue_size=1)
self.sub_encoder_ticks = rospy.Subscriber(
"wheels_driver_node/wheels_cmd_executed",
WheelsCmdStamped,
self.save_wheelscmdstamped,
queue_size=1)
def setGains(self):
self.v_bar_gain_ref = 0.5
v_bar_fallback = 0.25 # nominal speed, 0.25m/s
self.v_max = 1
k_theta_fallback = -2.0
k_d_fallback = -(k_theta_fallback**2) / (4.0 * self.v_bar_gain_ref)
theta_thres_fallback = math.pi / 6.0
d_thres_fallback = math.fabs(
k_theta_fallback / k_d_fallback) * theta_thres_fallback
d_offset_fallback = 0.0
k_theta_fallback = k_theta_fallback
k_d_fallback = k_d_fallback
k_Id_fallback = 2.5
k_Iphi_fallback = 1.25
self.fsm_state = None
self.cross_track_err = 0
self.heading_err = 0
self.cross_track_integral = 0
self.heading_integral = 0
self.cross_track_integral_top_cutoff = 0.3
self.cross_track_integral_bottom_cutoff = -0.3
self.heading_integral_top_cutoff = 1.2
self.heading_integral_bottom_cutoff = -1.2
#-1.2
self.time_start_curve = 0
use_feedforward_part_fallback = False
self.wheels_cmd_executed = WheelsCmdStamped()
self.actuator_limits = Twist2DStamped()
self.actuator_limits.v = 999.0 # to make sure the limit is not hit before the message is received
self.actuator_limits.omega = 999.0 # to make sure the limit is not hit before the message is received
self.omega_max = 999.0 # considering radius limitation and actuator limits # to make sure the limit is not hit before the message is received
self.use_radius_limit_fallback = True
self.flag_dict = {
"obstacle_detected": False,
"parking_stop": False,
"fleet_planning_lane_following_override_active": False,
"implicit_coord_velocity_limit_active": False
}
self.pose_msg = LanePose()
self.pose_initialized = False
self.pose_msg_dict = dict()
self.v_ref_possible = dict()
self.main_pose_source = None
self.active = True
self.sleepMaintenance = False
# overwrites some of the above set default values (the ones that are already defined in the corresponding yaml-file (see launch-file of this node))
self.v_bar = self.setupParameter("~v_bar",
v_bar_fallback) # Linear velocity
self.k_d = self.setupParameter("~k_d", k_d_fallback) # P gain for d
self.k_theta = self.setupParameter(
"~k_theta", k_theta_fallback) # P gain for theta
self.d_thres = self.setupParameter(
"~d_thres", d_thres_fallback) # Cap for error in d
self.theta_thres = self.setupParameter(
"~theta_thres", theta_thres_fallback) # Maximum desire theta
self.d_offset = self.setupParameter(
"~d_offset",
d_offset_fallback) # a configurable offset from the lane position
self.k_Id = self.setupParameter(
"~k_Id", k_Id_fallback) # gain for integrator of d
self.k_Iphi = self.setupParameter(
"~k_Iphi",
k_Iphi_fallback) # gain for integrator of phi (phi = theta)
#TODO: Feedforward was not working, go away with this error source! (Julien)
self.use_feedforward_part = self.setupParameter(
"~use_feedforward_part", use_feedforward_part_fallback)
self.omega_ff = self.setupParameter("~omega_ff", 0)
self.omega_max = self.setupParameter("~omega_max", 999)
self.omega_min = self.setupParameter("~omega_min", -999)
self.use_radius_limit = self.setupParameter(
"~use_radius_limit", self.use_radius_limit_fallback)
self.min_radius = self.setupParameter("~min_rad", 0.0)
self.d_ref = self.setupParameter("~d_ref", 0)
self.phi_ref = self.setupParameter("~phi_ref", 0)
self.object_detected = self.setupParameter("~object_detected", 0)
self.v_ref_possible["default"] = self.v_max
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 setGains(self):
self.v_bar_gain_ref = 0.5
self.v_max = 1
v_bar_fallback = 0.25 # nominal speed, 0.25m/s
k_theta_fallback = -2.0
k_d_fallback = -(k_theta_fallback**2) / (4.0 * self.v_bar_gain_ref)
theta_thres_fallback = math.pi / 6.0
d_thres_fallback = math.fabs(
k_theta_fallback / k_d_fallback) * theta_thres_fallback
d_offset_fallback = 0.0
k_Id_fallback = 2.5
k_Iphi_fallback = 1.25
self.fsm_state = None
self.cross_track_err = 0
self.heading_err = 0
self.cross_track_integral = 0
self.heading_integral = 0
self.cross_track_integral_top_cutoff = 0.3
self.cross_track_integral_bottom_cutoff = -0.3
self.heading_integral_top_cutoff = 1.2
self.heading_integral_bottom_cutoff = -1.2
self.time_start_curve = 0
self.wheels_cmd_executed = WheelsCmdStamped()
self.actuator_limits = Twist2DStamped()
self.actuator_limits.v = 999.0 # to make sure the limit is not hit before the message is received
self.actuator_limits.omega = 999.0 # to make sure the limit is not hit before the message is received
self.omega_max = 999.0 # considering radius limitation and actuator limits
self.use_radius_limit_fallback = True
self.pose_msg = LanePose()
self.pose_initialized = False
self.pose_msg_dict = dict()
self.v_ref_possible = {
'default': self.v_max,
'main_pose': v_bar_fallback
}
self.main_pose_source = None
self.active = True
# overwrites some of the above set default values (the ones that are already defined in the corresponding yaml-file (see launch-file of this node))
# Linear velocity
self.v_bar = self.setupParameter("~v_bar", v_bar_fallback)
# P gain for d
self.k_d = self.setupParameter("~k_d", k_d_fallback)
# P gain for theta
self.k_theta = self.setupParameter("~k_theta", k_theta_fallback)
# Cap for error in d
self.d_thres = self.setupParameter("~d_thres", d_thres_fallback)
# Maximum desired theta
self.theta_thres = self.setupParameter("~theta_thres",
theta_thres_fallback)
# A configurable offset from the lane position
self.d_offset = self.setupParameter("~d_offset", d_offset_fallback)
# Gain for integrator of d
self.k_Id = self.setupParameter("~k_Id", k_Id_fallback)
# Gain for integrator of phi (phi = theta)
self.k_Iphi = self.setupParameter("~k_Iphi", k_Iphi_fallback)
# setup backward parameters
self.k_d_back = self.setupParameter("~k_d_back", 3.0)
self.k_theta_back = self.setupParameter("~k_theta_back", 1.0)
self.k_Id_back = self.setupParameter("~k_Id_back", 1.0)
self.k_Itheta_back = self.setupParameter("~k_Itheta_back", -1.0)
self.omega_ff = self.setupParameter("~omega_ff", 0)
self.omega_max = self.setupParameter("~omega_max", 4.7)
self.omega_min = self.setupParameter("~omega_min", -4.7)
self.use_radius_limit = self.setupParameter(
"~use_radius_limit", self.use_radius_limit_fallback)
self.min_radius = self.setupParameter("~min_rad", 0.0)
self.d_ref = self.setupParameter("~d_ref", 0)
self.phi_ref = self.setupParameter("~phi_ref", 0)
self.object_detected = self.setupParameter("~object_detected", 0)
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)
