Pythonでソケット越しにコマンドを受け取ってRoombaをROSで制御する（自己位置設定編

引き続き、自前のシステムから指令を送ってRoombaをROS越しに制御するための準備。

Rvizを使えば””2D pose estimate”ボタンで自己位置を設定できるけど、
それもプログラム越しに行いたいのでどういう処理が行われているかを調べて実装テスト。
参考にしたのはこことかこことかここ。

要は、initialpose（PoseWithCovarianceStamped）をPublishすればよい。
以下のコードを通じて、ros-navigation2d-exampleで制御しているRoombaの位置を変更できることを確認した。
テストということで、何か文字列を受け取ったら特定の場所に自己位置を設定しているが、文字列を分解して処理すれば任意の位置に設定できるはず。
ただ、通信して最初のメッセージではうまく設定できず、2回目以降から正しく動く。おそらく初期化周りに問題があるんだろうけど、とりあえず動いたのでひとまずメモしておく。

#!/usr/bin/env python

from __future__ import print_function

import rospy
import roslib
from std_msgs.msg import String
from geometry_msgs.msg import PoseWithCovarianceStamped,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():
                        pose=rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=100)
                        initial_pose = PoseWithCovarianceStamped()
                        initial_pose.header.stamp=rospy.Time.now()
                        initial_pose.header.frame_id="map"
                        initial_pose.pose.pose.position.x = 5.0
                        initial_pose.pose.pose.position.y = 5.0
                        initial_pose.pose.pose.orientation.z = 1.0
                        initial_pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
                        pose.publish(initial_pose)
                        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 PoseWithCovarianceStamped,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():

pose=rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=100)

initial_pose = PoseWithCovarianceStamped()

initial_pose.header.stamp=rospy.Time.now()

initial_pose.header.frame_id="map"

initial_pose.pose.pose.position.x = 5.0

initial_pose.pose.pose.position.y = 5.0

initial_pose.pose.pose.orientation.z = 1.0

initial_pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]

pose.publish(initial_pose)

rospy.sleep(2)

finally:

for sock in readfds:

sock.close()

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

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