def _update_move_base_params(self, config): """ If parameter updates have not been called in the last 5 seconds allow the subscriber callback to set them """ if ((rospy.Time.now().to_sec() - self.last_move_base_update) > 5.0): self.update_base_local_planner = True if self.update_base_local_planner: self.update_base_local_planner = False self.last_move_base_update = rospy.Time.now().to_sec() try: dyn_reconfigure_client = Client("/move_base/DWAPlannerROS", timeout=1.0) changes = dict() changes['acc_lim_x'] = maximum_f( self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_y'] = maximum_f( self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2 changes['max_vel_x'] = self.valid_config.x_vel_limit_mps changes['max_vel_y'] = self.valid_config.y_vel_limit_mps changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps dyn_reconfigure_client.update_configuration(changes) dyn_reconfigure_client.close() except: pass rospy.loginfo( "Movo Driver updated move_base parameters to match machine parameters" )
def _update_move_base_params(self): """ If parameter updates have not been called in the last 5 seconds allow the subscriber callback to set them """ if ((rospy.Time.now().to_sec()-self.last_move_base_update) > 5.0): self.update_base_local_planner = True if self.update_base_local_planner: self.update_base_local_planner = False self.last_move_base_update = rospy.Time.now().to_sec() try: dyn_reconfigure_client= Client("/move_base/DWAPlannerROS",timeout=1.0) changes = dict() changes['acc_lim_x'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_y'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2 changes['max_vel_x'] = self.valid_config.x_vel_limit_mps changes['max_vel_y'] = self.valid_config.y_vel_limit_mps changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps dyn_reconfigure_client.update_configuration(changes) dyn_reconfigure_client.close() except: pass rospy.loginfo("Vector Driver updated move_base parameters to match machine parameters")
def init_dyn_recfg(): cli = tcc.TwistControllerReconfigureClient() cli.init() cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE) cli.set_config_param(tcc.NUM_FILT, False) cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY) cli.set_config_param(tcc.LAMBDA_MAX, 0.1) cli.set_config_param(tcc.W_THRESH, 0.05) # cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_LEAST_SINGULAR_VALUE) # cli.set_config_param(tcc.LAMBDA_MAX, 0.2) # cli.set_config_param(tcc.EPS_DAMP, 0.2) cli.set_config_param(tcc.EPS_TRUNC, 0.001) cli.set_config_param(tcc.PRIO_CA, 100) cli.set_config_param(tcc.PRIO_JLA, 50) cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS) cli.set_config_param(tcc.K_H, 1.0) cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA) cli.set_config_param(tcc.K_H_CA, -2.0) cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1) cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0) cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025) cli.set_config_param(tcc.DAMP_CA, 0.000001) cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA) cli.set_config_param(tcc.K_H_JLA, -1.0) cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0) cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0) cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0) cli.set_config_param(tcc.DAMP_JLA, 0.00001) cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION) cli.set_config_param(tcc.KEEP_DIR, True) cli.set_config_param(tcc.ENF_VEL_LIM, True) cli.set_config_param(tcc.ENF_POS_LIM, True) cli.update() cli.close() cli = Client('frame_tracker') ft_param = { 'cart_min_dist_threshold_lin': 0.2, 'cart_min_dist_threshold_rot': 0.2 } cli.update_configuration(ft_param) cli.close()
def init_dyn_recfg(): cli = tcc.TwistControllerReconfigureClient() cli.init() cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE) cli.set_config_param(tcc.NUM_FILT, False) cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_CONSTANT) cli.set_config_param(tcc.DAMP_FACT, 0.2) cli.set_config_param(tcc.EPS_TRUNC, 0.001) cli.set_config_param(tcc.PRIO_CA, 100) cli.set_config_param(tcc.PRIO_JLA, 50) cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS) cli.set_config_param(tcc.K_H, 1.0) cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA) cli.set_config_param(tcc.K_H_CA, -1.0) cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1) cli.set_config_param(tcc.ACTIV_BUF_CA, 25.0) cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025) cli.set_config_param(tcc.DAMP_CA, 0.000001) cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA) cli.set_config_param(tcc.K_H_JLA, -1.0) cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0) cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0) cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0) cli.set_config_param(tcc.DAMP_JLA, 0.00001) cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_BASE_ACTIVE) cli.set_config_param(tcc.EXT_RATIO, 0.05) cli.set_config_param(tcc.KEEP_DIR, True) cli.set_config_param(tcc.ENF_VEL_LIM, True) cli.set_config_param( tcc.ENF_POS_LIM, False) # To show that joint pos limits are violated if possible. cli.update() cli.close() cli = Client('frame_tracker') ft_param = { 'cart_min_dist_threshold_lin': 5.0, 'cart_min_dist_threshold_rot': 5.0 } cli.update_configuration(ft_param) cli.close()
def init_dyn_recfg(): cli = tcc.TwistControllerReconfigureClient() cli.init() cli.set_config_param(tcc.NUM_FILT, False) cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY) cli.set_config_param(tcc.LAMBDA_MAX, 0.1) cli.set_config_param(tcc.W_THRESH, 0.05) cli.set_config_param(tcc.EPS_TRUNC, 0.001) cli.set_config_param(tcc.PRIO_CA, 50) cli.set_config_param( tcc.PRIO_JLA, 100) # change priorities now CA constraint has highest prio cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS) cli.set_config_param(tcc.K_H, 1.0) cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA) cli.set_config_param(tcc.K_H_CA, -1.0) cli.set_config_param(tcc.DAMP_CA, 0.000001) cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1) cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0) cli.set_config_param(tcc.CRIT_THRESH_CA, 0.03) cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA) cli.set_config_param(tcc.K_H_JLA, -1.0) cli.set_config_param(tcc.DAMP_JLA, 0.000001) cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0) cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0) cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0) cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION) cli.set_config_param(tcc.KEEP_DIR, True) cli.set_config_param(tcc.ENF_VEL_LIM, True) cli.set_config_param(tcc.ENF_POS_LIM, True) cli.update() cli.close() cli = Client('frame_tracker') ft_param = { 'cart_min_dist_threshold_lin': 0.2, 'cart_min_dist_threshold_rot': 0.2 } cli.update_configuration(ft_param) cli.close()
def init_dyn_recfg(): cli = tcc.TwistControllerReconfigureClient() cli.init() cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE) cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY) cli.set_config_param(tcc.LAMBDA_MAX, 0.1) cli.set_config_param(tcc.W_THRESH, 0.05) cli.set_config_param(tcc.PRIO_CA, 100) cli.set_config_param(tcc.PRIO_JLA, 50) cli.set_config_param(tcc.SOLVER, tcc.TwistController_GPM) cli.set_config_param(tcc.K_H, 1.0) cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA) cli.set_config_param(tcc.K_H_CA, -2.0) cli.set_config_param(tcc.DAMP_CA, 0.000001) cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1) cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA) cli.set_config_param(tcc.K_H_JLA, -1.0) cli.set_config_param(tcc.DAMP_JLA, 0.00001) cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0) cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION) cli.set_config_param(tcc.KEEP_DIR, True) cli.set_config_param(tcc.ENF_VEL_LIM, True) cli.set_config_param( tcc.ENF_POS_LIM, False) # To show that joint pos limits are violated if possible. cli.update() cli.close() cli = Client('frame_tracker') ft_param = { 'cart_min_dist_threshold_lin': 0.2, 'cart_min_dist_threshold_rot': 0.2 } cli.update_configuration(ft_param) cli.close()
# from what you requested if you asked for something illegal). print("Configuration after setting int_ to 4:") print_config(client.update_configuration({'int_': 4})) time.sleep(1) print("Configuration after setting int_ to 0 and bool_ to True:") print_config(client.update_configuration({'int_': 0, 'bool_': True})) time.sleep(1) # You can access constants defined in Test.cfg file in the following way: import dynamic_reconfigure.cfg.TestConfig as Config print("Medium is a constant that is set to 1:", Config.Test_Medium) # This is useful for setting enums: print("Configuration after setting int_enum_ to Medium:") print_config(client.update_configuration({'int_enum_': Config.Test_Medium})) time.sleep(1) # You can use the get_config_callback and set_config_callback methods to # get the current callback or change the callback. It is sometimes useful # not to set the callback in the constructor, but to wait until more # initialization has been done first. Again, the callback will be called # immediately if a configuration is available. old_callback = client.get_config_callback() client.set_config_callback(new_config_callback) time.sleep(1) # When you are done, you can close the client. client.close()
class DynamicConfigurationParameters: def __init__(self): self.defaults = {} for param_name in rospy.get_param_names(): if not param_name.startswith(rospy.get_name()): continue value = str(rospy.get_param(param_name)) index = value.find(';') name = value[:index] path = value[index+1:] param_name = param_name.split('/')[2] self.defaults[param_name] = { 'name': name, 'path': path, } rospy.Service('/mercury/dynparam/defaults', Default, self.default_handler) rospy.Service('/mercury/dynparam/load', Load, self.load_handler) def load(self, name, path): if not os.path.exists(path): return False if not name.startswith('/'): name = '/' + name f = file(path, 'r') try: params = {} for doc in yaml.load_all(f.read(), Loader=yaml.FullLoader): params.update(doc) finally: f.close() try: self.client = DynamicReconfigureClient(name) try: self.client.update_configuration(params) except DynamicReconfigureCallbackException as err: logger.log_error(str(err)) return False except DynamicReconfigureParameterException as err: logger.log_error(str(err)) return False return True finally: self.client.close() def default_handler(self, request): key = request.name if key not in self.defaults: return False logger.log_warn("loading default parameters for %s ...", key) return self.load(self.defaults[key]['name'], self.defaults[key]['path']) def load_handler(self, request): if request.name == "": return False return self.load(request.name, request.path) def spin(self): rospy.spin()