2.5 通訊機制實操
2.5 通訊機制實操
本節主要是通過ROS內建的turtlesim案例，結合已經介紹ROS命令獲取節點、話題、話題訊息、服務、服務訊息與引數的資訊，最終再以編碼的方式實現烏龜運動的控制、烏龜位姿的訂閱、烏龜生成與烏龜窗體背景顏色的修改。
目的:熟悉、強化通訊模式應用
2.5.1 實操01_話題釋出
需求描述:編碼實現烏龜運動控制，讓小烏龜做圓周運動。
結果演示:
實現分析:
- 烏龜運動控制實現，關鍵節點有兩個，一個是烏龜運動顯示節點 turtlesim_node，另一個是控制節點，二者是訂閱釋出模式實現通訊的，烏龜運動顯示節點直接呼叫即可，運動控制節點之前是使用的 turtle_teleop_key通過鍵盤 控制，現在需要自定義控制節點。
- 控制節點自實現時，首先需要了解控制節點與顯示節點通訊使用的話題與訊息，可以使用ros命令結合計算圖來獲取。
- 瞭解了話題與訊息之後，通過 C++ 或 Python 編寫運動控制節點，通過指定的話題，按照一定的邏輯釋出訊息即可。
實現流程:
- 通過計算圖結合ros命令獲取話題與訊息資訊。
- 編碼實現運動控制節點。
- 啟動 roscore、turtlesim_node 以及自定義的控制節點，檢視執行結果。
1.話題與訊息獲取
準備:先啟動鍵盤控制烏龜運動案例。
1.1話題獲取
獲取話題:/turtle1/cmd_vel
通過計算圖檢視話題，啟動計算圖:
rqt_graph
或者通過 rostopic 列出話題:
rostopic list
1.2訊息獲取
獲取訊息型別:geometry_msgs/Twist
rostopic type /turtle1/cmd_vel
獲取訊息格式:
rosmsg info geometry_msgs/Twist
響應結果:
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
linear(線速度) 下的xyz分別對應在x、y和z方向上的速度(單位是 m/s)；
angular(角速度)下的xyz分別對應x軸上的翻滾、y軸上俯仰和z軸上偏航的速度(單位是rad/s)。
詳情請檢視補充資料。
2.實現釋出節點
建立功能包需要依賴的功能包: roscpp rospy std_msgs geometry_msgs
實現方案A:C++
/*
編寫 ROS 節點，控制小烏龜畫圓
準備工作:
1.獲取topic(已知: /turtle1/cmd_vel)
2.獲取訊息型別(已知: geometry_msgs/Twist)
3.執行前，注意先啟動 turtlesim_node 節點
實現流程:
1.包含標頭檔案
2.初始化 ROS 節點
3.建立釋出者物件
4.迴圈釋出運動控制訊息
*/
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 節點
ros::init(argc,argv,"control");
ros::NodeHandle nh;
// 3.建立釋出者物件
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);
// 4.迴圈釋出運動控制訊息
//4-1.組織訊息
geometry_msgs::Twist msg;
msg.linear.x = 1.0;
msg.linear.y = 0.0;
msg.linear.z = 0.0;
msg.angular.x = 0.0;
msg.angular.y = 0.0;
msg.angular.z = 2.0;
//4-2.設定傳送頻率
ros::Rate r(10);
//4-3.迴圈傳送
while (ros::ok())
{
pub.publish(msg);
ros::spinOnce();
}
return 0;
}
配置檔案此處略
實現方案B:Python
#! /usr/bin/env python
"""
編寫 ROS 節點，控制小烏龜畫圓
準備工作:
1.獲取topic(已知: /turtle1/cmd_vel)
2.獲取訊息型別(已知: geometry_msgs/Twist)
3.執行前，注意先啟動 turtlesim_node 節點
實現流程:
1.導包
2.初始化 ROS 節點
3.建立釋出者物件
4.迴圈釋出運動控制訊息
"""
import rospy
from geometry_msgs.msg import Twist
if __name__ == "__main__":
# 2.初始化 ROS 節點
rospy.init_node("control_circle_p")
# 3.建立釋出者物件
pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=1000)
# 4.迴圈釋出運動控制訊息
rate = rospy.Rate(10)
msg = Twist()
msg.linear.x = 1.0
msg.linear.y = 0.0
msg.linear.z = 0.0
msg.angular.x = 0.0
msg.angular.y = 0.0
msg.angular.z = 0.5
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()
許可權設定以及配置檔案此處略
3.執行
首先，啟動 roscore；
然後啟動烏龜顯示節點；
最後執行運動控制節點；
最終執行結果與演示結果類似。
補充資料1:
弧度:單位弧度定義為圓弧長度等於半徑時的圓心角。
補充資料2:偏航、翻滾與俯仰
座標系圖解:
偏航:
俯仰:
翻滾:
2.5.2 實操02_話題訂閱
需求描述:已知turtlesim中的烏龜顯示節點，會發布當前烏龜的位姿(窗體中烏龜的座標以及朝向)，要求控制烏龜運動，並時時列印當前烏龜的位姿。
結果演示:
實現分析:
- 首先，需要啟動烏龜顯示以及運動控制節點並控制烏龜運動。
- 要通過ROS命令，來獲取烏龜位姿釋出的話題以及訊息。
- 編寫訂閱節點，訂閱並列印烏龜的位姿。
實現流程:
- 通過ros命令獲取話題與訊息資訊。
- 編碼實現位姿獲取節點。
- 啟動 roscore、turtlesim_node 、控制節點以及位姿訂閱節點，控制烏龜運動並輸出烏龜的位姿。
1.話題與訊息獲取
獲取話題:/turtle1/pose
rostopic list
獲取訊息型別:turtlesim/Pose
rostopic type /turtle1/pose
獲取訊息格式:
rosmsg info turtlesim/Pose
響應結果:
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity
2.實現訂閱節點
建立功能包需要依賴的功能包: roscpp rospy std_msgs turtlesim
實現方案A:C++
/*
訂閱小烏龜的位姿: 時時獲取小烏龜在窗體中的座標並列印
準備工作:
1.獲取話題名稱 /turtle1/pose
2.獲取訊息型別 turtlesim/Pose
3.執行前啟動 turtlesim_node 與 turtle_teleop_key 節點
實現流程:
1.包含標頭檔案
2.初始化 ROS 節點
3.建立 ROS 控制代碼
4.建立訂閱者物件
5.回撥函式處理訂閱的資料
6.spin
*/
#include "ros/ros.h"
#include "turtlesim/Pose.h"
void doPose(const turtlesim::Pose::ConstPtr& p){
ROS_INFO("烏龜位姿資訊:x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f",
p->x,p->y,p->theta,p->linear_velocity,p->angular_velocity
);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 節點
ros::init(argc,argv,"sub_pose");
// 3.建立 ROS 控制代碼
ros::NodeHandle nh;
// 4.建立訂閱者物件
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
// 5.回撥函式處理訂閱的資料
// 6.spin
ros::spin();
return 0;
}
配置檔案此處略
實現方案B:Python
#! /usr/bin/env python
"""
訂閱小烏龜的位姿: 時時獲取小烏龜在窗體中的座標並列印
準備工作:
1.獲取話題名稱 /turtle1/pose
2.獲取訊息型別 turtlesim/Pose
3.執行前啟動 turtlesim_node 與 turtle_teleop_key 節點
實現流程:
1.導包
2.初始化 ROS 節點
3.建立訂閱者物件
4.回撥函式處理訂閱的資料
5.spin
"""
import rospy
from turtlesim.msg import Pose
def doPose(data):
rospy.loginfo("烏龜座標:x=%.2f, y=%.2f,theta=%.2f",data.x,data.y,data.theta)
if __name__ == "__main__":
# 2.初始化 ROS 節點
rospy.init_node("sub_pose_p")
# 3.建立訂閱者物件
sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=1000)
# 4.回撥函式處理訂閱的資料
# 5.spin
rospy.spin()
許可權設定以及配置檔案此處略
3.執行
首先，啟動 roscore；
然後啟動烏龜顯示節點，執行運動控制節點；
最後啟動烏龜位姿訂閱節點；
最終執行結果與演示結果類似。
2.5.3 實操03_服務呼叫
需求描述:編碼實現向 turtlesim 傳送請求，在烏龜顯示節點的窗體指定位置生成一烏龜，這是一個服務請求操作。
結果演示:
實現分析:
- 首先，需要啟動烏龜顯示節點。
- 要通過ROS命令，來獲取烏龜生成服務的服務名稱以及服務訊息型別。
- 編寫服務請求節點，生成新的烏龜。
實現流程:
- 通過ros命令獲取服務與服務訊息資訊。
- 編碼實現服務請求節點。
- 啟動 roscore、turtlesim_node 、烏龜生成節點，生成新的烏龜。
1.服務名稱與服務訊息獲取
獲取話題:/spawn
rosservice list
獲取訊息型別:turtlesim/Spawn
rosservice type /spawn
獲取訊息格式:
rossrv info turtlesim/Spawn
響應結果:
float32 x
float32 y
float32 theta
string name
---
string name
2.服務客戶端實現
建立功能包需要依賴的功能包: roscpp rospy std_msgs turtlesim
實現方案A:C++