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()
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()
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 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()
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()
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=TFMessage() msg1=TransformStamped() list_msg=[] for i in range(len(message['transforms'])) : msg1.header.seq=message['transforms'][i]['header']['seq'] msg1.header.stamp.secs=message['transforms'][i]['header']['stamp']['secs'] msg1.header.stamp.nsecs=message['transforms'][i]['header']['stamp']['nsecs'] msg1.header.frame_id=message['transforms'][i]['header']['frame_id'] msg1.child_frame_id=message['transforms'][i]['child_frame_id'] msg1.transform.translation.x=message['transforms'][i]['transform']['translation']['x'] msg1.transform.translation.y=message['transforms'][i]['transform']['translation']['y'] msg1.transform.translation.z=message['transforms'][i]['transform']['translation']['z'] msg1.transform.rotation.x=message['transforms'][i]['transform']['rotation']['x'] msg1.transform.rotation.y=message['transforms'][i]['transform']['rotation']['y'] msg1.transform.rotation.z=message['transforms'][i]['transform']['rotation']['z'] msg1.transform.rotation.w=message['transforms'][i]['transform']['rotation']['w'] list_msg.append(msg1) msg=TFMessage(list_msg) self._rosPub=rospy.Publisher(self._local_topic_name, TFMessage, queue_size=10) 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()
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) 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() # try: msg = OccupancyGrid() msg.header.seq = message['header']['seq'] msg.header.stamp = rospy.Time.now( ) #.secs=message['header']['stamp']['secs'] msg.header.frame_id = message['header']['frame_id'] #type unicode msg.info.map_load_time.secs = message['info']['map_load_time']['secs'] msg.info.map_load_time.nsecs = message['info']['map_load_time'][ 'nsecs'] msg.info.resolution = message['info']['resolution'] msg.info.width = message['info']['width'] msg.info.height = message['info']['height'] msg.info.origin.position.x = message['info']['origin']['position']['x'] msg.info.origin.position.y = message['info']['origin']['position']['y'] msg.info.origin.position.z = message['info']['origin']['position']['z'] msg.info.origin.orientation.x = message['info']['origin'][ 'orientation']['x'] msg.info.origin.orientation.y = message['info']['origin'][ 'orientation']['y'] msg.info.origin.orientation.z = message['info']['origin'][ 'orientation']['z'] msg.info.origin.orientation.w = message['info']['origin'][ 'orientation']['w'] msg.data = message['data'] # for i in range(len(message['data'])): # print(i) # msg.data[i]=message['data'][i] self._rosPub = rospy.Publisher(self._local_topic_name, OccupancyGrid, queue_size=10) 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()
class MainWindow(QMainWindow): on_run = pyqtSignal(dict) disconnected = pyqtSignal() def __init__(self): super(MainWindow, self).__init__() self.host, ok_pressed = QInputDialog.getText(self, '连接', '机器人地址', QLineEdit.Normal, '127.0.0.1') if not ok_pressed or len(self.host) < 7: exit(0) try: self.ros_client = Ros(self.host, 9090) self.ros_client.run(3) self.ros_client.on('close', self.on_lost_connect) except Exception as e: QMessageBox.critical(self, "错误", e.args[0]) exit(0) self.setWindowTitle('调试器') self.statusBar().setSizeGripEnabled(False) self.statusBar().setStyleSheet('border: 1px solid black;') self._toolMenu = self.menuBar().addMenu('工具') self._srv_action = QAction('Service表', self._toolMenu) self._srv_action.setCheckable(True) self._srv_action.toggled.connect(self.on_srv_table_widget) self._topics_action = QAction('Topics图', self._toolMenu) self._topics_action.setCheckable(True) self._topics_action.toggled.connect(self.on_topics_widget) self._remote_action = QAction('遥控', self._toolMenu) self._remote_action.setCheckable(True) self._remote_action.toggled.connect(self.on_remote_widget) self._logs_action = QAction('日志', self._toolMenu) self._logs_action.setCheckable(True) self._logs_action.toggled.connect(self.on_log_widget) self._image_action = QAction('图像', self._toolMenu) self._image_action.setCheckable(True) self._image_action.toggled.connect(self.on_image_widget) self._params_action = QAction('参数', self._toolMenu) self._params_action.setCheckable(True) self._params_action.toggled.connect(self.on_params_widget) self._toolMenu.addAction(self._srv_action) self._toolMenu.addAction(self._topics_action) self._toolMenu.addAction(self._remote_action) self._toolMenu.addAction(self._logs_action) self._toolMenu.addAction(self._image_action) self._toolMenu.addAction(self._params_action) self.menuBar().addSeparator() self._act_debug_action = self.menuBar().addAction('动作调试器') self._act_debug_action.triggered.connect(self.on_act_debug) self.menuBar().addSeparator() self._state_monitor_action = self.menuBar().addAction('状态监测器') self._state_monitor_action.triggered.connect(self.on_state_monitor) self.hostLabel = QLabel(self.host) self.hostLabel.setText(self.host) self.statusBar().addWidget(self.hostLabel) self.mainWidget = QWidget() self.main_layout = QVBoxLayout() self.main_splitter = QSplitter(Qt.Horizontal, self) self.left_splitter = QSplitter(Qt.Vertical, self.main_splitter) self.middle_splitter = QSplitter(Qt.Vertical, self.main_splitter) self.right_splitter = QSplitter(Qt.Vertical, self.main_splitter) self.main_splitter.addWidget(self.left_splitter) self.main_splitter.addWidget(self.middle_splitter) self.main_splitter.addWidget(self.right_splitter) self.boxqss = '''QGroupBox { border: 2px solid black; border-radius: 5px; margin-top:1ex; } QGroupBox::title { subcontrol-origin: margin; subcontrol-position: top center; padding: 0 3px; }''' self.main_layout.addWidget(self.main_splitter) self.mainWidget.setLayout(self.main_layout) self.setCentralWidget(self.mainWidget) self.action_debug_client = None self.on_run.connect(self.run_add_angles) self.disconnected.connect(self.on_disconnected) self.srvWidget = None self.graphWidget = None self.remoteWidget = None self.logWidget = None self.imageWidget = None self.paramsWidget = None self.on_srv_table_widget() self.on_topics_widget() self.on_remote_widget() self.on_log_widget() self.on_image_widget() self.on_params_widget() def on_recv_action(self, req, res): self.on_run.emit(req) return True def run_add_angles(self, req): try: run_service = Service(self.ros_client, '/add_angles', 'common/AddAngles') run_service.call(ServiceRequest(req)) except Exception as e: QMessageBox.critical(self, "错误", e.args[0]) 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 on_state_monitor(self): cmd = 'rosrun team_monitor team_monitor ' os.popen(cmd) def on_lost_connect(self, proto): self.disconnected.emit() def on_disconnected(self): QMessageBox.critical(self, "错误", '连接已断开') self.close() def on_srv_table_widget(self, show=None): if show is None: groupbox = QGroupBox('Service表') groupbox.setStyleSheet(self.boxqss) layout = QVBoxLayout() self.srvWidget = ServiceTableWidget(self.ros_client) layout.addWidget(self.srvWidget) groupbox.setLayout(layout) groupbox.hide() self.left_splitter.insertWidget(0, groupbox) elif show: self.left_splitter.widget(0).show() else: self.left_splitter.widget(0).hide() self.adjustSize() def on_topics_widget(self, show=None): if show is None: groupbox = QGroupBox('Topic拓扑图') groupbox.setStyleSheet(self.boxqss) layout = QVBoxLayout() self.graphWidget = TopicGraphWidget(self.ros_client) layout.addWidget(self.graphWidget) groupbox.setLayout(layout) groupbox.hide() self.left_splitter.insertWidget(1, groupbox) elif show: self.left_splitter.widget(1).show() else: self.left_splitter.widget(1).hide() self.adjustSize() def on_remote_widget(self, show=None): if show is None: groupbox = QGroupBox('遥控') groupbox.setStyleSheet(self.boxqss) layout = QVBoxLayout() self.remoteWidget = RemoteWidget(self.ros_client) layout.addWidget(self.remoteWidget) groupbox.setLayout(layout) groupbox.hide() self.middle_splitter.insertWidget(0, groupbox) elif show: self.middle_splitter.widget(0).show() else: self.middle_splitter.widget(0).hide() self.adjustSize() def on_log_widget(self, show=None): if show is None: groupbox = QGroupBox('运行日志') groupbox.setStyleSheet(self.boxqss) layout = QVBoxLayout() self.logWidget = LogWidget(self.ros_client) layout.addWidget(self.logWidget) groupbox.setLayout(layout) groupbox.hide() self.middle_splitter.insertWidget(1, groupbox) elif show: self.middle_splitter.widget(1).show() else: self.middle_splitter.widget(1).hide() self.adjustSize() def on_image_widget(self, show=None): if show is None: groupbox = QGroupBox('在线图像') groupbox.setStyleSheet(self.boxqss) layout = QVBoxLayout() self.imageWidget = ImageWidget(self.ros_client) layout.addWidget(self.imageWidget) groupbox.setLayout(layout) groupbox.hide() self.right_splitter.insertWidget(0, groupbox) elif show: self.right_splitter.widget(0).show() else: self.right_splitter.widget(0).hide() self.adjustSize() def on_params_widget(self, show=None): if show is None: groupbox = QGroupBox('参数') groupbox.setStyleSheet(self.boxqss) layout = QVBoxLayout() self.paramsWidget = ParamsWidget(self.ros_client) layout.addWidget(self.paramsWidget) groupbox.setLayout(layout) groupbox.hide() self.right_splitter.insertWidget(1, groupbox) elif show: self.right_splitter.widget(1).show() else: self.right_splitter.widget(1).hide() self.adjustSize() def closeEvent(self, a0: QtGui.QCloseEvent) -> None: if self.ros_client is not None: self.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._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) 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() try: msg=LaserScan() 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.angle_min=message['angle_min'] msg.angle_max=message['angle_max'] msg.angle_increment=message['angle_increment'] msg.time_increment=message['time_increment'] msg.scan_time=message['scan_time'] msg.range_min=message['range_min'] msg.range_max=message['range_max'] ranges_list=[] intensities_list=[] for i in range(len(message['ranges'])): ranges_list.append(float) if message['ranges'][i]==None: ranges_list[i]=float('NaN') else: ranges_list[i]=message['ranges'][i] for i in range(len(message['intensities'])): intensities_list.append(float) if message['intensities'][i]==None: intensities_list[i]=float('NaN') else: intensities_list[i]=message['intensities'][i] msg.ranges=ranges_list msg.intensities=intensities_list self._rosPub=rospy.Publisher(self._local_topic_name, LaserScan, queue_size=10) 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()