def callback(self, data):
target_angle = math.degrees(
np.arctan(data.expected_y / data.expected_x))
current_angle = math.degrees(data.current_angle)
print("Cardrive Target angle: ", target_angle, ", Current Angle: ",
current_angle)
self.projected_angle += target_angle
self.counter += 1
if self.counter < CONTROLLER_SKIP_RATE:
return
self.average_angle = self.projected_angle / self.counter
self.derivative = (self.average_angle - self.last_average)
self.last_average = self.derivative
#self.average_angle = target_angle
#self.derivative = (current_angle - target_angle) % 90
#print("Cardrive derivative", self.derivative)
control_variable = self.kp * self.average_angle + self.kd * self.derivative
self.projected_angle = 0
self.counter = 0
steering_command = steer.get_actuator_command(control_variable)
print("Cardrive derivative", self.derivative, ", Steering command: ",
steering_command)
self.pub_steering.publish(steering_command)
def callback(self, data):
lane_slope = data.slope
lane_intercept = data.intercept
image_width = data.width
image_height = data.height
# Camera image row along which detection happens
detection_row = image_height/2
# Point along the detection line that's used as an anchor
detection_point = image_width/2
# Image column where detected lane line actually crosses detection line
intersection_point = (detection_row - lane_intercept)/lane_slope
opp = detection_point-intersection_point
adj = image_height - detection_row
# Angle to the intersection point in degrees
# projected_direction = 90-math.degrees(math.atan2(opp, adj))
self.projected_direction += math.degrees(math.atan(float(opp)/adj))
self.counter += 1
if self.counter < CONTROLLER_SKIP_RATE:
return
averaged_direction = self.projected_direction/self.counter
self.counter = 0
self.projected_direction = 0
last_pd_error = self.pd_error
# When the car is positioned at the lane, an angle to the intersection approaches zero
# Thus, the target ideal direction is 0 degrees
self.pd_error = averaged_direction
self.derivative = self.pd_error - last_pd_error
control_variable = self.kp * self.pd_error + self.kd * self.derivative
steering_command = steer.get_actuator_command(control_variable)
print("Projected direction: {:.2f}, error: {:.2f}, derivative: {:.2f}, control var: {:.2f}, steering_command {}".format(
averaged_direction, self.pd_error, self.derivative, control_variable, steering_command))
info = ("Projected direction: {:.2f}, error: {:.2f}, derivative: {:.2f}, control var: {:.2f}, steering_command {}\n".format(
averaged_direction, self.pd_error, self.derivative, control_variable, steering_command))
with open('/home/oleksandra/Documents/catkin_ws_user/src/assignment7_line_detection_pd_control/src/pd_output.txt', 'a') as out:
out.write(info)
# Set only the wheel angle and use manual control publisher to start the car
self.pub_steering.publish(steering_command)
def callback(self, data):
angle_to_closest_line = min(map(angle_to_line, data.lines), key=abs)
self.projected_direction += angle_to_closest_line
self.counter += 1
if self.counter < CONTROLLER_SKIP_RATE:
return
averaged_direction = self.projected_direction / self.counter
self.counter = 0
self.projected_direction = 0
last_pd_error = self.pd_error
# When the car is positioned at the lane, an angle to the intersection approaches zero
# Thus, the target ideal direction is 0 degrees
self.pd_error = averaged_direction
self.derivative = self.pd_error - last_pd_error
control_variable = self.kp * self.pd_error + self.kd * self.derivative
steering_command = steer.get_actuator_command(control_variable)
print(
"Projected direction: {:.2f}, error: {:.2f}, derivative: {:.2f}, control var: {:.2f}, steering_command {}"
.format(averaged_direction, self.pd_error, self.derivative,
control_variable, steering_command))
info = (
"Projected direction: {:.2f}, error: {:.2f}, derivative: {:.2f}, control var: {:.2f}, steering_command {}\n"
.format(averaged_direction, self.pd_error, self.derivative,
control_variable, steering_command))
with open(
'/home/oleksandra/Documents/catkin_ws_user/src/assignment12_time_and_precision/src/pd_output.txt',
'a') as out:
out.write(info)
# Set only the wheel angle and use manual control publisher to start the car
self.pub_steering.publish(steering_command)
def callback(self, lines):
guide_line = lines.lines[2]
detection_row = guide_line.height / 2
detection_point = guide_line.width / 2
intersection_point = (detection_row -
guide_line.intercept) / guide_line.slope
opp = detection_point - intersection_point
adj = guide_line.height - detection_row
self.projected_direction += math.degrees(math.atan(float(opp) / adj))
self.counter += 1
if self.counter < self.CONTROLLER_SKIP_RATE:
return
averaged_direction = self.projected_direction / self.counter
self.counter = 0
self.projected_direction = 0
last_pd_error = self.pid_error
self.pid_error = averaged_direction
self.integral = self.integral + self.pid_error
self.derivative = self.pid_error - last_pd_error
control_variable = self.kp * self.pid_error + self.ki * self.integral + self.kd * self.derivative
steering_command = steer.get_actuator_command(control_variable)
msgDrive = MsgDrive()
msgDrive.angle = steering_command
if steering_command > 75 and steering_command < 105:
msgDrive.speed = self.max_speed
else:
msgDrive.speed = self.min_speed
self.pub_drive.publish(msgDrive)
def callback(self, data):
# Old approach:
# Folow the one line
#angle_to_closest_line = min(map(angle_to_line, data.lines), key=abs)
#=============================
# New approach:
# Drive between two lines: side line and dashed line to stay inside the road
line1 = data.line_set[0]
line1 = LaneDetection.end_start_points(line1.slope, line1.intercept,
line1.width)
line2 = data.line_set[1]
line2 = LaneDetection.end_start_points(line2.slope, line2.intercept,
line2.width)
van_point_x, van_point_y = LaneDetection.vanishing_point(line1, line2)
# guide line
center_point_x = abs((line2[1][0] - line1[1][0])) / 2 + line1[1][0]
guide_line = Line()
guide_line.slope = LaneDetection.get_slope(center_point_x,
data.line_set[0].height,
van_point_x, van_point_y)
guide_line.intercept = LaneDetection.get_intercept(
van_point_x, van_point_y, guide_line.slope)
guide_line.height = data.line_set[0].height
guide_line.width = data.line_set[0].width
detection_row = guide_line.height - van_point_y
angle_to_guide_line = get_angle_to_guide_line(guide_line,
detection_row)
self.projected_direction += angle_to_guide_line
self.counter += 1
if self.counter < CONTROLLER_SKIP_RATE:
return
averaged_direction = self.projected_direction / self.counter
self.counter = 0
self.projected_direction = 0
last_pd_error = self.pd_error
# When the car is positioned at the lane, an angle to the intersection approaches zero
# Thus, the target ideal direction is 0 degrees
self.pd_error = averaged_direction
self.derivative = self.pd_error - last_pd_error
control_variable = self.kp * self.pd_error + self.kd * self.derivative
steering_command = steer.get_actuator_command(control_variable)
print(
"Projected direction: {:.2f}, error: {:.2f}, derivative: {:.2f}, control var: {:.2f}, steering_command {}"
.format(averaged_direction, self.pd_error, self.derivative,
control_variable, steering_command))
info = (
"Projected direction: {:.2f}, error: {:.2f}, derivative: {:.2f}, control var: {:.2f}, steering_command {}\n"
.format(averaged_direction, self.pd_error, self.derivative,
control_variable, steering_command))
with open(
'/home/oleksandra/Documents/catkin_ws_user/src/assignment12_time_and_precision/src/pd_output.txt',
'a') as out:
out.write(info)
# Set only the wheel angle and use manual control publisher to start the car
self.pub_steering.publish(steering_command)
def lane_switch_callback(self, car_position):
#if lane == 'stop':
# self.speed_pub.publish(0)
#car_position = rospy.wait_for_message("/expected_coordinates", Coordinate)
# print("car_drive: &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&")
# print("car_drive: car_position:", car_position.current_x, car_position.current_y)
# print("car_drive: car_position_lane:", car_position.inner_x, car_position.inner_y)
# print("car_drive: car_lane_proj:", car_position.inner_proj_x, car_position.inner_proj_y)
# print("car_drive: lane: ", lane.data)
if car_position.lane == 'inner':
proj_point = utils.perpendicular_point(
car_position.inner_proj_x * 100,
car_position.inner_proj_y * 100,
car_position.inner_x * 100,
car_position.inner_y * 100,
self.dist_to_projection
)
#print("car_drive: inner: proj_point:", proj_point)
target_point_lane, distance = utils.get_closest_point(
proj_point[0],
proj_point[1],
self.inner_track
)
#print("car_drive: inner: target_point_lane:", target_point_lane)
target_point = utils.get_point_to_distance(
self.inner_track,
(proj_point[0], proj_point[1]),
target_point_lane,
self.dist_from_lane
)
else:
proj_point = utils.perpendicular_point(
car_position.outer_proj_x * 100,
car_position.outer_proj_y * 100,
car_position.outer_x * 100,
car_position.outer_y * 100,
self.dist_to_projection
)
#print("car_drive: outer: proj_point:", proj_point)
target_point_lane, distance = utils.get_closest_point(
proj_point[0],
proj_point[1],
self.inner_track
)
#print("car_drive: outer: target_point_lane:", proj_point)
target_point = utils.get_point_to_distance(
self.inner_track,
(proj_point[0], proj_point[1]),
target_point_lane,
-self.dist_from_lane
)
#print("car_drive: target_point: ", target_point)
adj = np.linalg.norm(
np.array((car_position.current_y, car_position.current_x)) - np.array((target_point[0], target_point[1]))
)
#Logging the position sent by the camera (current_x, current_y) and the calculated position (calc_x, calc_y)
info = ("{:.2f}, {:.2f}, {:.2f}, {:.2f}\n".format(
target_point[0],
target_point[1],
target_point_lane[0],
target_point_lane[1],
))
with open('/home/adripinto/catkin_ws_user/src/last_assignment/src/log.txt', 'a') as out:
out.write(info)
###############################################################################################################
target_point = np.array(target_point)
car_point = np.array((car_position.current_y * 100, car_position.current_x * 100))
middle_point = target_point - car_point
unit_point = np.array((0, 1))
# print(closest_point_vector)
# print((closest_point_vector))
# print(vector)
closest_angle = np.arccos(
np.dot(unit_point, middle_point) / (np.linalg.norm(unit_point) * np.linalg.norm(middle_point))
)
print("car_drive: ###################################################")
print("car_drive: car_position: current_angle:", np.rad2deg(car_position.current_angle))
print("car_drive: closest_angle:", np.rad2deg(closest_angle))
print("car_drive: middle_point:", middle_point)
# # print(yaw,closest_point_error)
if middle_point[0] >= 0:
closest_point_error = closest_angle * -1# / np.pi
theta = closest_point_error + car_position.current_angle
else:
closest_point_error = closest_angle
theta = closest_point_error - car_position.current_angle
if theta > np.pi:
theta -= 2 * np.pi
elif theta < -1 * np.pi:
theta += 2 * np.pi
print("car_drive: closest_point_error:", closest_point_error)
print("car_drive: thetha:", np.rad2deg(theta))
self.previous_angle = theta
steering_angle = self.kp * theta + self.kd * (theta - self.previous_angle)
steering_angle = np.rad2deg(steering_angle) + self.val # / 180
if steering_angle > 180:
steering_angle -= 180
if steering_angle > 270:
steering_angle -= 270
print("car_drive: steering_angle:", steering_angle)
self.all_angles.append(steering_angle)
# print(steering_angle)
self.counter += 1
if self.counter >= CONTROLLER_SKIP_RATE:
mean_error = np.median(self.all_angles)
current_angle = mean_error
print("car_drive: mean_error:", current_angle)
if current_angle > 180:
current_angle = 180
if current_angle < 0:
current_angle = 0
self.counter = 0
self.all_angles = []
print("car_drive: current_angle:", current_angle)
steering_command = steer.get_actuator_command(current_angle)
print("car_drive: steering_command:", steering_command)
self.pub_steering.publish(current_angle)