def leave_from_parking(self, howparked, path):
#if howparked =="forward_parking":
if howparked == "backward_parking":
data = self.scan
filtered_ranges, filtered_angles = [], []
ranges = data.ranges
angles = arange(data.angle_min,
data.angle_max + data.angle_increment,
data.angle_increment)
for i in range(len(ranges)):
if ranges[i] < 12:
filtered_ranges.append(ranges[i])
filtered_angles.append(angles[i])
idx_minangles = argmin(abs(array(filtered_angles)))
backward_dist = filtered_ranges[idx_minangles]
xr, yr, heading = self.__find_current_position()
goal_pos_x = xr - (backward_dist - 0.18) * cos(heading)
goal_pos_y = yr - (backward_dist - 0.18) * sin(heading)
self.path = adjustable_path_points("linear", (xr, yr),
goal=(goal_pos_x, goal_pos_y))
self.change_to_reversed()
self.__pure_pursuit()
rospy.sleep(1.0)
path.reverse()
self.path = path
self.ld = 0.35
self.change_to_forward()
self.__pure_pursuit()
def __init__(self):
self.path = adjustable_path_points('parking', (1.5, 1.5), (0, 0), heading = pi/2)
self.Estop = 0
self.car_heading = 0
# Subscribe to the topics
self.car_pose_sub = rospy.Subscriber("SVEA1/odom", Odometry, self.car_pose_cb)
self.lidar_sub = rospy.Subscriber("/scan", LaserScan, self.lidar_cb)
self.reversed = False
self.has_parking_spot = False
self.preparing_to_park = False
self.going_backwards = False
self.parking_identified = 0
self.parking_lot_start = [0, 0]
self.parking_lot_dist = 0
self.Atan_start = []
self.current_start_distance = 0.2 # Distance to first obstacle
self.pp_range = None
self.pp_angle = None
self.pp_coner = None
self.pp_heading = 0
rospy.loginfo(self.car_pose_sub)
# init Publisher
self.car_control_pub = rospy.Publisher("lli/ctrl_request", lli_ctrl_request, queue_size=10)
# goal = self.path[0]
self.ld = 0.32
self.xs = []
self.ys = []
self.__follow_then_park()
def parallell_parking_start(self):
#Car positioning
self.pp_heading = self.current_heading
parallell_distance = 0.5 # Distance in the car's direction between corner and starting point
outward_distance = 0.3 # Same, but to the left
xg = self.pp_corner[0] + (parallell_distance * cos(
self.current_heading)) - (outward_distance *
sin(self.current_heading))
yg = self.pp_corner[1] + (parallell_distance * sin(
self.current_heading)) + (outward_distance *
cos(self.current_heading))
print("GoalPos: " + str((xg, yg)))
xr, yr, __ = self.__find_current_position()
print("CarPos: ", str((xr, yr)))
start_path = adjustable_path_points("linear", (xg, yg), (xg, yg))
self.path = start_path
#Arctan start point
Atan_parallell_distance = 0.29 # Distance in the car's direction between corner and starting point
Atan_outward_distance = 0.4 # Same, but to the left
Atan_x = self.pp_corner[0] + (Atan_parallell_distance * cos(
self.current_heading)) - (Atan_outward_distance *
sin(self.current_heading))
Atan_y = self.pp_corner[1] + (Atan_parallell_distance * sin(
self.current_heading)) + (Atan_outward_distance *
cos(self.current_heading))
self.Atan_start = [Atan_x, Atan_y]
self.preparing_to_park = True
def parallell_parking_start(self, angle, range):
parallell_distance = 0.25 # Distance in the car's direction between corner and starting point
outward_distance = 0.4 # Same, but to the left
parallell_distance_to_travel = parallell_distance - cos(angle) * range
outward_distance_to_travel = outward_distance - sin(angle) * range
# Rotation into global frame
x_distance_to_travel = cos(self.current_heading) * parallell_distance_to_travel - \
sin(self.current_heading) * outward_distance_to_travel
y_distance_to_travel = sin(self.current_heading) * parallell_distance_to_travel + \
cos(self.current_heading) * outward_distance_to_travel
xr, yr = self.car_pose.pose.pose.position.x, self.car_pose.pose.pose.position.y
xg, yg = xr + x_distance_to_travel, yr + y_distance_to_travel
start_path = adjustable_path_points("linear", (xr, yr), (xg, yg))
self.path = start_path
parallell_distance_to_goal = -0.45 - cos(
angle) * range # Heavily subject to change
outward_distance_to_goal = -0.08 - sin(angle) * range
x_distance_to_goal = cos(self.current_heading) * parallell_distance_to_goal - \
sin(self.current_heading) * outward_distance_to_goal
y_distance_to_goal = sin(self.current_heading) * parallell_distance_to_goal + \
cos(self.current_heading) * outward_distance_to_goal
xp, yp = xr + x_distance_to_goal, yr + y_distance_to_goal
self.pp_goal = (xp, yp)
self.mgoal = ()
def __init__(self):
self.path = adjustable_path_points('parking', (1.5, 1.5), (0, 0),
heading=pi / 2)
self.Estop = 0
self.car_heading = 0
# Subscribe to the topics
self.car_pose_sub = rospy.Subscriber("SVEA1/odom", Odometry,
self.car_pose_cb)
self.lidar_sub = rospy.Subscriber("/scan", LaserScan, self.lidar_cb)
self.reversed = False
self.has_parking_spot = False
self.parking_identified = 0
self.parking_lot_start = [0, 0]
self.parking_lot_dist = 0
self.pp_goal = [0, 0]
self.mgoal = [0, 0]
self.obs_list = []
self.pp_range = None
self.pp_angle = None
rospy.loginfo(self.car_pose_sub)
# init Publisher
self.car_control_pub = rospy.Publisher("lli/ctrl_request",
lli_ctrl_request,
queue_size=10)
# goal = self.path[0]
self.ld = 0.32
self.xs = []
self.ys = []
self.change_to_reversed()
self.has_parking_spot = True
self.__pure_pursuit()
def leave_from_parking(self, howparked):
#preprocess the scan data, take away the inf datas
self.doing_forward_parking = False
self.has_parking_spot = True
data = self.scan
filtered_ranges, filtered_angles = [], []
ranges = data.ranges
angles = arange(data.angle_min, data.angle_max + data.angle_increment,
data.angle_increment)
for i in range(len(ranges)):
if ranges[i] < 12:
filtered_ranges.append(ranges[i])
filtered_angles.append(angles[i])
xr, yr, heading = self.__find_current_position(reversed=False)
idx_minangles = argmin(
abs(array(filtered_angles) + (heading - self.pp_heading)))
backward_dist = filtered_ranges[idx_minangles]
goal_pos_x = xr - (backward_dist - 0.18) * cos(self.pp_heading)
goal_pos_y = yr - (backward_dist - 0.18) * sin(self.pp_heading)
self.path = adjustable_path_points("linear", (xr, yr),
goal=(goal_pos_x, goal_pos_y))
print("I am here 3")
self.change_to_reversed()
self.__pure_pursuit()
print("I am here 4")
rospy.sleep(1.0)
if howparked == "forward_parking":
self.path = adjustable_path_points("parking",
(goal_pos_x, goal_pos_y),
heading=self.pp_heading + pi)
if howparked == "backward_parking":
print("i am here 5")
print(self.pp_heading)
self.path = adjustable_path_points("parking",
(goal_pos_x, goal_pos_y),
heading=self.pp_heading + pi)
self.ld = 0.32
self.change_to_forward()
self.__pure_pursuit()
def change_lane(self, parallell_distance, outward_distance):
initial_p_dist = 0.2
xr, yr, __ = self.__find_current_position()
heading = arctan2(self.path[1][1]-self.path[0][1], self.path[1][0] - self.path[0][0])
x_init = initial_p_dist * cos(heading) - outward_distance * sin(heading)
y_init = initial_p_dist * sin(heading) + outward_distance * cos(heading)
x_distance = parallell_distance * cos(heading) - outward_distance * sin(heading)
y_distance = parallell_distance * sin(heading) + outward_distance * cos(heading)
x_start, y_start = xr + x_init, yr + y_init
x_goal, y_goal = xr + x_distance, yr + y_distance
self.path = adjustable_path_points("linear", (x_start, y_start), (x_goal, y_goal))
def forward_parking(self):
outward_distance = 0.3
parallel_distance = -0.4
heading = arctan2(self.path[1][1] - self.path[0][1], self.path[1][0] - self.path[0][0])
start_x = parallel_distance * cos(heading) - outward_distance * sin(heading) + self.fp_corner[0]
start_y = parallel_distance * sin(heading) + outward_distance * cos(heading) + self.fp_corner[1]
print("Start position: ", (start_x, start_y))
self.path = adjustable_path_points("parking_forward", (start_x, start_y), heading=heading)
self.has_parking_spot = False
self.pp_heading = heading
self.parked = True
def parallell_parking_backwards(self):
self.preparing_to_park = False
self.going_backwards = True
rospy.sleep(1.0)
xr, yr = self.car_pose.pose.pose.position.x, self.car_pose.pose.pose.position.y
print("Planning path...")
savetxt("/home/nvidia/catkin_ws/obs_without.csv", self.obs_list, delimiter=",")
self.path = adjustable_path_points("parking", self.Atan_start, heading = self.pp_heading)
print("Building path...")
#self.path = steerings
self.ld = 0.32
self.change_to_reversed()
self.__pure_pursuit()
def parallell_parking_backwards(self):
# Readjust states
self.preparing_to_park = False
self.going_backwards = True
rospy.sleep(1.0)
# Create and save path
self.path = adjustable_path_points("parking", self.Atan_start, heading=self.pp_heading)
self.pp_path = copy(self.path) # Saves for use when going forwards
# Prepare for going backwards
self.ld = 0.35
self.change_to_reversed()
# Going backwards
self.__pure_pursuit()
self.parked = False
def parallell_parking_forwards(self):
# Changing states
self.going_backwards = False
self.going_forwards = True
rospy.sleep(1.0)
# Finds goal position
xr, yr, __ = self.__find_current_position()
outward_distance = -0.15
parallell_distance = -0.2
x_goal = self.pp_corner[0] + parallell_distance * cos(self.pp_heading) - outward_distance * sin(self.pp_heading)
y_goal = self.pp_corner[1] + parallell_distance * sin(self.pp_heading) + outward_distance * cos(self.pp_heading)
# Finds path to goal position
self.path = adjustable_path_points("linear", (xr, yr), goal=(x_goal, y_goal))
# Prepares
self.ld = 0.25
self.change_to_forward()
# Goes forward
self.__pure_pursuit()
def parallell_parking_forwards(self):
self.going_backwards = False
self.going_forwards = True
rospy.sleep(1.0)
xr, yr, __ = self.__find_current_position()
outward_distance = -0.15
parallell_distance = -0.2
x_goal = self.pp_corner[0] + parallell_distance * cos(
self.pp_heading) - outward_distance * sin(self.pp_heading)
y_goal = self.pp_corner[1] + parallell_distance * sin(
self.pp_heading) + outward_distance * cos(self.pp_heading)
self.path = adjustable_path_points("linear", (xr, yr),
goal=(x_goal, y_goal))
print("Building path...")
# self.path = steerings
self.ld = 0.25
self.change_to_forward()
self.__pure_pursuit()
def __init__(self):
self.path = adjustable_path_points('parking', (1.5, 1.5), (0, 0), heading = pi/2)
self.Estop = 0
self.car_heading = 0
# Subscribe to the MOCAP and lidar topics
self.car_pose_sub = rospy.Subscriber("SVEA1/odom", Odometry, self.car_pose_cb)
self.lidar_sub = rospy.Subscriber("/scan", LaserScan, self.lidar_cb)
self.reversed = False
# States of parking
self.has_parking_spot = False
self.preparing_to_park = False
self.going_backwards = False
self.going_forwards = False
self.doing_forward_parking = False
self.parking_identified = -1
self.parked = False
self.scan = None
self.pp_path = None
self.found_path = False
self.tried = False
# Parameters of parker
self.parking_lot_start = [0, 0]
self.parking_lot_dist = 0
self.Atan_start = []
self.current_start_distance = 0.2 # Distance to first obstacle
self.pp_range = None
self.pp_angle = None
self.pp_corner = None
self.pp_heading = 0
self.fp_corner = (0, 0)
rospy.loginfo(self.car_pose_sub)
# init Publisher to hardware
self.car_control_pub = rospy.Publisher("lli/ctrl_request", lli_ctrl_request, queue_size=10)
self.ld = 0.32 # Lookahead distance
self.xs = []
self.ys = []
self.__follow_then_park()
