Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(components): keep parameter values in sync with ros #111

Merged
merged 4 commits into from
Jun 11, 2024
Merged

fix(components): keep parameter values in sync with ros #111

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e18b938
fix: keep parameter values in sync with ros
domire8 May 31, 2024
1112680
fix: code review
domire8 Jun 10, 2024
b0e6e37
docs: changelog
domire8 Jun 10, 2024
ab1db2c
docs: update changelog
domire8 Jun 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 18 additions & 7 deletions source/modulo_components/include/modulo_components/ComponentInterface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -459,6 +459,13 @@ class ComponentInterface {
/**
* @brief Callback function to validate and update parameters on change.
* @param parameters The new parameter objects provided by the ROS interface
*/
void pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters);

/**
* @brief Callback function notify ROS about the validation result from the pre_set_parameters_callback step..
domire8 marked this conversation as resolved.
Show resolved Hide resolved
* @param parameters The new parameter objects provided by the ROS interface
* @see pre_set_parameters_callback()
* @return The result of the validation
*/
rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
Expand Down Expand Up @@ -574,8 +581,13 @@ class ComponentInterface {
std::map<std::string, std::function<void(void)>> periodic_callbacks_; ///< Map of periodic function callbacks

state_representation::ParameterMap parameter_map_; ///< ParameterMap for handling parameters
std::unordered_map<std::string, bool> read_only_parameters_;
std::shared_ptr<rclcpp::node_interfaces::PreSetParametersCallbackHandle>
pre_set_parameter_cb_handle_; ///< ROS callback function handle on pre set of parameters
std::shared_ptr<rclcpp::node_interfaces::OnSetParametersCallbackHandle>
parameter_cb_handle_; ///< ROS callback function handle for setting parameters
on_set_parameter_cb_handle_; ///< ROS callback function handle on set of parameters
rcl_interfaces::msg::SetParametersResult set_parameters_result_;
bool pre_set_parameter_callback_called_ = false; ///< Flag to indicate if pre_set_parameter_callback was called

std::shared_ptr<rclcpp::TimerBase> step_timer_; ///< Timer for the step function
std::shared_ptr<tf2_ros::Buffer> tf_buffer_; ///< TF buffer
Expand All @@ -591,8 +603,6 @@ class ComponentInterface {
std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> node_timers_;
std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> node_topics_;
rclcpp::QoS qos_ = rclcpp::QoS(10); ///< Quality of Service for ROS publishers and subscribers

bool set_parameter_callback_called_ = false; ///< Flag to indicate if on_set_parameter_callback was called
};

template<typename T>
Expand All @@ -619,10 +629,11 @@ inline T ComponentInterface::get_parameter_value(const std::string& name) const
template<typename T>
inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) {
try {
rcl_interfaces::msg::SetParametersResult result = this->node_parameters_->set_parameters(
{
modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))
}).at(0);
std::vector<rclcpp::Parameter> parameters{
modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))};
this->pre_set_parameters_callback(parameters);
this->pre_set_parameter_callback_called_ = true;
auto result = this->node_parameters_->set_parameters(parameters).at(0);
if (!result.successful) {
RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
"Failed to set parameter value of parameter '" << name << "': " << result.reason);
Expand Down
48 changes: 43 additions & 5 deletions source/modulo_components/modulo_components/component_interface.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,9 @@ def __init__(self, node_name: str, *args, **kwargs):
super().__init__(node_name, *args, **node_kwargs)
self.__step_lock = Lock()
self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {}
self.__read_only_parameters: Dict[str, bool] = {}
self.__pre_set_parameters_callback_called = False
self.__set_parameters_result = SetParametersResult()
self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {}
self._triggers: Dict[str, bool] = {}
self._periodic_callbacks: Dict[str, Callable[[], None]] = {}
Expand All @@ -60,6 +63,7 @@ def __init__(self, node_name: str, *args, **kwargs):

self._qos = QoSProfile(depth=10)

self.add_pre_set_parameters_callback(self.__pre_set_parameters_callback)
self.add_on_set_parameters_callback(self.__on_set_parameters_callback)
self.add_parameter(sr.Parameter("rate", 10.0, sr.ParameterType.DOUBLE),
"The rate in Hertz for all periodic callbacks")
Expand Down Expand Up @@ -142,16 +146,25 @@ def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, r
if not self.has_parameter(sr_parameter.get_name()):
self.get_logger().debug(f"Adding parameter '{sr_parameter.get_name()}'.")
self._parameter_dict[sr_parameter.get_name()] = parameter
self.__read_only_parameters[sr_parameter.get_name()] = False
eeberhard marked this conversation as resolved.
Show resolved Hide resolved
try:
descriptor = ParameterDescriptor(description=description, read_only=read_only)
self.__set_parameters_result = SetParametersResult(successful=True, reason="")
if sr_parameter.is_empty():
descriptor.dynamic_typing = True
descriptor.type = get_ros_parameter_type(sr_parameter.get_parameter_type()).value
self.declare_parameter(ros_param.name, None, descriptor=descriptor)
else:
self.declare_parameter(ros_param.name, ros_param.value, descriptor=descriptor)
new_parameters = self.__pre_set_parameters_callback([Node.get_parameter(self, ros_param.name)])
result = self.__on_set_parameters_callback(new_parameters)
if not result.successful:
self.undeclare_parameter(ros_param.name)
raise ParameterError(result.reason)
self.__read_only_parameters[sr_parameter.get_name()] = read_only
except Exception as e:
del self._parameter_dict[sr_parameter.get_name()]
del self.__read_only_parameters[sr_parameter.get_name()]
raise ParameterError(f"Failed to add parameter: {e}")
else:
self.get_logger().debug(f"Parameter '{sr_parameter.get_name()}' already exists.")
Expand Down Expand Up @@ -214,8 +227,10 @@ def set_parameter_value(self, name: str, value: T, parameter_type: sr.ParameterT
:param parameter_type: The type of the parameter
"""
try:
ros_param = write_parameter(sr.Parameter(name, value, parameter_type))
result = self.set_parameters([ros_param])[0]
parameters = [write_parameter(sr.Parameter(name, value, parameter_type))]
new_parameters = self.__pre_set_parameters_callback(parameters)
self.__pre_set_parameters_callback_called = True
result = self.set_parameters(new_parameters)[0]
if not result.successful:
self.get_logger().error(f"Failed to set parameter value of parameter '{name}': {result.reason}",
throttle_duration_sec=1.0)
Expand Down Expand Up @@ -249,30 +264,53 @@ def on_validate_parameter_callback(self, parameter: sr.Parameter) -> bool:
:return: The validation result
"""
return True

def __on_set_parameters_callback(self, ros_parameters: List[Parameter]) -> SetParametersResult:
def __pre_set_parameters_callback(self, ros_parameters: List[Parameter]) -> List[Parameter]:
"""
Callback function to validate and update parameters on change.

:param ros_parameters: The new parameter objects provided by the ROS interface
:return: The result of the validation
:return: The validated parameter objects
"""
if self.__pre_set_parameters_callback_called:
self.__pre_set_parameters_callback_called = False
return ros_parameters
new_parameters = []
result = SetParametersResult(successful=True)
for ros_param in ros_parameters:
try:
parameter = self.__get_component_parameter(ros_param.name)
if self.__read_only_parameters.get(ros_param.name):
self.get_logger().debug(f"Parameter '{ros_param.name}' is read only")
continue
new_parameter = read_parameter_const(ros_param, parameter)
if not self.__validate_parameter(new_parameter):
new_parameters.append(write_parameter(parameter))
result.successful = False
result.reason = f"Validation of parameter '{ros_param.name}' returned false!"
else:
if isinstance(self._parameter_dict[ros_param.name], str):
self.__setattr__(self._parameter_dict[ros_param.name], new_parameter)
else:
self._parameter_dict[ros_param.name] = new_parameter
new_parameters.append(write_parameter(new_parameter))
except Exception as e:
result.successful = False
result.reason += str(e)
self.__set_parameters_result = result
return new_parameters


def __on_set_parameters_callback(self, ros_parameters: List[Parameter]) -> SetParametersResult:
"""
Callback function notify ROS about the validation result from the pre_set_parameters_callback step
domire8 marked this conversation as resolved.
Show resolved Hide resolved

:param ros_parameters: The parameter objects provided by the ROS interface
:return: The result of the validation
"""
result = copy.copy(self.__set_parameters_result)
self.__set_parameters_result.successful = True
self.__set_parameters_result.reason = ""
return result

def add_predicate(self, name: str, value: Union[bool, Callable[[], bool]]):
Expand Down
61 changes: 39 additions & 22 deletions source/modulo_components/src/ComponentInterface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,10 @@ ComponentInterface::ComponentInterface(
node_services_(interfaces->get_node_services_interface()),
node_timers_(interfaces->get_node_timers_interface()),
node_topics_(interfaces->get_node_topics_interface()) {
// register the parameter change callback handler
this->parameter_cb_handle_ = this->node_parameters_->add_on_set_parameters_callback(
// register the parameter change callback handlers
this->pre_set_parameter_cb_handle_ = this->node_parameters_->add_pre_set_parameters_callback(
[this](std::vector<rclcpp::Parameter>& parameters) { return this->pre_set_parameters_callback(parameters); });
this->on_set_parameter_cb_handle_ = this->node_parameters_->add_on_set_parameters_callback(
[this](const std::vector<rclcpp::Parameter>& parameters) -> rcl_interfaces::msg::SetParametersResult {
return this->on_set_parameters_callback(parameters);
});
Expand Down Expand Up @@ -79,9 +81,7 @@ void ComponentInterface::on_step_callback() {}

void ComponentInterface::add_parameter(
const std::shared_ptr<state_representation::ParameterInterface>& parameter, const std::string& description,
bool read_only
) {
this->set_parameter_callback_called_ = false;
bool read_only) {
rclcpp::Parameter ros_param;
try {
ros_param = modulo_core::translators::write_parameter(parameter);
Expand All @@ -91,32 +91,37 @@ void ComponentInterface::add_parameter(
if (!this->node_parameters_->has_parameter(parameter->get_name())) {
RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding parameter '" << parameter->get_name() << "'.");
this->parameter_map_.set_parameter(parameter);
this->read_only_parameters_.insert_or_assign(parameter->get_name(), false);
eeberhard marked this conversation as resolved.
Show resolved Hide resolved
try {
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = description;
descriptor.read_only = read_only;
// since the pre_set_parameters_callback is not called on parameter declaration, this has to be true
this->set_parameters_result_.successful = true;
this->set_parameters_result_.reason = "";
if (parameter->is_empty()) {
descriptor.dynamic_typing = true;
descriptor.type = modulo_core::translators::get_ros_parameter_type(parameter->get_parameter_type());
this->node_parameters_->declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor);
} else {
this->node_parameters_->declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor);
}
if (!this->set_parameter_callback_called_) {
auto result =
this->on_set_parameters_callback({this->node_parameters_->get_parameters({parameter->get_name()})});
if (!result.successful) {
this->node_parameters_->undeclare_parameter(parameter->get_name());
throw ParameterException(result.reason);
}
std::vector<rclcpp::Parameter> ros_parameters{this->node_parameters_->get_parameters({parameter->get_name()})};
this->pre_set_parameters_callback(ros_parameters);
auto result = this->on_set_parameters_callback(ros_parameters);
if (!result.successful) {
this->node_parameters_->undeclare_parameter(parameter->get_name());
throw ParameterException(result.reason);
}
this->read_only_parameters_.at(parameter->get_name()) = read_only;
} catch (const std::exception& ex) {
this->parameter_map_.remove_parameter(parameter->get_name());
this->read_only_parameters_.erase(parameter->get_name());
throw ParameterException("Failed to add parameter: " + std::string(ex.what()));
}
} else {
RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(),
"Parameter '" << parameter->get_name() << "' already exists.");
RCLCPP_DEBUG_STREAM(
this->node_logging_->get_logger(), "Parameter '" << parameter->get_name() << "' already exists.");
}
}

Expand All @@ -129,33 +134,45 @@ ComponentInterface::get_parameter(const std::string& name) const {
}
}

rcl_interfaces::msg::SetParametersResult
ComponentInterface::on_set_parameters_callback(const std::vector<rclcpp::Parameter>& parameters) {
void ComponentInterface::pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters) {
if (this->pre_set_parameter_callback_called_) {
this->pre_set_parameter_callback_called_ = false;
return;
}
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto& ros_parameter : parameters) {
for (auto& ros_parameter : parameters) {
try {
if (ros_parameter.get_name().substr(0, 27) == "qos_overrides./tf.publisher") {
auto parameter = parameter_map_.get_parameter(ros_parameter.get_name());
if (this->read_only_parameters_.at(ros_parameter.get_name())) {
eeberhard marked this conversation as resolved.
Show resolved Hide resolved
RCLCPP_DEBUG_STREAM(
this->node_logging_->get_logger(), "Parameter '" << ros_parameter.get_name() << "' is read only.");
continue;
}
// get the associated parameter interface by name
auto parameter = parameter_map_.get_parameter(ros_parameter.get_name());

// convert the ROS parameter into a ParameterInterface without modifying the original
auto new_parameter = modulo_core::translators::read_parameter_const(ros_parameter, parameter);
if (!this->validate_parameter(new_parameter)) {
ros_parameter = modulo_core::translators::write_parameter(new_parameter);
eeberhard marked this conversation as resolved.
Show resolved Hide resolved
result.successful = false;
result.reason += "Validation of parameter '" + ros_parameter.get_name() + "' returned false!";
} else if (!new_parameter->is_empty()) {
// update the value of the parameter in the map
domire8 marked this conversation as resolved.
Show resolved Hide resolved
modulo_core::translators::copy_parameter_value(new_parameter, parameter);
ros_parameter = modulo_core::translators::write_parameter(new_parameter);
}
} catch (const std::exception& ex) {
result.successful = false;
result.reason += ex.what();
}
}
this->set_parameter_callback_called_ = true;
this->set_parameters_result_ = result;
}

rcl_interfaces::msg::SetParametersResult
ComponentInterface::on_set_parameters_callback(const std::vector<rclcpp::Parameter>& parameters) {
auto result = this->set_parameters_result_;
this->set_parameters_result_.successful = true;
this->set_parameters_result_.reason = "";
return result;
}

Expand Down
28 changes: 26 additions & 2 deletions source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,12 @@ class EmptyParameterInterface : public ComponentInterfacePublicInterface {
) : ComponentInterfacePublicInterface(node), allow_empty_(allow_empty) {
if (add_parameter) {
if (empty_parameter) {
this->add_parameter(std::make_shared<Parameter<std::string>>("name"), "Test parameter");
this->add_parameter(std::make_shared<Parameter<std::string>>("name"), "Name parameter");
} else {
this->add_parameter(std::make_shared<Parameter<std::string>>("name", "test"), "Test parameter");
this->add_parameter(std::make_shared<Parameter<std::string>>("name", "test"), "Name parameter");
}
}
this->add_parameter(std::make_shared<Parameter<double>>("value", 0.0), "Value parameter");
};

private:
Expand All @@ -30,6 +31,12 @@ class EmptyParameterInterface : public ComponentInterfacePublicInterface {
RCLCPP_ERROR(this->node_logging_->get_logger(), "Provide a non empty value for parameter 'name'");
return false;
}
} else if (parameter->get_name() == "value") {
auto value = parameter->get_parameter_value<double>();
if (value < 0) {
parameter->set_parameter_value<double>(std::abs(value));
}
return true;
}
domire8 marked this conversation as resolved.
Show resolved Hide resolved
return true;
}
Expand Down Expand Up @@ -180,4 +187,21 @@ TYPED_TEST(ComponentInterfaceEmptyParameterTest, ParameterOverridesEmpty) {
EXPECT_THROW(std::make_shared<EmptyParameterInterface<TypeParam>>(node, false, true, false),
modulo_utils::exceptions::ParameterException);
}

TYPED_TEST(ComponentInterfaceEmptyParameterTest, ValueParameter) {
EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value<double>(), 0);
this->component_->template set_parameter_value<double>("value", 1);
EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value<double>(), 1);
EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 1);
this->component_->template set_parameter_value<double>("value", -2);
EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value<double>(), 2);
EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 2);

this->component_->set_ros_parameter({"value", 3.0});
EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value<double>(), 3);
EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 3);
this->component_->set_ros_parameter({"value", -4.0});
EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value<double>(), 4);
EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 4);
}
} // namespace modulo_components
6 changes: 3 additions & 3 deletions source/modulo_components/test/cpp/test_component_interface_parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,20 +65,20 @@ TYPED_TEST(ComponentInterfaceParameterTest, AddParameterAgain) {
EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException);
this->component_->add_parameter(this->param_, "Test parameter");

// Adding an existing parameter again should just set the value
// Adding an existing parameter again should not change the parameter
this->component_->validate_parameter_was_called = false;
EXPECT_NO_THROW(this->component_->add_parameter("test", 2, "foo"));
EXPECT_FALSE(this->component_->validate_parameter_was_called);
this->template expect_parameter_value<int>(1);
}

TYPED_TEST(ComponentInterfaceParameterTest, SetParameter) {
// setting before adding should not work and parameter should not be created
// Setting before adding should not work and parameter should not be created
this->component_->set_parameter_value("test", 1);
EXPECT_THROW(
this->component_->template get_parameter_value<int>("test"), modulo_utils::exceptions::ParameterException);

// validation should not be invoked as the parameter did not exist
// Validation should not be invoked as the parameter did not exist
EXPECT_FALSE(this->component_->validate_parameter_was_called);
EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException);

Expand Down
Loading
Loading