[ROS Husky主題實作] 單元2 : 創建畫Husky Car移動軌跡的 Subscriber (訂閱者/節點)

<source : gazebo中的模擬截圖>

一、前言

本系列文章乃依據<clearpathrobotics>進行實作與紀錄。在前篇文章，我們實作了撰寫 Publisher 的技巧。既然有 Publisher，自然會希望有 Subscriber，這樣就可以看到節點進行完整的發布與訂閱動作。接下來，我們將會學習實作一個Subscriber的節點程式，訂閱 Husky Car的里程計Topic，並且畫出移動路徑。

不同以往撰寫程式碼，我們會透過 Github 來獲取所需的套件包。

二、實作解說

(一)、安裝所需工具 Git 和 python-pygame

$ sudo apt-get install git

$ sudo apt-get install python-pygame

(二)、git 工具初始化

Github是開發者常用的版本控制工具之一，大部份的ROS軟體都會連結到Github資料庫。使用者通常會從 Github 的伺服器 "拉" 檔案，修改，然後再將這些修改的內容 "推" 回伺服器。在本次實作中，我們也會練習如何使用Github來 "拉" ROS套件，接下來將會創建一個新的路徑來 "拉" 這些套件。

$ mkdir -p ~/catkin_ws/src/ros101

$ cd ~/catkin_ws/src/ros101

$ git init

(三)、"拉" Gitub 套件

若我們知道套件的 Github 網址，就能輕鬆地從 Github "拉" 套件。指令很簡單，只要 git pull + 網址即可，本例如下 :

$ git pull https://github.com/mcoteCPR/ROS101.git

"拉" 下套件後，我們在 ros101 的路徑上，就會看到 "src" 和 "launch" 資料夾，同時還有 Cmakelsits.txt 和 package.xml。

(四)、撰寫 Subscriber 程式

因為 Github 套件中，已有完整的程式碼，接下來我們只要確認內容是否符合我們的需求，因此透過文字編輯器開啟檔案的指令如下 :

$ gedit ~/catkin_ws/src/ros101/src/odom_graph.py

(五)、編譯套件包

$ cd ~/catkin_ws

$ catkin_make

(六)、啟動與 husky_gazebo 相關的Nodes 和編譯完成的 套件包

$ roslaunch husky_gazebo husky_emepty_world.launch

$ source ~/catkin_ws/devel/setup.bash

$ roslaunch ros101 odom_graph_test.launch

成功畫面如下圖所示，Husky Car會在Gazebo的環境中自主移動，而剛撰寫來記錄移動軌跡的Subscriber python程式將會在另一個名為 "Husky Odom Graph" 視窗執行。

三、有異於範例工作空間名稱的實作紀錄

本系列範例的工作空間為 "ros101"，但我們已習慣以 "catkin_ws" 的名稱來進行開發。因此，以上個部分的步驟來進行，到了 "catkin_make" 這個編譯的步驟就會遇到 Error。

只要進行2個小修改，就可以成功編譯。

1.將 Git Pull來的python檔案移到  "random_husky_driver/src"這個資料夾

和C++這種編譯式語言不同，Python是直譯式語言，因此在CMakeList.txt無須作額外的修改。

2.將 Git Pull來的launch檔案移到  "random_husky_driver"這個資料夾並修改

放的位置不是在 "src" 內，而是和 "random_husky_driver"的"src"是同一層，如下圖所示。

原本的 package name是 "ros101"，記得修改成你的套件名稱。沒改的話，還是會有錯誤。以本文為例，為 "random_husky_driver"。

你可以直接用滑鼠依照資料夾的階層，打開該檔案進行修改。或是用gedit指令，打開檔案修改。

完成上述步驟後，記得將"ros101"整個資料夾刪掉，回到"catkin_ws"再進行編譯。原則上，你就可以成功接到第二部分的第6步驟了。

四、程式碼解說

---

#!/usr/bin/env python

import rospy

from nav_msgs.msg import Odometry

import pygame, sys

from pygame.locals import *

#Initiates the display

pygame.init()

windowSurfaceObj= pygame.display.set_mode((600,600))

pygame.display.set_caption('Husky Odom Graph')

yellow = pygame.Color(245,210,0)

windowSurfaceObj.fill(pygame.Color(0,0,0))

pygame.display.update()

old_x = 300

old_y = 300

#Callback function, draws a line from the last odom point to a new one

def odomCB(msg):

global old_x

global old_y

new_x=int(abs((msg.pose.pose.position.x * 150)))+300

new_y=int(abs((msg.pose.pose.position.y * 150)))+300

pygame.draw.line(windowSurfaceObj, yellow, (old_x,old_y), (new_x, new_y), 2)

pygame.display.update()

old_x=new_x

old_y=new_y

def listener():

rospy.init_node('odom_graph', anonymous=True)

#     changed publish to read "husky_velocity_controller/cmd_vel"

#     change repairs issues under indigo 

rospy.Subscriber("husky_velocity_controller/odom", Odometry, odomCB)

rospy.spin()

if __name__=="__main__":

listener()

---

(一)

import rospy

from nav_msgs.msg import Odometry

載入相關ROS套件。rospy定義了ROS運作基本功能，nav_msgs.msg 則定義和里程計相關的訊息格式。

(二)

def odomCB(msg)

此函示為里程計的返回函式，每當Subscriber接收一個訊息則回傳。此函式定義的內容在上一次座標間讀取里程計位置訊息後，在我們的顯示畫面上畫線。

(三)

def listener()

定義一個listener的節點，"anonymous=True" 意指多個同樣的節點可以在同一個時間執行。

rospy.init_node('odom_graph', anonymous=True)

初始化節點，並讀取名為 "odom" topic的訊息，並呼叫 在odomCB函式接收訊息時呼叫它。

rospy.spin()

讓節點持續工作，直到被關閉。

---

參考資料 :

• ROS.org : WritingPublisherSubscriber(c++)
• clearpathrobotics
• [CSDN] ROS下的CMakeList.txt編寫