def parameter_callback(self, params):
for p in params:
if p.name == 'bar' and p.type_ == Parameter.Type.STRING:
self.get_logger().info('Parameter %s:%s:%s' %
(self.get_name(), p.name, p.value))
if p.name == 'foo' and p.type_ == Parameter.Type.DOUBLE:
self.get_logger().info('Parameter %s:%s:%s' %
(self.get_name(), p.name, p.value))
return SetParametersResult(successful=True)
def _set_parameters_callback(self, request, response):
for p in request.parameters:
param = Parameter.from_parameter_msg(p)
try:
result = self._node.set_parameters_atomically([param])
except ParameterNotDeclaredException as e:
result = SetParametersResult(successful=False, reason=str(e))
response.results.append(result)
return response
def parameterCallback(self, params):
for param in params:
if param.name == "gravity":
self.measurementHandler.setGravity(param.value)
elif param.name == "magneticIntensity":
self.measurementHandler.setMagneticIntensity(param.value)
elif param.name == "inclination":
self.measurementHandler.setInclination(param.value)
return SetParametersResult(successful=True)
def reconfigure_pid_parameters(self, params): # pylint: disable=function-redefined
"""Check and update the node's parameters."""
param_values = {p.name: p.value for p in params}
pid_param_names = {
"speed_Kp",
"speed_Ki",
"speed_Kd",
"accel_Kp",
"accel_Ki",
"accel_Kd",
}
common_names = pid_param_names.intersection(param_values.keys())
if not common_names:
# No work do to
return SetParametersResult(successful=True)
if any(p.value is None for p in params):
return SetParametersResult(
successful=False,
reason="Parameter must have a value assigned")
self.speed_controller.tunings = (
param_values.get("speed_Kp", self.speed_controller.Kp),
param_values.get("speed_Ki", self.speed_controller.Ki),
param_values.get("speed_Kd", self.speed_controller.Kd),
)
self.accel_controller.tunings = (
param_values.get("accel_Kp", self.accel_controller.Kp),
param_values.get("accel_Ki", self.accel_controller.Ki),
param_values.get("accel_Kd", self.accel_controller.Kd),
)
self.loginfo(
"Reconfigure Request: speed ({}, {}, {}), accel ({}, {}, {})".
format(self.speed_controller.tunings[0],
self.speed_controller.tunings[1],
self.speed_controller.tunings[2],
self.accel_controller.tunings[0],
self.accel_controller.tunings[1],
self.accel_controller.tunings[2]))
return SetParametersResult(successful=True)
def set_parameters_atomically(self, parameter_list):
result = None
if self._parameters_callback:
result = self._parameters_callback(parameter_list)
else:
result = SetParametersResult(successful=True)
if result.successful:
for param in parameter_list:
self._parameters[param.name] = param
return result
def _set_parameters_atomically_callback(self, request, response):
node = self._get_node()
try:
response.result = node.set_parameters_atomically([
Parameter.from_parameter_msg(p) for p in request.parameters])
except ParameterNotDeclaredException as e:
response.result = SetParametersResult(
successful=False,
reason=str(e)
)
return response
def set_parameters_validate_callback(
self, parameters: List[Parameter]) -> SetParametersResult:
"""Called when there is a request to set one or multiple parameters
Called even for the initial parameter declaration (if registered before the declaration).
Registered using self.add_on_set_parameters_callback(self.reconfigure_callback).
Called for each parameter separately (i.e. len(parameters) == 1),
unless multiple parameters are set using set_parameters_atomically (then len(parameters) >= 1).
Before this callback is called, parameters' values are validated against their specified constraints (if any).
If type or constraints validation fails, this callback will not be called at all.
If this callback returns SetParametersResult(successful=False), the values will not be set.
"""
# self.get_logger().info('set_parameters_validate_callback')
# validate parameters
for param in parameters:
# print(f' param={param.name} value={param.value}')
if param.value is None:
return SetParametersResult(
successful=False, reason=f'missing value for {param.name}')
# validate run_mode
if param.name == 'run_mode':
if not RunMode.has_name(param.value.upper()):
return SetParametersResult(
successful=False,
reason=f'invalid run_mode=\'{param.value}\'')
pass
# if we pass successful=False, parameter value will not be set
# if parameter set was attempted using self.set_parameter*, node will exit with an error
# if parameter set was attempted remotely, the remote caller is just passed the result
# (failure and the optional configurable reason='why it was unsuccessful')
return SetParametersResult(successful=True)
pass
def _dynamic_reconfigure_callback(self, params):
"""
Callback for the dynamic reconfigure configuration.
:param config: New _config
:param level: The level is a definable int in the Vision.cfg file. All changed params are or ed together by dynamic reconfigure.
"""
config = deepcopy(self._config)
for param in params:
config[param.name] = param.value
self._configure_vision(config)
return SetParametersResult(successful=True)
def validate_parameters(self, params) -> SetParametersResult:
for param in params:
if param.name == CMD and param.type_ == Parameter.Type.STRING:
if param.value == CMD_START:
self._target_pwm = self._start_pwm
self.get_logger().info('start, ramp to {}'.format(self._target_pwm))
return SetParametersResult(successful=True)
elif param.value == CMD_INC:
self._target_pwm = self._target_pwm + self._inc_pwm
self.get_logger().info('increment, ramp to {}'.format(self._target_pwm))
return SetParametersResult(successful=True)
elif param.value == CMD_DEC:
self._target_pwm = self._target_pwm - self._inc_pwm
self.get_logger().info('decrement, ramp to {}'.format(self._target_pwm))
return SetParametersResult(successful=True)
elif param.value == CMD_STOP:
self._target_pwm = 1500
self.get_logger().info('stop, ramp to {}'.format(self._target_pwm))
return SetParametersResult(successful=True)
return SetParametersResult(successful=False)
def _on_set_parameters(self, params):
"""Handle Parameter events especially for side"""
result = SetParametersResult()
try:
for param in params:
if param.name == "side":
self.get_logger().warn(f"Side changed {param.value}")
self.side = param
else:
setattr(self, param.name, param)
(
self.robot_pose.pose.position.x,
self.robot_pose.pose.position.y,
theta,
) = self.fetchStartPosition()
self.robot_pose.pose.orientation = euler_to_quaternion(theta)
self.update_transform()
result.successful = True
except BaseException as e:
result.reason = e
return result
def _on_parameter_event(self, parameter_list):
for parameter in parameter_list:
if parameter.name == USE_SIM_TIME_NAME:
if parameter.type_ == Parameter.Type.BOOL:
self.ros_time_is_active = parameter.value
else:
self._node.get_logger().error(
'{} parameter set to something besides a bool'
.format(USE_SIM_TIME_NAME))
break
return SetParametersResult(successful=True)
def _on_parameter_event(self, parameter_list):
for parameter in parameter_list:
if parameter.name == 'use_sim_time':
if parameter.type_ == Parameter.Type.BOOL:
self.ros_time_is_active = parameter.value
else:
self._node.get_logger().error(
'use_sim_time parameter set to something besides a bool'
)
break
return SetParametersResult(successful=True)
def callback_params(self, data):
"""Handle updated configuration values."""
for parameter in data:
#if parameter.name == "name":
#if parameter.type_ == Parameter.Type.DOUBLE:
self.config[parameter.name] = parameter.value
# Config has changed, reset PID controllers
self.create_pids(self.config)
self.get_logger().warn("Parameters dynamically changed...")
return SetParametersResult(successful=True)
def set_parameters_atomically(
self, parameter_list: List[Parameter]) -> SetParametersResult:
"""
Atomically set parameters for the node.
If a callback was registered previously with :func:`set_parameters_callback`, it will be
called prior to setting the parameters for the node.
If the parameters are set successfully, a :class:`ParameterEvent` message is
published.
:param parameter_list: The list of parameters to set.
"""
result = None
if self._parameters_callback:
result = self._parameters_callback(parameter_list)
else:
result = SetParametersResult(successful=True)
if result.successful:
parameter_event = ParameterEvent()
# Add fully qualified path of node to parameter event
if self.get_namespace() == '/':
parameter_event.node = self.get_namespace() + self.get_name()
else:
parameter_event.node = self.get_namespace(
) + '/' + self.get_name()
for param in parameter_list:
if Parameter.Type.NOT_SET == param.type_:
if Parameter.Type.NOT_SET != self.get_parameter(
param.name).type_:
# Parameter deleted. (Parameter had value and new value is not set)
parameter_event.deleted_parameters.append(
param.to_parameter_msg())
# Delete any unset parameters regardless of their previous value.
# We don't currently store NOT_SET parameters so this is an extra precaution.
if param.name in self._parameters:
del self._parameters[param.name]
else:
if Parameter.Type.NOT_SET == self.get_parameter(
param.name).type_:
# Parameter is new. (Parameter had no value and new value is set)
parameter_event.new_parameters.append(
param.to_parameter_msg())
else:
# Parameter changed. (Parameter had a value and new value is set)
parameter_event.changed_parameters.append(
param.to_parameter_msg())
self._parameters[param.name] = param
parameter_event.stamp = self._clock.now().to_msg()
self._parameter_event_publisher.publish(parameter_event)
return result
def parameters_callback(params):
global JETBOT_MAX_LIN_VEL
global JETBOT_MAX_ANG_VEL
for param in params:
if param.name == 'max_linear_vel':
self.max_linear_vel = param.value
elif param.name == 'max_angular_vel':
self.max_angular_vel = param.value
else:
raise ValueError(f'unknown parameter {param.name}')
return SetParametersResult(successful=True)
def param_cb(self, params):
for param in params:
if param.name in self.param_table:
self.node.get_logger().debug('param callback ' + param.name)
element = self.param_table[param.name]['element']
prop_name = self.param_table[param.name]['prop_name']
if param.value != element.get_property(prop_name):
element.set_property(prop_name, param.value)
else:
self.node.get_logger().warn(
'simple pipeline ' + self.name +
' param_cb: unrecognised parameter "' + param.name + '"')
return SetParametersResult(successful=True)
def parameters_callback(self, params):
for param in params:
if param.name == 'left_trim':
self.left_trim = param.value
elif param.name == 'right_trim':
self.right_trim = param.value
elif param.name == 'max_pwm':
self.max_pwm = param.value
elif param.name == 'wheel_separation':
self.wheel_separation = param.value
else:
raise ValueError(f'unknown parameter {param.name}')
return SetParametersResult(successful=True)
def cb_params(self, data):
republish = False
for parameter in data:
if parameter.name == "template_file":
if parameter.type_ == Parameter.Type.STRING:
if(parameter.value != ''):
try:
filename = os.path.join(self.template_file_path, parameter.value)
self.get_logger().info(f"loading filename {filename}")
with open(filename) as j2_file_:
self.template_data = j2_file_.read()
self.template = Template(self.template_data)
self.get_logger().info("loaded template file")
except:
self.get_logger().error("load template file failed")
if parameter.name == "template_data":
if parameter.type_ == Parameter.Type.STRING:
try:
self.template_data = parameter.value
self.template = Template(self.template_data)
self.get_logger().info("loaded template data")
except:
self.get_logger().error("load template data failed")
if parameter.name == "template_params":
if parameter.value != None:
self.get_logger().warn("ignoring param 'template_params', try 'template_params.foo' ")
# XXX jsonloads?
elif parameter.name.startswith("template_params"):
param_name = parameter.name[len("template_params."):]
if param_name in self.template_params:
if self.template_params[param_name] != parameter.value:
republish = True
self.template_params[param_name] = parameter.value
if parameter.name == "message_module":
self.get_logger().error("jinjaturtle does not support changing subscription type after load")
if parameter.name == "message_class":
self.get_logger().error("jinjaturtle does not support changing subscription type after load")
if republish:
if self.last_message != None:
self.pub_message(self.last_message)
return SetParametersResult(successful=True)
def callback(parameter_list):
nonlocal callback_called
callback_called = True
r = SetParametersResult(successful=True)
for p in parameter_list:
if p.type_ != Parameter.Type.INTEGER:
r.successful = False
r.reason = 'Integer parameters only'
return r
if p.value % 2 != 0:
r.successful = False
r.reason = 'Integer must be even'
return r
return r
def set_parameters_atomically(self, parameter_list):
result = None
if self._parameters_callback:
result = self._parameters_callback(parameter_list)
else:
result = SetParametersResult(successful=True)
if result.successful:
parameter_event = ParameterEvent()
# Add fully qualified path of node to parameter event
if self.get_namespace() == '/':
parameter_event.node = self.get_namespace() + self.get_name()
else:
parameter_event.node = self.get_namespace(
) + '/' + self.get_name()
for param in parameter_list:
if Parameter.Type.NOT_SET == param.type_:
if Parameter.Type.NOT_SET != self.get_parameter(
param.name).type_:
# Parameter deleted. (Parameter had value and new value is not set)
parameter_event.deleted_parameters.append(
param.to_parameter_msg())
# Delete any unset parameters regardless of their previous value.
# We don't currently store NOT_SET parameters so this is an extra precaution.
if param.name in self._parameters:
del self._parameters[param.name]
else:
if Parameter.Type.NOT_SET == self.get_parameter(
param.name).type_:
# Parameter is new. (Parameter had no value and new value is set)
parameter_event.new_parameters.append(
param.to_parameter_msg())
else:
# Parameter changed. (Parameter had a value and new value is set)
parameter_event.changed_parameters.append(
param.to_parameter_msg())
self._parameters[param.name] = param
parameter_event.stamp = self._clock.now().to_msg()
self._parameter_event_publisher.publish(parameter_event)
return result
def callback(parameter_list):
nonlocal callback_called
callback_called = True
return SetParametersResult(successful=False)
def __init__(self, **kwargs):
assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \
"Invalid arguments passed to constructor: %r" % kwargs.keys()
from rcl_interfaces.msg import SetParametersResult
self.result = kwargs.get('result', SetParametersResult())
def reconfigure_callback(
self, parameters: List[Parameter]) -> SetParametersResult:
"""Called when any parameter changes
Registered using self.add_on_set_parameters_callback(self.reconfigure_callback).
Called for each parameter separately
(unless multiple parameters are set using set_parameters_atomically).
Before this callback is called, parameter value is validated against constraints (if any).
If constraints validation fails, this callback will not be called at all.
"""
# self.get_logger().info('reconfigure_callback')
for param in parameters:
if param.name not in self.config_map:
return SetParametersResult(
successful=False,
reason=f'param \'{param.name}\' not in config_map')
param_handler: Union[str, Tuple[str, Callable[
[Any], Any]]] = self.config_map[param.name]
if isinstance(param_handler, str):
setattr(self, param_handler, param.value)
continue
if isinstance(param_handler, tuple):
attr_name, attr_mapper = param_handler
setattr(self, attr_name, attr_mapper(param.value))
continue
return SetParametersResult(
successful=False,
reason=f'param_handler for param \'{param.name}\' is not valid'
)
pass
# if we pass successful=False, parameter value will not be set
# if parameter set was attempted using self.set_parameter*, node will exit with an error
# if parameter set was attempted remotely, the remote caller is just passed the result
# (failure and the optional configurable reason='why it was unsuccessful')
return SetParametersResult(successful=True)
# self.get_logger().info(f'''Reconfigure Request:
# Speed
# MAX: {config.Speed_max}
# MIN: {config.Speed_min}
# AVOID: {config.Speed_avoid}
# Turn-L
# MIN: {config.TurnL_min}
# MAX: {config.TurnL_max}
# AVOID: {config.TurnL_avoid}
# Turn-R
# MIN: {config.TurnR_min}
# MAX: {config.TurnR_max}
# AVOID: {config.TurnR_avoid}
# Switch 1-2
# Angle: {config.Switch_L12}
# Hysteresis: {config.Hysteresis_L12}
# Switch 2-3
# Angle: {config.Switch_L23}
# Hysteresis: {config.Hysteresis_L23}
# ''')
# rospy.loginfo(
# """Reconfigure Request: \n""" +
# """\tSpeed\n"""
# """\t\tMAX: {Speed_max}\n"""
# """\t\tMIN: {Speed_min}\n"""
# """\t\tAVOID: {Speed_avoid}\n"""
# """\tTurn-L\n"""
# """\t\tMIN: {TurnL_min}\n"""
# """\t\tMAX: {TurnL_max}\n"""
# """\t\tAVOID: {TurnL_avoid}\n"""
# """\tTurn-R\n"""
# """\t\tMIN: {TurnR_min}\n"""
# """\t\tMAX: {TurnR_max}\n"""
# """\t\tAVOID: {TurnR_avoid}\n"""
# """\tSwitch 1-2\n"""
# """\t\tAngle: {Switch_L12}\n"""
# """\t\tHysteresis: {Hysteresis_L12}\n"""
# """\tSwitch 2-3\n"""
# """\t\tAngle: {Switch_L23}\n"""
# """\t\tHysteresis: {Hysteresis_L23}"""
# .format(**config)
# )
pass
def _set_parameters_atomically(
self, parameter_list: List[Parameter]) -> SetParametersResult:
"""
Set the given parameters, all at one time, and then aggregate result.
This method does not check if the parameters were declared beforehand, and is intended
for internal use of this class.
If a callback was registered previously with :func:`set_parameters_callback`, it will be
called prior to setting the parameters for the node only once for all parameters.
If the callback prevents the parameters from being set, then it will be reflected in the
returned result; no exceptions will be raised in this case.
For each successfully set parameter, a :class:`ParameterEvent` message is
published.
If the value type of the parameter is NOT_SET, and the existing parameter type is
something else, then the parameter will be implicitly undeclared.
:param parameter_list: The list of parameters to set.
:return: Aggregate result of setting all the parameters atomically.
"""
result = None
if self._parameters_callback:
result = self._parameters_callback(parameter_list)
else:
result = SetParametersResult(successful=True)
if result.successful:
parameter_event = ParameterEvent()
# Add fully qualified path of node to parameter event
if self.get_namespace() == '/':
parameter_event.node = self.get_namespace() + self.get_name()
else:
parameter_event.node = self.get_namespace(
) + '/' + self.get_name()
for param in parameter_list:
if Parameter.Type.NOT_SET == param.type_:
if Parameter.Type.NOT_SET != self.get_parameter_or(
param.name).type_:
# Parameter deleted. (Parameter had value and new value is not set)
parameter_event.deleted_parameters.append(
param.to_parameter_msg())
# Delete any unset parameters regardless of their previous value.
# We don't currently store NOT_SET parameters so this is an extra precaution.
if param.name in self._parameters:
del self._parameters[param.name]
if param.name in self._descriptors:
del self._descriptors[param.name]
else:
if Parameter.Type.NOT_SET == self.get_parameter_or(
param.name).type_:
# Parameter is new. (Parameter had no value and new value is set)
parameter_event.new_parameters.append(
param.to_parameter_msg())
else:
# Parameter changed. (Parameter had a value and new value is set)
parameter_event.changed_parameters.append(
param.to_parameter_msg())
self._parameters[param.name] = param
parameter_event.stamp = self._clock.now().to_msg()
self._parameter_event_publisher.publish(parameter_event)
return result
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
from rcl_interfaces.msg import SetParametersResult
self.result = kwargs.get('result', SetParametersResult())
def parameter_callback(gait_selection, gait_state_machine, parameters):
"""
A callback function that is used to update the parameters that are a part of the
gait selection node. Since some of these parameters are from the
gait_state_machine, some from gait_selection and some from both, this is
implemented here.
:param gait_selection: The current GaitSelection object
:param gait_state_machine: The current GaitStateMachine
:param parameters: the parameters to update
:return: Whether the callback was successful
"""
gait_selection.get_logger().info("Parameters are updated")
gaits_updated = False
for param in parameters:
if param.name == "balance" and param.type_ == Parameter.Type.BOOL:
gait_selection._balance_used = param.value
elif param.name == "dynamic_gait" and param.type == Parameter.Type.BOOL:
gait_selection._dynamic_gait = param.value
elif (param.name == "early_schedule_delay"
and param.type_ == Parameter.Type.DOUBLE):
if param.value < 0:
return SetParametersResult(successful=False)
gait_selection._early_schedule_duration = Duration(
seconds=param.value)
elif (param.name == "first_subgait_delay"
and param.type_ == Parameter.Type.DOUBLE):
if param.value < 0:
return SetParametersResult(successful=False)
gait_selection._first_subgait_delay = Duration(seconds=param.value)
elif param.name == "dynamic_subgait_duration":
gait_selection.dynamic_subgait_duration = param.value
dynamic_gait_updated = True
elif param.name == "middle_point_fraction":
gait_selection.middle_point_fraction = param.value
dynamic_gait_updated = True
elif param.name == "middle_point_height":
gait_selection.middle_point_height = param.value
dynamic_gait_updated = True
elif param.name == "minimum_stair_height":
gait_selection.minimum_stair_height = param.value
dynamic_gait_updated = True
elif param.name == "push_off_fraction":
gait_selection.push_off_fraction = param.value
dynamic_gait_updated = True
elif param.name == "push_off_position":
gait_selection.push_off_position = param.value
dynamic_gait_updated = True
elif param.name == "gait_package" and param.type_ == Parameter.Type.STRING:
gait_selection._gait_package = param.value
gaits_updated = True
elif param.name == "gait_directory" and param.type_ == Parameter.Type.STRING:
gait_selection._directory_name = param.value
gaits_updated = True
elif param.name == "timer_period" and param.type_ == Parameter.Type.DOUBLE:
gait_state_machine.timer_period = param.value
if gait_state_machine.update_timer is not None:
gait_state_machine.update_timer.destroy()
gait_state_machine.run()
# Seperate update function for dynamic gait to avoid time performance issues
if dynamic_gait_updated:
gait_selection.dynamic_setpoint_gait.update_parameters()
gait_selection.get_logger().info("Dynamic gait parameters updated.")
elif gaits_updated:
gait_selection.update_gaits()
gait_state_machine._generate_graph()
gait_selection.get_logger().info("Gaits were updated")
return SetParametersResult(successful=True)
def reject_parameter_callback(self, parameter_list):
rejected_parameters = (param for param in parameter_list
if 'reject' in param.name)
return SetParametersResult(successful=(not any(rejected_parameters)))
