How can I publish to a Gazebo Topic from Python
I am trying to build a publisher in python, for now I am using the 'Topic visualization' tool in gzclient as my subscriber.
I am trying to build a publisher in python, for now I am using the 'Topic visualization' tool in gzclient as my subscriber.
That took a while to figure out, but this works on Gazebo 1.4.
Note that I copied all of the necessary message .proto files from the gazebo source directory into a directory './proto/msg' and compiled them with:
protoc -I='./proto/msg' --python_out='./proto' ./proto/msg/*.proto
I then made that directory a python library with
touch ./proto/__init__.py
Finally I used this script to publish some bogus data:
#!/usr/bin/env python
import socket
import time
import sys
from datetime import datetime
from proto.packet_pb2 import Packet
from proto.publish_pb2 import Publish
from proto.request_pb2 import Request
from proto.response_pb2 import Response
from proto.pose_pb2 import Pose
from proto.subscribe_pb2 import Subscribe
MASTER_TCP_IP = '127.0.0.1'
MASTER_TCP_PORT = 11345
NODE_TCP_IP = '127.0.0.1'
NODE_TCP_PORT = 11451
TCP_BUFFER_SIZE = 40960
# Listen for Subscribers
s_sub = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s_sub.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s_sub.bind((NODE_TCP_IP, NODE_TCP_PORT))
s_sub.listen(5)
# Register as a Publisher with Gazebo
pk = Packet()
pk.stamp.sec = int(time.time())
pk.stamp.nsec = datetime.now().microsecond
pk.type = "advertise"
pub = Publish()
pub.topic = "/gazebo/default/Pioneer3AT/cmd_vel"
pub.msg_type = Pose.DESCRIPTOR.full_name
pub.host = NODE_TCP_IP
pub.port = NODE_TCP_PORT
pk.serialized_data = pub.SerializeToString()
s_reg = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s_reg.connect((MASTER_TCP_IP, MASTER_TCP_PORT))
s_reg.send(hex(pk.ByteSize()).rjust(8))
s_reg.send(pk.SerializeToString())
# Respond to a subscriber
try:
conn, address = s_sub.accept()
data = conn.recv(TCP_BUFFER_SIZE)
# Decode Incomming Packet
pk_sub = Packet()
pk_sub.ParseFromString(data[8:])
print "Packet:\n", pk_sub
# Decode Subscription Request
sub = Subscribe()
sub.ParseFromString(pk_sub.serialized_data)
print "Sub:\n", sub
# Pack Data for Reply
cmd_vel = Pose()
cmd_vel.name = "Pioneer3AT"
cmd_vel.position.x = 0.1
cmd_vel.position.y = 0.2
cmd_vel.position.z = 0.3
cmd_vel.orientation.x = 1
cmd_vel.orientation.y = 2
cmd_vel.orientation.z = 3
cmd_vel.orientation.w = 4
# Publish Packet to Subscriber
while 1:
pk_pub = Packet()
pk_pub.stamp.sec = int(time.time())
pk_pub.stamp.nsec = datetime.now().microsecond
pk_pub.type = Pose.DESCRIPTOR.full_name
pk_pub.serialized_data = cmd_vel.SerializeToString()
conn.send(hex(cmd_vel.ByteSize()).rjust(8))
conn.send(cmd_vel.SerializeToString())
time.sleep(0.2)
finally:
#This connection must remain open
s_reg.close()
Asked: 2013-03-04 09:09:15 -0600
Seen: 1,535 times
Last updated: Mar 04 '13
Are there any default model topics?
How to avoid Queue limit reached warning when subscribing to a topic.
Google Protobuf Version between Gazebo 1.3 and Gazebo 1.4
ROS-Gazebo-Stage compatibility
Is it possible to control the GUI camera with python to follow a model?
Unable to install or run rospy