def testmultibytestring(self):
client = dynamic_reconfigure.client.Client("ref_server", timeout=5)
config = client.get_configuration(timeout=5)
self.assertEqual('bar', config['mstr_'])
# Kanji for konnichi wa (hello)
str_ = u"今日は"
client.update_configuration({"mstr_": str_})
rospy.sleep(1.0)
config = client.get_configuration(timeout=5)
self.assertEqual(u"今日は", config['mstr_'])
self.assertEqual(type(u"今日は"), type(config['mstr_']))
self.assertEqual(u"今日は", rospy.get_param('/ref_server/mstr_'))
# Hiragana for konnichi wa (hello)
str_ = u"こんにちは"
client.update_configuration({"mstr_": str_})
rospy.sleep(1.0)
config = client.get_configuration(timeout=5)
self.assertEqual(u"こんにちは", config['mstr_'])
self.assertEqual(type(u"こんにちは"), type(config['mstr_']))
self.assertEqual(u"こんにちは", rospy.get_param('/ref_server/mstr_'))
def testmultibytestring(self):
client = dynamic_reconfigure.client.Client("ref_server", timeout=5)
config = client.get_configuration(timeout=5)
self.assertEqual('bar', config['mstr_'])
# Kanji for konnichi wa (hello)
str_ = u"今日は"
client.update_configuration(
{"mstr_": str_}
)
rospy.sleep(1.0)
config = client.get_configuration(timeout=5)
self.assertEqual(u"今日は", config['mstr_'])
self.assertEqual(type(u"今日は"), type(config['mstr_']))
self.assertEqual(u"今日は", rospy.get_param('/ref_server/mstr_'))
# Hiragana for konnichi wa (hello)
str_ = u"こんにちは"
client.update_configuration(
{"mstr_": str_}
)
rospy.sleep(1.0)
config = client.get_configuration(timeout=5)
self.assertEqual(u"こんにちは", config['mstr_'])
self.assertEqual(type(u"こんにちは"), type(config['mstr_']))
self.assertEqual(u"こんにちは", rospy.get_param('/ref_server/mstr_'))
def testsimple(self):
client = dynamic_reconfigure.client.Client("ref_server", timeout=5)
config = client.get_configuration(timeout=5)
self.assertEqual(0, config['int_'])
self.assertEqual(0.0, config['double_'])
self.assertEqual('foo', config['str_'])
self.assertEqual(False, config['bool_'])
int_ = 7
double_ = 0.75
str_ = 'bar'
bool_ = True
client.update_configuration(
{"int_": int_, "double_": double_, "str_": str_,
"bool_": bool_}
)
rospy.sleep(1.0)
config = client.get_configuration(timeout=5)
self.assertEqual(int_, config['int_'])
self.assertEqual(double_, config['double_'])
self.assertEqual(str_, config['str_'])
self.assertEqual(bool_, config['bool_'])
def testsimple(self):
client = dynamic_reconfigure.client.Client("ref_server", timeout=5)
config = client.get_configuration(timeout=5)
self.assertEqual(0, config['int_'])
self.assertEqual(0.0, config['double_'])
self.assertEqual('foo', config['str_'])
self.assertEqual(False, config['bool_'])
self.assertEqual(1.0, config['double_no_minmax'])
self.assertEqual(2.0, config['double_no_max'])
self.assertEqual(-1.0, config['deep_var_double'])
int_ = 7
double_ = 0.75
str_ = 'bar'
bool_ = True
double_no_max_ = 1e+300
double_no_minmax_ = -1e+300
client.update_configuration(
{"int_": int_, "double_": double_, "str_": str_,
"bool_": bool_,
"double_no_max": double_no_max_, "double_no_minmax": double_no_minmax_}
)
rospy.sleep(1.0)
config = client.get_configuration(timeout=5)
self.assertEqual(int_, config['int_'])
self.assertEqual(double_, config['double_'])
self.assertEqual(str_, config['str_'])
self.assertEqual(type(str_), type(config['str_']))
self.assertEqual(bool_, config['bool_'])
self.assertEqual(double_no_max_, config['double_no_max'])
self.assertEqual(double_no_minmax_, config['double_no_minmax'])
def cb(msg):
global expected_n_cluster
global reconfig_n_limit, reconfig_n_times
global default_tolerance
global sub_kcluster, sub_ncluster
with lock:
if reconfig_n_times > reconfig_n_limit:
sub_kcluster.unregister()
sub_ncluster.unregister()
return
if expected_n_cluster is None:
return
cfg = client.get_configuration(timeout=None)
tol_orig = cfg['tolerance']
delta = tol_orig * reconfig_eps
if msg.data == expected_n_cluster:
print('Expected/Actual n_cluster: {0}, Tolerance: {1}'
.format(msg.data, tol_orig))
reconfig_n_times = reconfig_n_limit + 1
return
elif msg.data > expected_n_cluster:
cfg['tolerance'] += delta
else:
cfg['tolerance'] -= delta
if cfg['tolerance'] < 0.001:
print('Invalid tolerance, resetting')
cfg['tolerance'] = default_tolerance
print('''\
Expected n_cluster: {0}
Actual n_cluster: {1}
tolerance: {2} -> {3}
'''.format(expected_n_cluster, msg.data, tol_orig, cfg['tolerance']))
client.update_configuration(cfg)
reconfig_n_times += 1
def get_joint_state(client):
config = client.get_configuration()
q1 = config['joint1']
q2 = config['joint2']
q3 = config['joint3']
q4 = config['joint4']
return [q1, q2, q3, q4]
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 cb(msg):
global expected_n_cluster, reconfig_n_limit, reconfig_n_times
global sub_kcluster, sub_ncluster
with lock:
if reconfig_n_times > reconfig_n_limit:
sub_kcluster.unregister()
sub_ncluster.unregister()
return
if expected_n_cluster is None:
return
cfg = client.get_configuration(timeout=None)
tol_orig = cfg['tolerance']
delta = tol_orig * reconfig_eps
if msg.data == expected_n_cluster:
print('Expected/Actual n_cluster: {0}, Tolerance: {1}'
.format(msg.data, tol_orig))
reconfig_n_times = reconfig_n_limit + 1
return
elif msg.data > expected_n_cluster:
cfg['tolerance'] += delta
else:
cfg['tolerance'] -= delta
if cfg['tolerance'] < 0.001:
print('Invalid tolerance, resetting')
cfg['tolerance'] = 0.02
print('''\
Expected n_cluster: {0}
Actual n_cluster: {1}
tolerance: {2} -> {3}
'''.format(expected_n_cluster, msg.data, tol_orig, cfg['tolerance']))
client.update_configuration(cfg)
reconfig_n_times += 1
def initialise(self):
"""
Get various dyn reconf configurations and cache/set the new variables.
"""
self.logger.debug("%s.initialise()" % self.__class__.__name__)
for name, client in self._dynamic_reconfigure_clients.iteritems():
self._dynamic_reconfigure_configurations[
name] = client.get_configuration()
try:
self.safety_sensors_enable = self._dynamic_reconfigure_configurations[
"safety_sensors"]["enable"]
self._dynamic_reconfigure_clients[
"safety_sensors"].update_configuration({"enable": True})
except dynamic_reconfigure.DynamicReconfigureParameterException:
self.feedback_message = "failed to configure the 'enable' parameter [safety_sensors]"
self.initialised = False
try:
self.rotate_duration = self._dynamic_reconfigure_configurations[
"rotate"]["duration"]
self._dynamic_reconfigure_clients["rotate"].update_configuration(
{"duration": 8.0})
except dynamic_reconfigure.DynamicReconfigureParameterException:
self.feedback_message = "failed to configure the 'duration' parameter [rotate]"
self.initialised = False
self.initialised = True
self.feedback_message = "reconfigured the context for scanning"
def initialise(self):
self.initial_config = {}
self.edge_config = {}
for namespace in self.namespaces:
rospy.loginfo("Edge Reconfigure Manager: Getting initial configuration for {}".format(namespace))
client = dynamic_reconfigure.client.Client(namespace, timeout=2.0)
try:
config = client.get_configuration()
except rospy.ServiceException as e:
rospy.logwarn("Edge Reconfigure Manager: Caught service exception: {}".format(e))
continue
self.initial_config[namespace] = {}
self.edge_config[namespace] = {}
for param in self.edge["config"]:
if param["namespace"] == namespace:
self.edge_config[namespace][param["name"]] = param["value"]
reset = True if "reset" not in param else param["reset"]
if reset:
self.initial_config[namespace][param["name"]] = config[param["name"]]
def getDrcConfig(dump_config=False):
""" Fetch configuration. """
global client
params = client.get_configuration()
if dump_config is True:
# dump to file
dumpConfig(params)
return params
def store_initial_parameters(self):
for mb_action, client in self.rcnfclient.iteritems():
try:
self.init_dynparams[mb_action] = client.get_configuration()
except Exception as e:
rospy.logwarn(
"I couldn't store initial move_base parameters. Caught exception: %s. will continue with previous params",
exc)
def tolerance_up(userdata):
node_to_reconfigure = "/move_base/PalLocalPlanner"
client = dynamic_reconfigure.client.Client(node_to_reconfigure)
original_config = client.get_configuration()
userdata.original_config = original_config.yaw_goal_tolerance
new_params = {'yaw_goal_tolerance': 3.1416}
client.update_configuration(new_params)
return succeeded
def change_projector_mode(self, on):
client = dynamic_reconfigure.client.Client("camera_synchronizer_node")
node_config = client.get_configuration()
node_config["projector_mode"] = 2
if on:
node_config["narrow_stereo_trig_mode"] = 3
else:
node_config["narrow_stereo_trig_mode"] = 4
client.update_configuration(node_config)
def change_projector_mode(self, on):
client = dynamic_reconfigure.client.Client("camera_synchronizer_node")
node_config = client.get_configuration()
node_config["projector_mode"] = 2
if on:
node_config["narrow_stereo_trig_mode"] = 3
else:
node_config["narrow_stereo_trig_mode"] = 4
client.update_configuration(node_config)
def testsimple(self):
client = dynamic_reconfigure.client.Client("ref_server", timeout=5)
config = client.get_configuration(timeout=5)
self.assertEqual(0, config['int_'])
self.assertEqual(0.0, config['double_'])
self.assertEqual('foo', config['str_'])
self.assertEqual(False, config['bool_'])
self.assertEqual(1.0, config['double_no_minmax'])
self.assertEqual(2.0, config['double_no_max'])
self.assertEqual(-1.0, config['deep_var_double'])
int_ = 7
double_ = 0.75
str_ = 'bar'
bool_ = True
double_no_max_ = 1e+300
double_no_minmax_ = -1e+300
client.update_configuration({
"int_": int_,
"double_": double_,
"str_": str_,
"bool_": bool_,
"double_no_max": double_no_max_,
"double_no_minmax": double_no_minmax_
})
rospy.sleep(1.0)
config = client.get_configuration(timeout=5)
self.assertEqual(int_, config['int_'])
self.assertEqual(double_, config['double_'])
self.assertEqual(str_, config['str_'])
self.assertEqual(type(str_), type(config['str_']))
self.assertEqual(bool_, config['bool_'])
self.assertEqual(double_no_max_, config['double_no_max'])
self.assertEqual(double_no_minmax_, config['double_no_minmax'])
def update_reached_precision(self, goal_with_prirority):
return
tolerance_param = 'xy_goal_tolerance'
priority_idx = int(goal_with_prirority.priority / 100)
precision = self.priority_to_precision[priority_idx]
# http://wiki.ros.org/base_local_planner#Goal_Tolerance_Parameters
client = dynamic_reconfigure.client.Client("/move_base/TrajectoryPlannerROS")
old_config = client.get_configuration(0.05)
if old_config is None or old_config[tolerance_param] != precision:
params = { tolerance_param : precision }
new_config = client.update_configuration(params)
rospy.loginfo("Goal xy tolerance set to %s", str(precision))
else:
rospy.loginfo("Goal tolerance not updated")
def update_reached_precision(self, goal_with_prirority):
return
tolerance_param = 'xy_goal_tolerance'
priority_idx = int(goal_with_prirority.priority / 100)
precision = self.priority_to_precision[priority_idx]
# http://wiki.ros.org/base_local_planner#Goal_Tolerance_Parameters
client = dynamic_reconfigure.client.Client(
"/move_base/TrajectoryPlannerROS")
old_config = client.get_configuration(0.05)
if old_config is None or old_config[tolerance_param] != precision:
params = {tolerance_param: precision}
new_config = client.update_configuration(params)
rospy.loginfo("Goal xy tolerance set to %s", str(precision))
else:
rospy.loginfo("Goal tolerance not updated")
def get_configuration(self, request):
"""
Returns node configuration with values in json
:param request:
:return:
"""
node = request.node
if node in self.configurable_nodes:
logger.info('get_node_configuration service call received for node: ' + node)
client = dynamic_reconfigure.client.Client(node)
configuration = client.get_configuration()
del configuration['groups']
return srv.NodeConfigurationResponse(json.dumps(configuration))
else:
logger.info('get_node_configuration service: Invalid node name')
return srv.NodeConfigurationResponse(json.dumps({}))
def handle_get_camera_param(self,req):
"""
Handles requests to get the parameters for a given camera.
"""
response = GetCameraParamResponse()
response.flag = True
response.message = ''
node = get_node_from_camera_name(req.camera_name)
if node is not None:
client = dynamic_reconfigure.client.Client(node)
config = client.get_configuration()
response.brightness = config['brightness']
response.shutter = config['shutter']
response.gain = config['gain']
else:
response.flag = False
response.message = 'camera, {0}, not found'.format(req.camera_name)
return response
def talker():
global cur_rate
pub = rospy.Publisher('chatter', env, queue_size=10)
rospy.init_node('talker', anonymous=True)
server_ip = "192.168.1.100"
rospy.set_param("img_pub_rate", 10)
client = dynamic_reconfigure.client.Client("/camera/left/image/compressed")
info = client.get_configuration()
params = {"jpeg_quality": 10}
client.update_configuration(params)
print info
while not rospy.is_shutdown():
img_pub_rate = rospy.get_param("img_pub_rate")
rate = rospy.Rate(img_pub_rate)
value = env()
value.atmo = 1
value.temp = 2
value.hum = 3
pub.publish(value)
# rate.sleep()
time.sleep(2)
def get_configuration(self, request):
"""
Returns node configuration with values in json
:param request:
:return:
"""
node = request.node
if node in self.configurable_nodes:
logger.info(
'get_node_configuration service call received for node: ' +
node)
client = dynamic_reconfigure.client.Client(node)
configuration = client.get_configuration()
del configuration['groups']
return srv.NodeConfigurationResponse(json.dumps(configuration))
else:
logger.info('get_node_configuration service: Invalid node name')
return srv.NodeConfigurationResponse(json.dumps({}))
#!/usr/bin/env python
import rospy
import tf
from PyKDL import Rotation
import dynamic_reconfigure.client
# find the rotation between the glass frame and the face detection frame, and
# rotate the face detection frame by its inverse
if __name__ == '__main__':
rospy.init_node('frame_adjust')
client = dynamic_reconfigure.client.Client(rospy.myargv()[1])
listener = tf.TransformListener()
listener.waitForTransform('face_detection', 'glass', rospy.Time(), rospy.Duration(10))
trans, rot = listener.lookupTransform('glass', 'face_detection', rospy.Time(0))
rot = Rotation.Quaternion(*rot)
r, p, y = rot.Inverse().GetRPY()
config = client.get_configuration()
config.yaw -= y
# config.pitch -= p
# config.roll -= r
client.update_configuration(config)
def get_params(node, timeout=None):
client = dynamic_reconfigure.client.Client(node, timeout=timeout)
return client.get_configuration(timeout=timeout)
rospy.logerr("Couldn't set the profile on the laser. Exception %s" % e)
return False
return True
def configure_laser():
#TODO: remove hack to get things working in gazebo
try:
rospy.wait_for_service('tilt_hokuyo_node/set_parameters', 10.0)
except rospy.exceptions.ROSException, e:
rospy.logerr("Couldn't set parameters %s" % e)
return
#end TODO
client = dynamic_reconfigure.client.Client('tilt_hokuyo_node')
global old_config
old_config = client.get_configuration(2.0)
new_config = {'skip': 0, 'intensity': 0, 'min_ang': -1.57, 'max_ang': 1.57, 'calibrate_time': 1, 'cluster': 1, 'time_offset': 0.0}
rospy.loginfo('Setting laser to the navigation configuration: %s' % new_config)
client.update_configuration(new_config)
if __name__ == '__main__':
name = 'setting_laser'
rospy.init_node(name)
#create a client to the tilt laser
rospy.wait_for_service('laser_tilt_controller/set_traj_cmd')
tilt_profile_client = rospy.ServiceProxy('laser_tilt_controller/set_traj_cmd', SetLaserTrajCmd)
## set_tilt_profile([1.05, -.7, 1.05], [0.0, 1.8, 2.0125 + .3]) ## original
set_tilt_profile([1.05, -.7, 1.05], [0.0, 2.4, 2.4 + .2125 + .3])
configure_laser()
return False
return True
def configure_laser():
#TODO: remove hack to get things working in gazebo
try:
rospy.wait_for_service('tilt_hokuyo_node/set_parameters', 10.0)
except rospy.exceptions.ROSException, e:
rospy.logerr("Couldn't set parameters %s" % e)
return
#end TODO
client = dynamic_reconfigure.client.Client('tilt_hokuyo_node')
global old_config
old_config = client.get_configuration(2.0)
new_config = {
'skip': 0,
'intensity': 0,
'min_ang': -1.57,
'max_ang': 1.57,
'calibrate_time': 1,
'cluster': 1,
'time_offset': 0.0
}
rospy.loginfo('Setting laser to the navigation configuration: %s' %
new_config)
client.update_configuration(new_config)
if __name__ == '__main__':
# rpy_from_imu_to_global = ac.get_rpy_of_imu_in_global()
# target_depth = ac.current_depth
# target_angles = [roll_in_deg, pitch_in_deg, rpy_from_imu_to_global[2]*180/pi]
# vx = 0
# vz = 0.9
#
# ac.do_lateral_motion(dt_in_sec, target_angles, target_depth, vx, vz)
#
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
# print e
client = dynamic_reconfigure.client.Client("/localAP",
timeout=5,
config_callback=callback)
original_params = client.get_configuration(timeout=5)
print 'Previous value of ' + search_parameter + ' was: ' + str(
original_params[search_parameter])
for val in val_options:
rospy.loginfo('Setting ' + search_parameter + ' to ' + str(val))
client.update_configuration({search_parameter: float(val)})
rospy.loginfo("Returned from update_configuration")
ui_pub.publish(search_parameter + '\n' + str(val))
try:
print 'Going laterally'
