def handle_updates(self, ros: "Ros") -> Callable[[], None]: """Handle incoming update for `/scan` topic and update robot's ranges accordingly. Args: ros (Ros): ROS client. Returns: A callback for unsubscribing the topic. """ topic = Topic(ros, "/scan", "sensor_msgs/LaserScan") topic.subscribe(self.__scan_handler) return lambda: topic.unsubscribe()
def handle_updates(self, ros: "Ros") -> Callable[[], None]: """Handle incoming update for `/odom` topic and update robot's position accordingly. Args: ros (Ros): ROS client. Returns: A callback for unsubscribing the topic. """ topic = Topic(ros, "/odom", "nav_msgs/Odometry") topic.subscribe(self.__odom_handler) return lambda: topic.unsubscribe()
def handle_updates(self, ros: "Ros") -> Callable[[], None]: """Handle incoming update for `/camera/rgb/image_raw/compressed` topic and update robot's image accordingly. Args: ros (Ros): ROS client. Returns: A callback for unsubscribing the topic. """ topic = Topic(ros, "/camera/rgb/image_raw/compressed", "sensor_msgs/CompressedImage") topic.subscribe(self.__image_handler) return lambda: topic.unsubscribe()
class DirectUrActionClient(EventEmitterMixin): """Direct UR Script action client to simulate an action interface on arbitrary URScript commands. """ def __init__(self, ros, timeout=None, omit_feedback=False, omit_status=False, omit_result=False): super(DirectUrActionClient, self).__init__() self.server_name = '/ur_driver/URScript' self.ros = ros self.timeout = timeout self.omit_feedback = omit_feedback self.omit_status = omit_status self.omit_result = omit_result self.goal = None self._received_status = False # Advertise the UR Script topic self._urscript_topic = Topic(ros, self.server_name, 'std_msgs/String') self._urscript_topic.advertise() # Create the topics associated with actionlib self.status_listener = Topic(ros, '/tool_velocity', 'geometry_msgs/TwistStamped') # Subscribe to the status topic if not self.omit_status: self.status_listener.subscribe(self._on_status_message) # If timeout specified, emit a 'timeout' event if the action server does not respond if self.timeout: self.ros.call_later(self.timeout / 1000., self._trigger_timeout) self._internal_state = 'idle' def _on_status_message(self, message): self._received_status = True self.goal.emit('status', message) twist = message['twist'] total_velocity = math.fsum(list(twist['linear'].values()) + list(twist['angular'].values())) in_motion = total_velocity != 0.0 if self._internal_state == 'idle': if in_motion: self._internal_state = 'executing_goal' elif self._internal_state == 'executing_goal': if not in_motion: self._internal_state = 'stopped' self.goal.emit('result', message) def _trigger_timeout(self): if not self._received_status: self.emit('timeout') def add_goal(self, goal): """Add a goal to this action client. Args: goal (:class:`.Goal`): Goal to add. """ self.goal = goal def cancel(self): """Cancel all goals associated with this action client.""" pass # self.cancel_topic.publish(Message()) def dispose(self): """Unsubscribe and unadvertise all topics associated with this action client.""" # And the UR Script topic self._urscript_topic.unadvertise() if not self.omit_status: self.status_listener.unsubscribe(self._on_status_message) def send_goal(self, goal, result_callback=None, timeout=None): """Send goal to the action server. Args: goal (:class:`URGoal`): The goal containing the script lines timeout (:obj:`int`): Timeout for the goal's result expressed in milliseconds. callback (:obj:`callable`): Function to be called when a result is received. It is a shorthand for hooking on the ``result`` event. """ self._internal_state == 'idle' if result_callback: goal.on('result', result_callback) ur_script = goal.goal_message['goal']['script'] message = Message({'data': ur_script}) self._urscript_topic.publish(message) if timeout: self.ros.call_later( timeout / 1000., goal._trigger_timeout)
class ImageWidget(QWidget): on_image = pyqtSignal(str) def __init__(self, ros: Ros, parent=None): super(ImageWidget, self).__init__(parent) if not os.path.exists(IMAGE_SAVE_DIR): os.mkdir(IMAGE_SAVE_DIR) self.ros_client = ros layout = QVBoxLayout() self._image_label = ImageLabel() self._image_label.shot.connect(self.on_shot) layout.addWidget(self._image_label) btn_layout = QHBoxLayout() self._subscribe_btn = QPushButton('订阅图像') self._subscribe_btn.clicked.connect(self.subscribe_image) self._type_combo_box = QComboBox() self._type_combo_box.addItems(['None', 'Origin', 'Result']) self._type_combo_box.currentIndexChanged.connect(self.on_type_changed) self._func_combo_box = QComboBox() self._func_combo_box.addItems(['function1', 'function2']) self._save_btn = QPushButton('保存') self._save_btn.clicked.connect(self.on_save_clicked) self._auto_save_check_box = QCheckBox('自动保存') self._auto_save_check_box.toggled.connect(self.on_auto_save_check) btn_layout.addWidget(self._subscribe_btn) btn_layout.addWidget(self._type_combo_box) btn_layout.addWidget(self._func_combo_box) btn_layout.addWidget(self._save_btn) btn_layout.addWidget(self._auto_save_check_box) layout.addLayout(btn_layout) self.setLayout(layout) self._save = False self._auto_save = False self._subscribe = False self._image_topic = Topic(self.ros_client, '/result/vision/compressed', 'sensor_msgs/CompressedImage') self.on_image.connect(self.on_recv_image) self._last_seq = 0 def check_connect(self): if not self.ros_client.is_connected: QMessageBox.critical(self, "错误", '连接中断,请重新连接') return False return True def subscribe_image(self): if not self.check_connect(): return self._subscribe = not self._subscribe if self._subscribe: self._image_topic.subscribe(self.recv_image) self._subscribe_btn.setText('取消订阅') t = self.ros_client.get_param('image') if t is not None: self._type_combo_box.setCurrentIndex(t) else: self._image_topic.unsubscribe() self._subscribe_btn.setText('订阅图像') def recv_image(self, msg): base64_bytes = msg['data'].encode('ascii') image_bytes = base64.b64decode(base64_bytes) fmt = msg['format'] seq = msg['header']['seq'] name = IMAGE_SAVE_DIR + 'temp.{}'.format(fmt) if self._auto_save or self._save: if seq - self._last_seq > 100 or self._save: self._save = False name = IMAGE_SAVE_DIR + datetime.datetime.now().strftime( '%Y%m%d%H%M%S') + '.' + fmt self._last_seq = seq with open(name, 'wb') as image_file: image_file.write(image_bytes) self.on_image.emit(name) def on_recv_image(self, name): self._image_label.set_image(name) def on_auto_save_check(self, val): self._auto_save = val def on_save_clicked(self): self._save = True 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 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])