def twist_from_frame(self, image, dt): # prepare image img_warped = imagefunctions.warp(image) hsv = cv2.cvtColor(img_warped, cv2.COLOR_BGR2HSV) ret, img_bin = cv2.threshold(hsv[:, :, 1], 100, 255, cv2.THRESH_BINARY) # pick points for interpolation #pts_x, pts_y = imagefunctions.pickpoints(img_bin) pts_x, pts_y = imagefunctions.pickpoints2(img_bin, self.minx - 10, self.miny - 10, self.maxx + 10, self.maxy + 10) # fit polynomial if (len(pts_x) > line_pixel_threshold and len(pts_y) > line_pixel_threshold): # update search region self.minx = min(pts_x) self.maxx = max(pts_x) self.miny = min(pts_y) self.maxy = max(pts_y) # fit linr z = np.polyfit(pts_y, pts_x, 1) p = np.poly1d(z) # generate plot coordinates ploty = [min(pts_y), max(pts_y)] plotx = p(ploty) pts = np.stack((plotx, ploty)) pts = np.transpose(pts) pts = pts.reshape((-1, 1, 2)) ptsplot = pts.astype(int) # plot line on image lines_img = cv2.cvtColor(img_bin, cv2.COLOR_GRAY2RGB) cv2.polylines(lines_img, [ptsplot], False, (0, 255, 0)) cv2.line(lines_img, (int(IMG_WIDTH / 2), IMG_HEIGHT - 1), (int(IMG_WIDTH / 2), int(IMG_HEIGHT / 2)), (0, 0, 255), 1) out_tile = np.hstack([img_warped, lines_img]) # publish robot's view try: imgmsg = self.bridge.cv2_to_imgmsg(out_tile, "bgr8") imgmsg.header.stamp = rospy.get_rostime() self.image_pub.publish(imgmsg) except CvBridgeError as e: print(e) # cross track error dist_to_line = p(IMG_HEIGHT) - ( IMG_WIDTH / 2) # +ve: line is to the right of car slope = z[0] # np.arctan2 ang_deviation = -slope # +ve: line deviates to right of car cte = wt_dist * dist_to_line + wt_ang * ang_deviation # Controllers throttle = MAX_THROTTLE_GAIN steering = self.steercontroller.step(cte, dt) # Twist Command vel = Twist() vel.linear.x = min(self.max_linear_vel, throttle) vel.angular.z = steering print 'dist=' + str(dist_to_line) + " ang=" + str( ang_deviation) + " => throttle=" + str( vel.linear.x) + ", steer=" + str( vel.angular.z) + ", state=" + str(self.drive_state) # update Twist Command self.my_twist_command = vel else: # publish robot's view # plot line on image lines_img = cv2.cvtColor(img_bin, cv2.COLOR_GRAY2RGB) cv2.line(lines_img, (int(IMG_WIDTH / 2), IMG_HEIGHT - 1), (int(IMG_WIDTH / 2), int(IMG_HEIGHT / 2)), (0, 0, 255), 1) out_tile = np.hstack([img_warped, lines_img]) # publish robot's view try: imgmsg = self.bridge.cv2_to_imgmsg(out_tile, "bgr8") imgmsg.header.stamp = rospy.get_rostime() self.image_pub.publish(imgmsg) except CvBridgeError as e: print(e) print 'xxxxxxxxxx LOST LINE xxxxxxxx'
def twist_from_frame(self, image, dt): # prepare image img_warped = imagefunctions.warp(image) hsv = cv2.cvtColor(img_warped, cv2.COLOR_BGR2HSV) ret, img_bin = cv2.threshold(hsv[:, :, 1], 100, 255, cv2.THRESH_BINARY) # pick points for interpolation #pts_x, pts_y = imagefunctions.pickpoints(img_bin) pts_x, pts_y = imagefunctions.pickpoints2(img_bin, self.minx - 10, self.miny - 10, self.maxx + 10, self.maxy + 10) # fit polynomial if (len(pts_x) > line_pixel_threshold and len(pts_y) > line_pixel_threshold): # update search region self.minx = min(pts_x) self.maxx = max(pts_x) self.miny = min(pts_y) self.maxy = max(pts_y) # fit linr z = np.polyfit(pts_y, pts_x, 1) p = np.poly1d(z) # generate plot coordinates ploty = [min(pts_y), max(pts_y)] plotx = p(ploty) pts = np.stack((plotx, ploty)) pts = np.transpose(pts) pts = pts.reshape((-1, 1, 2)) ptsplot = pts.astype(int) # plot line on image lines_img = cv2.cvtColor(img_bin, cv2.COLOR_GRAY2RGB) cv2.polylines(lines_img, [ptsplot], False, (0, 255, 0)) cv2.line(lines_img, (int(IMG_WIDTH / 2), IMG_HEIGHT - 1), (int(IMG_WIDTH / 2), int(IMG_HEIGHT / 2)), (0, 0, 255), 1) out_tile = np.hstack([img_warped, lines_img]) # publish robot's view try: imgmsg = self.bridge.cv2_to_imgmsg(out_tile, "bgr8") imgmsg.header.stamp = rospy.get_rostime() self.image_pub.publish(imgmsg) except CvBridgeError as e: print(e) ############################################################################ # TO-DO: # Design a meaningful value for cross track error (cte) such that: # +ve values means we should steer the car RIGHT to bring it back to the center. # Tips: # Calculate the car's distance to center, and # Heading direction deviation from the direction of the fitted line. # Use weights wt_dist and wt_ang defined in lines 27-28 to combine errors linearly. ############################################################################ cte = 0.0 # Controllers throttle = MAX_THROTTLE_GAIN steering = cte # Twist Command vel = Twist() vel.linear.x = min(self.max_linear_vel, throttle) vel.angular.z = steering print 'dist=' + str(dist_to_line) + " ang=" + str( ang_deviation) + " => throttle=" + str( vel.linear.x) + ", steer=" + str( vel.angular.z) + ", state=" + str(self.drive_state) # update Twist Command self.my_twist_command = vel else: # publish robot's view # plot line on image lines_img = cv2.cvtColor(img_bin, cv2.COLOR_GRAY2RGB) cv2.line(lines_img, (int(IMG_WIDTH / 2), IMG_HEIGHT - 1), (int(IMG_WIDTH / 2), int(IMG_HEIGHT / 2)), (0, 0, 255), 1) out_tile = np.hstack([img_warped, lines_img]) # publish robot's view try: imgmsg = self.bridge.cv2_to_imgmsg(out_tile, "bgr8") imgmsg.header.stamp = rospy.get_rostime() self.image_pub.publish(imgmsg) except CvBridgeError as e: print(e) print 'xxxxxxxxxx LOST LINE xxxxxxxx'