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()