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