Udacity机器人软件工程师课程笔记（五）-样本搜索和找回-基于漫游者号模拟器-自主驾驶
9.自主驾驶
在接下来的环节中，我们要实现漫游者号的自动驾驶功能。
完成这个功能我们需要四个程序，第一个为感知程序，其对摄像头输入的图片进行变换处理和坐标变换使用。第二个程序为决策程序，功能是帮助漫游者号根据当前条件和状态进行相应的决策，以实现漫游者号前进，后退，转向等功能。第三个是支持程序，来定义一些关于漫游者号状态的类等。最后为主程序，来调用三个函数对漫游者号进行控制的。
Udacity提供的初始程序在Udacity机器人软件工程师课程笔记（三）中有所提供。
经过前面的学习，我们已经清楚了感知程序和决策程序，因为之前我都把它写成了一个程序，这样导致程序显得非常复杂，可读性不是太高。现在针对各个功能程序进行程序的划分，来方便最后的主程序的调用。以下的初始程序都放在RoboND-Rover-Project文件夹下的code文件夹中。
（1） 感知部分perception.py
首先是感知部分。感知部分perception.py应当具有三个功能，
- 图像阈值处理
- 透视变换
- 坐标变换
这些功能在之前的程序中已经多次使用了，但是我们需要调用这些功能去编写一个综合感知函数 perception_step() 来对图像进行处理，它需要完成以下功能：
1）定义透视变换的源点和目标点
2）应用透视变换
3）应用颜色阈值来识别可导航的地形/障碍物/岩石样本
4）更新Rover.vision_image（这个图像显示在屏幕的左侧）
5）将地图图像像素值转换为以漫游者号为中心的坐标
6）将以漫游者号为中心的像素值坐标转换为世界坐标
7）更新漫游者号的世界地图（这个图像显示在屏幕右侧）
8）将以漫游者号为中心的像素位置转换为极坐标
感知部分的程序如下
import numpy as np
import cv2
import matplotlib.pyplot as plt##图像处理
# 定义二值化图像函数
def color_thresh(img, rgb_thresh=(160, 160, 160)):img_thresh = np.zeros_like(img[:, :, 0])above_thresh = (img[:, :, 0] > rgb_thresh[0]) & (img[:, :, 1] > rgb_thresh[1]) & (img[:, :, 2] > rgb_thresh[2])img_thresh[above_thresh] = 255return img_thresh##透视变换
# 定义图像映射函数，将摄像头的图像映射到平面坐标中去
def perspect_transform(img, src, dst):M = cv2.getPerspectiveTransform(src, dst) # 定义变换矩阵img_perspect = cv2.warpPerspective(img, M, (img.shape[1], img.shape[0]))return img_perspect##坐标变换
# 定义从图像坐标转换成漫游者号坐标函数
def rover_coords(binary_img):ypos, xpos = binary_img.nonzero()x_pixel = -(ypos - binary_img.shape[0]).astype(np.float)y_pixel = -(xpos - binary_img.shape[1]/2 ).astype(np.float)return x_pixel, y_pixel# 定义旋转操作函数
def rotate_pix(xpix, ypix, yaw):yaw_rad = yaw * np.pi / 180xpix_rotated = (xpix * np.cos(yaw_rad)) - (ypix * np.sin(yaw_rad))ypix_rotated = (xpix * np.sin(yaw_rad)) + (ypix * np.cos(yaw_rad))return xpix_rotated, ypix_rotated# 定义平移操作函数
def translate_pix(xpix_rot, ypix_rot, xpos, ypos, scale):xpix_translated = (xpix_rot / scale) + xposypix_translated = (ypix_rot / scale) + yposreturn xpix_translated, ypix_translated# 定义综合函数，将旋转和平移函数进行结合，并限制了图像范围
def pix_to_world(xpix, ypix, xpos, ypos, yaw, world_size, scale):xpix_rot, ypix_rot = rotate_pix(xpix, ypix, yaw)xpix_tran, ypix_tran = translate_pix(xpix_rot, ypix_rot, xpos, ypos, scale)x_pix_world = np.clip(np.int_(xpix_tran), 0, world_size - 1)y_pix_world = np.clip(np.int_(ypix_tran), 0, world_size - 1)return x_pix_world, y_pix_world# 定义转换为极坐标函数
def to_polar_coords(xpix, ypix):dist = np.sqrt(xpix**2 + ypix ** 2)angles = np.arctan2(ypix, xpix)return dist, anglesdef perception_step(Rover):# (1)定义透视变换的原点和目标点# 参考图像filename = 'E:/2019/RoboND-Rover-Project-master/calibration_images/example_grid1.jpg'image = cv2.imread(filename)dst_size = 5bottom_offset = 0src = np.float32([[14, 140], [301, 140], [200, 96], [118, 96]])dst = np.float32([[image.shape[1] / 2 - dst_size, image.shape[0] - bottom_offset],[image.shape[1] / 2 + dst_size, image.shape[0] - bottom_offset],[image.shape[1] / 2 + dst_size, image.shape[0] - 2 * dst_size - bottom_offset],[image.shape[1] / 2 - dst_size, image.shape[0] - 2 * dst_size - bottom_offset],])# （2）应用透视变换img_perspect = perspect_transform(Rover.img, src, dst)# (3)应用颜色阈值来识别可导航的地形/障碍物/岩石样本img_thresh_ter = color_thresh(img_perspect, rgb_thresh=(160, 160, 160))img_thresh_obs = color_thresh(img_perspect, rgb_thresh=(160, 160, 160))img_thresh_roc = color_thresh(img_perspect, rgb_thresh=(160, 130, 0))# (4)更新ROver.vision_imageRover.vision_image[:, :, 0] = img_thresh_rocRover.vision_image[:, :, 1] = img_thresh_obsRover.vision_image[:, :, 2] = img_thresh_ter# (5)将地图像素值转换为以漫游者号为中心的坐标xpix, ypix = rover_coords(img_thresh_ter)# (6)将以漫游者号为中心的像素值坐标转化为世界地图坐标x_pix_world, y_pix_world = pix_to_world(xpix, ypix, xpos=Rover.pos[0], ypos=Rover.pos[1], yaw=Rover.yaw, world_size=200, scale=10)# (7)更新漫游者号的世界地图Rover.worldmap[x_pix_world, y_pix_world] += 1# (8)将以漫游者号为中心的像素位置转换为极坐标rover_distances, rover_angles = to_polar_coords(xpix, ypix)# 可导航地形像素的角度的数组Rover.nav_angles = rover_angles# 获取转向角，使用极坐标系角度的平均值，这样可以运行，但是会有意外情况avg_angle_degrees = np.mean((rover_angles ) * 180 / np.pi)Rover.steer= np.clip(avg_angle_degrees, -15, 15)# 获取油门速度，使用平均极坐标系的平均值Rover.throttle = np.mean(rover_distances)print('steer:', Rover.steer, 'Rover.throttle:', Rover.throttle)return Rover
（2） 决策部分decision.py
使用决策树逻辑编写的简单判别系统，控制漫游者号对目前的状态和环境做出相应的判断，并执行相应的加速，转向，停止等操作。
import numpy as np# 命令基于perception_step（）函数的输出
def decision_step(Rover):if Rover.nav_angles is not None:# 检查 Rover.mode 状态if Rover.mode == 'forward':# 检查可导航地形的范围if len(Rover.nav_angles) >= Rover.stop_forward: # 如果为forward模式，可导航地图是好的，速度如果低于最大值，则加速if Rover.vel < Rover.max_vel:# 设置油门值Rover.throttle = Rover.throttle_setelse:Rover.throttle = 0Rover.brake = 0# 将转向设置为平均夹角，范围为 +/- 15Rover.steer = np.clip(np.mean(Rover.nav_angles * 180/np.pi), -15, 15)# 如果缺少地形图，则设置为停止模式elif len(Rover.nav_angles) < Rover.stop_forward:# 设置到停止模式并刹车Rover.throttle = 0# 设置制动值Rover.brake = Rover.brake_setRover.steer = 0Rover.mode = 'stop'# 如果我们已经处于“停止”模式，那么做出不同的决定elif Rover.mode == 'stop':# 如果我们处于停止模式但仍然继续制动if Rover.vel > 0.2:Rover.throttle = 0Rover.brake = Rover.brake_setRover.steer = 0# 如果我们没有移动（vel <0.2），那就做点别的elif Rover.vel <= 0.2:# 现在为停止状态，依靠视觉数据，判断是否有前进的道路if len(Rover.nav_angles) < Rover.go_forward:Rover.throttle = 0# 松开制动器以便转动Rover.brake = 0# 转弯范围为+/- 15度，当停止时，下一条线将引起4轮转向Rover.steer = -15#如果停下来但看到前面有足够的可导航地形，那就启动if len(Rover.nav_angles) >= Rover.go_forward:# 将油门设置回存储值Rover.throttle = Rover.throttle_setRover.brake = 0# 将转向设置为平均角度Rover.steer = np.clip(np.mean(Rover.nav_angles * 180/np.pi), -15, 15)Rover.mode = 'forward'else:Rover.throttle = Rover.throttle_setRover.steer = 0Rover.brake = 0# 在可拾取岩石的状态下发送拾取命if Rover.near_sample and Rover.vel == 0 and not Rover.picking_up:Rover.send_pickup = Truereturn Rover
(3)项目支持函数
提供一些支持项目运行的函数，比如创建输出图像等等，具体的功能可以看程序。
程序如下：
import numpy as np
import cv2
from PIL import Image
from io import BytesIO, StringIO
import base64
import time# 定义一个函数，将遥测字符串转换为浮点数，与十进制约定无关
def convert_to_float(string_to_convert):if ',' in string_to_convert:float_value = np.float(string_to_convert.replace(',', '.'))else:float_value = np.float(string_to_convert)return float_valuedef update_rover(Rover, data):# 初始化开始时间和样本位置if Rover.start_time is None:Rover.start_time = time.time()Rover.total_time = 0samples_xpos = np.int_([convert_to_float(pos.strip())
更多相关：
-
有一天，我写了一个自信满满的自定义组件myComponent，在多个页面import使用了，结果控制台给我来这个
我特么裤子都脱了，你给我来这个提示是几个意思
仔细一看 The Component 'MyComponentComponent' is declared by more than one NgModule...
-
创建一个带路由的项目，依次执行下面每行代码
ng n RouingApp --routingcd RouingAppng g c components/firstng g c components/secondng g m components/second --routing
代码拷贝：
import {NgModul...
-
cnpm install vue-quill-editor
cnpm install quill-image-drop-module
cnpm install quill-image-resize-module
执行上面的命令安装，然后在main.js下面加入
//引入quill-editor编辑器import...
-
首先要理解Vue项目加载顺序： index.html → main.js → App.vue → nav.json→ routes.js → page1.vue
index.html建议加入样式
-
简单记录平时画图用到的python 便捷小脚本
1. 从单个文件输入 绘制坐标系图
#!/usr/bin/python
# coding: utf-8
import matplotlib.pyplot as plt
import numpy as np
import matplotlib as mpl
import sysf...