def _refresh_ctrlers_cb(self):
try:
# refresh the list of controller managers we can find
srv_list = rosservice.get_service_list()
ctrlman_ns_list_cur = []
for srv_name in srv_list:
if 'controller_manager/list_controllers' in srv_name:
srv_type = rosservice.get_service_type(srv_name)
if srv_type == 'controller_manager_msgs/ListControllers':
ctrlman_ns = srv_name.split(
'/controller_manager/list_controllers')[0]
if ctrlman_ns == '':
ctrlman_ns = '/'
# ctrlman_ns is a Controller Manager namespace
if ctrlman_ns not in self.list_ctrlers:
# we haven't connected to it yet, create the service proxies
self.controller_manager_connect(ctrlman_ns)
ctrlman_ns_list_cur.append(ctrlman_ns)
# remove every controller manager which isn't up anymore
for ctrlman_ns_old in self.list_ctrlers.keys():
if ctrlman_ns_old not in ctrlman_ns_list_cur:
self.controller_manager_disconnect(ctrlman_ns_old)
# refresh the controller list for the current controller manager
self.refresh_loadable_ctrlers()
self.refresh_ctrlers()
except Exception as e:
self.sig_sysmsg.emit(e.message)
def submitPendingTargets(self):
# if there are people actually subscribed to this topic
if '/imaging/target' not in rosservice.get_service_list():
# print(Submission service not yet published")
return
# if the service exists on the network
if self.submit_image_ is None:
# if this is our first time connecting to the image submit service
self.submit_image_ = rospy.ServiceProxy('/imaging/target',
SubmitImage)
target_dao = SubmittedTargetDAO(self.configPath)
# if there are targets waiting to be submitted to the judges
if not target_dao.areTargetsPending():
print("no targets pending")
return
# then lets submit them
pending = target_dao.getAllPendingTargets()
if pending is None or not pending:
return
for target in pending:
imageMsg = self.targetToInteropMsg(target)
resp = self.submit_image_(
**
imageMsg) # map dictionary into function args for submit_image
if resp.success:
# only set a target as submitted if interop says it was successful
target_dao.setTargetSubmitted(target.target, target.autonomous)
def _refresh_ctrlers_cb(self):
try:
# refresh the list of controller managers we can find
srv_list = rosservice.get_service_list()
ctrlman_ns_list_cur = []
for srv_name in srv_list:
if 'controller_manager/list_controllers' in srv_name:
srv_type = rosservice.get_service_type(srv_name)
if srv_type == 'controller_manager_msgs/ListControllers':
ctrlman_ns = srv_name.split('/controller_manager/list_controllers')[0]
if ctrlman_ns == '':
ctrlman_ns = '/'
# ctrlman_ns is a Controller Manager namespace
if ctrlman_ns not in self.list_ctrlers:
# we haven't connected to it yet, create the service proxies
self.controller_manager_connect(ctrlman_ns)
ctrlman_ns_list_cur.append(ctrlman_ns)
# remove every controller manager which isn't up anymore
for ctrlman_ns_old in self.list_ctrlers.keys():
if ctrlman_ns_old not in ctrlman_ns_list_cur:
self.controller_manager_disconnect(ctrlman_ns_old)
# refresh the controller list for the current controller manager
self.refresh_loadable_ctrlers()
self.refresh_ctrlers()
except Exception as e:
self.sig_sysmsg.emit(e.message)
def execute(self, inputs, outputs, gvm):
self.logger.info("Checking ros services")
# check services
servicesToCheck = [
'/tts/speak', '/stt_google', '/nlp/yesno', '/nlp/general'
]
serviceList = rosservice.get_service_list()
serviceNotFound = False
for service in servicesToCheck:
if service not in serviceList:
self.logger.info("Service not found: {}".format(service))
serviceNotFound = True
# check topics
# todo
if serviceNotFound is True:
return -1
else:
self.logger.info("All services were found!")
return 0
def _sloppy_get_controller_managers(namespace):
"""
Get list of I{potential} active controller manager namespaces.
The method name is prepended with I{sloppy}, and the returned list contains
I{potential} active controller managers because it does not perform a
thorough check of the expected ROS API.
It rather tries to minimize the number of ROS master queries.
This method has the overhead of one ROS master query.
@param namespace: Namespace where to look for controller managers.
@type namespace: str
@return: List of I{potential} active controller manager namespaces.
@rtype: [str]
"""
try:
# refresh the list of controller managers we can find
srv_list = rosservice.get_service_list(namespace=namespace)
except rosservice.ROSServiceIOException:
return []
ns_list = []
for srv_name in srv_list:
match_str = '/' + _LIST_CONTROLLERS_STR
if match_str in srv_name:
ns = srv_name.split(match_str)[0]
if ns == '':
# controller manager services live in root namespace
# unlikely, but still possible
ns = '/'
ns_list.append(ns)
return ns_list
def __init__(self, use_controller, joy_topic):
# rospy.loginfo("INITIALIZING CONTROLLER NODE NOW.")
# rospy.loginfo("waiting for update_params service...")
# rospy.wait_for_service('update_params')
# rospy.loginfo("found update_params service")
# self._update_params = rospy.ServiceProxy('update_params', UpdateParams)
# rospy.loginfo("waiting for emergency service")
# rospy.wait_for_service('emergency')
# rospy.loginfo("found emergency service")
# self._emergency = rospy.ServiceProxy('emergency', Empty)
print(rosservice.get_service_list())
if use_controller:
rospy.loginfo("waiting for land service")
rospy.wait_for_service('/land')
rospy.loginfo("found land service")
self._land = rospy.ServiceProxy('/land', Empty)
rospy.loginfo("waiting for takeoff service")
rospy.wait_for_service('/takeoff')
rospy.loginfo("found takeoff service")
self._takeoff = rospy.ServiceProxy('/takeoff', Empty)
else:
self._land = None
self._takeoff = None
# subscribe to the joystick at the end to make sure that all required
# services were found
self._buttons = None
print("Value in init, ", self._takeoff)
rospy.loginfo("subscribing to joy_topic...")
rospy.Subscriber(joy_topic, Joy, self._joyChanged)
rospy.loginfo("subscribed to joy_topic.")
def rtabmap_service_exists(self):
service_list = rosservice.get_service_list()
reset_odom_service = '/' + self.rtabmap_namespace + '/reset_odom'
if (reset_odom_service in service_list):
return True
else:
return False
def trigger_configuration(self):
"""
Callback when the configuration button is clicked
"""
image_topics = sorted(rostopic.find_by_type('sensor_msgs/Image'))
try:
topic_index = image_topics.index(self._sub.resolved_name)
except (AttributeError, ValueError):
topic_index = 0
topic_name, ok = QInputDialog.getItem(self._widget, "Select topic name", "Topic name",
image_topics, topic_index)
if ok:
self._create_subscriber(topic_name)
available_rosservices = []
for s in rosservice.get_service_list():
try:
if rosservice.get_service_type(s) in _SUPPORTED_SERVICES:
available_rosservices.append(s)
except:
pass
try:
srv_index = available_rosservices.index(self._srv.resolved_name)
except (AttributeError, ValueError):
srv_index = 0
srv_name, ok = QInputDialog.getItem(self._widget, "Select service name", "Service name", available_rosservices,
srv_index)
if ok:
self._create_service_client(srv_name)
def _get_init_services(self):
self._init_services = []
service_names = rosservice.get_service_list()
for service_name in service_names:
if "initialize_msf" in service_name:
self._init_services.append(
SrvWidget(self._widget.srv_container_layout, service_name))
def create_reconfigure_client(self, mb_action):
"""
Creates the dynamic reconfigure clients for the given actions
"""
rcnfsrvrname = rospy.get_namespace(
) + mb_action + '/' + self.move_base_reconf_service
test_service = rcnfsrvrname + '/set_parameters'
service_created = False
service_created_tries = 50
while service_created_tries > 0 and not self.cancelled:
service_names = rosservice.get_service_list()
if test_service in service_names:
rospy.loginfo("Creating Reconfigure Client %s" % rcnfsrvrname)
client = dynamic_reconfigure.client.Client(rcnfsrvrname,
timeout=10)
self.rcnfclient[mb_action] = client
self.init_dynparams[mb_action] = client.get_configuration()
return True
else:
service_created_tries -= 1
if service_created_tries > 0:
rospy.logwarn(
"I couldn't create reconfigure client %s. remaining tries %d"
% (rcnfsrvrname, service_created_tries))
rospy.sleep(1)
else:
rospy.logerr(
"I couldn't create reconfigure client %s. is %s running?"
% (rcnfsrvrname, rcnfsrvrname))
return False
def _sloppy_get_controller_managers(namespace):
"""
Get list of I{potential} active controller manager namespaces.
The method name is prepended with I{sloppy}, and the returned list contains
I{potential} active controller managers because it does not perform a
thorough check of the expected ROS API.
It rather tries to minimize the number of ROS master queries.
This method has the overhead of one ROS master query.
@param namespace: Namespace where to look for controller managers.
@type namespace: str
@return: List of I{potential} active controller manager namespaces.
@rtype: [str]
"""
try:
# refresh the list of controller managers we can find
srv_list = rosservice.get_service_list(namespace=namespace)
except ROSServiceIOException:
return []
ns_list = []
for srv_name in srv_list:
match_str = '/' + _LIST_CONTROLLERS_STR
if match_str in srv_name:
ns = srv_name.split(match_str)[0]
if ns == '':
# controller manager services live in root namespace
# unlikely, but still possible
ns = '/'
ns_list.append(ns)
return ns_list
def generate_tasks(self, start_time, end_time):
task_list = []
exploration_service_names = rosservice.get_service_list(
namespace="/exploration_services")
print exploration_service_names
exploration_service_list = [
rospy.ServiceProxy(name, GetExplorationTasks)
for name in exploration_service_names
]
for (name, service) in zip(exploration_service_names,
exploration_service_list):
try:
rospy.loginfo("Calling exploration service " + name)
response = service(start_time, end_time)
service_name = name.split('/')[-1]
print service_name
method_name = "create_tasks_for_" + service_name
if hasattr(self, method_name) and callable(
getattr(self, method_name)):
method = getattr(self, method_name)
task_list += method(response.task_definition,
response.task_score, start_time,
end_time)
else:
rospy.logwarn(
"No method defined to create tasks for service " +
name)
except rospy.ServiceException, e:
rospy.logwarn("Error calling service " + name + ":" + str(e) +
". Skipping...")
def dashboard_connect():
service_list = rosservice.get_service_list(include_nodes=True)
simulation_nodes = filter(
lambda entry: "fmu_simulation" in entry[0] and "set_configuration" in
entry[0], service_list)
# simulation nodes list is: [ ["service_name",[node_name]],... ]
return [e[1][0] for e in simulation_nodes]
def check_services(self):
service_list = rosservice.get_service_list()
if '/piiq/get_camera_data' in service_list and \
'/piiq/excute_move' in service_list and \
'/piiq/components_vision' in service_list and \
'/piiq/load_environment' in service_list :
return True
return False
def get_services_list(self):
'''
Returns the list of service names that are registered by the node
:return _srv: list of services
:return type: [string]
'''
self._srv = get_service_list('/' + self._node_name)
return self._srv
def rtabmap_service_exists():
global rtabmap_namespace
service_list = rosservice.get_service_list()
reset_odom_service = '/' + rtabmap_namespace + '/reset_odom'
if (reset_odom_service in service_list):
return True
else:
return False
def run_tests(tests):
msg_params = {'timeout_secs': 5}
results = []
params_strs = set([test['params_str'] for test in tests])
for params_str in params_strs:
rec_tests = [test for test in tests if test['params_str'] == params_str]
themes = [test_types[test['type']]['listener_theme'] for test in rec_tests]
msg_retriever = CWaitForMessage(msg_params)
print ('*'*30)
print ('Running the following tests: %s' % ('\n' + '\n'.join([test['name'] for test in rec_tests])))
print ('*'*30)
num_of_startups = 5
is_node_up = False
for run_no in range(num_of_startups):
print
print ('*'*8 + ' Starting ROS ' + '*'*8)
print ('running node (%d/%d)' % (run_no, num_of_startups))
cmd_params = ['roslaunch', 'realsense2_camera', 'rs_from_file.launch'] + params_str.split(' ')
print ('running command: ' + ' '.join(cmd_params))
p_wrapper = subprocess.Popen(cmd_params, stdout=None, stderr=None)
time.sleep(2)
service_list = rosservice.get_service_list()
is_node_up = len([service for service in service_list if 'realsense2_camera/' in service]) > 0
if is_node_up:
print ('Node is UP')
break
print ('Node is NOT UP')
print ('*'*8 + ' Killing ROS ' + '*'*9)
p_wrapper.terminate()
p_wrapper.wait()
print ('DONE')
if is_node_up:
listener_res = msg_retriever.wait_for_messages(themes)
if 'static_tf' in [test['type'] for test in rec_tests]:
print ('Gathering static transforms')
frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame', 'camera_fisheye_frame', 'camera_pose']
tf_listener = tf.TransformListener()
listener_res['static_tf'] = dict([(xx, get_tf(tf_listener, xx[0], xx[1])) for xx in itertools.combinations(frame_ids, 2)])
print ('*'*8 + ' Killing ROS ' + '*'*9)
p_wrapper.terminate()
p_wrapper.wait()
else:
listener_res = dict([[theme_name, {}] for theme_name in themes])
print ('*'*30)
print ('DONE run')
print ('*'*30)
for test in rec_tests:
try:
res = run_test(test, listener_res)
except Exception as e:
print ('Test %s Failed: %s' % (test['name'], e))
res = False, '%s' % e
results.append([test['name'], res])
return results
def list_services():
"""
GET /api/<version>/service
GET a list of all available ROS services.
Services are listed in the "results" field of the JSON response body.
"""
return jsonify({"results": rosservice.get_service_list()})
def isColliding(self, jointCoordinates):
"""
Check if the NICO is in collision with itself or the environment
for a certain joint configuration
:param jointCoordinates: List with the joint value for each joint in the correct order,
or a dictionary mapping joint names to joint values
(the default value for joints that are not included in the mapping is their current value)
:type jointCoordinates: list of floats or dict mapping str to float
:return: Returns True if the NICO is in collision at the given joint angles
and returns False otherwise
:rtype: boolean
"""
service_list = rosservice.get_service_list()
if "/moveit/collision_check" not in service_list:
print("Error: /moveit/collision_check service is not available")
return None
if type(jointCoordinates) == list:
jointCoordinates = dict(
zip(self.group.get_active_joints(), jointCoordinates))
# Get current joint values
dic = OrderedDict(
zip(self.group.get_active_joints(),
self.group.get_current_joint_values()))
for key in jointCoordinates:
dic[key] = nicoToRosAngle(key, jointCoordinates[key],
self.jsonConfig, self.vrep)
jointCoordinates = dic.values()
print(jointCoordinates)
jointValues = FloatList()
jointValues.data = []
for jointValue in jointCoordinates:
floatMsg = std_msgs.msg.Float32()
floatMsg.data = jointValue
jointValues.data.append(floatMsg)
groupName = std_msgs.msg.String()
groupName.data = self.groupName
try:
collision_check_service = rospy.ServiceProxy(
"moveit/collision_check", collision_check)
rsp = collision_check_service(groupName, jointValues)
if rsp.collision_state.data:
print("The planning group " + self.groupName +
" is in a collision state")
else:
print("The planning group " + self.groupName +
" is not in a collision state")
return rsp.collision_state.data
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
return None
def list_services():
"""
GET /api/<version>/service
GET a list of all available ROS services.
Services are listed in the "results" field of the JSON response body.
"""
return jsonify({"results": rosservice.get_service_list()})
def __init__(self):
service_name = '/svox_tts/speech'
while service_name not in rosservice.get_service_list():
print 'waiting for ' + service_name
rospy.sleep(5)
self.service = rospy.ServiceProxy(service_name, Speech)
self.service(2, 'test', 100)
self.service(3, 'test', 140)
def listen(self, services):
rospy.loginfo("Tapping services...")
services = rosservice.get_service_list(include_nodes=True)
for (service, node) in services:
# ignore irrelevant services
if node == 'rostrace' or service.endswith(
'/get_loggers') or service.endswith('/set_logger_level'):
continue
self.listen_to(service)
rospy.loginfo("Tapped services")
def get_services():
try:
services = get_service_list()
content = []
for i in services:
content.append([i, get_service_type(i), get_service_node(i)])
return jsonify(status=0, result=content)
except:
return jsonify(status=-1, result=[])
def get_controller_namespaces():
service_list = rosservice.get_service_list()
for service in service_list:
try:
service_type = rosservice.get_service_type(service)
except rosservice.ROSServiceIOException:
pass
else:
if service_type == 'controller_manager_msgs/ListControllers':
yield service.split('/')[1]
def _create_detect_service_client(self):
srv_name = "/detect"
if self._srv_detect:
self._srv_detect.close()
if srv_name in rosservice.get_service_list():
rospy.loginfo("Creating proxy for service '%s'" % srv_name)
self._srv_detect = rospy.ServiceProxy(
srv_name, rosservice.get_service_class_by_name(srv_name))
def _create_service_client(self, srv_name):
"""
Method that creates a client service proxy to call either the GetFaceProperties.srv or the Recognize.srv
:param srv_name:
"""
if self._srv:
self._srv.close()
if srv_name in rosservice.get_service_list():
rospy.loginfo("Creating proxy for service '%s'" % srv_name)
self._srv = rospy.ServiceProxy(srv_name, rosservice.get_service_class_by_name(srv_name))
def get_node_names(self):
"""
Gets a list of available services via a ros service call.
:returns: a list of all nodes that provide the set_logger_level service, ''list(str)''
"""
set_logger_level_nodes = []
nodes = rosnode.get_node_names()
for name in sorted(nodes):
for service in rosservice.get_service_list(name):
if service == name + '/set_logger_level':
set_logger_level_nodes.append(name)
return set_logger_level_nodes
def on_refresh_services_button_clicked(self):
service_names = rosservice.get_service_list()
self._services = {}
for service_name in service_names:
try:
self._services[service_name] = rosservice.get_service_class_by_name(service_name)
#qDebug('ServiceCaller.on_refresh_services_button_clicked(): found service %s using class %s' % (service_name, self._services[service_name]))
except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e:
qWarning('ServiceCaller.on_refresh_services_button_clicked(): could not get class of service %s:\n%s' % (service_name, e))
self.service_combo_box.clear()
self.service_combo_box.addItems(sorted(service_names))
def update_service_list(self):
self.av_stop_services=[]
self.av_activate_services=[]
self.service_names = rosservice.get_service_list()
for i in self.stop_services:
if i in self.service_names:
self.av_stop_services.append(i)
for i in self.activate_services :
if i in self.service_names :
self.av_activate_services.append(i)
def _create_service_client(self, srv_name):
"""
Create a service client proxy
:param srv_name: Name of the service
"""
if self._srv:
self._srv.close()
if srv_name in rosservice.get_service_list():
rospy.loginfo("Creating proxy for service '%s'" % srv_name)
self._srv = rospy.ServiceProxy(
srv_name, rosservice.get_service_class_by_name(srv_name))
def get_node_names(self):
"""
Gets a list of available services via a ros service call.
:returns: a list of all nodes that provide the set_logger_level service, ''list(str)''
"""
set_logger_level_nodes = []
nodes = rosnode.get_node_names()
for name in sorted(nodes):
for service in rosservice.get_service_list(name):
if service == name + '/set_logger_level':
set_logger_level_nodes.append(name)
return set_logger_level_nodes
def update_service_list(self):
self.av_stop_services = []
self.av_activate_services = []
self.service_names = rosservice.get_service_list()
for i in self.stop_services:
if i in self.service_names:
self.av_stop_services.append(i)
for i in self.activate_services:
if i in self.service_names:
self.av_activate_services.append(i)
def rosViewer():
service_list = rosservice.get_service_list()
print service_list
rospy.wait_for_service('/gazebo/get_model_state')
print "finished waiting for service..."
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
model = GetModelStateRequest()
model.model_name = 'piano2'
while not rospy.is_shutdown():
result = get_model_srv(model)
print '[', result.pose.position.x, ',', result.pose.position.y, ',', result.pose.position.z, ']'
time.sleep(1)
def check_status(status, srv):
# Check if service exists in ROS services
if any(srv in s for s in rosservice.get_service_list()):
status.level = DiagnosticStatus.OK
status.message = "OK"
status.values = [KeyValue(key="Update Status", value="OK")]
else:
status.level = DiagnosticStatus.ERROR
status.message = "Error"
status.values = [
KeyValue(key="Update Status", value="Error"),
KeyValue(key="%s" % srv, value="Service not found")
]
def update_service_list(self):
self.av_stop_services=[]
self.av_activate_services=[]
#print rosservice.get_service_list()
self.service_names = rosservice.get_service_list()#[x[0] for x in rosgraph.masterapi.Master('/no_go_nodes_stop').getSystemState()[2]]
#print service_names
for i in self.stop_services:
if i in self.service_names:
self.av_stop_services.append(i)
for i in self.activate_services :
if i in self.service_names :
self.av_activate_services.append(i)
def update_service_list(self):
self.av_stop_services=[]
self.av_activate_services=[]
#print rosservice.get_service_list()
self.service_names = rosservice.get_service_list()#[x[0] for x in rosgraph.masterapi.Master('/no_go_nodes_stop').getSystemState()[2]]
#print service_names
for i in self.stop_services:
if i in self.service_names:
self.av_stop_services.append(i)
for i in self.activate_services :
if i in self.service_names :
self.av_activate_services.append(i)
def on_refresh_services_button_clicked(self):
service_names = rosservice.get_service_list()
self._services = {}
for service_name in service_names:
try:
self._services[service_name] = rosservice.get_service_class_by_name(service_name)
#qDebug('ServiceCaller.on_refresh_services_button_clicked(): found service %s using class %s' % (service_name, self._services[service_name]))
except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e:
qWarning('ServiceCaller.on_refresh_services_button_clicked(): could not get class of service %s:\n%s' % (service_name, e))
except Exception as e:
qWarning('ServiceCaller.on_refresh_services_button_clicked(): failed to load class of service %s:\n%s' % (service_name, e))
self.service_combo_box.clear()
self.service_combo_box.addItems(sorted(self._services.keys()))
def run(self):
state = None
while True:
sm = self.sm()
if sm is None:
break
time.sleep(0.2)
try:
new_state = rosservice.get_service_list()
if new_state != state:
state = new_state
self.refresh.emit(state)
except Exception:
pass
def check_firmware_version() -> str:
services = rosservice.get_service_list()
if rospy.resolve_name("firmware/get_firmware_version") in services:
get_firmware_version = rospy.ServiceProxy(
"firmware/get_firmware_version", Trigger)
# Support legacy firmware
elif rospy.resolve_name("core2/get_firmware_version") in services:
get_firmware_version = rospy.ServiceProxy("core2/get_firmware_version",
Trigger)
else:
return "<unknown>"
return get_firmware_version().message
def on_combobox(self, event):
print 'combobox event'
if self.cb_in_sight == 0 :
print str(self.combobox.GetValue())
#print rosservice.get_service_list()
for s in rosservice.get_service_list():
if s.endswith(self.output_ns+ '/switch'): #'camera_dispatcher/switch'
switch_cam = rospy.ServiceProxy(self.output_ns+ '/switch', Switch) # 'camera_dispatcher/switch' | camera_pose_toolkits.srv.Switch
resp = switch_cam ( str(self.combobox.GetValue()))
break
else:
wx.MessageBox('Service not ready. Try again later.', 'Info!')
else:
wx.MessageBox('Remove checkerboard from '+ self.output_ns +'\' field of view before switching camera', 'Info!')
def on_pushButton_refresh_clicked(self):
self._services = {}
# Select calibration services
service_names = rosservice.get_service_list()
for service_name in service_names:
try:
current_service = rosservice.get_service_class_by_name(service_name)
if current_service.__name__ == CALIBRATION_SERVICE_TYPE_NAME:
self._services[service_name] = current_service
except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e:
qWarning('on_pushButton_refresh_clicked(): could not get class of service %s:\n%s' % (service_name, e))
self._widget.comboBox_services.clear()
self._widget.comboBox_services.addItems(sorted(self._services.keys()))
def __init__(self):
super(YesNoButtonWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
'resource', 'yes_no_button.ui')
loadUi(ui_file, self)
self.setObjectName('YesNoButtonUi')
self.yes_button.clicked[bool].connect(self._clicked_yes_button)
self.no_button.clicked[bool].connect(self._clicked_no_button)
self.yes_button.setEnabled(False)
self.no_button.setEnabled(False)
self.yes = None
service_name = rospy.get_namespace() + 'rqt_yn_btn'
if service_name in rosservice.get_service_list():
rospy.logwarn('{} is already advertised'.format(service_name))
return
self.srv = rospy.Service('rqt_yn_btn', YesNo, self._handle_yn_btn)
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(service_name)
tool_tip += '\n %s [%s]' % (service_name, service_type)
except rosservice.ROSServiceIOException, e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
def _get_init_services(self):
self._init_services = []
service_names = rosservice.get_service_list()
for service_name in service_names:
if "initialize_msf" in service_name:
self._init_services.append(SrvWidget(self._widget.srv_container_layout, service_name))
def __init__(self) :
self.cancelled = False
self.preempted = False
#self.aborted = False
self.current_action = 'none'
self.current_route = None
self.n_tries = rospy.get_param('~retries', 3)
rospy.on_shutdown(self._on_node_shutdown)
self.move_base_actions = ['move_base','human_aware_navigation']
self.needed_actions=[]
self.navigation_activated=False
self._action_name = '/topological_navigation/execute_policy_mode'
self.stats_pub = rospy.Publisher('/execute_policy_mode/Statistics', NavStatistics, queue_size=1)
self.lnodes = []
rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback)
rospy.loginfo("Waiting for Topological map ...")
while len(self.lnodes) == 0 :
rospy.sleep(rospy.Duration.from_sec(0.05))
rospy.loginfo(" ...done")
self.needed_move_base_actions = [x for x in self.needed_actions if x in self.move_base_actions]
if 'move_base' not in self.needed_move_base_actions:
self.needed_move_base_actions.append('move_base')
#Creating monitored navigation client
rospy.loginfo("Creating monitored navigation client.")
self.monNavClient= actionlib.SimpleActionClient('monitored_navigation', MonitoredNavigationAction)
self.monNavClient.wait_for_server()
rospy.loginfo(" ...done")
#Subscribing to Localisation Topics
rospy.loginfo("Waiting for Localisation Topics")
self.current_node = rospy.wait_for_message('/current_node', String)
self.closest_node = rospy.wait_for_message('/closest_node', String)
rospy.loginfo("Subscribing to Localisation Topics")
rospy.Subscriber('/closest_node', String, self.closestNodeCallback)
rospy.Subscriber('/current_node', String, self.currentNodeCallback)
rospy.loginfo(" ...done")
mb_service_created=False
self.rcnfclient={}
config = {}
#Creating Reconfigure Client
for i in self.needed_move_base_actions:
client = None
rcnfsrvrname= '/'+i+'/DWAPlannerROS'
test_service = rcnfsrvrname+'/set_parameters'
service_created=False
service_created_tries=50
while service_created_tries>0 and not self.cancelled :
service_names = rosservice.get_service_list()
if test_service in service_names:
rospy.loginfo("Creating Reconfigure Client %s" %rcnfsrvrname)
client = dynamic_reconfigure.client.Client(rcnfsrvrname, timeout=10)
self.rcnfclient[i] = client
config[i] = self.rcnfclient[i].get_configuration()
service_created=True
service_created_tries=0
else:
service_created_tries-=1
if service_created_tries>0:
rospy.logwarn("I couldn't create reconfigure client %s. remaining tries %d" %(rcnfsrvrname,service_created_tries))
rospy.sleep(1)
else:
rospy.logerr("I couldn't create reconfigure client %s. is %s running?" %(rcnfsrvrname, i))
if service_created and i == 'move_base':
mb_service_created=True
if mb_service_created:
self.dyt = config['move_base']['yaw_goal_tolerance']
else:
while not mb_service_created and not self.cancelled:
rcnfsrvrname= '/move_base/DWAPlannerROS'
test_service = rcnfsrvrname+'/set_parameters'
rospy.logwarn("%s must be created! will keep trying until its there" %rcnfsrvrname)
service_names = rosservice.get_service_list()
if test_service in service_names:
rospy.loginfo("Creating Reconfigure Client %s" %rcnfsrvrname)
client = dynamic_reconfigure.client.Client(rcnfsrvrname, timeout=10)
self.rcnfclient['move_base'] = client
config['move_base'] = self.rcnfclient['move_base'].get_configuration()
mb_service_created=True
else:
rospy.sleep(1)
if not self.cancelled:
self.dyt = config['move_base']['yaw_goal_tolerance']
rospy.loginfo("default yaw tolerance %f" %self.dyt)
#Creating Action Server
rospy.loginfo("Creating action server.")
self._as = actionlib.SimpleActionServer(self._action_name, strands_navigation_msgs.msg.ExecutePolicyModeAction, execute_cb = self.executeCallback, auto_start = False)
self._as.register_preempt_callback(self.preemptCallback)
rospy.loginfo(" ...starting")
self._as.start()
rospy.loginfo(" ...done")
rospy.loginfo("EPM All Done ...")
rospy.spin()
def list_services():
return {'services': get_service_list(None, None)}
def find_reconfigure_services():
return sorted(
[s[: -len("/set_parameters")] for s in rosservice.get_service_list() if s.endswith("/set_parameters")]
)
def get_services():
""" Returns a list of all the services advertised in the ROS system """
return get_service_list()
#! /usr/bin/env python
import rosservice
servList = rosservice.get_service_list()
maestorServs = []
serviceArgs = dict()
for serv in servList:
if "/get_loggers" not in serv and "/set_logger_level" not in serv:
maestorServs.append(serv)
for serv in maestorServs:
serviceArgs[serv] = rosservice.get_service_args(serv)
f = open("/opt/ros/fuerte/stacks/maestor/console/maestorapi.py", "w")
# Begin writing the console dynamically. WOO
f.write("#! /usr/bin/env python\n")
f.write("import rlcompleter\n")
f.write("import readline\n")
f.write("import rosservice\n\n")
f.write('readline.parse_and_bind("tab: complete")\n\n')
f.write("def ls():\n")
f.write(" return " + str(maestorServs) + "\n\n")
for serv in serviceArgs:
f.write("def " + serv.replace("/", "") + " (" + serviceArgs[serv].replace(" ", ", ") + "):\n")
def _list(self):
"""
Override Namespace._list()
"""
# hide rosh services
return [s for s in rosservice.get_service_list(namespace=self._ns) if not s.startswith('/rosh_')]
def find_reconfigure_services():
import rosservice
return sorted([s[:-len('/set_parameters')] for s in rosservice.get_service_list() if s.endswith('/set_parameters')])
def __GetServices(self): return rosservice.get_service_list()
