Gazebo筆記(三)
2019年7月16日(二)
目錄：
一、參考網站
二、一些小發現、心得
三、建立發布節點 (使用 python)
四、建立接受器節點 (使用 python)
一、參考網站
(1) Tutorial: ROS Control
二、一些小發現、心得
(1) 發布消息到 /rrbot/joint1_position_controller/command ，使手臂動作
/rrbot/joint1_position_controller/command 是話題名稱。
std_msgs/Float64 是消息類型。
'0' 是發布的資料。(2) 印在螢幕上
rospy.loginfo("%f\n",speed_linear)(3) 話題上的消息類型
/rrbot/joint1_position_controller/command
rostopic type /rrbot/joint1_position_controller/command
----------------------------------------------------
std_msgs/Float64(分隔線)rosmsg show std_msgs/Float64
---------------------------------------------------
float64 data(分隔線)rostopic hz /rrbot/joint1_position_controller/command
----------------------------------------------------
average rate: 15.307
min: 0.000s max: 0.162s std dev: 0.03397s window: 168
所以 頻率約是 15Hz
/rrbot/joint1_position_controller/state
rostopic type /rrbot/joint1_position_controller/state
--------------------------------------------------------
control_msgs/JointControllerState(分隔線)rosmsg show control_msgs/JointControllerState
--------------------------------------------------------
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float64 set_point
float64 process_value
float64 process_value_dot
float64 error
float64 time_step
float64 command
float64 p
float64 i
float64 d
float64 i_clamp
bool antiwindup
/turtle1/cmd_vel
rostopic type /turtle1/cmd_vel
------------------------------------------------
geometry_msgs/Twist(分隔線)rosmsg show geometry_msgs/Twist
-----------------------------------------------
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
(4) 刪掉 package
rm -r name(5) 依賴項之後補加的加法
三、建立發布節點 (使用 python)
(1) 注意事項
＊ 是 usr ，而不是 user
(2) 建立 my_rrbot package，並加上依賴項(可以放消息類型相關、rospy)。
std_msgs、control_msgs 都和消息有關。
rospy 和 python 有關。
cd ~/catkin_ws/src
catkin_create_pkg my_rrbot std_msgs control_msgs rospy之後，my_rrbot pkg裡面自動會有src資料夾，把程式放在這裡就可以。
(3) 建立 rrbot_pub.py
功能： 這個程式可以發布 -1 ～ 1 的 sin波消息，讓 rrbot的joint1 去轉動。
分析：
(a) python 相關檔案要加上這樣的開頭：
#!/usr/bin/env python(b) import 和消息類型有關的東西：
因為我們想要對 /rrbot/joint1_position_controller/command 話題發布消息，而它使用的消息類型是 std_msgs/Float64 ，所以需要如下的 import：
from std_msgs.msg import Float64(c) 宣告 節點名稱：
宣告這個節點名稱為 rrbot_command，且其後會附加隨機亂數，確保該節點為獨一無二的。
rospy.init_node("rrbot_command", anonymous=True)(d) 發布消息到話題 /rrbot/joint1_position_controller/command
消息類型為 Float64 ： (注意：rospy.Publisher 是大寫！！)
move_publisher = rospy.Publisher("/rrbot/joint1_position_controller/command", Float64, queue_size=10)(e) 宣告消息變數的方法
move_msg = Float64()(f) 設定發布頻率：
經由用rostopic hz 測量 rqt_gui的發布頻率，發現其發布頻率是 15 hz，所以我這裡的發布器頻率也設為 15 hz。 ( rospy.Rate 是大寫)
rate = rospy.Rate(15) #15 hz
及
在loop中的 rate.sleep()(g) 設定消息值
由之前的 rosmsg show ，我們可以觀察到：
rosmsg show std_msgs/Float64
---------------------------------------------------
float64 data可以使用以下方式，來設定消息值( .data )：
move_msg.data = angle(e) 剩下就是一直發布消息了
(4) 執行 rrbot_pub.py
chmod +x rrbot_pub.py
rosrun my_rrbot rrbot_pub.py以下的指令是，執行所有的程式：
roslaunch rrbot_gazebo rrbot_world.launch //開啟 rrbot
roslaunch rrbot_control rrbot_control.launch //開啟 controller
rosrun my_rrbot rrbot_pub.py //發布轉動命令到topic這時候，rrbot的 joint 1會開始來回轉動( -1 ～ 1 間)。
看一下 rqt_graph：
可以看到我們的 rrbot_command節點，發布了消息到 /rrbot/joint1_position_controller/command 話題，進行joint的轉動。
(5) 講述如何補上依賴項，要實際操作
待……補 (先別看)
更改Cmakelist、package.xml的內容
待……補
(6) 同時發布 two joint 轉動 (做好玩的)
這個程式可以使兩個 joint 同時轉動，程式如下：
rosrun my_rrbot rrbot_two_pub.py看一下 rqt_graph：
觀察第二張 rqt 圖。可以看到 rrbot_command節點，發布消息到兩個話題。
四、建立接受器節點 (使用 python)
(1) 建立 rrbot_sub.py (都是放在 /catkin_ws/src/my_rrbot/src)
程式功能：在 rrbot_pub.py 運行的情況下，執行這個程式。這個節點可以訂閱 /rrbot/joint1_position_controller/command 話題，接收到 command的值。
程式：
分析：
(a) 宣告節點
宣告這個節點名稱為 rrbot_command_sub。
rospy.init_node('rrbot_command_sub', anonymous=True)(b) 訂閱話題
訂閱 /rrbot/joint1_position_controller/command 話題，接收消息類型為 Float64的消息。若接收到消息，則呼叫 callback 函式。
(注意 Subscriber大寫)
rospy.Subscriber('/rrbot/joint1_position_controller/command', Float64, callback)(c) rospy.spin()
確保節點不會退出，直到該節點被終止。
rospy.spin() simply keeps your node from exiting until the node has been shutdown. (tutorial的)
rospy.spin()(d) call back 函式
用 subdata.data，就像 rrbot_pub.py 中的寫法那樣。
def callback(subdata):
rospy.loginfo("command is %f", subdata.data)(2) 執行 rrbot_sub.py
roscore
rosrun my_rrbot rrbot_pub.py
rosrun my_rrbot rrbot_sub.py執行結果如下：
左為 rrbot_pub.py，右為 rrbot_sub.py。用螢幕截圖，可以發現它接收的非常精準。
(3) 建立 rrbot_two_sub.py
程式功能：rrbot_two_sub.py 的節點可以訂閱兩個話題，分別是 /rrbot/joint1_position_controller/command 話題和 /rrbot/joint1_position_controller/state 話題。訂閱第一個話題，可以得到發布的目標位置。訂閱第二個話題，可以得到 joint的實際位置。
程式：
分析：
(a) 宣告節點
宣告這個節點的名稱為 rrbot_command_sub
rospy.init_node('rrbot_command_sub', anonymous=True)(b) 訂閱話題
rospy.Subscriber('/rrbot/joint1_position_controller/command', Float64, callback)(c) 訂閱話題
訂閱 /rrbot/joint1_position_controller/state 話題。由之前的測量(二、一些小發現、心得)，可知道這個 topic 使用 JointControllerState 消息類型。若有接收到消息，則呼叫 callbackstate 函式。
rospy.Subscriber('/rrbot/joint1_position_controller/state', JointControllerState, callbackstate)(d) callbackstate 函式
我們的目標是讀取出 process_value，因為這個是 joint的實際位置讀值。
由之前的測量(二、一些小發現、心得)，可以推論出為什麼要寫subdata_state.process_value 。
def callbackstate(subdata_state):
rospy.loginfo("process value is %f", subdata_state.process_value)(4) 執行 rrbot_two_sub.py
執行所有程式：
roslaunch rrbot_gazebo rrbot_world.launch
roslaunch rrbot_control rrbot_control.launch
rosrun my_rrbot rrbot_pub.py
rosrun my_rrbot rrbot_two_sub.py執行結果：
註： 如果 command的值和process_value的值差一些些，可以開啟 rqt_gui，去調整他的PID，像我就是把P調成20000。
如果 command的值和process_value的值差很多(差不多6.28)，則可以去調整rrbot_pub.py 的發布值，把它加6.28或減6.28即可。
看一下rqt_graph：
我們可以看到，rrbot_command_sub節點訂閱了兩個話題！
rrbot_command節點發布了消息到一個話題！
(5) rospy.spin() 的擺放位置
spin要放在最後面。
如果把程式放在spin後面的話，程式就不會執行到那邊。
以下是正確的程式( rrbot_two_sub_test.py)：
