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'