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)
