def __init__(self): self.client = mqtt.Client() self.client2 = mqtt.Client() self.ros_client = Ros(host='10.205.240.34', port=9090) self.publisher = Topic(self.ros_client, '/chatter', 'std_msgs/String') self.muse_left = "DC11" self.muse_right = "C00E" self.status_left = False self.status_right = False self.action_mode = 0 self.client.on_connect = self.on_connect self.client.on_message = self.on_message self.client.connect("localhost", 1883, 60) t = threading.Thread(target=self.worker) t.start() self.client2.on_connect = self.on_connect2 self.client2.on_message = self.on_message self.client2.connect("localhost", 1883, 60) t2 = threading.Thread(target=self.worker2) t2.start() self.ros_client.on_ready(self.subscribing, run_in_thread=True) t3 = threading.Thread(target=self.worker3) t3.start() while True: pass
def on_act_debug(self): if not self.ros_client.is_connected: QMessageBox.critical(self, "错误", '尚未连接到ROS') return if self.host == '127.0.0.1' or self.host == 'localhost': self.action_debug_client = self.ros_client cmd = 'rosrun action action_debuger' else: cmd = 'roslaunch start start_action_debug_robot.launch' try: if self.action_debug_client is None: print("run action_debug_client at 127.0.0.1:9090") self.action_debug_client = Ros('127.0.0.1', 9090) self.action_debug_client.run() elif not self.action_debug_client.is_connected: print("connecting action debug client") self.action_debug_client.connect() except Exception as e: QMessageBox.critical(self, "错误", '无法连接到本地调试器 %s' % str(e)) return act_service = Service(self.action_debug_client, '/debug/action/run', 'common/AddAngles') act_service.advertise(self.on_recv_action) os.popen(cmd)
def __init__(self): self.ros_client = Ros('127.0.0.1', 9090) print("wait for server") self.publisher = Topic(self.ros_client, '/cmd_vel', 'geometry_msgs/Twist') self.listener = Topic(self.ros_client, '/odom', 'nav_msgs/Odometry') self.listener.subscribe(self.callback) self.ros_client.on_ready(self.start_thread, run_in_thread=True) self.ros_client.run_forever()
def __init__(self, ip, port, local_topic_name, remote_topic_name, message_type): self._ip = ip self._port = port self._local_topic_name = local_topic_name self._remote_topic_name = remote_topic_name self._message_type = message_type self._ros_client= Ros(self._ip,self._port) self._rosPub = None # Should be specified by the automation engine self._listener = None self._logger = logging.getLogger(self.__class__.__name__) self._logger.setLevel(logging.DEBUG)
def run_topic_pubsub(): context = {'counter': 0} ros_client = Ros('127.0.0.1', 9090) listener = Topic(ros_client, '/chatter', 'std_msgs/String') publisher = Topic(ros_client, '/chatter', 'std_msgs/String') def receive_message(message): context['counter'] += 1 assert message['data'] == 'hello world', 'Unexpected message content' if context['counter'] == 3: listener.unsubscribe() # Give it a bit of time, just to make sure that unsubscribe # really unsubscribed and counter stays at the asserted value ros_client.call_later(2, ros_client.terminate) def start_sending(): while True: if not ros_client.is_connected: break publisher.publish(Message({'data': 'hello world'})) time.sleep(0.1) publisher.unadvertise() def start_receiving(): listener.subscribe(receive_message) ros_client.on_ready(start_receiving, run_in_thread=True) ros_client.on_ready(start_sending, run_in_thread=True) ros_client.run_forever() assert context[ 'counter'] >= 3, 'Expected at least 3 messages but got ' + str( context['counter'])
def run_empty_service(): ros_client = Ros('127.0.0.1', 9090) ros_client.run() service = Service(ros_client, '/test_empty_service', 'std_srvs/Empty') service.advertise(lambda req, resp: True) time.sleep(1) client = Service(ros_client, '/test_empty_service', 'std_srvs/Empty') client.call(ServiceRequest()) service.unadvertise() time.sleep(2) service.ros.terminate()
class ROSCloudSubBridge(object): def __init__(self, ip, port, local_topic_name, remote_topic_name, message_type, file): self._ip = ip self._port = port self._local_topic_name = local_topic_name self._remote_topic_name = remote_topic_name self._message_type = message_type self._ros_client = Ros(self._ip, self._port) self._rosPub = None # Should be specified by the automation engine self._listener = None # Should be specified by the automation engine self._service = None self._logger = logging.getLogger(self.__class__.__name__) self._logger.setLevel(logging.DEBUG) self._file = file def _run_topic_pubsub(self): try: self._listener = Topic(self._ros_client, self._remote_topic_name, self._message_type) rospy.init_node('cloudsub', anonymous=True) self._ros_client.send_on_ready(Message(self._file)) global my my = rospy.get_time() self._ros_client.on_ready(self._start_receiving, run_in_thread=True) self._ros_client.run_event_loop() except: self._logger.exception("Fatal error in constructor") def _receive_message(self, message): global my rospy.loginfo(rospy.get_caller_id() + " Message type %s ", self._message_type) rospy.loginfo( rospy.get_caller_id() + " Time from previous message %s ", (rospy.get_time() - my)) my = rospy.get_time() try: msg = Clock() msg.clock.secs = message['clock']['secs'] msg.clock.nsecs = message['clock']['nsecs'] self._rosPub = rospy.Publisher( self._local_topic_name, Clock, queue_size=10 ) #message type is String instead of msg_stds/String self._rosPub.publish(msg) except: print('Error') def _start_receiving(self): self._listener.subscribe(self._receive_message) def start(self): signal.signal(signal.SIGINT, lambda *x: self.stop()) self._run_topic_pubsub() def stop(self): if self._ros_client is not None: self._ros_client.terminate()
def connect(self, host: str, port: int) -> None: """Connect to the robot via ROS bridge. Args: host (str): Name or IP address of the ROS bridge host, e.g. `127.0.0.1`. port (int): ROS bridge port, e.g. `9090`. """ ros = Ros(host, port) ros.run() self._ros = ros def _set_connecting_flag(*args): self._connected = True self._ros.on_ready(_set_connecting_flag)
def __init__(self): OVBox.__init__(self) self.signalHeader = None self.ros_client = Ros(host='10.204.231.147', port=9090) #self.listener = Topic(self.ros_client, '/chatter', 'std_msgs/String') self.publisher = Topic(self.ros_client, '/chatter', 'std_msgs/String') self.ros_client.on_ready(self.subscribing, run_in_thread=True) self.phase = 0 self.parameters = { "count": "4", "scenario": "4", "gender": "Male", "age": "25", "subjectno": "55", "phase": 5, "type": 2 }
def test_rosapi_topics_blocking(): ros = Ros(host, port) ros.run() topic_list = ros.get_topics() print(topic_list) assert ('/rosout' in topic_list) ros.close()
def run_rosapi_topics_blocking(*args, **kwargs): ros_client = Ros(*args, **kwargs) ros_client.run() topic_list = ros_client.get_topics() print(topic_list) assert ('/rosout' in topic_list) ros_client.terminate()
def run_reconnect_does_not_trigger_on_client_close(): ros = Ros('127.0.0.1', 9090) ros.run() assert ros.is_connected, "ROS initially connected" time.sleep(0.5) event = threading.Event() ros.on('close', lambda m: event.set()) ros.close() event.wait(5) assert not ros.is_connected, "Successful disconnect" assert not ros.is_connecting, "Not trying to re-connect"
class ROSCloudSubBridge(object): def __init__(self, ip, port, local_topic_name, remote_topic_name, message_type): self._ip = ip self._port = port self._local_topic_name = local_topic_name self._remote_topic_name = remote_topic_name self._message_type = message_type self._ros_client= Ros(self._ip,self._port) self._rosPub = None # Should be specified by the automation engine self._listener = None # Should be specified by the automation engine self._logger = logging.getLogger(self.__class__.__name__) self._logger.setLevel(logging.DEBUG) self.br =tf2_ros.StaticTransformBroadcaster() self.t = [] def _run_topic_pubsub(self): try: self._listener = Topic(self._ros_client, self._remote_topic_name, self._message_type) rospy.init_node('cloudsub', anonymous=True) global my my=rospy.get_time() self._ros_client.on_ready(self._start_receiving, run_in_thread=True) self._ros_client.run_event_loop() except: self._logger.exception("Fatal error in constructor") def _receive_message(self,message): global my rospy.loginfo(rospy.get_caller_id() + " Message type %s ",self._message_type) rospy.loginfo(rospy.get_caller_id() + " Time from previous message %s ",(rospy.get_time()-my)) my=rospy.get_time() for i in range(len(message['transforms'])): self.t.append(TransformStamped()) self.t[-1].header.stamp = rospy.Time.now() self.t[-1].header.frame_id = message['transforms'][i]['header']['frame_id'] self.t[-1].child_frame_id = message['transforms'][i]['child_frame_id'] self.t[-1].transform.translation.x = message['transforms'][i]['transform']['translation']['x'] self.t[-1].transform.translation.y = message['transforms'][i]['transform']['translation']['y'] self.t[-1].transform.translation.z = message['transforms'][i]['transform']['translation']['z'] self.t[-1].transform.rotation.x = message['transforms'][i]['transform']['rotation']['x'] self.t[-1].transform.rotation.y = message['transforms'][i]['transform']['rotation']['y'] self.t[-1].transform.rotation.z = message['transforms'][i]['transform']['rotation']['z'] self.t[-1].transform.rotation.w = message['transforms'][i]['transform']['rotation']['w'] self.br.sendTransform(self.t) def _start_receiving(self): self._listener.subscribe(self._receive_message) def start(self): signal.signal(signal.SIGINT, lambda *x: self.stop()) self._run_topic_pubsub() def stop(self): if self._ros_client is not None: self._ros_client.terminate()
def run_add_two_ints_service(): ros_client = Ros('127.0.0.1', 9090) service = Service(ros_client, '/test_server', 'rospy_tutorials/AddTwoInts') def dispose_server(): service.unadvertise() service.ros.call_later(1, service.ros.terminate) def add_two_ints(request, response): response['sum'] = request['a'] + request['b'] if response['sum'] == 42: service.ros.call_later(2, dispose_server) return True def check_sum(result): assert (result['sum'] == 42) def invoke_service(): client = Service(ros_client, '/test_server', 'rospy_tutorials/AddTwoInts') client.call(ServiceRequest({'a': 2, 'b': 40}), check_sum, print) service.advertise(add_two_ints) ros_client.call_later(1, invoke_service) ros_client.run_event_loop()
class ROSCloudSubBridge(object): def __init__(self, ip, port, local_topic_name, remote_topic_name, message_type): self._ip = ip self._port = port self._local_topic_name = local_topic_name self._remote_topic_name = remote_topic_name self._message_type = message_type self._ros_client= Ros(self._ip,self._port) self._rosPub = None # Should be specified by the automation engine self._listener = None self._logger = logging.getLogger(self.__class__.__name__) self._logger.setLevel(logging.DEBUG) def _run_topic_pubsub(self): try: self._listener = Topic(self._ros_client, self._remote_topic_name, self._message_type) self._listener2= Topic(self._ros_client, self._remote_topic_name, self._message_type) rospy.init_node('cloudsub', anonymous=True) global my my=rospy.get_time() self._ros_client.on_ready(self._start_receiving, run_in_thread=True) self._ros_client.run_event_loop() except: self._logger.exception("Fatal error in constructor") def _receive_message(self,message): global my rospy.loginfo(rospy.get_caller_id() + " I heard a message of %s",self._message_type) rospy.loginfo(rospy.get_caller_id() + " Time from previous message %s",(rospy.get_time()-my) )#edw mou leei unicode type my=rospy.get_time() try: msg=Image() msg.header.seq=message['header']['seq'] msg.header.stamp.secs=message['header']['stamp']['secs'] msg.header.stamp.nsecs=message['header']['stamp']['nsecs'] msg.header.frame_id=message['header']['frame_id'] msg.height=message['height'] msg.width=message['width'] msg.encoding=str(message['encoding']) msg.is_bigendian=message['is_bigendian'] msg.step=message['step'] msg.data=message['data'].decode('base64') self._rosPub=rospy.Publisher(self._local_topic_name, Image, queue_size=10) #message type is String instead of msg_stds/String self._rosPub.publish(msg) except: print('Error') def _start_receiving(self): self._listener.subscribe(self._receive_message) def start(self): signal.signal(signal.SIGINT, lambda *x: self.stop()) self._run_topic_pubsub() def stop(self): if self._ros_client is not None: self._ros_client.terminate()
def run_param_manipulation(): ros_client = Ros('127.0.0.1', 9090) def delete_param_done(_): ros_client._test_param_del = True def get_param_done(value): ros_client._test_param_value = value delete_param = Param(ros_client, 'test_param') delete_param.delete(delete_param_done) def set_param_done(_): ros_client._test_param_set = True check_param = Param(ros_client, 'test_param') check_param.get(get_param_done) param = Param(ros_client, 'test_param') param.set('test_value', set_param_done) def verify(): assert(ros_client._test_param_set is True) assert(ros_client._test_param_value == 'test_value') assert(ros_client._test_param_del is True) ros_client.terminate() ros_client.call_later(1, verify) ros_client.run_forever()
def run_topic_pubsub(): context = {'counter': 0} ros_client = Ros('192.168.1.106', 9090) listener = Topic(ros_client, '/chatter', 'std_msgs/String') publisher = Topic(ros_client, '/chatter', 'std_msgs/String') def receive_message(message): print("============================") context['counter'] += 1 assert message['data'] == 'hello world', 'Unexpected message content' if context['counter'] == 3: listener.unsubscribe() # Give it a bit of time, just to make sure that unsubscribe # really unsubscribed and counter stays at the asserted value ros_client.call_later(2, ros_client.terminate) # def callback(topic_list): # print(topic_list) # assert('/rosout' in topic_list['topics']) # time.sleep(1) # ros_client.terminate() def start_sending(): while True: print("---------------------") if not ros_client.is_connected: break publisher.publish(Message({'data': 'hello world'})) print(listener.is_subscribed) time.sleep(0.1) ros_client.terminate break # ros_client.get_topics(callback) publisher.unadvertise() def start_receiving(): listener.subscribe(receive_message) ros_client.on_ready(start_receiving, run_in_thread=True) ros_client.on_ready(start_sending, run_in_thread=True) ros_client.run_forever() assert context[ 'counter'] >= 3, 'Expected at least 3 messages but got ' + str( context['counter'])
def run_add_two_ints_service(): ros_client = Ros('127.0.0.1', 9090) ros_client.run() def add_two_ints(request, response): response['sum'] = request['a'] + request['b'] return False service_name = '/test_sum_service' service_type = 'rospy_tutorials/AddTwoInts' service = Service(ros_client, service_name, service_type) service.advertise(add_two_ints) time.sleep(1) client = Service(ros_client, service_name, service_type) result = client.call(ServiceRequest({'a': 2, 'b': 40})) assert (result['sum'] == 42) service.unadvertise() time.sleep(2) service.ros.terminate()
def test_rosapi_topics(): context = dict(wait=threading.Event(), result=None) ros = Ros(host, port) ros.run() def callback(topic_list): context['result'] = topic_list context['wait'].set() ros.get_topics(callback) if not context['wait'].wait(5): raise Exception assert ('/rosout' in context['result']['topics']) ros.close()
class rosbridge_client: def __init__(self): self.ros_client = Ros('127.0.0.1', 9090) print("wait for server") self.publisher = Topic(self.ros_client, '/cmd_vel', 'geometry_msgs/Twist') self.listener = Topic(self.ros_client, '/odom', 'nav_msgs/Odometry') self.listener.subscribe(self.callback) self.ros_client.on_ready(self.start_thread, run_in_thread=True) self.ros_client.run_forever() def callback(self, message): x = message["pose"]["pose"]["position"]["x"] y = message["pose"]["pose"]["position"]["y"] print(x, y) def start_thread(self): while True: if self.ros_client.is_connected: self.publisher.publish( Message({ 'linear': { 'x': 0.5, 'y': 0, 'z': 0 }, 'angular': { 'x': 0, 'y': 0, 'z': 0.5 } })) else: print("Disconnect") break time.sleep(1.0)
def test_closing_event(): ros = Ros(url) ros.run() ctx = dict(closing_event_called=False, was_still_connected=False) def handle_closing(): ctx['closing_event_called'] = True ctx['was_still_connected'] = ros.is_connected time.sleep(1.5) ts_start = time.time() ros.on('closing', handle_closing) ros.close() ts_end = time.time() closing_was_handled_synchronously_before_close = ts_end - ts_start >= 1.5 assert ctx['closing_event_called'] assert ctx['was_still_connected'] assert closing_was_handled_synchronously_before_close
def run_rosapi_topics(): ros_client = Ros('127.0.0.1', 9090) def callback(topic_list): assert ('/rosout' in topic_list['topics']) ros_client.terminate() def get_topics(): ros_client.get_topics(callback) ros_client.on_ready(get_topics) ros_client.run_event_loop()
def run_rosapi_topics(*args, **kwargs): ros_client = Ros(*args, **kwargs) def callback(topic_list): print(topic_list) assert('/rosout' in topic_list['topics']) time.sleep(1) ros_client.terminate() def get_topics(): ros_client.get_topics(callback) ros_client.on_ready(get_topics) ros_client.run_forever()
def test_param_manipulation(): ros = Ros('127.0.0.1', 9090) ros.run() param = Param(ros, 'test_param') assert param.get() is None param.set('test_value') assert param.get() == 'test_value' param.delete() assert param.get() is None ros.close()
def run_tf_test(): ros_client = Ros('127.0.0.1', 9090) tf_client = TFClient(ros_client, fixed_frame='world') def callback(message): assert message['translation'] == dict( x=0.0, y=0.0, z=0.0), 'Unexpected translation received' assert message['rotation'] == dict( x=0.0, y=0.0, z=0.0, w=1.0), 'Unexpected rotation received' ros_client.terminate() tf_client.subscribe(frame_id='/world', callback=callback) ros_client.call_later(2, ros_client.terminate) ros_client.run_forever()
def test_topic_pubsub(): context = dict(wait=threading.Event(), counter=0) ros = Ros('127.0.0.1', 9090) ros.run() listener = Topic(ros, '/chatter', 'std_msgs/String') publisher = Topic(ros, '/chatter', 'std_msgs/String') def receive_message(message): context['counter'] += 1 assert message['data'] == 'hello world', 'Unexpected message content' if context['counter'] == 3: listener.unsubscribe() context['wait'].set() def start_sending(): while True: if context['counter'] >= 3: break publisher.publish(Message({'data': 'hello world'})) time.sleep(0.1) publisher.unadvertise() def start_receiving(): listener.subscribe(receive_message) t1 = threading.Thread(target=start_receiving) t2 = threading.Thread(target=start_sending) t1.start() t2.start() if not context['wait'].wait(10): raise Exception t1.join() t2.join() assert context[ 'counter'] >= 3, 'Expected at least 3 messages but got ' + str( context['counter']) ros.close()
def test_tf_test(): context = dict(wait=threading.Event(), counter=0) ros = Ros('127.0.0.1', 9090) ros.run() tf_client = TFClient(ros, fixed_frame='world') def callback(message): context['message'] = message context['counter'] += 1 context['wait'].set() tf_client.subscribe(frame_id='/world', callback=callback) if not context['wait'].wait(5): raise Exception assert context['counter'] > 0 assert context['message']['translation'] == dict( x=0.0, y=0.0, z=0.0), 'Unexpected translation received' assert context['message']['rotation'] == dict( x=0.0, y=0.0, z=0.0, w=1.0), 'Unexpected rotation received' ros.close()
if not callback or ('cbs' in frame and len(frame['cbs']) == 0): self.frame_info.pop(frame_id) def dispose(self): """Unsubscribe and unadvertise all topics associated with this instance.""" if self.current_topic: self.current_topic.unsubscribe() if __name__ == '__main__': from roslibpy import Ros FORMAT = '%(asctime)-15s [%(levelname)s] %(message)s' logging.basicConfig(level=logging.DEBUG, format=FORMAT) ros_client = Ros('127.0.0.1', 9090) def run_tf_example(): tfclient = TFClient(ros_client, fixed_frame='world', angular_threshold=0.01, translation_threshold=0.01) tfclient.subscribe('turtle2', print) def dispose_server(): tfclient.dispose() ros_client.call_later(10, dispose_server) ros_client.call_later(11, ros_client.close) ros_client.call_later(12, ros_client.terminate)
class ROSCloudSubBridge(object): def __init__(self, ip, port, local_topic_name, remote_topic_name, message_type): self._ip = ip self._port = port self._local_topic_name = local_topic_name self._remote_topic_name = remote_topic_name self._message_type = message_type self._ros_client = Ros(self._ip, self._port) self._rosPub = None # Should be specified by the automation engine self._listener = None # Should be specified by the automation engine # self._2listener = None # Should be specified by the automation engine self._logger = logging.getLogger(self.__class__.__name__) self._logger.setLevel(logging.DEBUG) def _run_topic_pubsub(self): try: self._listener = Topic(self._ros_client, self._remote_topic_name, self._message_type) # self._2listener = Topic(self._ros_client, self._remote_topic_name, self._message_type) rospy.init_node('cloudsub', anonymous=True) global my # global my2 # my2=rospy.get_time() my = rospy.get_time() self._ros_client.on_ready(self._start_receiving, run_in_thread=True) self._ros_client.run_event_loop() except: self._logger.exception("Fatal error in constructor") def _receive_message(self, message): global my rospy.loginfo(rospy.get_caller_id() + " Message type %s ", self._message_type) rospy.loginfo( rospy.get_caller_id() + " Time from previous message %s ", (rospy.get_time() - my)) my = rospy.get_time() try: msg = Status() msg.header.seq = message['header']['seq'] msg.header.stamp.secs = message['header']['stamp']['secs'] msg.header.stamp.nsecs = message['header']['stamp']['nsecs'] msg.header.frame_id = message['header']['frame_id'] msg.id = message['id'] msg.active = message['active'] msg.heartbeat_timeout = message['heartbeat_timeout'] msg.heartbeat_period = message['heartbeat_period'] msg.instance_id = message['instance_id'] # for i in range(0,8): # msg.orientation_covariance[i]=message['orientation_covariance'][i] # msg.angular_velocity_covariance[i]=message['angular_velocity_covariance'][i] # msg.linear_acceleration_covariance[i]=message['linear_acceleration_covariance'][i] self._rosPub = rospy.Publisher(self._local_topic_name, Status, queue_size=10) self._rosPub.publish(msg) except: print('Error') # def _2receive_message(self,message): # global my2 # rospy.loginfo(rospy.get_caller_id() + "I heard a message %s",(rospy.get_time()-my2)) # my2=rospy.get_time() # try: # msg=Clock() # msg.clock.secs=message['clock']['secs'] # msg.clock.nsecs=message['clock']['nsecs'] # self._rosPub=rospy.Publisher('/test/clock2', Clock, queue_size=10) #message type $ # self._rosPub.publish(msg) # except: # print('Error') def _start_receiving(self): self._listener.subscribe(self._receive_message) # self._listener2.subscribe(self._2receive_message) def start(self): signal.signal(signal.SIGINT, lambda *x: self.stop()) self._run_topic_pubsub() def stop(self): if self._ros_client is not None: self._ros_client.terminate()
def test_connection_fails_when_schema_not_ws(): with pytest.raises(Exception): Ros(u'http://%s:%d' % (host, port))