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()