def init_rosout():
logger = logging.getLogger("rospy.rosout")
try:
global _rosout_pub
if _rosout_pub is None:
logger.info("initializing %s core topic"%_ROSOUT)
_rosout_pub = Publisher(_ROSOUT, Log, latch=True)
logger.info("connected to core topic %s"%_ROSOUT)
return True
except Exception as e:
logger.error("Unable to initialize %s: %s\n%s", _ROSOUT, e, traceback.format_exc())
return False
def test_Publisher(self):
import rospy
from rospy.impl.registration import get_topic_manager, Registration
from rospy.topics import Publisher, DEFAULT_BUFF_SIZE
# Publisher(self, name, data_class, subscriber_listener=None, tcp_nodelay=False, latch=False, headers=None)
name = 'foo'
rname = rospy.resolve_name('foo')
data_class = test_rospy.msg.Val
# test invalid params
for n in [None, '', 1]:
try:
Publisher(n, data_class)
self.fail("should not allow invalid name")
except ValueError:
pass
for d in [None, 1, TestRospyTopics]:
try:
Publisher(name, d)
self.fail("should now allow invalid data_class")
except ValueError:
pass
try:
Publisher(name, None)
self.fail("None should not be allowed for data_class")
except ValueError:
pass
# round 1: test basic params
pub = Publisher(name, data_class)
self.assertEquals(rname, pub.resolved_name)
# - pub.name is left in for backwards compatiblity, but resolved_name is preferred
self.assertEquals(rname, pub.name)
self.assertEquals(data_class, pub.data_class)
self.assertEquals('test_rospy/Val', pub.type)
self.assertEquals(data_class._md5sum, pub.md5sum)
self.assertEquals(Registration.PUB, pub.reg_type)
# verify impl as well
impl = get_topic_manager().get_impl(Registration.PUB, rname)
self.assert_(impl == pub.impl)
self.assertEquals(rname, impl.resolved_name)
self.assertEquals(data_class, impl.data_class)
self.failIf(impl.is_latch)
self.assertEquals(None, impl.latch)
self.assertEquals(0, impl.seq)
self.assertEquals(1, impl.ref_count)
self.assertEquals('', impl.buff.getvalue())
self.failIf(impl.closed)
self.failIf(impl.has_connections())
# check publish() fall-through
from test_rospy.msg import Val
impl.publish(Val('hello world-1'))
# check stats
self.assertEquals(0, impl.message_data_sent)
# check acquire/release don't bomb
impl.acquire()
impl.release()
# do a single publish with connection override. The connection
# override is a major cheat as the object isn't even an actual
# connection. I will need to add more integrated tests later
co1 = ConnectionOverride('co1')
self.failIf(impl.has_connection('co1'))
impl.add_connection(co1)
self.assert_(impl.has_connection('co1'))
self.assert_(impl.has_connections())
impl.publish(Val('hello world-1'), connection_override=co1)
import cStringIO
buff = cStringIO.StringIO()
Val('hello world-1').serialize(buff)
# - check equals, but strip length field first
self.assertEquals(co1.data[4:], buff.getvalue())
self.assertEquals(None, impl.latch)
# Now enable latch
pub = Publisher(name, data_class, latch=True)
impl = get_topic_manager().get_impl(Registration.PUB, rname)
# have to verify latching in pub impl
self.assert_(impl == pub.impl)
self.assertEquals(True, impl.is_latch)
self.assertEquals(None, impl.latch)
self.assertEquals(2, impl.ref_count)
co2 = ConnectionOverride('co2')
self.failIf(impl.has_connection('co2'))
impl.add_connection(co2)
for n in ['co1', 'co2']:
self.assert_(impl.has_connection(n))
self.assert_(impl.has_connections())
v = Val('hello world-2')
impl.publish(v, connection_override=co2)
self.assert_(v == impl.latch)
buff = cStringIO.StringIO()
Val('hello world-2').serialize(buff)
# - strip length and check value
self.assertEquals(co2.data[4:], buff.getvalue())
# test that latched value is sent to connections on connect
co3 = ConnectionOverride('co3')
self.failIf(impl.has_connection('co3'))
impl.add_connection(co3)
for n in ['co1', 'co2', 'co3']:
self.assert_(impl.has_connection(n))
self.assert_(impl.has_connections())
self.assertEquals(co3.data[4:], buff.getvalue())
# TODO: tcp_nodelay
# TODO: suscribe listener
self.assert_(impl.has_connection('co1'))
impl.remove_connection(co1)
self.failIf(impl.has_connection('co1'))
self.assert_(impl.has_connections())
# TODO: need to validate DeadTransport better
self.assert_(
[d for d in impl.dead_connections if d.endpoint_id == 'co1'])
self.assert_(impl.has_connection('co3'))
impl.remove_connection(co3)
self.failIf(impl.has_connection('co3'))
self.assert_(impl.has_connections())
for id in ['co1', 'co3']:
self.assert_(
[d for d in impl.dead_connections if d.endpoint_id == id])
self.assert_(impl.has_connection('co2'))
impl.remove_connection(co2)
self.failIf(impl.has_connection('co2'))
self.failIf(impl.has_connections())
for id in ['co1', 'co2', 'co3']:
self.assert_(
[d for d in impl.dead_connections if d.endpoint_id == id])
# test publish() latch on a new Publisher object (this was encountered in testing, so I want a test case for it)
pub = Publisher('bar', data_class, latch=True)
v = Val('no connection test')
pub.impl.publish(v)
self.assert_(v == pub.impl.latch)
# test connection header
h = {'foo': 'bar', 'fuga': 'hoge'}
pub = Publisher('header_test', data_class, headers=h)
self.assertEquals(h, pub.impl.headers)
end_publishers[robot].publish(msg2pub)
if __name__ == "__main__":
robots = rospy.get_param("robots_names", default=[])
rospy.init_node("fake_joy", anonymous=False) # Initialize keyboard node
models_service.wait_for_service()
for r in robots:
#Get trajectory clients
if 'UAV' in r:
trajectory_clients[r] = SimpleActionClient(
"/{}/approach_server".format(r), ExecuteDroneApproachAction)
trajectory_clients[r].wait_for_server()
elif 'pioneer3at' in r:
trajectory_clients[r] = Publisher(
"/{}/cmd_vel".format(r), Twist,
queue_size=10) # Server name = robot_name/move_base
#Get tele END publishers
end_publishers[r] = rospy.Publisher("/{}/maneuvers/in".format(r),
events_message,
queue_size=10)
st_sub[r] = rospy.Subscriber("/{}/maneuvers/in".format(r),
events_message, maneuver_event, r)
rospy.spin()
