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()
# 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()
