Pythonでソケット越しにコマンドを受け取ってRoombaをROSで制御する

自前のシステムから指令を送ってRoombaをROS越しに制御するための準備。
まずはテストということで、Pythonで何か文字列を受け取ったら移動コマンドを発行する簡単なプログラムを作成してみた。
ROSではじめるロボットプログラミングを参考に、パッケージの作成からコンパイルまで以下の通りやってみた。
文字列を解釈してTopicの内容を変える…とかは，ここまで出来れば後は簡単なはず。

（~/rostestをワークスペースとしている）
mkdir -p ~/rostest/src
cd ~/rostest/src
catkin_init_workspace
cd ~/rostest
catkin_make
source ~/rostest/devel/setup.bach
（必要なら~/.bashrcに追記して読み込みを自動化する）
cd ~/rostest/src
catkin_create_pkg python_test rospy roscpp std_msgs
cd ~/rostest
catkin_make
source ~/rostest/devel/setup.bach
cd ~/rostest/src/python_test
（ここにtalker.pyを作る。コードは後で）
chmod 755 talker.py
（roscore, rosrun roomba_500_series roomba500_light_nodeを別ターミナルで実行しておく）
rosrun python_test talker.py

以上を実行したうえで、クライアントのプログラムから何か文字列を送ればtalker.pyからTopicが送られてRoombaが左方向に2秒、右方向に2秒動くはず。自分はTCP/IPテストツールを使った。

以下、talker.pyの中身。

#!/usr/bin/env python

from __future__ import print_function

import rospy
import roslib
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import socket
import select

rospy.init_node('talker')
pub=rospy.Publisher('chatter',String, queue_size=10)
rate = rospy.Rate(10)

host = 'localhost'
port = 12000
backlog = 10
bufsize = 4096

server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
readfds = set([server_sock])
try:
    server_sock.bind((host, port))
    server_sock.listen(backlog)

    while True:
        rready, wready, xready = select.select(readfds, [], [])
        for sock in rready:
            if sock is server_sock:
                conn, address = server_sock.accept()
                readfds.add(conn)
            else:
                msg = sock.recv(bufsize)
                if len(msg) == 0:
                    sock.close()
                    readfds.remove(sock)
                else:
                    print(msg)
                    #sock.send(msg)
                    #何かを受け取ったら、以下の回転・静止コマンドを発行
                    if not rospy.is_shutdown():
                        pub.publish(msg)
                        p=rospy.Publisher('cmd_vel', Twist, queue_size=10)
                        twist=Twist()
                        twist.angular.z = 0.5
                        p.publish(twist)
                        rospy.sleep(2)
                        twist=Twist()
                        twist.angular.z = -0.5
                        p.publish(twist) 
                        rospy.sleep(2)
                        twist=Twist()
                        p.publish(twist)
                        rospy.sleep(2)
finally:
    for sock in readfds:
            sock.close()

#!/usr/bin/env python

from __future__ import print_function

import rospy

import roslib

from std_msgs.msg import String

from geometry_msgs.msg import Twist

import socket

import select

rospy.init_node('talker')

pub=rospy.Publisher('chatter',String, queue_size=10)

rate = rospy.Rate(10)

host = 'localhost'

port = 12000

backlog = 10

bufsize = 4096

server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

readfds = set([server_sock])

try:

server_sock.bind((host, port))

server_sock.listen(backlog)

while True:

rready, wready, xready = select.select(readfds, [], [])

for sock in rready:

if sock is server_sock:

conn, address = server_sock.accept()

readfds.add(conn)

else:

msg = sock.recv(bufsize)

if len(msg) == 0:

sock.close()

readfds.remove(sock)

else:

print(msg)

#sock.send(msg)

#何かを受け取ったら、以下の回転・静止コマンドを発行

if not rospy.is_shutdown():

pub.publish(msg)

p=rospy.Publisher('cmd_vel', Twist, queue_size=10)

twist=Twist()

twist.angular.z = 0.5

p.publish(twist)

rospy.sleep(2)

twist=Twist()

twist.angular.z = -0.5

p.publish(twist)

rospy.sleep(2)

twist=Twist()

p.publish(twist)

rospy.sleep(2)

finally:

for sock in readfds:

sock.close()

コメントする コメントをキャンセル

コメントするコメントをキャンセル