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 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'