class WallFollower: # Import ROS parameters from the "params.yaml" file. # Access these variables in class functions with self: # i.e. self.CONSTANT SCAN_TOPIC = "/scan" DRIVE_TOPIC = "cmd_vel" SIDE = -1 # -1 right is and +1 is left VELOCITY = 1.6 DESIRED_DISTANCE = 0.5 def __init__(self): # Create a node that # Subscribes to the laser scan topic, # Publishes to drive topic - to move the vehicle. # Initialize subscriber to laser scan. rospy.Subscriber(self.SCAN_TOPIC, LaserScan, self.LaserCb) # Initialize a publisher of drive commands. self.drive_pub = rospy.Publisher(self.DRIVE_TOPIC, Twist, queue_size=10) # Variables to keep track of drive commands being sent to robot. self.seq_id = 0 # Class variables for following. self.side_angle_window_fwd_ = math.pi * 0.1 self.side_angle_window_bwd_ = math.pi - math.pi * 0.3 self.point_buffer_x_ = np.array([]) self.point_buffer_y_ = np.array([]) self.num_readings_in_buffer_ = 0 self.num_readings_for_fit_ = 2 self.reject_dist = 0.7 self.steer_cmd = 0 self.vel_cmd = self.VELOCITY self.pid = PID() def GetLocalSideWallCoords(self, ranges, angle_min, angle_max, angle_step): # Slice out the interesting samples from our scan. pi/2 radians from pi/4 to (pi - pi/4) radians for the right side. positive_start_angle = self.side_angle_window_fwd_ positive_end_angle = self.side_angle_window_bwd_ if self.SIDE == -1: #"right": start_angle = -positive_start_angle end_angle = -positive_end_angle elif self.SIDE == 1: #"left": start_angle = positive_start_angle end_angle = positive_end_angle start_ix_ = int((-angle_min + start_angle) / angle_step) end_ix_ = int((-angle_min + end_angle) / angle_step) start_ix = min(start_ix_, end_ix_) end_ix = max(start_ix_, end_ix_) side_ranges = ranges[min(start_ix, end_ix):max(start_ix, end_ix)] x_values = np.array([ ranges[i] * math.cos(angle_min + i * angle_step) if i < len(ranges) else ranges[(i - len(ranges))] * math.cos(angle_min + (i - len(ranges)) * angle_step) for i in range(start_ix, end_ix) ]) y_values = np.array([ ranges[i] * math.sin(angle_min + i * angle_step) if i < len(ranges) else ranges[i - len(ranges)] * math.sin(angle_min + (i - len(ranges)) * angle_step) for i in range(start_ix, end_ix) ]) # Check that the values for the points are within 1 meter from each other. Discard any point that is not within one meter form the one before it. out_x = [] out_y = [] for ix in range(0, len(x_values)): new_point = (x_values[ix], y_values[ix]) # This conditional handles points with infinite value. if side_ranges[ix] < 2.5 and side_ranges[ix] > 0 and abs( new_point[1]) < 7000 and abs(new_point[0]) < 7000: out_x.append(new_point[0]) out_y.append(new_point[1]) return np.array(out_x), np.array(out_y) def LaserCb(self, scan_data): # This function is called every time we get a laser scan. # This is the plan: # * Get scan data. # * Convert it to x,y coordinates in the local frame of the robot. # * Find a least squares - best fit line through those points with numpy. # Consider using data from multiple scans in one least-squares fit cycle. # This is a line equation, with respect to the car at (0,0), with the x axis being the heading. # Get vector theta for the line, and theta_0 as the y intersection. # * Find the distance from the line to the origin with ( theta_T dot [[0],[0]] + theta_0 ) / (norm theta) # TLDR, We have a vector theta for the line we have found, and a distance to that wall. # TODO(yorai): Handle erroneous scan values. If one is too big, or too small, use past value. # Do not do this for too many in a row, maybe just throw scan away if too many corrections. angle_step = scan_data.angle_increment angle_min = scan_data.angle_min angle_max = scan_data.angle_max ranges = scan_data.ranges # Get data for side ranges. Add to buffer. wall_coords_local = self.GetLocalSideWallCoords( ranges, angle_min, angle_max, angle_step) ####### #Find mean and throw out everything that is 1 meter away from mean distance, no outliers. # If one differs by more than a meter from the previous one, gets thrown out from both x and y. Distnaces as we go along vector of points. # but print the things first. ####### self.point_buffer_x_ = np.append(self.point_buffer_x_, wall_coords_local[0]) self.point_buffer_y_ = np.append(self.point_buffer_y_, wall_coords_local[1]) self.num_readings_in_buffer_ += 1 # If we have enough data, then find line of best fit. if self.num_readings_in_buffer_ >= self.num_readings_for_fit_: # Find line of best fit. # self.point_buffer_x_ = np.array([0, 1, 2, 3]) # self.point_buffer_y_ = np.array([-1, 0.2, 0.9, 2.1]) A = np.vstack( [self.point_buffer_x_, np.ones(len(self.point_buffer_x_))]).T m, c = np.linalg.lstsq(A, self.point_buffer_y_, rcond=0.001)[0] # Find angle from heading to wall. # Vector of wall. Call wall direction vector theta. th = np.array([[m], [1]]) th /= np.linalg.norm(th) # Scalar to define the (hyper) plane th_0 = c # Distance to wall is (th.T dot x_0 + th_0)/(norm(th)) dist_to_wall = abs(c / np.linalg.norm(th)) # Angle between heading and wall. angle_to_wall = math.atan2(m, 1) # Clear scan buffers. self.point_buffer_x_ = np.array([]) self.point_buffer_y_ = np.array([]) self.num_readings_in_buffer_ = 0 # Simple Proportional controller. # Feeding the current angle ERROR(with target 0), and the distance ERROR to wall. Desired error to be 0. print("ANGLE", angle_to_wall, "DIST", dist_to_wall) steer = self.pid.GetControl(0.0 - angle_to_wall, self.DESIRED_DISTANCE - dist_to_wall, self.SIDE) # Publish control to /drive topic. drive_msg = Twist() # drive_msg.header.seq = self.seq_id # self.seq_id += 1 # Populate the command itself. drive_msg.linear.x = self.VELOCITY drive_msg.angular.z = steer # drive_msg.drive.steering_angle = steer # drive_msg.drive.steering_angle_velocity = 0.1 # drive_msg.drive.speed = self.VELOCITY # drive_msg.drive.acceleration = 1 # drive_msg.drive.acceleration = 0.5 self.drive_pub.publish(drive_msg)