期末項(xiàng)目?jī)?nèi)容：移動(dòng)小車操作入門，老師提供了三個(gè)選題：

1）控制小車尋線，并在三條黑線的交界處停止

2）控制小車尋找aruco碼,先到達(dá)aruco碼處，按照固定距離到達(dá)目標(biāo)點(diǎn)

3）控制小車尋找aruco碼,通過(guò)視覺(jué)圖像得到三維信息，根據(jù)目標(biāo)點(diǎn)和aruco碼的相對(duì)距離，直接到達(dá)目標(biāo)點(diǎn)。

https://www.bilibili.com/video/BV1r24y137xX/

我嘗試了1、2 兩種任務(wù)，任務(wù)1失敗，任務(wù)2不完全成功。

（一）小車控制介紹：

????小車控制時(shí)需要打開底盤并進(jìn)行底盤連接：

????roslaunch jubot_driver jubot_driver.launch?

????這個(gè)命令會(huì)創(chuàng)建一個(gè)節(jié)點(diǎn)： /jubot_driver ,這個(gè)節(jié)點(diǎn)會(huì)持續(xù)接收 名為/cmd_vel 的話題的信息。（圖中最后一個(gè)圈）

? ?那么我們也可以通過(guò) /cmd_vel 話題能夠?qū)崿F(xiàn)小車的控制。采用訂閱與發(fā)布的方式，創(chuàng)建一個(gè)話題發(fā)布者，在 /cmd_vel 上發(fā)布對(duì)應(yīng)數(shù)據(jù)類型，并打開底盤控制命令（接受信息），就能實(shí)現(xiàn)小車移動(dòng)的控制。查詢?cè)掝}格式，發(fā)現(xiàn)/cmd_vel 的數(shù)據(jù)類型為：geometry_msgs::Twist twist; Twist的信息則在此查詢：geometry_msgs/Twist Documentation (ros.org)：

????Vector3的的內(nèi)容則是：

我們就能夠采用這樣的方式發(fā)布信息，并實(shí)現(xiàn)控制小車運(yùn)動(dòng)了。

linear 為線速度；angular為角度度（.z 表示繞z軸轉(zhuǎn)動(dòng)）

#include <geometry_msgs/Twist.h>

???? ????geometry_msgs::Twist twist;

? ? ? ? geometry_msgs::Vector3 linear;

????????geometry_msgs::Vector3 angular;

????????linear.x=0;linear.y=0; linear.z=0;angular.x=0; angular.y=0;angular.z=0.1;

????????twist.linear=linear;twist.angular=angular;?

ros::Subscriber arSub = node_handle.subscribe("/ar_pose_marker",1, doMsg);

?????????cmdpub.publish(twist);

（二）任務(wù)二、三過(guò)程：

任務(wù)二的重點(diǎn)是通過(guò)aruco碼來(lái)獲取小車的位姿信息，我們可以通過(guò)

?roslaunch jubot_ar_track jubot_ar_track.launch 調(diào)起aruco識(shí)別程序，通過(guò)查看話題和讀取話題信息，我發(fā)現(xiàn) 這個(gè)程序通過(guò) /ar_pose_marker 節(jié)點(diǎn)發(fā)布aruco碼的位姿信息，但是發(fā)布的信息讀取遇到了困難，時(shí)常報(bào)錯(cuò)：pose不存在；[ ]?操作符不存了。。。

花了很長(zhǎng)的時(shí)間，終于搞清楚了如何讀取：

ros::NodeHandle node_handle;

ros::Subscriber arSub = node_handle.subscribe("/ar_pose_marker",1, doMsg);

void doMsg(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg){

//ROS_INFO("get:%f", msg->markers[].pose.pose.position.x);

if(msg->markers.size()!=0){

int aruco_id = msg->markers[0].id;

geometry_msgs::Pose aruco_pose = msg->markers[0].pose.pose;

//可以通過(guò)aruco.pose.x /aruco.position.x 得到位置信息和四元素

}}

從/ar_pose_marker得到的數(shù)據(jù)包括: position?和 orientation ；position為以aruco碼為坐標(biāo)系，小車在該坐標(biāo)系的坐標(biāo)；orientation是四元素，使用方法如下：

通過(guò)四元素轉(zhuǎn)換得到aruco碼相對(duì)小車坐標(biāo)，就可以控制小車移動(dòng)到目標(biāo)點(diǎn)了。

（三）任務(wù)一過(guò)程：

任務(wù)一主要是尋線和終止條件的判斷，通過(guò)調(diào)用opencv 的庫(kù)，在自己電腦上進(jìn)行測(cè)試時(shí)，巡線部分能夠較簡(jiǎn)單實(shí)現(xiàn)，但是上小車測(cè)試，地面的反光和分辨率卻對(duì)識(shí)別帶來(lái)很大的影響：

上圖是小車測(cè)試結(jié)果；下圖為手機(jī)拍攝圖測(cè)試效果。我嘗試過(guò)hsv數(shù)值修改；數(shù)值大了會(huì)導(dǎo)致出現(xiàn)很多無(wú)關(guān)的點(diǎn)被選擇；而數(shù)值小了之后識(shí)別的路徑就會(huì)被截?cái)唷?/p>

我對(duì)圖像的處理操作如下：

x_size = 800?

y_size = 600

frame = CV2.imread("E:/1.jpg")
compress = CV2.resize(frame, (x_size, y_size), interpolation=CV2.INTER_CUBIC)#標(biāo)準(zhǔn)模板

gs_frame = CV2.GaussianBlur(compress, (5, 5), 0)#?高斯模糊

sv_image = CV2.cvtColor(gs_frame, CV2.COLOR_BGR2HSV) #轉(zhuǎn)換為hsv圖像

# hsv 篩選顏色
black_lower = np.array([0, 0, 0])
black_high = np.array([180, 255, 145])

mask = CV2.inRange(hsv_image, black_lower, black_high)

#腐蝕和膨脹的核心
kernel_dilate = np.ones((6,6),dtype =np.uint8)
kernel_erosion = np.ones((3, 3), dtype=np.uint8)

#腐蝕操作，去除毛刺
n = 0
while (n < 3):
? ?# 膨脹
? ?dilate = CV2.dilate(mask, kernel_dilate ,4 )
? ?# 腐蝕操作
? ?erosion = CV2.erode(dilate, kernel_erosion, iterations=2)
? ? mask = erosion
? ? n = n + 1

contours, hierarchy = CV2.findContours(mask, CV2.RETR_TREE, CV2.CHAIN_APPROX_NONE) #輪廓提取

indx = ChoiceTheLength(contours)#選取出最長(zhǎng)的輪廓
drawMyContours("find contours", mask, contours[indx], True)#并展示

????如果要用于巡線，思路是選取屏幕下方符合條件的點(diǎn)并框選，如果選框中心靠左的位置，則左偏移（linear.z= 0.1）；中間則直行，直到中心點(diǎn)逐漸偏左或偏右。

????如果要用于判斷終點(diǎn)；則是檢測(cè)到三線的交點(diǎn)（Y字形），則認(rèn)為即將到達(dá)目標(biāo)點(diǎn)，可以開個(gè)計(jì)時(shí)器，時(shí)間到了就停止運(yùn)動(dòng)；或者移動(dòng)固定的距離。

但是視覺(jué)上小車測(cè)試的效果太差了，讀取的hsv圖根本沒(méi)法用來(lái)識(shí)別，所以放棄了這個(gè)操作。

（四）困難與總結(jié)：

1. CMakeLists.txt 在ros中是真的重要，因?yàn)樵谖募绣e(cuò)誤的鏈接內(nèi)容，還有沒(méi)有鏈接足夠的庫(kù)，一直導(dǎo)致編譯錯(cuò)誤還發(fā)現(xiàn)不了。一開始在檢查的時(shí)候也沒(méi)有看cmakelist.txt 的東西，被這個(gè)編譯卡了很久。

2. catkin_make 有時(shí)候沒(méi)有報(bào)錯(cuò)，只是警告，但是也會(huì)導(dǎo)致編譯失敗。我意識(shí)到這一點(diǎn)時(shí)已經(jīng)過(guò)了一天。

3.這此項(xiàng)目讓我們了解了ROS系統(tǒng)相關(guān)知識(shí)，學(xué)會(huì)了控制小車移動(dòng)，初步認(rèn)識(shí)了坐標(biāo)系轉(zhuǎn)換等知識(shí)