機(jī)器人操作系統(tǒng)（ROS）課件匯 曾慶喜 第7-11章 ROS 基礎(chǔ)應(yīng)用實(shí)例-ROS-2簡(jiǎn)介

機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）遙控小海龜運(yùn)動(dòng)7.1機(jī)器人操作系統(tǒng)（ROS）7.1遙控小海龜運(yùn)動(dòng)

我們?cè)趧傞_始學(xué)習(xí)ROS的時(shí)候，接觸到的第一個(gè)功能包是turtlesim，該功能包可以通過(guò)鍵盤控制小海龜在界面中移動(dòng)。這個(gè)功能包的核心是turtlesim_node節(jié)點(diǎn)，提供一個(gè)可視化的海龜仿真器。本章中我們將學(xué)習(xí)在ROS中如何使用遙控器來(lái)控制小海龜運(yùn)動(dòng)，實(shí)驗(yàn)分別以紅外遙控器和游戲手柄為例來(lái)實(shí)現(xiàn)小海龜?shù)倪\(yùn)動(dòng)控制。

紅外遙控器

游戲手柄

仿真小海龜機(jī)器人操作系統(tǒng)（ROS）7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)硬件基礎(chǔ)

實(shí)驗(yàn)用到的硬件包括：紅外遙控器、紅外解碼模塊、USB轉(zhuǎn)TTL模塊。紅外解碼模塊可將紅外遙控器發(fā)射的紅外編碼信號(hào)解碼成相應(yīng)的16進(jìn)制數(shù)并通過(guò)串口進(jìn)行輸出。USB轉(zhuǎn)TTL模塊與紅外解碼模塊通過(guò)杜邦線進(jìn)行連接，通過(guò)USB端口連接至上位機(jī)。

紅外遙控器紅外解碼器USB轉(zhuǎn)TTL模塊紅外遙控器按鍵對(duì)應(yīng)的16進(jìn)制編碼紅外遙控器按鍵對(duì)應(yīng)的16進(jìn)制編碼，其中用戶碼為“00FF”，串口輸出數(shù)據(jù)為“用戶碼+鍵位碼”，例如我們按下“OK”鍵，串口則輸出“00FF1C”。機(jī)器人操作系統(tǒng)（ROS）軟件基礎(chǔ)在終端中輸入以下命令安裝ROS串口功能包：$catkin_create_pkgteleoperationstd_msgsrospyroscpp輸入以下命令檢測(cè)serial包是否安裝好：$rospackfindserial若終端顯示serial的路徑(/opt/ros/melodic/share/serial)，則說(shuō)明安裝成功。7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）在~/catkin_ws/src下輸入以下命令創(chuàng)建一個(gè)名為serialport的功能包：$catkin_create_pkgserialportroscpprospystd_msgsserial在serialport/src文件夾下創(chuàng)建名為infrared_remote.cpp的文件，并填入以下代碼：#include<ros/ros.h>#include<serial/serial.h>//ROS已經(jīng)內(nèi)置了的串口包#include<geometry_msgs/Twist.h>serial::Serialser;//聲明串口對(duì)象

uint8_tbuffer[3];//定義串口數(shù)據(jù)存放數(shù)組

uint8_ta;intmain(intargc,char**argv){ros::init(argc,argv,"serial_remote_node");//初始化節(jié)點(diǎn)ros::NodeHandlenh;//聲明節(jié)點(diǎn)句柄 ros::Publisherread_pub=nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1);//發(fā)布話題……7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)軟件基礎(chǔ)機(jī)器人操作系統(tǒng)（ROS）代碼解釋ros/ros.h包含大部分ROS中通用的頭文件，serial/serial.h即ROS中已經(jīng)內(nèi)置了的串口包，通過(guò)它可以與串口設(shè)備進(jìn)行通訊，另外節(jié)點(diǎn)會(huì)發(fā)布geometry_msgs/Twist類型的消息，所以需要加入包含該類型的頭文件Twist.h，這個(gè)頭文件根據(jù)Twist.msg的消息結(jié)構(gòu)定義自動(dòng)生成。1.頭文件部分#include<ros/ros.h>#include<serial/serial.h>#include<geometry_msgs/Twist.h>7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）代碼解釋在main函數(shù)中，通過(guò)init函數(shù)初始化ros節(jié)點(diǎn)，節(jié)點(diǎn)名稱為serial_remote_node，該名稱在運(yùn)行的ROS中是唯一的，不允許同時(shí)存在名稱相同的兩個(gè)節(jié)點(diǎn)。通過(guò)ros::NodeHandlenh聲明節(jié)點(diǎn)句柄，最后創(chuàng)建一個(gè)名為read_pub的發(fā)布者，發(fā)布者向話題/turtle1/cmd_vel中發(fā)布Twist類型的消息，其中緩存區(qū)大小為1。2.主函數(shù)部分intmain(intargc,char**argv){ros::init(argc,argv,"serial_remote_node");ros::NodeHandlenh;

ros::Publisherread_pub=nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1);......}7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）代碼解釋進(jìn)入trycatch語(yǔ)句，設(shè)置串口設(shè)備號(hào)為/dev/ttyUSB0，串口波特率為9600b/s，串口超時(shí)時(shí)間為1000ms，設(shè)置完成后打開串口。當(dāng)串口打開錯(cuò)誤時(shí)，在終端報(bào)錯(cuò)："Unabletoopenport"并返回-1。若串口成功打開，則在終端輸出："SerialPortinitialized"。2.主函數(shù)部分try{ser.setPort("/dev/ttyUSB0");ser.setBaudrate(9600);serial::Timeoutto=serial::Timeout::simpleTimeout(1000);ser.setTimeout(to);ser.open();}catch(serial::IOException&e){ROS_ERROR_STREAM("Unabletoopenport");return-1;}7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）代碼解釋ros::Rateloop_rate(50);geometry_msgs::Twistvel;指定程序循環(huán)的頻率為50hz，并創(chuàng)建一個(gè)geometry_msgs::Twist類型的變量vel，用于存儲(chǔ)需要發(fā)布的線速度和角速度數(shù)據(jù)。while(ros::ok()){if(ser.available()){ ...... } ros::spinOnce loop_rate.sleep();}進(jìn)入while循環(huán)，通過(guò)ser.available()函數(shù)接收串口緩沖區(qū)中當(dāng)前剩余的字符個(gè)數(shù)，判斷串口的緩沖區(qū)有無(wú)數(shù)據(jù)。當(dāng)ser.available()>0時(shí)，說(shuō)明串口接收到了數(shù)據(jù)，可以進(jìn)行數(shù)據(jù)解析，否則等待串口數(shù)據(jù)接收。7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)2.主函數(shù)部分機(jī)器人操作系統(tǒng)（ROS）編譯功能包在CMakeLists.txt文件中添加：add_executable(infrared_remotesrc/infrared_remote.cpp)

add_dependencies(infrared_remote${${PROJECT_NAME}_EXPORTED_TARGETS}

${catkin_EXPORTED_TARGETS})

target_link_libraries(infrared_remote${catkin_LIBRARIES})修改完成之后，在工作空間中進(jìn)行編譯：$cd~/catkin_ws

$catkin_make7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）開啟終端輸入以下命令啟動(dòng)小海龜節(jié)點(diǎn)，在啟動(dòng)前不要忘記啟動(dòng)roscore命令：$roscore$rosrunturtlesimturtlesim_node插入紅外接收探頭，另啟終端輸入以下命令開啟紅外遙控節(jié)點(diǎn)：$source

~/catkin_ws/devel/setup.bash$rosrunserialportinfrared_remote按動(dòng)紅外遙控器上的按鍵，將出現(xiàn)信息輸出日志：信息輸出日志結(jié)果7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)可通過(guò)如下命令查看遙控器發(fā)送的/turtle1/cmd_vel話題消息：$rostopicecho/turtle1/cmd_vel話題/turtle1/cmd_vel內(nèi)容機(jī)器人操作系統(tǒng)（ROS）7.1.1紅外遙控器控制小海龜運(yùn)動(dòng)遙控小海龜運(yùn)動(dòng)情況此時(shí)我們就可以通過(guò)紅外遙控器控制小海龜?shù)倪\(yùn)動(dòng)，小海龜運(yùn)動(dòng)情況如圖所示。讀者可以根據(jù)自己的需要定義遙控器上其他按鍵的功能，編寫屬于自己的控制命令。本實(shí)驗(yàn)遙控器的按鍵信息對(duì)應(yīng)如下：上下左右鍵分別對(duì)應(yīng)前進(jìn)、后退、左轉(zhuǎn)和右轉(zhuǎn)，OK鍵對(duì)應(yīng)停止。機(jī)器人操作系統(tǒng)（ROS）7.1.2游戲手柄控制小海龜運(yùn)動(dòng)硬件基礎(chǔ)

除了上一節(jié)中討論的紅外遙控器控制小海龜運(yùn)動(dòng)，我們還可以通過(guò)游戲手柄更加真實(shí)而有趣的控制屏幕里的小海龜。微軟XBOX360手柄機(jī)器人操作系統(tǒng)（ROS）軟件基礎(chǔ)在終端中輸入以下命令安裝手柄驅(qū)動(dòng)功能包：$sudoapt-getinstallros-melodic-joystick-drivers輸入以下命令檢測(cè)serial包是否安裝好：$rospackfindserial安裝完成后將手柄連接到電腦，運(yùn)行如下命令，查看游戲手柄是否被識(shí)別，js0即為手柄端口，查看端口號(hào)結(jié)果如圖。$ls/dev/input查看端口號(hào)7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）使用如下命令檢查它是否工作，其中jstest是虛擬搖桿工具：$sudojstest/dev/input/js0得到以下輸出，說(shuō)明游戲手柄正常工作，微軟X-BOX360手柄有8個(gè)軸向輸入和11個(gè)按鈕，操作手柄上的搖桿或按鈕將會(huì)發(fā)生相應(yīng)數(shù)值變化，如圖所示：軟件基礎(chǔ)查看手柄數(shù)據(jù)7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）軟件基礎(chǔ)當(dāng)確定好游戲手柄能夠正常工作以后，使用joy和joy_node功能包測(cè)試它的功能。輸入如下命令并得到以下輸出，說(shuō)明所有配置都是正確的，節(jié)點(diǎn)運(yùn)行結(jié)果如圖所示：$roscore$rosrunjoyjoy_node我們可以通過(guò)如下命令查看joy話題發(fā)布的消息，話題/joy內(nèi)容如圖所示：$rostopicecho/joy話題/joy內(nèi)容查看手柄數(shù)據(jù)7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）

通過(guò)以下命令查看Joy類型的消息，Joy類型消息如圖所示：$rostopictype/joy$rosmsgshowsensor_msgs/Joy軟件基礎(chǔ)查看話題類型7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）通過(guò)如下命令啟動(dòng)仿真小海龜，運(yùn)行小海龜節(jié)點(diǎn)如圖所示，小海龜界面如圖所示：$rosrunturtlesimturtlesim_node軟件基礎(chǔ)運(yùn)行節(jié)點(diǎn)結(jié)果圖小海龜界面7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）通過(guò)rostopiclist命令查看所有話題，其中/turtle1/cmd_vel是控制小海龜移動(dòng)的話題，我們可以通過(guò)rostopictype命令查看其消息類型：通過(guò)如下指令查看geometry_msgs/Twist消息類型$rosmsgshowgeometry_msgs/Twist查看話題類型geometry_msgs/Twist消息類型其中，linear代表三個(gè)方向上的線速度，angular代表三個(gè)方向的角速度。7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）在serialport/src文件夾下創(chuàng)建一個(gè)新的文件xbox_remote.cpp，并填入以下內(nèi)容：$catkin_create_pkgserialportroscpprospystd_msgsserial在serialport/src文件夾下創(chuàng)建名為infrared_remote.cpp的文件，并填入以下代碼：#include<ros/ros.h>#include<sensor_msgs/Joy.h>#include<geometry_msgs/Twist.h>usingnamespacestd;classTeleopJoy{ public: TeleopJoy();private: voidcallBack(constsensor_msgs::Joy::ConstPtr&joy); ros::NodeHandlen; ros::Publisherpub; ros::Subscribersub; inti_velLinear,i_velAngular;};……軟件基礎(chǔ)7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）代碼解釋#include<ros/ros.h>#include<sensor_msgs/Joy.h>#include<geometry_msgs/Twist.h>ros/ros.h包含大部分ROS中通用的頭文件。節(jié)點(diǎn)會(huì)發(fā)布sensor_msgs/Joy和geometry_msgs/Twist類型的消息，所以需要加入包含該類型的頭文件Joy.h和Twist.h，這兩個(gè)頭文件根據(jù)Joy.msg和Twist.msg的消息結(jié)構(gòu)定義自動(dòng)生成。1.頭文件部分7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）代碼解釋在main函數(shù)中，通過(guò)init函數(shù)初始化ros節(jié)點(diǎn)，節(jié)點(diǎn)名稱為xbox_remote，該名稱在運(yùn)行的ROS中是唯一的，不允許同時(shí)存在名稱相同的兩個(gè)節(jié)點(diǎn)。同時(shí)創(chuàng)建TeleopJoy類的一個(gè)實(shí)例：teleop_turtle，實(shí)現(xiàn)遙控器數(shù)據(jù)的接收和小海龜控制消息的發(fā)布。2.主函數(shù)部分intmain(intargc,char**argv){ ros::init(argc,argv,"xbox_remote"); TeleopJoyteleop_turtle; ros::spin()}7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）代碼解釋在TeleopJoy()的類中，創(chuàng)建了一個(gè)節(jié)點(diǎn)句柄，用于使用和管理節(jié)點(diǎn)資源。同時(shí)定義了回調(diào)函數(shù)、線速度變量和角速度變量、發(fā)布者和訂閱者。3.類和函數(shù)部分classTeleopJoy{public:TeleopJoy();private:voidcallBack(constsensor_msgs::Joy::ConstPtr&joy);ros::NodeHandlen;ros::Publisherpub;ros::Subscribersub;inti_velLinear,i_velAngular;};7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）代碼解釋在構(gòu)造函數(shù)中，對(duì)參數(shù)進(jìn)行初始化。線速度和角速度通過(guò)參數(shù)服務(wù)器獲取變量的值。發(fā)布者pub向話題/turtle1/cmd_vel中發(fā)布Twist類型的消息，緩存區(qū)大小為1。訂閱者sub通過(guò)名為joy的話題接收sensor_msgs::Joy類型的消息數(shù)據(jù)，緩存區(qū)大小為10，其中TeleopJoy::callBack為需要執(zhí)行的回調(diào)函數(shù)。3.類和函數(shù)部分TeleopJoy::TeleopJoy(){ n.param("axis_linear",i_velLinear,i_velLinear); n.param("axis_angular",i_velAngular,i_velAngular); pub=n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1); sub=n.subscribe<sensor_msgs::Joy>("joy",10,&TeleopJoy::callBack,this);}7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）代碼解釋回調(diào)函數(shù)是訂閱節(jié)點(diǎn)接收消息的基礎(chǔ)機(jī)制，當(dāng)話題收到消息時(shí)會(huì)自動(dòng)以消息指針作為參數(shù)，調(diào)用回調(diào)函數(shù)，完成對(duì)消息內(nèi)容的處理。如上，我們?cè)诨卣{(diào)函數(shù)中創(chuàng)建一個(gè)geometry_msgs::Twist類型的變量vel，用于存儲(chǔ)需要發(fā)布的線速度和角速度數(shù)據(jù)。當(dāng)接收到Xbox手柄的軸向角速度和線速度時(shí)，更新vel中對(duì)應(yīng)的值。最后，通過(guò)調(diào)用publish函數(shù)將消息發(fā)布出去。4.回調(diào)函數(shù)部分voidTeleopJoy::callBack(constsensor-msgs::Joy::ConstPtr&joy){ geometry_msgs::Twistvel; vel.angular.z=joy->axes[i_velAngular]; vel.linear.x=joy->axes[i_velLinear]; pub.publish(vel);}7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）編譯功能包打開功能包中的CMakeLists.txt文件，并添加如下內(nèi)容：add_executable(xbox_remotesrc/xbox_remote.cpp)

target_link_libraries(xbox_remote${catkin_LIBRARIES})修改完成之后，在工作空間中進(jìn)行編譯：$cd~/catkin_ws

$catkin_make7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）我們創(chuàng)建一個(gè)xbox_remote.launch的啟動(dòng)文件，為參數(shù)服務(wù)器聲明一些變量，并同時(shí)啟動(dòng)turtlesim、joy和xbox_remote三個(gè)節(jié)點(diǎn)。創(chuàng)建啟動(dòng)文件<launch> <nodename="sim"pkg="turtlesim"type="turtlesim_node"/> <nodename="xbox_remote"pkg="serialport"type="example1"/> <paramname="axis_linear"value="1"type="int"/> <paramname="axis_angular"value="0"type="int"/>

<noderespawn="true"pkg="joy"type="joy_node"name="teleopJoy"><paramname="dev"type="string"value="/dev/input/js0"/><paramname="deadzone"value="0.12"/></node></launch>7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）在啟動(dòng)文件中啟動(dòng)一個(gè)節(jié)點(diǎn)需要三個(gè)屬性：pkg、type和name。其中pkg定義節(jié)點(diǎn)所在的功能包名稱，type定義節(jié)點(diǎn)的可執(zhí)行文件名稱，name屬性用來(lái)定義節(jié)點(diǎn)運(yùn)行的名稱，這將覆蓋節(jié)點(diǎn)中init()函數(shù)賦予節(jié)點(diǎn)的名稱。在啟動(dòng)joy_node節(jié)點(diǎn)時(shí)，我們可以修改兩個(gè)參數(shù)：dev：游戲手柄設(shè)備地址；默認(rèn)為/dev/input/js0。deadzone：死區(qū)（雙精度，默認(rèn)值：0.12）。死區(qū)是操縱桿能夠被識(shí)別到偏離軸中心之前必須移動(dòng)的量。0.12意味著操縱桿必須移動(dòng)12%到軸范圍的邊緣，然后該軸才會(huì)輸出非零值。解釋啟動(dòng)文件7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）輸入以下命令運(yùn)行xbox_remote.launch啟動(dòng)文件：$sourcecatkin_ws/devel/setup.bash$roslaunchserialportxbox_remote.launch此時(shí)，你就可以通過(guò)游戲手柄操控小海龜了，另起終端運(yùn)行：$rostopicecho/turtle1/cmd_vel通過(guò)如下命令可查看當(dāng)前ROS運(yùn)行的節(jié)點(diǎn)圖：$rqt_graph

遙控小海龜界面節(jié)點(diǎn)圖遙控小海龜日志信息運(yùn)行啟動(dòng)文件7.1.2游戲手柄控制小海龜運(yùn)動(dòng)機(jī)器人操作系統(tǒng)（ROS）7.1.3本節(jié)小結(jié)本節(jié)我們分別用了紅外遙控器和游戲手柄來(lái)控制小海龜運(yùn)動(dòng)。在這兩個(gè)實(shí)驗(yàn)中深入的理解了從遙控設(shè)備中獲取數(shù)據(jù)（訂閱），生成能控制小海龜速度的指令（發(fā)布）。并通過(guò)編寫launch文件，同時(shí)啟動(dòng)多個(gè)節(jié)點(diǎn)，并通過(guò)rqt_graph工具查看當(dāng)前ROS運(yùn)行的節(jié)點(diǎn)圖，進(jìn)一步熟悉了ROS的通訊機(jī)制。機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）Ublox-GPS模塊的使用及坐標(biāo)轉(zhuǎn)換7.2機(jī)器人操作系統(tǒng)（ROS）7.2.1GPS簡(jiǎn)介

GPS是由一種具有全方位、全天候、全時(shí)段、高精度的基于空間的衛(wèi)星導(dǎo)航系統(tǒng)，能在任何時(shí)間、任何天氣、任何地點(diǎn)為全球用戶提供低成本、高精度的三維位置、速度和精確定時(shí)等導(dǎo)航信息。GPS中最有用的信息是在GGA中，提供了通用的Fix數(shù)據(jù)以及GPS的3D位置。完整的一幀GPS數(shù)據(jù)如圖所示。$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47WhereGGA GlobalpositioningSystemFixData(全球定位修正數(shù)據(jù))123519 Fixtakenat12:35:19UTC(修正)4807.038,N Latitude48deg07.038’N(緯度)01131.

000,E Longitude11deg31.000’E(經(jīng)度)1Fixquality: 0=invalid(修正質(zhì)量)1=GPSfix(SPS)(修正)2=DGPSfix(修正)3=PPSfix(修正)4=RealTimeKinematic(實(shí)時(shí)運(yùn)動(dòng)學(xué))5=FloatRTK6=estimated(deadreckoning)(2.3feature)(估計(jì)航路推算)7=Manualinputmode(手動(dòng)輸入模式)8=Simulationmode(仿真模式)08 Numberofsatellitiesbeingtracked(已跟蹤衛(wèi)星數(shù))0.9 Horizontaldilutionofposition(水平位置精度)545.4,MAltitude,Meters,abovemeansealevel(高度，米，平均海平面)46.9,M Heightofgeoid(meansealevel)aboveWGS84ellipsoid(大地水準(zhǔn)面高（平均海平面）高于WGS84橢球)(emptyfield)timeinsecondssincelastDGPSupdate(自上一次DGPS更新時(shí)間間隔，以秒為單位)(emptyfield)DGPSstationIDnumber(DGPS基站ID號(hào))*47 thechecksumdata,alwaysbeginswith*(校驗(yàn)和數(shù)據(jù))GPGGA格式機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換

地面運(yùn)動(dòng)機(jī)器人一般采用平面直接坐標(biāo)系，因此需要將GPS接收機(jī)輸出的經(jīng)緯度信息轉(zhuǎn)換為大地平面坐標(biāo)來(lái)進(jìn)行導(dǎo)航，常用的坐標(biāo)轉(zhuǎn)換方法為高斯-克呂格投影變換。

高斯-克呂格投影示意圖如圖所示，該投影是用一個(gè)假想的橢圓柱筒橫置于地球表面，與地球上的某一經(jīng)線相切，即投影后的中央經(jīng)線，該橢圓柱的中心軸位于赤道平面內(nèi)，然后按照一定投影條件將地球橢球面中央子午線兩側(cè)規(guī)定范圍內(nèi)的點(diǎn)投影到橢球圓柱面上，從而得到地球橢球面上各個(gè)點(diǎn)的高斯投影。高斯投影示意圖機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換為了有效控制投影變形，可采用分帶投影的方法。先按一定的經(jīng)度差將地球表面劃分為若干投影帶，再使圓柱面依次和每一帶的中央經(jīng)線相切，并把各帶中央經(jīng)線東西兩側(cè)一定經(jīng)度差范圍內(nèi)的經(jīng)緯線網(wǎng)投影到圓柱上，然后從兩極將該圓柱面切開展平，構(gòu)成地球各帶經(jīng)緯線網(wǎng)在平面上的圖形。6度帶和3度帶的分帶投影示意圖如圖所示，6度分帶從0度經(jīng)線起自西向東每6度分為一個(gè)投影帶，將地球劃分為60個(gè)投影帶，并依次進(jìn)行編號(hào)。6度帶和3度帶的分帶投影機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換defcallback(gps): globalframe_id #gps提供的經(jīng)緯度數(shù)據(jù)轉(zhuǎn)換為以度為單位的經(jīng)緯度格式

gps_jingdu_1=float(gps.longitude)/100 gps_weidu_1=float(gps.latitude)/100 gps_jingdu_2=((gps_jingdu_1-math.floor(gps_jingdu_1))*100)/60 gps_weidu_2=((gps_weidu_1-math.floor(gps_weidu_1))*100)/60 gps_jingdu=math.floor(gps_jingdu_1)+gps_jingdu_2 gps_weidu=math.floor(gps_weidu_1)+gps_weidu_2 #經(jīng)緯度轉(zhuǎn)換為大地坐標(biāo)系下的坐標(biāo)

gps_x=LatLonToXY(gps_weidu,gps_jingdu)[1] gps_y=LatLonToXY(gps_weidu,gps_jingdu)[0] gps_msg=Point() gps_msg.y=gps_x gps_msg.x=gps_y gps_pub.publish(gps_msg)defgps_convert(): #初始化節(jié)點(diǎn)

rospy.init_node('get_GPS',anonymous=True) #訂閱話題/fix gps_sub=message_filters.Subscriber('/fix',NavSatFix) ts=message_filters.ApproximateTimeSynchronizer([gps_sub],10,0.1) ts.registerCallback(callback) rospy.loginfo("gpsdataconvertfunctionsuccessfullyinitialized!") rospy.spin()if__name__=='__main__’: listener() rospy.loginfo("datasuccessfullysaved!")機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyimportcv2importnumpyasnpimportmessage_filtersimportmathfromsensor_msgs.msgimportNavSatFixfromgeometry_msgs.msgimportPoseStamped,Pose,Quaternion,Point#定義參數(shù)frame_id=0RefCenterLon=118.79232402#lon融合中心經(jīng)度RefCenterLat=31.93936970#lat融合中心緯度#發(fā)布話題，話題名稱/gps/flat，數(shù)據(jù)類型為Point，隊(duì)列長(zhǎng)度為1gps_pub=rospy.Publisher('/gps/flat',Point,queue_size=1)#經(jīng)緯度轉(zhuǎn)換為大地坐標(biāo)系下的坐標(biāo)函數(shù)defLatLonToXY(Lat,Lon): fi=Lat/180*math.pi la=(Lon-RefCenterLon)/180*math.pi zi=RefCenterLat/180*math.pi a=6378137#長(zhǎng)半軸 b=6356752.3142#短半軸 c=6399593.6258#極點(diǎn)處子午線的曲率半徑 f=1/298.257223563#橢圓率 E2=0.00669437999013#第一偏心率的平方 Eta2=0.00673949674227#第二偏心率的平方 V=math.sqrt(1+Eta2) N=c/V beta0=1.0-3.0/4.0*E2+45.0/64.0*E2**2.0-175.0/256.0*E2**3+11025.0/16384.0*E2**4 beta2=beta0-1 beta4=15.0/32.0*E2**2-175.0/384.0*E2**3+3675.0/8192.0*E2**4 beta6=-35.0/96.0*E2**3+735.0/2048.0*E2**4 beta8=315.0/1024.0*E2**4 Sz=c*(beta0*zi+(beta2*math.cos(zi)+beta4*math.cos(zi)**3+beta6*math.cos(zi)**5+beta8*math.cos(zi)**7)*math.sin(zi)) S=c*(beta0*fi+(beta2*math.cos(fi)+beta4*math.cos(fi)**3+beta6*math.cos(fi)**5+beta8*math.cos(fi)**7)*math.sin(fi)) X=S+la**2*N/2.0*math.sin(fi)*math.cos(fi)+la**4*N/24.0*math.sin(fi)*math.cos(fi)**3.0*(5.0-math.tan(fi)**2+9.0*Eta2+4*Eta2**2)+la**6*N/720.0*math.sin(fi)*math.cos(fi)**5*(61-58*math.tan(fi)**2+math.tan(fi)**4) Y=la*N*math.cos(fi)+la**3*N/6.0*math.cos(fi)**3*(1-math.tan(fi)**2+Eta2)+la**5*N/120.0*math.cos(fi)**5*(5-18*math.tan(fi)**2+math.tan(fi)**4) Z=Sz+la**2*N/2.0*math.sin(zi)*math.cos(zi)+la**4*N/24.0*math.sin(zi)*math.cos(zi)**3.0*(5.0-math.tan(zi)**2+9.0*Eta2+4*Eta2**2)+la**6*N/720.0*math.sin(zi)*math.cos(zi)**5*(61-58*math.tan(zi)**2+math.tan(zi)**4) X=X-Z return[Y,X]本節(jié)提供經(jīng)緯度坐標(biāo)轉(zhuǎn)換示例代碼getGPS.py。getGPS.py的主要功能為將經(jīng)緯度坐標(biāo)轉(zhuǎn)換為大地平面坐標(biāo)，訂閱/fix話題（經(jīng)緯度原始數(shù)據(jù)），發(fā)布/gps/flat話題（轉(zhuǎn)換后的大地坐標(biāo)位置數(shù)據(jù)）。機(jī)器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換啟動(dòng)GPS后，運(yùn)行如下命令進(jìn)行經(jīng)緯度轉(zhuǎn)換：$rosrungps_pubgetGPS.py運(yùn)行如下命令，查看話題列表，話題/gps/flat內(nèi)容為經(jīng)緯度轉(zhuǎn)換后的坐標(biāo)，如圖所示：$rostopiclist

查看話題列表機(jī)器人操作系統(tǒng)（ROS）7.2.3GPS的使用本節(jié)使用的是GPS接收模塊Ublox，如圖所示，通過(guò)USB轉(zhuǎn)TTL模塊進(jìn)行Ublox和計(jì)算機(jī)的連接，安裝相應(yīng)驅(qū)動(dòng)，可以從這個(gè)設(shè)備中得到相應(yīng)的經(jīng)度、緯度和高層信息。Ublox實(shí)物圖機(jī)器人操作系統(tǒng)（ROS）7.2.3GPS的使用

在開始之前我們需要安裝NMEAGPS的安裝包，安裝完后運(yùn)行rosstack和rospack配置文件，運(yùn)行指令如下：$sudoapt-getinstallros-melodic-nmea-*$rosstackprofile&rospackprofile安裝結(jié)果圖

rosstack和rospack配置結(jié)果圖機(jī)器人操作系統(tǒng)（ROS）7.2.3GPS的使用通過(guò)如下命令查看GPS模塊的端口號(hào)$ls/dev查看端口號(hào)通過(guò)如下命令修改端口權(quán)限為可讀可寫可執(zhí)行，每次拔插Ublox模塊之后需重新設(shè)置：$sudochmod777/dev/ttyUSB0機(jī)器人操作系統(tǒng)（ROS）7.2.3GPS的使用啟動(dòng)roscore命令，然后運(yùn)行nmea_serial_driver.py文件，啟動(dòng)GPS驅(qū)動(dòng)，這里需要注意GPS端口號(hào)和波特率：$roscore$rosrunnmea_navsat_drivernmea_serial_driver_port:/dev/ttyUSB0_baud:=9600啟動(dòng)GPS驅(qū)動(dòng)機(jī)器人操作系統(tǒng)（ROS）7.2.3GPS的使用運(yùn)行如下指令可以查看話題列表和GPS對(duì)應(yīng)話題的數(shù)據(jù)類型：$rostopiclist$rostopictype/fix查看話題和數(shù)據(jù)類型/fix話題的消息類型是sensor_msgs/NavSatFix，用來(lái)表明設(shè)備的經(jīng)度、緯度、高度、質(zhì)量以及協(xié)方差矩陣。機(jī)器人操作系統(tǒng)（ROS）7.2.3GPS的使用通過(guò)運(yùn)行如下命令查看/fix話題發(fā)布的具體消息：$rostopicecho/fix話題/fix內(nèi)容通過(guò)運(yùn)行如下命令查看/fix話題發(fā)送的頻率，平均頻率為5Hz：$rostopichz/fix查看發(fā)送頻率機(jī)器人操作系統(tǒng)（ROS）7.3.3本節(jié)小結(jié)本部分介紹了GPS和高斯-克呂格投影變換，其次使用Ublox模塊得到經(jīng)緯度信息進(jìn)行定位，并提供了一個(gè)高斯-克呂格投影變換相關(guān)代碼，將經(jīng)緯度信息轉(zhuǎn)換為大地坐標(biāo)系下的坐標(biāo)，為機(jī)器人提供位置信息進(jìn)行導(dǎo)航。機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）多傳感器數(shù)據(jù)同步7.3機(jī)器人操作系統(tǒng)（ROS）7.3.1message_filters程序庫(kù)介紹message_filters是一個(gè)用于roscpp和rospy的實(shí)用程序庫(kù)，類似于一個(gè)消息緩存器，當(dāng)消息到達(dá)消息過(guò)濾器時(shí)，可能并不會(huì)立即輸出，而是在稍后的時(shí)間點(diǎn)里滿足一定條件下輸出。message_filters庫(kù)中包含有TimeSynchronizer時(shí)間同步器，它接收來(lái)自多個(gè)源的不同類型的消息，并且僅當(dāng)它們?cè)诰哂邢嗤瑫r(shí)間戳的每個(gè)源上接收到消息時(shí)才輸出它們，也就是起到了一個(gè)消息同步輸出的效果。

對(duì)齊傳感器數(shù)據(jù)時(shí)間戳有兩種策略：

①時(shí)間戳完全對(duì)齊策略(ExactTimePolicy)

②時(shí)間戳相近策略(ApproximateTimePolicy)機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)

本實(shí)驗(yàn)使用相機(jī)和IMU，時(shí)間同步示意圖如圖所示，分別定義IMU、相機(jī)兩個(gè)傳感器節(jié)點(diǎn)，分別發(fā)布/imu/angle，/usb_cam/image_raw話題，對(duì)應(yīng)的發(fā)布頻率分別為100Hz和30Hz。

通過(guò)TimeSynchronizer統(tǒng)一接收這兩個(gè)話題，當(dāng)兩個(gè)話題的時(shí)間戳對(duì)齊時(shí)，進(jìn)入callback回調(diào)函數(shù)。采用時(shí)間戳相近的對(duì)齊方式可通過(guò)設(shè)定一個(gè)時(shí)間參數(shù)，如0.1s，如果兩個(gè)話題的時(shí)間戳相差在0.1s之內(nèi)則認(rèn)為已經(jīng)對(duì)齊。

數(shù)據(jù)同步的頻率以頻率最低的話題為準(zhǔn)，即/usb_cam/image_raw的30Hz頻率?；赥imeSynchronizer的時(shí)間同步示意圖機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyfromgeometry_msgs.msgimportPoseStampeddata_z=0.0defangle_publisher(): #ROS節(jié)點(diǎn)初始化

rospy.init_node('angle_publisher',anonymous=True) #發(fā)布話題，話題名為/imu/angle，類型為PoseStamped，隊(duì)列長(zhǎng)度為10 angle_pub=rospy.Publisher('/imu/angle',PoseStamped,queue_size=10) #設(shè)置循環(huán)的頻率 rate=rospy.Rate(100) whilenotrospy.is_shutdown(): globaldata_z angle=PoseStamped() angle.header.frame_id="angle" angle.header.stamp=rospy.Time.now() data_z+=0.01 angle.pose.position.z=data_z #發(fā)布消息

angle_pub.publish(angle) rospy.loginfo("Publshangle%0.2fdeg",angle.pose.position.z) #按照循環(huán)頻率延時(shí)

rate.sleep()if__name__=='__main__': try: angle_publisher() exceptrospy.ROSInterruptException: pass建立IMU發(fā)布節(jié)點(diǎn)(angle_pub.py)：angle_pub.py的功能是模擬IMU不停地發(fā)布角度信息。它發(fā)布/imu/angle話題消息，類型為geometry_msgs/PoseStamped。機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyimportcv2importnumpyasnpimportmessage_filtersimportosfromos.pathimportjoinfromsensor_msgs.msgimportImagefromgeometry_msgs.msgimportPoseStampedfromcv_bridgeimportCvBridge,CvBridgeError#初始化參數(shù)frame_id=0gps_id=0#設(shè)置保存路徑，nuaa-frl為本機(jī)用戶名，讀者需自行修改path="/home/nuaa-frl/Dataset/"img_path="/home/nuaa-frl/Dataset/imgs"bridge=CvBridge()flag=Trueglobalpath_filepath_file='/home/nuaa-frl/temp'建立數(shù)據(jù)同步節(jié)點(diǎn)(CAM_IMU_test.py)：CAM_IMU_test.py的功能是將相機(jī)和IMU的數(shù)據(jù)進(jìn)行同步采集并保存。它訂閱/imu/angle話題和/usb_cam/image_raw話題，并通過(guò)時(shí)間同步器TimeSynchronizer進(jìn)行數(shù)據(jù)同步采集，最終保存到本地文件中。機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)defcallback(angle,image): ifflag==True: rospy.loginfo("datarecieved!") globalframe_id globalbridge imu_yaw=angle.pose.position.z #創(chuàng)建data.txt，保存imu角度數(shù)據(jù)

data="{}{}\n".format(frame_id,imu_yaw) pose_file=open(join(path,'data.txt'),'a') pose_file.writelines(data) pose_file.close() #保存圖片

down_img=bridge.imgmsg_to_cv2(image) image_name=join(img_path,str(frame_id)+".jpg") cv2.imwrite(image_name,down_img) frame_id+=1 rospy.loginfo("datarecorded!")deflistener(): #初始化節(jié)點(diǎn)

rospy.init_node('listener',anonymous=True) time=rospy.Time().now() #訂閱/imu/angle話題

angle_sub=message_filters.Subscriber('/imu/angle',PoseStamped) #訂閱/usb_cam/image_raw話題

image_sub=message_filters.Subscriber('/usb_cam/image_raw',Image) #如果兩個(gè)話題的時(shí)間戳相差在0.1s之內(nèi)則認(rèn)為時(shí)間戳已經(jīng)對(duì)齊

ts=message_filters.ApproximateTimeSynchronizer([angle_sub,image_sub],1000,0.1) ts.registerCallback(callback) rospy.loginfo("successfullyinitialized!") rospy.spin()if__name__=='__main__': #清空存放圖片的文件夾

foriinos.listdir(img_path):#os.listdir()返回一個(gè)列表，包含有指定路徑下的目錄和文件的名稱

path_file=os.path.join(img_path,i)#os.path.join()用于路徑拼接文件路徑

ifos.path.isfile(path_file):#os.path.isfile()判斷某一對(duì)象(需提供絕對(duì)路徑)是否為文件

os.remove(path_file)#os.remove()刪除指定路徑的文件

else: forfinos.listdir(path_file): path_file2=os.path.join(path_file,f) ifos.path.isfile(path_file2): os.remove(path_file2)#清空（創(chuàng)建）txt withopen(join(path,'data.txt'),'a')asf:#打開data.txt，如果該文件已存在，新的內(nèi)容將會(huì)被寫入到已有內(nèi)容之后。如果該文件不存在，創(chuàng)建新文件進(jìn)行寫入

f.seek(0)#從文件開頭讀取文件

f.truncate()#截?cái)辔募?/p>

listener() rospy.loginfo("datasuccessfullysaved!")機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)運(yùn)行以下命令下載相機(jī)驅(qū)動(dòng)程序(具體版本視安裝的ros版本決定)：$sudoapt-getinstallros-melodic-usb-cam接入相機(jī)，運(yùn)行以下命令啟動(dòng)相機(jī)節(jié)點(diǎn)：$roslaunchusb_camusb_cam-test.launch安裝相機(jī)驅(qū)動(dòng)啟動(dòng)相機(jī)節(jié)點(diǎn)機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)運(yùn)行以下命令啟動(dòng)IMU節(jié)點(diǎn)：$rosrunCAM_IMU_SYNCangle_pub.py運(yùn)行如下命令查看相機(jī)和IMU節(jié)點(diǎn)發(fā)布頻率：$rostopichz/usb_cam/image_raw$rostopichz/imu/angle啟動(dòng)IMU節(jié)點(diǎn)相機(jī)話題發(fā)布頻率IMU話題發(fā)布頻率機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)運(yùn)行以下命令啟動(dòng)數(shù)據(jù)同步節(jié)點(diǎn):$rosrunCAM_IMU_SYNCCAM_IMU_test.py

運(yùn)行數(shù)據(jù)同步節(jié)點(diǎn)數(shù)據(jù)實(shí)現(xiàn)同步采集，圖片數(shù)據(jù)保存在Dataset/imgs文件夾下面，IMU數(shù)據(jù)將存保在Dataset/data.txt里面機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)若我們關(guān)閉其中一個(gè)節(jié)點(diǎn)，話題將缺失，因此將無(wú)法進(jìn)入回調(diào)函數(shù)，進(jìn)而同步失敗，終端將一直卡在如圖所示界面。運(yùn)行結(jié)果圖機(jī)器人操作系統(tǒng)（ROS）7.3.3本節(jié)小結(jié)

本部分編寫了一個(gè)IMU節(jié)點(diǎn)來(lái)模擬IMU發(fā)布的角度信息，并和相機(jī)進(jìn)行數(shù)據(jù)同步采集，掌握了如何通過(guò)ROS實(shí)現(xiàn)數(shù)據(jù)同步，三種及以上傳感器的數(shù)據(jù)同步方法類似，讀者可以自行嘗試。機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）基于G29的移動(dòng)機(jī)器人遙操作實(shí)驗(yàn)7.4機(jī)器人操作系統(tǒng)（ROS）7.4.1遙操作(Teleoperation)介紹

遙操作即遠(yuǎn)程操作，操作者通過(guò)操縱主端設(shè)備，再由主端設(shè)備控制從端設(shè)備來(lái)完成一系列工作任務(wù)。

一個(gè)簡(jiǎn)單的遙操作系統(tǒng)通常由一個(gè)操作員、一個(gè)主操作器、一個(gè)主控制器、一個(gè)通信信道、一個(gè)從控制器、一個(gè)從操作器和一個(gè)環(huán)境組成，如圖所示。遙操作的基本結(jié)構(gòu)圖CarSim小型駕駛模擬器太空遙操作主從機(jī)器人模擬室機(jī)器人操作系統(tǒng)（ROS）7.4.2實(shí)驗(yàn)環(huán)境基礎(chǔ)硬件基礎(chǔ)在本實(shí)驗(yàn)中，使用的設(shè)備是G29方向盤總成，包括方向盤本體、腳踏板以及換擋桿，結(jié)構(gòu)如圖所示。G29方向盤旋轉(zhuǎn)角度與真實(shí)汽車方向盤旋轉(zhuǎn)圈數(shù)相同，并帶有力反饋效果，具有六檔變速桿，模擬真實(shí)駕駛體驗(yàn)。G29方向盤總成圖機(jī)器人操作系統(tǒng)（ROS）7.4.2實(shí)驗(yàn)環(huán)境基礎(chǔ)軟件基礎(chǔ)創(chuàng)建功能包，使用如下命令創(chuàng)建一個(gè)名為teleoperation的文件夾：$catkin_create_pkgteleoperationstd_msgsrospyroscpp創(chuàng)建完成后，代碼空間src會(huì)生成一個(gè)teleoperation功能包，里面包含package.xml和CMakelist.txt文件?；氐焦ぷ骺臻g的根目錄下使用catkin_make進(jìn)行編譯，然后使用如下命令設(shè)置環(huán)境變量：$source~/catkin_ws/devel/setup.bash以上就創(chuàng)建好了一個(gè)工作空間和功能包，接下來(lái)的遙操作例程就在這個(gè)teleoperation功能包里進(jìn)行。機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)準(zhǔn)備工作首先安裝功能包：$sudoapt-getinstallros-melodic-joy接下來(lái)，需要查看G29是否能正常使用，先運(yùn)行如下命令：$ls/dev/input可以看到結(jié)果如圖所示，其中若識(shí)別成功，可以看到有一個(gè)jsX的設(shè)備，本實(shí)驗(yàn)中為js0。程序運(yùn)行結(jié)果圖機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)準(zhǔn)備工作方向盤數(shù)據(jù)信息結(jié)果圖運(yùn)行如下命令查看方向盤的數(shù)據(jù)：$rosrunjoyjoy_node$rostopicecho/joy現(xiàn)在轉(zhuǎn)動(dòng)方向盤，或者按下套件中的按鍵，將會(huì)觀察到相應(yīng)的輸出，如圖所示，方向盤套件的每個(gè)按鈕都對(duì)應(yīng)一個(gè)buttons數(shù)值，方向盤、油門、剎車以及離合均對(duì)應(yīng)一個(gè)axes數(shù)值。機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)建立數(shù)據(jù)解析節(jié)點(diǎn)獲取到方向盤數(shù)據(jù)后，需要對(duì)方向盤數(shù)據(jù)進(jìn)行解析并轉(zhuǎn)換為控制車運(yùn)動(dòng)的速度和轉(zhuǎn)向角。首先，在teleoperation功能包的launch文件夾里使用如下命令創(chuàng)建一個(gè)g29.launch文件：$sudogeditg29.launch在文檔里添加如下內(nèi)容：<?xmlversion="1.0"?><launch><groupns="G29"><nodepkg="joy"type="joy_node"name="joy"><paramname="coalesce_interval"type="double"value="0.02"/><paramname="default_trig_val"value="true"/><!--paramname="deadzone"value="0.0"/--><!--paramname="dev"value="/dev/input/js0"type="string"/--><paramname="dev"value="/dev/input/js0"type="string"/><paramname="deadzone"value="0.03"type="double"/><!--aramname="autorepeat_rate"value="10"type="double"/--></node></group></launch>機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)建立數(shù)據(jù)解析節(jié)點(diǎn)將joy節(jié)點(diǎn)歸為一個(gè)名為“G29”的組里，<group>標(biāo)簽可以對(duì)節(jié)點(diǎn)分組，具有ns屬性，可以讓節(jié)點(diǎn)歸屬某個(gè)命名空間。下面來(lái)解釋一下各個(gè)參數(shù)代表的含義：dev：G29方向盤設(shè)備地址。數(shù)據(jù)類型是字符串，默認(rèn)位置在/dev/input/js0。deadzone：死區(qū)（雙精度，默認(rèn)值：0.03）。死區(qū)是操縱桿能夠被識(shí)別到偏離軸中心之前必須移動(dòng)的量。此參數(shù)相對(duì)于規(guī)范化在-1和1之間的軸指定。因此，0.1意味著操縱桿必須移動(dòng)10%到軸范圍的邊緣，然后該軸才會(huì)輸出非零值。autorepeat_rate：狀態(tài)不變的操縱桿將自動(dòng)發(fā)送上一次發(fā)送的方向盤數(shù)據(jù)的速率（以赫茲為單位）。其數(shù)據(jù)類型是雙精度型，默認(rèn)值為10。coalesce_interval：在合并時(shí)間間隔（秒）內(nèi)接收到的axisevent將在單個(gè)ROS消息中發(fā)送。由于內(nèi)核將每個(gè)axis的運(yùn)動(dòng)作為一個(gè)單獨(dú)的事件發(fā)送，合并后大大降低了消息的發(fā)送速率。此選項(xiàng)還可用于限制傳出消息的速率。機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)建立數(shù)據(jù)解析節(jié)點(diǎn)通過(guò)以下指令運(yùn)行l(wèi)aunch文件：$roslaunchteleoperationg29.launch在teleoperation功能包的scripts文件夾里，創(chuàng)建steeringwheel_cmd_dt.py文件，建立方向盤數(shù)據(jù)解析節(jié)點(diǎn)，并將如下內(nèi)容添加到文件中：#!/usr/bin/envpython#coding=utf-8-*-importrospyimportthreadingimporttimeimportmathimportserialfromgeometry_msgs.msgimportTwistfromsensor_msgs.msgimportJoy

cmd_pub=rospy.Publisher('/remote/cmd_vel',Twist,queue_size=1)twist_msg=Twist()globalbrakebrake=0globalwheel_dirwheel_dir=1.0globalscalescale=1.0機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)建立數(shù)據(jù)解析節(jié)點(diǎn)defpub_twist_thread():globalbrakewhileTrue: cmd_pub.publish(twist_msg)time.sleep(0.2)

defjoy_cb(msg):globalbrakeglobalwheel_dirglobalscaleifmsg.buttons[18]==1: wheel_dir=-1.0 scale=1.0else: wheel_dir=1.0

ifmsg.buttons[12]==0andmsg.buttons[13]==0andmsg.buttons[14]==0andmsg.buttons[15]==0andmsg.buttons[16]==0andmsg.buttons[17]==0andmsg.buttons[18]==0: scale=0.0twist_msg.angular.z=msg.axes[0]*4.0twist_msg.linear.x=(msg.axes[2]+1.0)*0.25*wheel_dir*scaleif(msg.axes[3]>-0.95): brake=1else: brake=0if(brake==1):twist_msg.linear.x=0.0

defdata_rev():rospy.init_node('joy_receiver',anonymous=True)joy_sub=rospy.Subscriber('/G29/joy',Joy,joy_cb)t_twist=threading.Thread(target=pub_twist_thread)t_twist.start()rospy.spin()

if__name__=='__main__':data_rev()整體程序的流程圖機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)建立數(shù)據(jù)解析節(jié)點(diǎn)——代碼解釋第一行確保了這個(gè)腳本會(huì)被看作Python腳本，第二行聲明python代碼的文本格式是utf-8編碼的，也即告訴python解釋器要按照utf-8編碼的方式來(lái)讀取程序。接下來(lái)的import和fromimport是引用了一些我們?cè)谀_本中需要用到的組件，importrospy是引用ROS的核心Python庫(kù)。threading、time、math依次是線程模塊、時(shí)間模塊、數(shù)學(xué)模塊。在當(dāng)前例子中，我們需要用到ROS的geometry_msgs包中的Twist消息類型以及sensor_msgs包里的Joy消息類型。1.頭文件部分#!/usr/bin/envpython#coding=utf-8-*-importrospyimportthreadingimporttimeimportmathimportserialfromgeometry_msgs.msgimportTwistfromsensor_msgs.msgimportJoy機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)建立數(shù)據(jù)解析節(jié)點(diǎn)——代碼解釋這里定義了一個(gè)用來(lái)發(fā)布Twist類型的消息給/remote/cmd_vel話題的ROS發(fā)布者，隊(duì)列大小為1，若發(fā)布的消息超過(guò)隊(duì)列大小，則丟棄最早發(fā)布的消息。下面是定義的全局變量brake（剎車）、wheel_dir（車輪前進(jìn)指令）、scale（放大系數(shù)）、serial_str（串口發(fā)送的字符串?dāng)?shù)據(jù)）。2.參數(shù)定義cmd_pub=rospy.Publisher('/remote/cmd_vel',Twist,queue_size=1)twist_msg=Twist()globalbrakebrake=0globalwheel_dirwheel_dir=1.0globalscalescale=1.0機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)建立數(shù)據(jù)解析節(jié)點(diǎn)——代碼解釋定義一個(gè)發(fā)布Twist消息數(shù)據(jù)的線程，當(dāng)線程打開時(shí)，發(fā)布器cmd_pub循環(huán)發(fā)送twist_msg消息，循環(huán)發(fā)送間隔為0.2s。3.消息發(fā)布函數(shù)defpub_twist_thread():globalbrakewhileTrue: cmd_pub.publish(twist_msg)time.sleep(0.2)機(jī)器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實(shí)驗(yàn)建立數(shù)據(jù)解析節(jié)點(diǎn)——代碼解釋定義一個(gè)回調(diào)函數(shù)joy_cb()，先初始化全局變量brake、wheel_dir、scale、serial_str。若msg.buttons[18]為1,則將-1.0賦值給wheel_dir，scale為1,若msg.buttons[18]不為1（為0），則將1賦值給wheel_dir，這說(shuō)明了msg.buttons[18]決定小車掛的是前進(jìn)擋還是倒擋。接下來(lái)的if和elif說(shuō)明了msg.buttons[12]—[17]依次代表一擋到六擋。他們的scale是依次遞增的。若msg.buttons[12]到msg.buttons[18]都為0，即小車是靜止的，故scale為0。將msg.axes[0]的值放大4倍賦值給twist_msg.angular.z，作為小車運(yùn)動(dòng)的z軸角速度。將msg.axes[2]的值加一（使之變?yōu)檎担┏艘?.25再乘以車輪前進(jìn)方向最后乘以放大倍數(shù)得到的值賦給twist_msg.linear.x，作為小車運(yùn)動(dòng)的x軸線速度。若msg.axes[3]大于-0.95，brake為1，否則為0。若此時(shí)brake為1,那么z軸角速度和x軸線速度都為0，若twist_msg.linear.x為0那么z軸角速度也為0。最后一行語(yǔ)句表達(dá)了serial_str的組成，注意，整型數(shù)據(jù)要轉(zhuǎn)化成字符型數(shù)據(jù)，保留三位小數(shù)。4.回調(diào)函數(shù)defjoy_cb(msg):globalbrakeglobalwheel_dirglobalscaleifmsg.buttons[1