def test_rosparam_list(self):
cmd = 'rosparam'
params = ['/string', '/int', '/float',
'/g1/string', '/g1/int', '/g1/float',
'/g2/string', '/g2/int', '/g2/float',
]
l = rosparam.list_params('')
for t in params:
self.assert_(t in l)
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list'])
self._check(params, tolist(b))
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list', '/'])
self._check(params, tolist(b))
# test with namespace
g1p = [p for p in params if p.startswith('/g1/')]
not_g1p = [p for p in params if not p.startswith('/g1/')]
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list', '/g1'])
self._check(g1p, tolist(b))
self._notcheck(not_g1p, tolist(b))
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list', '/g1/'])
self._check(g1p, tolist(b))
self._notcheck(not_g1p, tolist(b))
# test with no match
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list', '/not/a/namespace/'])
self.assertEquals([], tolist(b))
def __init__(self, name):
#check if activity recognition node is alive
self.name = name
if 'rgb' in name:
self.type_class = 'rgb'
if 'flow' in name:
self.type_class = 'flow'
while not '/%s/class_tsn_%s/alive' % (
name, self.type_class) in rosparam.list_params(
'/'): # the naming here is dumb..
rospy.logwarn(
'classifier not started. waiting 3 seconds for it to start')
time.sleep(3)
while rosparam.get_param('/%s/class_tsn_%s/alive' %
(name, self.type_class)) is not 1:
rospy.loginfo(
'classifier started, but not alive. waiting 1 second for it to start'
)
time.sleep(1)
self.start_act = rospy.ServiceProxy('/%s/start_vidscores' % (name),
Empty)
self.stop_act = rospy.ServiceProxy('/%s/stop_vidscores' % (name),
Empty)
rospy.Subscriber('/%s/action_own_label' % (name),
Action,
self.getslatest_activity,
queue_size=1)
rospy.Subscriber('/%s/action_own_label_dic' % (name),
ActionDic,
self.getslatest_activityDIC,
queue_size=1)
self.actact = []
self.dic = []
def refresh_plugin_descriptions(self):
# clear old status
self.plugin_descriptions = []
for cb in self.plugin_cb:
cb.blockSignals(True)
cb.clear()
self.plugin_manager_widget.addPluginPushButton.setEnabled(False)
# collect all plugin descriptions from manager
if self.get_plugin_descriptions_client.wait_for_server(rospy.Duration(1.0)):
self.get_plugin_descriptions_client.send_goal(GetPluginDescriptionsGoal())
if self.get_plugin_descriptions_client.wait_for_result(rospy.Duration(5.0)):
self.plugin_descriptions = self.get_plugin_descriptions_client.get_result().descriptions
# collect all plugins loaded into param server
all_params = rosparam.list_params(self.namespace)
for pname in all_params:
# remove the plugin manager namespace
if self.namespace == '/':
pname_sub = pname
else:
pname_sub = pname[len(self.namespace):]
psplit = pname_sub.split('/')
# get plugin description from param server
if len(psplit) >= 2 and psplit[1] == 'type_class':
description = PluginDescription()
description.name.data = psplit[0]
description.type_class.data = rospy.get_param(pname)
if rospy.has_param(self.namespace+psplit[0]+'/type_class_package'):
description.type_class_package.data = rospy.get_param(self.namespace+psplit[0]+'/type_class_package')
if rospy.has_param(self.namespace+psplit[0]+'/base_class'):
description.base_class.data = rospy.get_param(self.namespace+psplit[0]+'/base_class')
if rospy.has_param(self.namespace+psplit[0]+'/base_class_package'):
description.base_class_package.data = rospy.get_param(self.namespace+psplit[0]+'/base_class_package')
self.plugin_descriptions.append(description)
# prepare combo box item texts
description = [[''] for i in range(self._NUM_DESC_ATTRIBUTES)]
for pd in self.plugin_descriptions:
for i in range(self._NUM_DESC_ATTRIBUTES):
description[i].append(self._get_data_from_description(pd, i))
# update combo boxes
for i in range(len(self.plugin_cb)):
description[i] = sorted(list(set(description[i])))
self.plugin_cb[i].addItems(description[i])
for cb in self.plugin_cb:
cb.blockSignals(False)
def publish_config(self):
""" Publish the rosparam server config through a topic """
self.published_config = True
pub = rospy.Publisher("safety/configuration", String, queue_size=2)
msg = String()
parameters_list = rosparam.list_params('/')
data = '\n'
for parameter in parameters_list:
data = data + 'Parameter: ' + parameter + '\n'
data = data + 'Value: ' + str(rospy.get_param(parameter)) + '\n\n'
msg.data = data
pub.publish(msg)
def refresh_loadable_ctrlers(self):
if self.cm_namespace_combo.count() == 0:
# no controller managers found so there are no loadable controllers
# remove old loadables
for old_loadable_text in self.loadable_params.keys():
self.remove_loadable_from_list(old_loadable_text)
return
ctrlman_ns = self.cm_namespace_combo.currentText()
if self.ctrlman_ns_cur != ctrlman_ns:
# new controller manager selected
# remove old loadables from list from last CM
for old_loadable_text in self.loadable_params.keys():
self.remove_loadable_from_list(old_loadable_text)
rospy.wait_for_service(
ctrlman_ns + '/controller_manager/list_controller_types', 0.2)
try:
resp = self.list_types[ctrlman_ns].call(
ListControllerTypesRequest())
except rospy.ServiceException as e:
# TODO: display warning somehow
return
ctrler_types = resp.types
loadable_params_cur = []
all_params = rosparam.list_params('/')
# for every parameter
for pname in all_params:
# remove the controller manager namespace
if ctrlman_ns == '/':
pname_sub = pname
else:
pname_sub = pname[len(ctrlman_ns):]
psplit = pname_sub.split('/')
if len(psplit) > 2 and psplit[2] == 'type':
loadable_type = rosparam.get_param(pname)
if loadable_type in ctrler_types:
load_text = pname[:-5] + ' - ' + loadable_type
loadable_params_cur.append(load_text)
if load_text not in self.loadable_params:
self.loadable_params[load_text] = psplit[1]
self.load_ctrl_combo.addItem(load_text)
# remove loadable parameters no longer in the parameter server
for load_text_old in self.loadable_params.keys():
if load_text_old not in loadable_params_cur:
self.remove_loadable_from_list(load_text_old)
def refresh_loadable_ctrlers(self):
if self.cm_namespace_combo.count() == 0:
# no controller managers found so there are no loadable controllers
# remove old loadables
for old_loadable_text in self.loadable_params.keys():
self.remove_loadable_from_list(old_loadable_text)
return
ctrlman_ns = self.cm_namespace_combo.currentText()
if self.ctrlman_ns_cur != ctrlman_ns:
# new controller manager selected
# remove old loadables from list from last CM
for old_loadable_text in self.loadable_params.keys():
self.remove_loadable_from_list(old_loadable_text)
rospy.wait_for_service(ctrlman_ns + '/controller_manager/list_controller_types', 0.2)
try:
resp = self.list_types[ctrlman_ns].call(ListControllerTypesRequest())
except rospy.ServiceException as e:
# TODO: display warning somehow
return
ctrler_types = resp.types
loadable_params_cur = []
all_params = rosparam.list_params('/')
# for every parameter
for pname in all_params:
# remove the controller manager namespace
if ctrlman_ns == '/':
pname_sub = pname
else:
pname_sub = pname[len(ctrlman_ns):]
psplit = pname_sub.split('/')
if len(psplit) > 2 and psplit[2] == 'type':
loadable_type = rosparam.get_param(pname)
if loadable_type in ctrler_types:
load_text = pname[:-5] + ' - ' + loadable_type
loadable_params_cur.append(load_text)
if load_text not in self.loadable_params:
self.loadable_params[load_text] = psplit[1]
self.load_ctrl_combo.addItem(load_text)
# remove loadable parameters no longer in the parameter server
for load_text_old in self.loadable_params.keys():
if load_text_old not in loadable_params_cur:
self.remove_loadable_from_list(load_text_old)
def get_robot_description_ns_list():
result = []
for param in rosparam.list_params('/'):
value = rosparam.get_param(param)
if not isinstance(value, str):
continue
if '<?xml' not in value:
continue
try:
assert etree.fromstring(value).xpath('/robot') != None
except:
traceback.print_exc()
continue
result.append(param)
return result
def __init__(self):
self.load_controller = rospy.ServiceProxy('pr2_controller_manager/load_controller',
LoadController)
self.unload_controller = rospy.ServiceProxy('pr2_controller_manager/unload_controller',
UnloadController)
self.switch_controller_srv = rospy.ServiceProxy('pr2_controller_manager/switch_controller',
SwitchController)
self.list_controllers_srv = rospy.ServiceProxy('pr2_controller_manager/list_controllers',
ListControllers)
self.load_controller.wait_for_service()
self.unload_controller.wait_for_service()
self.switch_controller_srv.wait_for_service()
self.list_controllers_srv.wait_for_service()
for arm in ['r', 'l']:
if LOADED_CTRLS_PARAMS[arm] not in rosparam.list_params(""):
possible_ctrls = []
for ctrl in LOADED_ARM_CONTROLLERS:
possible_ctrls.append(ctrl % arm)
rosparam.set_param_raw(LOADED_CTRLS_PARAMS[arm], possible_ctrls)
rospy.loginfo("[pr2_controller_switcher] ControllerSwitcher ready.")
def test_rosparam_list(self):
cmd = 'rosparam'
params = [
'/string',
'/int',
'/float',
'/g1/string',
'/g1/int',
'/g1/float',
'/g2/string',
'/g2/int',
'/g2/float',
]
l = rosparam.list_params('')
for t in params:
self.assert_(t in l)
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list'])
self._check(params, tolist(b))
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list', '/'])
self._check(params, tolist(b))
# test with namespace
g1p = [p for p in params if p.startswith('/g1/')]
not_g1p = [p for p in params if not p.startswith('/g1/')]
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list', '/g1'])
self._check(g1p, tolist(b))
self._notcheck(not_g1p, tolist(b))
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list', '/g1/'])
self._check(g1p, tolist(b))
self._notcheck(not_g1p, tolist(b))
# test with no match
with fakestdout() as b:
rosparam.yamlmain([cmd, 'list', '/not/a/namespace/'])
self.assertEquals([], tolist(b))
def get_rosparam_controller_names(namespace="/"):
"""
Get list of ROS parameter names that potentially represent a controller
configuration.
Example usage:
- Assume the following parameters exist in the ROS parameter:
server:
- C{/foo/type}
- C{/xxx/type/xxx}
- C{/ns/bar/type}
- C{/ns/yyy/type/yyy}
- The potential controllers found by this method are:
>>> names = get_rosparam_controller_names() # returns ['foo']
>>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar']
@param namespace: Namespace where to look for controllers.
@type namespace: str
@return: Sorted list of ROS parameter names.
@rtype: [str]
"""
import rosparam
list_out = []
all_params = rosparam.list_params(namespace)
for param in all_params:
# Remove namespace from parameter string
if not namespace or namespace[-1] != "/":
namespace += "/"
param_no_ns = param.split(namespace, 1)[1]
# Check if parameter corresponds to a controller configuration
param_split = param_no_ns.split("/")
if len(param_split) == 2 and param_split[1] == "type":
list_out.append(param_split[0]) # It does!
return sorted(list_out)
def main():
rospy.init_node("param_setting_gui")
print("parameters: ")
params_all = rosparam.list_params("/")
for p in params_all:
split_p = p.split("/")
pararent = "/".join(split_p[:-1])
if pararent == "":
pararent = "/"
if len(split_p):
if split_p[1] in ("camera", "move_group", "crane_x7",
"robot_description_planning",
"robot_description_semantic",
"robot_description_kinematics", "roslaunch"):
continue
param_dict[pararent].append(p)
print(" ", pararent, p)
# param name selection gui
frame = tk.Frame(root)
frame.pack()
tk.Label(frame, text="namespce:").pack(side="left")
text_ns.set(list(param_dict.keys())[0])
opt = tk.OptionMenu(frame, text_ns, *param_dict.keys())
opt.config(width=50)
opt.pack(side="left")
tk.Button(frame, text='refresh', command=button_refresh).pack(side="left")
frame_param.pack()
root.mainloop()
def get_rosparam_controller_names(namespace='/'):
"""
Get list of ROS parameter names that potentially represent a controller
configuration.
Example usage:
- Assume the following parameters exist in the ROS parameter:
server:
- C{/foo/type}
- C{/xxx/type/xxx}
- C{/ns/bar/type}
- C{/ns/yyy/type/yyy}
- The potential controllers found by this method are:
>>> names = get_rosparam_controller_names() # returns ['foo']
>>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar']
@param namespace: Namespace where to look for controllers.
@type namespace: str
@return: Sorted list of ROS parameter names.
@rtype: [str]
"""
import rosparam
list_out = []
all_params = rosparam.list_params(namespace)
for param in all_params:
# Remove namespace from parameter string
if not namespace or namespace[-1] != '/':
namespace += '/'
param_no_ns = param.split(namespace, 1)[1]
# Check if parameter corresponds to a controller configuration
param_split = param_no_ns.split('/')
if (len(param_split) == 2 and param_split[1] == 'type'):
list_out.append(param_split[0]) # It does!
return sorted(list_out)
def reset_params(self):
for p in rosparam.list_params(self.name):
rosparam.delete_param(p)
def load_params(self, namespace):
if not rosparam.list_params(namespace):
print "> No data to load"
else:
data = rosparam.get_param(namespace)
self.load_data(data)
def __clean_paramserver(self):
params = list_params("/")
for param in params:
delete_param(param, True)
def __init__(self, *args, **kwargs):
agent_id = 1
all_names = rospy.get_param_names()
if '/agent_id' in all_names:
rospy.logwarn('retrieve agent_id from param')
agent_id = rospy.get_param('/agent_id')
rospy.logwarn('agent : %s' % agent_id)
# initialize super class
GoalSubscriber.__init__(self)
RobotSubscriber.__init__(
self, '/nubot' + str(agent_id) + '/omnivision/OmniVisionInfo',
agent_id)
### create nav path subscriber
self.sub = {
'pos': TrajectorySubscriber('robosoccer_trajectory_pos'),
'vel': TrajectorySubscriber('robosoccer_trajectory_vel')
}
self.sub['pos'].register_callback(self.trajectory_callback)
# self.pub = rospy.Publisher('/nubot'+str(agent_id)+'/nubotcontrol/velcmd', nubotmsg.VelCmd, queue_size=3)
self.pub = rospy.Publisher('/nubot' + str(agent_id) +
'/nubotcontrol/velcmd',
nubotmsg.VelCmd,
queue_size=3)
self.info_pub = rospy.Publisher('/tracker_info',
trackmsg.ControlInfo,
queue_size=3)
self.error = {
'x': .0,
'y': .0,
'w': .0,
'sum': {
# sum for compute integral part of control
'x': .0,
'y': .0,
'w': .0,
}
}
self.control = {
'x': .0,
'y': .0,
'w': .0,
'com': np.array([0., 0., 0.])
}
self.pid = {
# initial value, will be overwritten
'p': np.diag([1., 1., 1.]),
'i': np.diag([.05, .05, .05])
}
# characteristic polynomial for computing gain
## default value
poly = {'p': [.5, .5, .5], 'i': [.05, .05, .05]}
## check if we have in rosparam
params = rosparam.list_params(rospy.get_name())
rospy.loginfo('parameter in %s : %s' % (rospy.get_name(), params))
if any('char_poly' in x for x in params):
rospy.logwarn('loading char poly from param')
poly = rosparam.get_param(rospy.get_name() + '/char_poly')
rospy.logwarn('poly : %s' % poly)
try:
self.enable_tracking = rosparam.get_param(rospy.get_name() +
'/enable')
rospy.logwarn('tracking enable : %s' % self.enable_tracking)
except:
rospy.logwarn('tracking not set, default to false')
self.enable_tracking = False
self.char_poly = {
# some tuning parameter
# 'p' : np.diag([0., 0., 0.]),
# 'i' : np.diag([0., 0., 0.])
'p': np.diag(poly['p']) * -1.,
'i': np.diag(poly['i']) * -1.,
# 'p' : np.diag([-.5, -.5, -.5]),
# 'i' : np.diag([-.05, -.05, -.05])
}
# command (or reference if you like)
self.command = {
# 'pos' : {'x' : .0, 'y' : .0, 'w' : .0,},
# 'vel' : {'x' : .0, 'y' : .0, 'w' : .0,}
'pos': np.array([0., 0., 0.]),
'vel': np.array([0., 0., 0.]),
}
self.last_update = None
# this enable is for waiting first goal to be published
self.enable = False
# saturation
self.saturate = {
'enabled': True,
'saturated': False,
'limit': {
'trans': 3.,
'rot': 1.5
}
}
self.ref_time = rospy.get_time()
self.ctrl_time = rospy.get_time()
rospy.logwarn('READY')
def refresh_plugin_descriptions(self):
# clear old status
self.plugin_descriptions = []
for cb in self.plugin_cb:
cb.blockSignals(True)
cb.clear()
self.plugin_manager_widget.addPluginPushButton.setEnabled(False)
# collect all plugin descriptions from manager
if self.get_plugin_descriptions_client.wait_for_server(
rospy.Duration(1.0)):
self.get_plugin_descriptions_client.send_goal(
GetPluginDescriptionsGoal())
if self.get_plugin_descriptions_client.wait_for_result(
rospy.Duration(5.0)):
self.plugin_descriptions = self.get_plugin_descriptions_client.get_result(
).descriptions
# collect all plugins loaded into param server
all_params = rosparam.list_params(self.namespace)
for pname in all_params:
# remove the plugin manager namespace
if self.namespace == '/':
pname_sub = pname
else:
pname_sub = pname[len(self.namespace):]
psplit = pname_sub.split('/')
# get plugin description from param server
if len(psplit) >= 2 and psplit[1] == 'type_class':
description = PluginDescription()
description.name.data = psplit[0]
description.type_class.data = rospy.get_param(pname)
if rospy.has_param(self.namespace + psplit[0] +
'/type_class_package'):
description.type_class_package.data = rospy.get_param(
self.namespace + psplit[0] + '/type_class_package')
if rospy.has_param(self.namespace + psplit[0] + '/base_class'):
description.base_class.data = rospy.get_param(
self.namespace + psplit[0] + '/base_class')
if rospy.has_param(self.namespace + psplit[0] +
'/base_class_package'):
description.base_class_package.data = rospy.get_param(
self.namespace + psplit[0] + '/base_class_package')
self.plugin_descriptions.append(description)
# prepare combo box item texts
description = [[''] for i in range(self._NUM_DESC_ATTRIBUTES)]
for pd in self.plugin_descriptions:
for i in range(self._NUM_DESC_ATTRIBUTES):
description[i].append(self._get_data_from_description(pd, i))
# update combo boxes
for i in range(len(self.plugin_cb)):
description[i] = sorted(list(set(description[i])))
self.plugin_cb[i].addItems(description[i])
for cb in self.plugin_cb:
cb.blockSignals(False)
def _list(ns=''):
ns = '/' + ns
try:
return response_ok({'parameters': sorted(rp.list_params(ns))})
except Exception as e:
return response_error(str(e))
