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 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()
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 call(self, client, request, callback, errback): def inner_handler(response_msg): if not compas.PY3: # IronPython sometimes decides it's a good idea to use an old-style class # to pass the response_msg around, so we need to make sure we turn it into # a proper ServiceResponse again # https://github.com/compas-dev/compas_fab/issues/235 is_old_style_class = isinstance(response_msg, types.InstanceType) if is_old_style_class: response_msg = dict(response_msg) response_object = self.response_class.from_msg(response_msg) # Validate the response if there's a validator function assigned if self.validator: try: self.validator(response_object) except Exception as e: errback(RosValidationError(e, response_object)) return callback(response_object) if isinstance(request, tuple): request_msg = self.request_class(*request) else: request_msg = self.request_class(**request) srv = Service(client, self.name, self.type) srv.call(ServiceRequest(request_msg.msg), callback=inner_handler, errback=errback)
def on_imu_reset(self): if not self.check_connect(): return try: service = Service(self.ros_client, '/imu_reset', 'std_srvs/Empty') service.call(ServiceRequest({})) except Exception as e: QMessageBox.critical(self, "错误", e.args[0])
def on_type_changed(self, idx): if not self.check_connect(): return try: service = Service(self.ros_client, '/setting/sendtype', 'common/SetInt') service.call(ServiceRequest({'number': idx})) except Exception as e: QMessageBox.critical(self, "错误", e.args[0])
def get_planning_scene(self, callback, components): """ """ reqmsg = GetPlanningSceneRequest(PlanningSceneComponents(components)) def receive_message(msg): response = GetPlanningSceneResponse.from_msg(msg) callback(response) srv = Service(self, '/get_planning_scene', 'moveit_msgs/GetPlanningScene') request = ServiceRequest(reqmsg.msg) srv.call(request, receive_message, receive_message)
def call(self, client, request, callback, errback): def inner_handler(response_msg): callback(self.response_class.from_msg(response_msg)) if isinstance(request, tuple): request_msg = self.request_class(*request) else: request_msg = self.request_class(**request) srv = Service(client, self.name, self.type) srv.call(ServiceRequest(request_msg.msg), callback=inner_handler, errback=errback)
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()
def on_shot(self, rect: QRect): if not self.check_connect(): return try: service = Service(self.ros_client, '/debug/image/snap', 'common/ImageSnap') result = service.call( ServiceRequest({ 'type': self._func_combo_box.currentIndex(), 'info': { 'x': rect.topLeft().x(), 'y': rect.topLeft().y(), 'w': rect.width(), 'h': rect.height() } })) except Exception as e: QMessageBox.critical(self, "错误", e.args[0])
def call(self, client, request, callback, errback): def inner_handler(response_msg): response_object = self.response_class.from_msg(response_msg) # Validate the response if there's a validator function assigned if self.validator: try: self.validator(response_object) except Exception as e: errback(RosValidationError(e, response_object)) return callback(response_object) if isinstance(request, tuple): request_msg = self.request_class(*request) else: request_msg = self.request_class(**request) srv = Service(client, self.name, self.type) srv.call(ServiceRequest(request_msg.msg), callback=inner_handler, errback=errback)
def load_actions(self): self._acts_widget.clear() if not self.check_connect(): return try: service = Service(self.ros_client, '/get_actions', 'common/GetActions') result = service.call(ServiceRequest({})) actions = result['actions'] self._acts_widget.clear() for action in actions: litem = QListWidgetItem() aitem = QLabel(action) aitem.setFixedHeight(30) aitem.setStyleSheet('QLabel{margin-left: 5px;}') aitem.show() self._acts_widget.addItem(litem) self._acts_widget.setItemWidget(litem, aitem) litem.setSizeHint( QSize(aitem.rect().width(), aitem.rect().height())) except Exception as e: QMessageBox.critical(self, "错误", e.args[0])
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 invoke_service(): client = Service(ros_client, '/test_server', 'rospy_tutorials/AddTwoInts') client.call(ServiceRequest({'a': 2, 'b': 40}), check_sum, print)