外部設備整合包External Process Handler

external_process_handler 外部設備整合包是一套完整的程序,可以提供使用者去執行外部的任務,整合到Seirios的介面上。

如何運用外部設備整合包,來設置您的任務呢?

Pre-setup: 請確保你的Seirios運行正常。

本指南向您展示如何使用 external_process_handler 外部設備整合包,將您自己的程序作為插件整合到 Seirios 中。

您將會需要:

  1. Seirios 導航軟體

  2. 您自備的程式碼

0. 安裝 Movel Seirios 訊息

請確認您的ROS版本。

  • 若您是 Ubuntu 20.04, ROS Noetic的使用者 :

請至以下連結下載 .deb package: https://drive.google.com/drive/folders/1hpi5NaSkFyr6QVXs83taxeHjWLulw8SH

移動 .deb 此安裝包的你的資料夾,打開終端視窗,請執行此指令 sudo apt install ./ros-noetic-movel-seirios-msgs_0.0.0-0focalXXX64.deb 以下載安裝。

  • 其餘版本的使用者:

Git clone this repo here: https://gitlab.com/movelai_public/movel_seirios_msgs/-/tree/master/srv into your catkin workspace.

Run catkin_make this repo to compile so that it will be usable.

1.請將程式碼加進你自己的程式中

你需要功能去與 external_program_handler溝通,以執行你啟動的指令和停止的指令。

你的程式中必須有一些東西可以告訴 Seirios 啟動程序和停止程序。 啟動時,可以使用 ROS 服務或啟動文件來完成。 需要停止時,可以使用 ROS 服務或訂閱主題來完成

以下為程式碼範例 (python and c++)

請確保您正在複製正確類型的程式碼。 不要將 c++ 代碼複製到 python 程序中,反之亦然。

1.1. 使用ROS service 去啟動 & 停止一個程式

啟動Starting

在您的程序中,添加一個服務,將其命名為 service_start 。 編寫一個處理程序 start_handler作為回調來處理您的啟動程式碼。

停止Stopping

將另一個服務初始化為service_stop。 同樣,編寫一個回調 stop_handler來處理您的程式碼以停止程序。

必須使用Seirios程式包。

movel_seirios_msgs以執行service_start和service_stop。要使用該程式包,請在下面添加導入語句。 如果您使用 C++,還必須包括其他配置。

from movel_seirios_msgs.srv import StringTrigger, StringTriggerResponse # like setbool

C++ 的額外配置

對於 C++ 程序,您還必須在自己包中的 package.xml 中添加 movel_seirios_msgs 包作為相依

<depend>movel_seirios_msgs</depend>

在自己的程式包CMakeLists.txt中, 添加 movel_seirios_msgs ,在此 find_package 之下。

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  movel_seirios_msgs
)

1.2. 使用主題來停止程序

如果您是從 UI 發布停止主題來停止程序,請參閱本節。 否則,忽略它。

請撰寫訂閱者 topic_process_cancel去訂閱此UI主題, 其中 handle_publish_cancel的回調中,請包含您的停止程式碼。

請確保topic_process_cancel 訂閱正確的UI主題。

1.3. 發布客戶的狀態

如果您從啟動文件啟動程序,請參閱本節。 否則,忽略它。

這樣做的目的,是要檢查從啟動文件,啟動程序後您的程序,是否有運行成功。

請撰寫發布者 client_status ,如果運行成功,請發佈2 ; 如果運行失敗,請發布3。

2. 範例程式碼

您可以參考範例程式碼,了解如何使用外部設備整合包,來去啟動程序和停止程序。

回調函數中的程式碼,只是用於測試 external_process_handler 的填充代碼。 可以使用您自己的開始程式碼和停止程式碼來替換它們。

#!/usr/bin/env python
import rospy
from movel_seirios_msgs.srv import StringTrigger, StringTriggerResponse # like setbool
from std_msgs.msg import Bool, UInt8


class Pytest:

    def __init__(self):
        print('starting test node')
        self.node = rospy.init_node('python_test')

        self.service_start = rospy.Service('/external_process/trigger_detecting', StringTrigger, self.start_handler)
        self.service_stop = rospy.Service('/external_process/stop_detecting', StringTrigger, self.stop_handler)
        
        # IGNORE dummy_publisher - just to simulate UI cancel topic
        self.dummy_publisher = rospy.Publisher('/cancel_publish', Bool, queue_size=1)

        self.topic_process_cancel = rospy.Subscriber('/cancel_publish', Bool, self.handle_publish_cancel)
        self.client_status = rospy.Publisher('/external_process/client_status', UInt8, queue_size=1)

        self.flag = True

    def start_handler(self, req):
        print('inside start handler')

        start_msg = StringTriggerResponse() 
        start_msg.success = True
        start_msg.message = 'starting detect'

        self.flag = True
        self.publish_client_status()

        while(self.flag):
            print('executing something')
            rospy.sleep(1)
    

        return start_msg # return must be [success, message format]

    def stop_handler(self, req):
        print('inside stop handler')

        self.flag = False
        print('stop handler called')
        stop_msg = StringTriggerResponse()
        stop_msg.success = False
        stop_msg.message = 'stopping detect'

        return stop_msg
    
    # IGNORE if you are not using a topic to stop your code
    def handle_publish_cancel(self, req):
        print("inside publish cancel", req.data)

        cancel_trigger = req.data
        if(cancel_trigger):
            print('executing cancel stuff') # replace print statement
    
    
    # IGNORE if you are not using launch files
    def publish_client_status(self):
        print("publish client status called")
        
        cstatus = UInt8()
        
        #Dummy code
        #Decide how do you determine if your launch file is successful
        #And replace the if-else statement
        if (self.flag == True):
            cstatus.data = 2 # if start_handler successfully called, task is successful and return 2
        else:
            cstatus.data = 3 # else task not successful, return 3

        self.client_status.publish(cstatus)


if __name__ == '__main__':

    py_test = Pytest()
    #py_test.publish_cancel()

    rospy.spin()

3. 在Seirios裡更改檔案

更改您自己的程式碼後,你還需要去配置task_supervisor,才能讓Seirios在你的程式上運行。

3.1. 使用參數 external_process_handler

  watchdog_rate: 2.0
  watchdog_timeout: 0
  service_req: true
  # Below 4 params are COMPULSORY if service_req is true
  service_start: "/external_process/trigger_detecting" 
  service_start_msg: "" # Optional, may put blank string
  service_stop: "/external_process/stop_detecting" 
  service_stop_msg: "" # Optional, may put blank string
  launch_req: false
  # Below 3 params COMPULSORY if launch_req is true
  launch_package: 
  launch_file: 
  launch_nodes: 
  topic_cancel_req: true
  # topic_process_cancel required if topic_cancel_req is true
  topic_process_cancel: "external_process/cancel_publish"
  client_status: # Required if launch_req true, optional otherwise

根據您啟動/停止程序的方式設置 service_reqlaunch_reqtopic_cancel_req 的布爾狀態。

參數 service_startservice_stoptopic_process_cancelclient_status 必須與您在自己的文件中提供的主題和服務的名稱相匹配。

3.2. 添加task supervisor 作為插件

將您的程式作為插件添加到task supervisor,以便 Seirios 運行,將配置插入 task_supervisor.yaml

Filepath: ../config/task_supervisor/config/

File to modify: task_supervisor.yaml

警告:yaml 文件禁止tab鍵。 編輯 yaml 文件時,不要使用tab鍵來縮排。

在task supervisor的插件部分下,插入以下程式碼。 縮排必須與其他插件對齊。 您可以提供任何nametype,但要確保它們不會與其他插件衝突。 請不要更改class

  plugins:
   - {name: detecting, type: 10, class: 'external_process_handler::ExternalProcessHandler'}

在文件的末尾中,使用您在上面提供的name創建一個新部分。 請貼上該參數。

detecting:
  watchdog_rate: 2.0
  watchdog_timeout: 0
  service_req: true
  launch_req: false
  service_start: "/external_process/trigger_detecting" #required if service_req is true
  service_start_msg: "" #optional  if there is any particular msg needs to sent or else empty string 
  service_stop: "/external_process/stop_detecting" #required if service_req is true
  service_start_msg: "" #optional  if there is any particular msg needs to sent or else empty string 
  #launch_package: "yocs_velocity_smoother" #required if launch_req is true
  #launch_file: "standalone.launch"  #required if launch_req is true
  #launch_nodes: "/nodelet_manager /velocity_smoother" #required if launch_req is true
  topic_cancel_req: true
  topic_process_cancel: "external_process/cancel_publish"

其他注意事項

對於 C++ 程式,請記住在您自己的程式包中,要正確設置 CMakeLists.txtpackage.xml

欲了解更多信息,請點擊此處。 click here.

Last updated