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