機器人操作系統(tǒng)（ROS）課件匯 曾慶喜 第7-11章 ROS 基礎應用實例-ROS-2簡介

機器人操作系統(tǒng)（ROS）機器人操作系統(tǒng)（ROS）遙控小海龜運動7.1機器人操作系統(tǒng)（ROS）7.1遙控小海龜運動

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

紅外遙控器

游戲手柄

仿真小海龜機器人操作系統(tǒng)（ROS）7.1.1紅外遙控器控制小海龜運動硬件基礎

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

紅外遙控器紅外解碼器USB轉(zhuǎn)TTL模塊紅外遙控器按鍵對應的16進制編碼紅外遙控器按鍵對應的16進制編碼，其中用戶碼為“00FF”，串口輸出數(shù)據(jù)為“用戶碼+鍵位碼”，例如我們按下“OK”鍵，串口則輸出“00FF1C”。機器人操作系統(tǒng)（ROS）軟件基礎在終端中輸入以下命令安裝ROS串口功能包：$catkin_create_pkgteleoperationstd_msgsrospyroscpp輸入以下命令檢測serial包是否安裝好：$rospackfindserial若終端顯示serial的路徑(/opt/ros/melodic/share/serial)，則說明安裝成功。7.1.1紅外遙控器控制小海龜運動機器人操作系統(tǒng)（ROS）在~/catkin_ws/src下輸入以下命令創(chuàng)建一個名為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;//聲明串口對象

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

uint8_ta;intmain(intargc,char**argv){ros::init(argc,argv,"serial_remote_node");//初始化節(jié)點ros::NodeHandlenh;//聲明節(jié)點句柄 ros::Publisherread_pub=nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1);//發(fā)布話題……7.1.1紅外遙控器控制小海龜運動軟件基礎機器人操作系統(tǒng)（ROS）代碼解釋ros/ros.h包含大部分ROS中通用的頭文件，serial/serial.h即ROS中已經(jīng)內(nèi)置了的串口包，通過它可以與串口設備進行通訊，另外節(jié)點會發(fā)布geometry_msgs/Twist類型的消息，所以需要加入包含該類型的頭文件Twist.h，這個頭文件根據(jù)Twist.msg的消息結(jié)構(gòu)定義自動生成。1.頭文件部分#include<ros/ros.h>#include<serial/serial.h>#include<geometry_msgs/Twist.h>7.1.1紅外遙控器控制小海龜運動機器人操作系統(tǒng)（ROS）代碼解釋在main函數(shù)中，通過init函數(shù)初始化ros節(jié)點，節(jié)點名稱為serial_remote_node，該名稱在運行的ROS中是唯一的，不允許同時存在名稱相同的兩個節(jié)點。通過ros::NodeHandlenh聲明節(jié)點句柄，最后創(chuàng)建一個名為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紅外遙控器控制小海龜運動機器人操作系統(tǒng)（ROS）代碼解釋進入trycatch語句，設置串口設備號為/dev/ttyUSB0，串口波特率為9600b/s，串口超時時間為1000ms，設置完成后打開串口。當串口打開錯誤時，在終端報錯："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紅外遙控器控制小海龜運動機器人操作系統(tǒng)（ROS）代碼解釋ros::Rateloop_rate(50);geometry_msgs::Twistvel;指定程序循環(huán)的頻率為50hz，并創(chuàng)建一個geometry_msgs::Twist類型的變量vel，用于存儲需要發(fā)布的線速度和角速度數(shù)據(jù)。while(ros::ok()){if(ser.available()){ ...... } ros::spinOnce loop_rate.sleep();}進入while循環(huán)，通過ser.available()函數(shù)接收串口緩沖區(qū)中當前剩余的字符個數(shù)，判斷串口的緩沖區(qū)有無數(shù)據(jù)。當ser.available()>0時，說明串口接收到了數(shù)據(jù)，可以進行數(shù)據(jù)解析，否則等待串口數(shù)據(jù)接收。7.1.1紅外遙控器控制小海龜運動2.主函數(shù)部分機器人操作系統(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})修改完成之后，在工作空間中進行編譯：$cd~/catkin_ws

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

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

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

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

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

$catkin_make7.1.2游戲手柄控制小海龜運動機器人操作系統(tǒng)（ROS）我們創(chuàng)建一個xbox_remote.launch的啟動文件，為參數(shù)服務器聲明一些變量，并同時啟動turtlesim、joy和xbox_remote三個節(jié)點。創(chuà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游戲手柄控制小海龜運動機器人操作系統(tǒng)（ROS）在啟動文件中啟動一個節(jié)點需要三個屬性：pkg、type和name。其中pkg定義節(jié)點所在的功能包名稱，type定義節(jié)點的可執(zhí)行文件名稱，name屬性用來定義節(jié)點運行的名稱，這將覆蓋節(jié)點中init()函數(shù)賦予節(jié)點的名稱。在啟動joy_node節(jié)點時，我們可以修改兩個參數(shù)：dev：游戲手柄設備地址；默認為/dev/input/js0。deadzone：死區(qū)（雙精度，默認值：0.12）。死區(qū)是操縱桿能夠被識別到偏離軸中心之前必須移動的量。0.12意味著操縱桿必須移動12%到軸范圍的邊緣，然后該軸才會輸出非零值。解釋啟動文件7.1.2游戲手柄控制小海龜運動機器人操作系統(tǒng)（ROS）輸入以下命令運行xbox_remote.launch啟動文件：$sourcecatkin_ws/devel/setup.bash$roslaunchserialportxbox_remote.launch此時，你就可以通過游戲手柄操控小海龜了，另起終端運行：$rostopicecho/turtle1/cmd_vel通過如下命令可查看當前ROS運行的節(jié)點圖：$rqt_graph

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

GPS是由一種具有全方位、全天候、全時段、高精度的基于空間的衛(wèi)星導航系統(tǒng)，能在任何時間、任何天氣、任何地點為全球用戶提供低成本、高精度的三維位置、速度和精確定時等導航信息。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(實時運動學)5=FloatRTK6=estimated(deadreckoning)(2.3feature)(估計航路推算)7=Manualinputmode(手動輸入模式)8=Simulationmode(仿真模式)08 Numberofsatellitiesbeingtracked(已跟蹤衛(wèi)星數(shù))0.9 Horizontaldilutionofposition(水平位置精度)545.4,MAltitude,Meters,abovemeansealevel(高度，米，平均海平面)46.9,M Heightofgeoid(meansealevel)aboveWGS84ellipsoid(大地水準面高（平均海平面）高于WGS84橢球)(emptyfield)timeinsecondssincelastDGPSupdate(自上一次DGPS更新時間間隔，以秒為單位)(emptyfield)DGPSstationIDnumber(DGPS基站ID號)*47 thechecksumdata,alwaysbeginswith*(校驗和數(shù)據(jù))GPGGA格式機器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換

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

高斯-克呂格投影示意圖如圖所示，該投影是用一個假想的橢圓柱筒橫置于地球表面，與地球上的某一經(jīng)線相切，即投影后的中央經(jīng)線，該橢圓柱的中心軸位于赤道平面內(nèi)，然后按照一定投影條件將地球橢球面中央子午線兩側(cè)規(guī)定范圍內(nèi)的點投影到橢球圓柱面上，從而得到地球橢球面上各個點的高斯投影。高斯投影示意圖機器人操作系統(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度分為一個投影帶，將地球劃分為60個投影帶，并依次進行編號。6度帶和3度帶的分帶投影機器人操作系統(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)換為大地坐標系下的坐標

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é)點

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!")機器人操作系統(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，隊列長度為1gps_pub=rospy.Publisher('/gps/flat',Point,queue_size=1)#經(jīng)緯度轉(zhuǎn)換為大地坐標系下的坐標函數(shù)defLatLonToXY(Lat,Lon): fi=Lat/180*math.pi la=(Lon-RefCenterLon)/180*math.pi zi=RefCenterLat/180*math.pi a=6378137#長半軸 b=6356752.3142#短半軸 c=6399593.6258#極點處子午線的曲率半徑 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)緯度坐標轉(zhuǎn)換示例代碼getGPS.py。getGPS.py的主要功能為將經(jīng)緯度坐標轉(zhuǎn)換為大地平面坐標，訂閱/fix話題（經(jīng)緯度原始數(shù)據(jù)），發(fā)布/gps/flat話題（轉(zhuǎn)換后的大地坐標位置數(shù)據(jù)）。機器人操作系統(tǒng)（ROS）7.2.2高斯-克呂格投影變換啟動GPS后，運行如下命令進行經(jīng)緯度轉(zhuǎn)換：$rosrungps_pubgetGPS.py運行如下命令，查看話題列表，話題/gps/flat內(nèi)容為經(jīng)緯度轉(zhuǎn)換后的坐標，如圖所示：$rostopiclist

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

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

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

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

①時間戳完全對齊策略(ExactTimePolicy)

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

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

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

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

rospy.init_node('angle_publisher',anonymous=True) #發(fā)布話題，話題名為/imu/angle，類型為PoseStamped，隊列長度為10 angle_pub=rospy.Publisher('/imu/angle',PoseStamped,queue_size=10) #設置循環(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)頻率延時

rate.sleep()if__name__=='__main__': try: angle_publisher() exceptrospy.ROSInterruptException: pass建立IMU發(fā)布節(jié)點(angle_pub.py)：angle_pub.py的功能是模擬IMU不停地發(fā)布角度信息。它發(fā)布/imu/angle話題消息，類型為geometry_msgs/PoseStamped。機器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實驗#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyimportcv2importnumpyasnpimportmessage_filtersimportosfromos.pathimportjoinfromsensor_msgs.msgimportImagefromgeometry_msgs.msgimportPoseStampedfromcv_bridgeimportCvBridge,CvBridgeError#初始化參數(shù)frame_id=0gps_id=0#設置保存路徑，nuaa-frl為本機用戶名，讀者需自行修改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é)點(CAM_IMU_test.py)：CAM_IMU_test.py的功能是將相機和IMU的數(shù)據(jù)進行同步采集并保存。它訂閱/imu/angle話題和/usb_cam/image_raw話題，并通過時間同步器TimeSynchronizer進行數(shù)據(jù)同步采集，最終保存到本地文件中。機器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實驗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é)點

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) #如果兩個話題的時間戳相差在0.1s之內(nèi)則認為時間戳已經(jīng)對齊

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()返回一個列表，包含有指定路徑下的目錄和文件的名稱

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

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

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)容將會被寫入到已有內(nèi)容之后。如果該文件不存在，創(chuàng)建新文件進行寫入

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

f.truncate()#截斷文件

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

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

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

遙操作即遠程操作，操作者通過操縱主端設備，再由主端設備控制從端設備來完成一系列工作任務。

一個簡單的遙操作系統(tǒng)通常由一個操作員、一個主操作器、一個主控制器、一個通信信道、一個從控制器、一個從操作器和一個環(huán)境組成，如圖所示。遙操作的基本結(jié)構(gòu)圖CarSim小型駕駛模擬器太空遙操作主從機器人模擬室機器人操作系統(tǒng)（ROS）7.4.2實驗環(huán)境基礎硬件基礎在本實驗中，使用的設備是G29方向盤總成，包括方向盤本體、腳踏板以及換擋桿，結(jié)構(gòu)如圖所示。G29方向盤旋轉(zhuǎn)角度與真實汽車方向盤旋轉(zhuǎn)圈數(shù)相同，并帶有力反饋效果，具有六檔變速桿，模擬真實駕駛體驗。G29方向盤總成圖機器人操作系統(tǒng)（ROS）7.4.2實驗環(huán)境基礎軟件基礎創(chuàng)建功能包，使用如下命令創(chuàng)建一個名為teleoperation的文件夾：$catkin_create_pkgteleoperationstd_msgsrospyroscpp創(chuàng)建完成后，代碼空間src會生成一個teleoperation功能包，里面包含package.xml和CMakelist.txt文件?；氐焦ぷ骺臻g的根目錄下使用catkin_make進行編譯，然后使用如下命令設置環(huán)境變量：$source~/catkin_ws/devel/setup.bash以上就創(chuàng)建好了一個工作空間和功能包，接下來的遙操作例程就在這個teleoperation功能包里進行。機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗準備工作首先安裝功能包：$sudoapt-getinstallros-melodic-joy接下來，需要查看G29是否能正常使用，先運行如下命令：$ls/dev/input可以看到結(jié)果如圖所示，其中若識別成功，可以看到有一個jsX的設備，本實驗中為js0。程序運行結(jié)果圖機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗準備工作方向盤數(shù)據(jù)信息結(jié)果圖運行如下命令查看方向盤的數(shù)據(jù)：$rosrunjoyjoy_node$rostopicecho/joy現(xiàn)在轉(zhuǎn)動方向盤，或者按下套件中的按鍵，將會觀察到相應的輸出，如圖所示，方向盤套件的每個按鈕都對應一個buttons數(shù)值，方向盤、油門、剎車以及離合均對應一個axes數(shù)值。機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗建立數(shù)據(jù)解析節(jié)點獲取到方向盤數(shù)據(jù)后，需要對方向盤數(shù)據(jù)進行解析并轉(zhuǎn)換為控制車運動的速度和轉(zhuǎn)向角。首先，在teleoperation功能包的launch文件夾里使用如下命令創(chuàng)建一個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>機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗建立數(shù)據(jù)解析節(jié)點將joy節(jié)點歸為一個名為“G29”的組里，<group>標簽可以對節(jié)點分組，具有ns屬性，可以讓節(jié)點歸屬某個命名空間。下面來解釋一下各個參數(shù)代表的含義：dev：G29方向盤設備地址。數(shù)據(jù)類型是字符串，默認位置在/dev/input/js0。deadzone：死區(qū)（雙精度，默認值：0.03）。死區(qū)是操縱桿能夠被識別到偏離軸中心之前必須移動的量。此參數(shù)相對于規(guī)范化在-1和1之間的軸指定。因此，0.1意味著操縱桿必須移動10%到軸范圍的邊緣，然后該軸才會輸出非零值。autorepeat_rate：狀態(tài)不變的操縱桿將自動發(fā)送上一次發(fā)送的方向盤數(shù)據(jù)的速率（以赫茲為單位）。其數(shù)據(jù)類型是雙精度型，默認值為10。coalesce_interval：在合并時間間隔（秒）內(nèi)接收到的axisevent將在單個ROS消息中發(fā)送。由于內(nèi)核將每個axis的運動作為一個單獨的事件發(fā)送，合并后大大降低了消息的發(fā)送速率。此選項還可用于限制傳出消息的速率。機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗建立數(shù)據(jù)解析節(jié)點通過以下指令運行l(wèi)aunch文件：$roslaunchteleoperationg29.launch在teleoperation功能包的scripts文件夾里，創(chuàng)建steeringwheel_cmd_dt.py文件，建立方向盤數(shù)據(jù)解析節(jié)點，并將如下內(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機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗建立數(shù)據(jù)解析節(jié)點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()整體程序的流程圖機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗建立數(shù)據(jù)解析節(jié)點——代碼解釋第一行確保了這個腳本會被看作Python腳本，第二行聲明python代碼的文本格式是utf-8編碼的，也即告訴python解釋器要按照utf-8編碼的方式來讀取程序。接下來的import和fromimport是引用了一些我們在腳本中需要用到的組件，importrospy是引用ROS的核心Python庫。threading、time、math依次是線程模塊、時間模塊、數(shù)學模塊。在當前例子中，我們需要用到ROS的geometry_msgs包中的Twist消息類型以及sensor_msgs包里的Joy消息類型。1.頭文件部分#!/usr/bin/envpython#coding=utf-8-*-importrospyimportthreadingimporttimeimportmathimportserialfromgeometry_msgs.msgimportTwistfromsensor_msgs.msgimportJoy機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗建立數(shù)據(jù)解析節(jié)點——代碼解釋這里定義了一個用來發(fā)布Twist類型的消息給/remote/cmd_vel話題的ROS發(fā)布者，隊列大小為1，若發(fā)布的消息超過隊列大小，則丟棄最早發(fā)布的消息。下面是定義的全局變量brake（剎車）、wheel_dir（車輪前進指令）、scale（放大系數(shù)）、serial_str（串口發(fā)送的字符串數(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機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗建立數(shù)據(jù)解析節(jié)點——代碼解釋定義一個發(fā)布Twist消息數(shù)據(jù)的線程，當線程打開時，發(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)機器人操作系統(tǒng)（ROS）7.4.3指令解析轉(zhuǎn)發(fā)實驗建立數(shù)據(jù)解析節(jié)點——代碼解釋定義一個回調(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，這說明了msg.buttons[18]決定小車掛的是前進擋還是倒擋。接下來的if和elif說明了msg.buttons[12]—[17]依次代表一擋到六擋。他們的scale是依次遞增的。若msg.buttons[12]到msg.buttons[18]都為0，即小車是靜止的，故scale為0。將msg.axes[0]的值放大4倍賦值給twist_msg.angular.z，作為小車運動的z軸角速度。將msg.axes[2]的值加一（使之變?yōu)檎担┏艘?.25再乘以車輪前進方向最后乘以放大倍數(shù)得到的值賦給twist_msg.linear.x，作為小車運動的x軸線速度。若msg.axes[3]大于-0.95，brake為1，否則為0。若此時brake為1,那么z軸角速度和x軸線速度都為0，若twist_msg.linear.x為0那么z軸角速度也為0。最后一行語句表達了serial_str的組成，注意，整型數(shù)據(jù)要轉(zhuǎn)化成字符型數(shù)據(jù)，保留三位小數(shù)。4.回調(diào)函數(shù)defjoy_cb(msg):globalbrakeglobalwheel_dirglobalscaleifmsg.buttons[1