ROS<=>RobView のプログラムの追加方法

RobView のサブプログラム追加方法(ようやくここまで来ました)

ず,robViewMode の番号を決めます.何か系統立てて番号付けしたいですね.
今のところ,1がomniDriveでの移動,2がpositionDriverでの移動,3がOdometryの値の設定 となっています.
ここでは,練習で10番を追加してみます.

■サービスの追加
~/catkin_ws/src/ros-rcll_btr_msgs/scripts/robotino.py を編集します.
main の rospy.init_node()の次の当たりに,srv** = rospy.Service(…) と言う項目があるので,サービス番号を追加して,サービス名と,受け取る型,そして,呼び出す関数を書きます.

srv04 = rospy.Service('rvw2/goToMPSCenter', Empty, goToMPSCenter)

引数無しの時は,Empty を指定します.自分で型を作る(C構造体的な感じ)場合は,ros-rcll_btr_msgs/srv の下に宣言ファイルを書いてcatkin_make します.
なお,Emptyを使うには,ファイルの最初の方でimportする必要があります.他の型も初めての使用であればimportを追加して下さい.

from std_srvs.srv import Empty, EmptyResponse

次に,呼び出される関数を書きます.mainより前に書きましょう.

def goToMPSCenter(data):
    global robViewMode
    robViewMode = 10
    udp.view3Send[1] = robViewMode # mode number
    print("goToMPSCenter")
    sendRobView()
    return EmptyReponse()

一番シンプルなのは,上記です.モード番号を設定してsendRobView()を呼び出すだけです.
引数を扱いたいときは,data の値を受け取って,udp.view3Send の5-7で渡して下さい.
robotino.py の修正は以上です.

これをメインプログラムから呼び出します.今回は,btr2021/rcll2021/python/refbox.py です.
こちらでも受け渡しの型を使いますので,import していない場合は追加して下さい.

from std_srvs.srv import Empty, EmptyResponse

呼び出し部分を関数として用意します(一回しか呼び出さないのであれば,直接書いても良いけど)

def goToMPSCenter():
    rospy.wait_for_service('/rvw2/goToMPSCenter')
    goToMPSCenter = rospy.ServiceProxy('/rvw2/goToMPSCenter', Empty)
    print("goToMPSCenter")
    resp = goToMPSCenter()
    print("reached")

先ほどのpub**で用意したサービスをwaitして,Proxy で呼び出します.
引数が必要な場合は,ここで与えます(今回はEmpty)
あとは,main にて,この関数を呼び出せば,robView はモード10を動かします.
というわけで,view3のプログラムを書けばできあがりです.

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

*