Skip to content

Commit b953bdd

Browse files
Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (#1947)
* Support add_pre_set_parameter and add_post_set_parameter callbacks in addition to add_on_set_parameter_callback in Node API Signed-off-by: deepanshu <deepanshubansal01@gmail.com>
1 parent 64e4c72 commit b953bdd

11 files changed

Lines changed: 991 additions & 60 deletions

File tree

164 KB
Loading
Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
# Proposed node parameters callback Design
2+
3+
## Introduction:
4+
5+
The original requirement came in **gazebo_ros_pkgs** for setting individual wheel slip parameters based on global wheel slip value [link to original issue](https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1365).
6+
7+
The main requirement is to set one or more parameters after another parameter is set successfully.
8+
9+
Additionally, it would be nice if users could be notified locally (via a callback) when parameters have been set successfully (i.e. post validation).
10+
11+
Related discussion can be found in [#609](https://github.com/ros2/rclcpp/issues/609) [#1789](https://github.com/ros2/rclcpp/pull/1789)
12+
13+
With the current parameters API, the `add_on_set_parameters_callback` is intended for validation of parameter values before they are set, it should **not** cause any side-effects.
14+
15+
There is also the `ParameterEventHandler` that publishes changes to node parameters on `/parameter_events` topic for external nodes to see. Though the node could subscribe to the `/parameter_events` topic to be notified of changes to its own parameters, it is less than ideal since there is a delay caused by waiting for an executor to process the callback.
16+
17+
We propose adding a `PostSetParametersCallbackHandle` for successful parameter set similar to `OnSetParametersCallbackHandle` for parameter validation. Also, we propose adding a `PreSetParametersCallbackHandle` useful for modifying list of parameters being set.
18+
19+
The validation callback is often abused to trigger side effects in the code, for instance updating class attributes even before a parameter has been set successfully. Instead of relying on the `/parameter_events` topic to be notified of parameter changes, users can register a callback with a new API, `add_post_set_parameters_callback`.
20+
21+
It is possible to use the proposed `add_post_set_parameters_callback` for setting additional parameters, but this might result in infinite recursion and does not allow those additional parameters to be set atomically with the original parameter(s) changed.
22+
To workaround these issues, we propose adding a "pre set" callback type that can be registered with `add_pre_set_parameters_callback`, which will be triggered before the validation callbacks and can be used to modify the parameter list.
23+
24+
![Desgin API](https://github.com/ros2/rclcpp/blob/deepanshu/local-param-changed-callback-support/rclcpp/doc/param_callback_design.png?raw=true)
25+
26+
## Alternatives
27+
28+
* Users could call `set_parameter` while processing a message from the `/parameter_events` topic, however, there is extra overhead in having to create subscription (as noted earlier).
29+
* Users could call `set_parameter` inside the "on set" parameters callback, however it is not well-defined how side-effects should handle cases where parameter validation fails.

rclcpp/include/rclcpp/node.hpp

Lines changed: 229 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -370,11 +370,21 @@ class Node : public std::enable_shared_from_this<Node>
370370
*
371371
* If `ignore_override` is `true`, the parameter override will be ignored.
372372
*
373-
* This method, if successful, will result in any callback registered with
374-
* add_on_set_parameters_callback to be called.
373+
* This method will result in any callback registered with
374+
* `add_on_set_parameters_callback` and `add_post_set_parameters_callback`
375+
* to be called for the parameter being set.
376+
*
377+
* If a callback was registered previously with `add_on_set_parameters_callback`,
378+
* it will be called prior to setting the parameter for the node.
375379
* If that callback prevents the initial value for the parameter from being
376380
* set then rclcpp::exceptions::InvalidParameterValueException is thrown.
377381
*
382+
* If a callback was registered previously with `add_post_set_parameters_callback`,
383+
* it will be called after setting the parameter successfully for the node.
384+
*
385+
* This method will _not_ result in any callbacks registered with
386+
* `add_pre_set_parameters_callback` to be called.
387+
*
378388
* The returned reference will remain valid until the parameter is
379389
* undeclared.
380390
*
@@ -491,11 +501,22 @@ class Node : public std::enable_shared_from_this<Node>
491501
* If `ignore_overrides` is `true`, all the overrides of the parameters declared
492502
* by the function call will be ignored.
493503
*
504+
* This method will result in any callback registered with
505+
* `add_on_set_parameters_callback` and `add_post_set_parameters_callback`
506+
* to be called once for each parameter.
507+
*
494508
* This method, if successful, will result in any callback registered with
495-
* add_on_set_parameters_callback to be called, once for each parameter.
509+
* `add_on_set_parameters_callback` to be called, once for each parameter.
496510
* If that callback prevents the initial value for any parameter from being
497511
* set then rclcpp::exceptions::InvalidParameterValueException is thrown.
498512
*
513+
* If a callback was registered previously with `add_post_set_parameters_callback`,
514+
* it will be called after setting the parameters successfully for the node,
515+
* once for each parameter.
516+
*
517+
* This method will _not_ result in any callbacks registered with
518+
* `add_pre_set_parameters_callback` to be called.
519+
*
499520
* \param[in] namespace_ The namespace in which to declare the parameters.
500521
* \param[in] parameters The parameters to set in the given namespace.
501522
* \param[in] ignore_overrides When `true`, the parameters overrides are ignored.
@@ -533,8 +554,9 @@ class Node : public std::enable_shared_from_this<Node>
533554

534555
/// Undeclare a previously declared parameter.
535556
/**
536-
* This method will not cause a callback registered with
537-
* add_on_set_parameters_callback to be called.
557+
* This method will _not_ cause a callback registered with any of the
558+
* `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and
559+
* `add_post_set_parameters_callback` to be called.
538560
*
539561
* \param[in] name The name of the parameter to be undeclared.
540562
* \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter
@@ -568,11 +590,24 @@ class Node : public std::enable_shared_from_this<Node>
568590
* Parameter overrides are ignored by set_parameter.
569591
*
570592
* This method will result in any callback registered with
571-
* add_on_set_parameters_callback to be called.
593+
* `add_pre_set_parameters_callback`, add_on_set_parameters_callback` and
594+
* `add_post_set_parameters_callback` to be called once for the parameter
595+
* being set.
596+
*
597+
* This method will result in any callback registered with
598+
* `add_on_set_parameters_callback` to be called.
572599
* If the callback prevents the parameter from being set, then it will be
573600
* reflected in the SetParametersResult that is returned, but no exception
574601
* will be thrown.
575602
*
603+
* If a callback was registered previously with `add_pre_set_parameters_callback`,
604+
* it will be called once prior to the validation of the parameter for the node.
605+
* If this callback makes modified parameter list empty, then it will be reflected
606+
* in the returned result; no exceptions will be raised in this case.
607+
*
608+
* If a callback was registered previously with `add_post_set_parameters_callback`,
609+
* it will be called once after setting the parameter successfully for the node.
610+
*
576611
* If the value type of the parameter is rclcpp::PARAMETER_NOT_SET, and the
577612
* existing parameter type is something else, then the parameter will be
578613
* implicitly undeclared.
@@ -609,11 +644,25 @@ class Node : public std::enable_shared_from_this<Node>
609644
* corresponding SetParametersResult in the vector returned by this function.
610645
*
611646
* This method will result in any callback registered with
612-
* add_on_set_parameters_callback to be called, once for each parameter.
647+
* `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and
648+
* `add_post_set_parameters_callback` to be called once for each parameter.
649+
650+
* If a callback was registered previously with `add_pre_set_parameters_callback`,
651+
* it will be called prior to the validation of parameters for the node,
652+
* once for each parameter.
653+
* If this callback makes modified parameter list empty, then it will be reflected
654+
* in the returned result; no exceptions will be raised in this case.
655+
*
656+
* This method will result in any callback registered with
657+
* `add_on_set_parameters_callback` to be called, once for each parameter.
613658
* If the callback prevents the parameter from being set, then, as mentioned
614659
* before, it will be reflected in the corresponding SetParametersResult
615660
* that is returned, but no exception will be thrown.
616661
*
662+
* If a callback was registered previously with `add_post_set_parameters_callback`,
663+
* it will be called after setting the parameters successfully for the node,
664+
* once for each parameter.
665+
*
617666
* Like set_parameter() this method will implicitly undeclare parameters
618667
* with the type rclcpp::PARAMETER_NOT_SET.
619668
*
@@ -640,11 +689,25 @@ class Node : public std::enable_shared_from_this<Node>
640689
* If the exception is thrown then none of the parameters will have been set.
641690
*
642691
* This method will result in any callback registered with
643-
* add_on_set_parameters_callback to be called, just one time.
692+
* `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and
693+
* `add_post_set_parameters_callback` to be called only 'once' for all parameters.
694+
*
695+
* If a callback was registered previously with `add_pre_set_parameters_callback`,
696+
* it will be called prior to the validation of node parameters, just one time
697+
* for all parameters.
698+
* If this callback makes modified parameter list empty, then it will be reflected
699+
* in the returned result; no exceptions will be raised in this case.
700+
*
701+
* This method will result in any callback registered with
702+
* 'add_on_set_parameters_callback' to be called, just one time.
644703
* If the callback prevents the parameters from being set, then it will be
645704
* reflected in the SetParametersResult which is returned, but no exception
646705
* will be thrown.
647706
*
707+
* If a callback was registered previously with `add_post_set_parameters_callback`,
708+
* it will be called after setting the node parameters successfully, just one time
709+
* for all parameters.
710+
*
648711
* If you pass multiple rclcpp::Parameter instances with the same name, then
649712
* only the last one in the vector (forward iteration) will be set.
650713
*
@@ -893,12 +956,82 @@ class Node : public std::enable_shared_from_this<Node>
893956
rcl_interfaces::msg::ListParametersResult
894957
list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
895958

959+
using PreSetParametersCallbackHandle =
960+
rclcpp::node_interfaces::PreSetParametersCallbackHandle;
961+
using PreSetParametersCallbackType =
962+
rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType;
963+
896964
using OnSetParametersCallbackHandle =
897965
rclcpp::node_interfaces::OnSetParametersCallbackHandle;
898-
using OnParametersSetCallbackType =
899-
rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
966+
using OnSetParametersCallbackType =
967+
rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType;
968+
using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] =
969+
OnSetParametersCallbackType;
970+
971+
using PostSetParametersCallbackHandle =
972+
rclcpp::node_interfaces::PostSetParametersCallbackHandle;
973+
using PostSetParametersCallbackType =
974+
rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType;
900975

901-
/// Add a callback for when parameters are being set.
976+
/// Add a callback that gets triggered before parameters are validated.
977+
/**
978+
* This callback can be used to modify the original list of parameters being
979+
* set by the user.
980+
*
981+
* The modified list of parameters is then forwarded to the "on set parameter"
982+
* callback for validation.
983+
*
984+
* The callback is called whenever any of the `set_parameter*` methods are called
985+
* or when a set parameter service request is received.
986+
*
987+
* The callback takes a reference to the vector of parameters to be set.
988+
*
989+
* The vector of parameters may be modified by the callback.
990+
*
991+
* One of the use case of "pre set callback" can be updating additional parameters
992+
* conditioned on changes to a parameter.
993+
*
994+
* For an example callback:
995+
*
996+
*```cpp
997+
* void
998+
* preSetParameterCallback(std::vector<rclcpp::Parameter> & parameters)
999+
* {
1000+
* for (auto & param : parameters) {
1001+
* if (param.get_name() == "param1") {
1002+
* parameters.push_back(rclcpp::Parameter("param2", 4.0));
1003+
* }
1004+
* }
1005+
* }
1006+
* ```
1007+
* The above callback appends 'param2' to the list of parameters to be set if
1008+
* 'param1' is being set by the user.
1009+
*
1010+
* All parameters in the vector will be set atomically.
1011+
*
1012+
* Note that the callback is only called while setting parameters with `set_parameter`,
1013+
* `set_parameters`, `set_parameters_atomically`, or externally with a parameters service.
1014+
*
1015+
* The callback is not called when parameters are declared with `declare_parameter`
1016+
* or `declare_parameters`.
1017+
*
1018+
* The callback is not called when parameters are undeclared with `undeclare_parameter`.
1019+
*
1020+
* An empty modified parameter list from the callback will result in "set_parameter*"
1021+
* returning an unsuccessful result.
1022+
*
1023+
* The `remove_pre_set_parameters_callback` can be used to deregister the callback.
1024+
*
1025+
* \param callback The callback to register.
1026+
* \returns A shared pointer. The callback is valid as long as the smart pointer is alive.
1027+
* \throws std::bad_alloc if the allocation of the PreSetParametersCallbackHandle fails.
1028+
*/
1029+
RCLCPP_PUBLIC
1030+
RCUTILS_WARN_UNUSED
1031+
PreSetParametersCallbackHandle::SharedPtr
1032+
add_pre_set_parameters_callback(PreSetParametersCallbackType callback);
1033+
1034+
/// Add a callback to validate parameters before they are set.
9021035
/**
9031036
* The callback signature is designed to allow handling of any of the above
9041037
* `set_parameter*` or `declare_parameter*` methods, and so it takes a const
@@ -937,6 +1070,8 @@ class Node : public std::enable_shared_from_this<Node>
9371070
* this callback, so when checking a new value against the existing one, you
9381071
* must account for the case where the parameter is not yet set.
9391072
*
1073+
* The callback is not called when parameters are undeclared with `undeclare_parameter`.
1074+
*
9401075
* Some constraints like read_only are enforced before the callback is called.
9411076
*
9421077
* The callback may introspect other already set parameters (by calling any
@@ -965,7 +1100,77 @@ class Node : public std::enable_shared_from_this<Node>
9651100
RCLCPP_PUBLIC
9661101
RCUTILS_WARN_UNUSED
9671102
OnSetParametersCallbackHandle::SharedPtr
968-
add_on_set_parameters_callback(OnParametersSetCallbackType callback);
1103+
add_on_set_parameters_callback(OnSetParametersCallbackType callback);
1104+
1105+
/// Add a callback that gets triggered after parameters are set successfully.
1106+
/**
1107+
* The callback is called when any of the `set_parameter*` or `declare_parameter*`
1108+
* methods are successful.
1109+
*
1110+
* The callback takes a reference to a const vector of parameters that have been
1111+
* set successfully.
1112+
*
1113+
* The post callback can be valuable as a place to cause side-effects based on
1114+
* parameter changes.
1115+
* For instance updating internally tracked class attributes once parameters
1116+
* have been changed successfully.
1117+
*
1118+
* For an example callback:
1119+
*
1120+
* ```cpp
1121+
* void
1122+
* postSetParameterCallback(const std::vector<rclcpp::Parameter> & parameters)
1123+
* {
1124+
* for(const auto & param:parameters) {
1125+
* // the internal class member can be changed after
1126+
* // successful change to param1 or param2
1127+
* if(param.get_name() == "param1") {
1128+
* internal_tracked_class_parameter_1_ = param.get_value<double>();
1129+
* }
1130+
* else if(param.get_name() == "param2") {
1131+
* internal_tracked_class_parameter_2_ = param.get_value<double>();
1132+
* }
1133+
* }
1134+
* }
1135+
* ```
1136+
*
1137+
* The above callback takes a const reference to list of parameters that have been
1138+
* set successfully and as a result of this updates the internally tracked class attributes
1139+
* `internal_tracked_class_parameter_1_` and `internal_tracked_class_parameter_2_`
1140+
* respectively.
1141+
*
1142+
* This callback should not modify parameters.
1143+
*
1144+
* The callback is called when parameters are declared with `declare_parameter`
1145+
* or `declare_parameters`. See `declare_parameter` or `declare_parameters` above.
1146+
*
1147+
* The callback is not called when parameters are undeclared with `undeclare_parameter`.
1148+
*
1149+
* If you want to make changes to parameters based on changes to another, use
1150+
* `add_pre_set_parameters_callback`.
1151+
*
1152+
* The `remove_post_set_parameters_callback` can be used to deregister the callback.
1153+
*
1154+
* \param callback The callback to register.
1155+
* \returns A shared pointer. The callback is valid as long as the smart pointer is alive.
1156+
* \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
1157+
*/
1158+
RCLCPP_PUBLIC
1159+
RCUTILS_WARN_UNUSED
1160+
PostSetParametersCallbackHandle::SharedPtr
1161+
add_post_set_parameters_callback(PostSetParametersCallbackType callback);
1162+
1163+
/// Remove a callback registered with `add_pre_set_parameters_callback`.
1164+
/**
1165+
* Delete a handler returned by `add_pre_set_parameters_callback`.
1166+
*
1167+
* \param handler The callback handler to remove.
1168+
* \throws std::runtime_error if the handler was not created with `add_pre_set_parameters_callback`,
1169+
* or if it has been removed before.
1170+
*/
1171+
RCLCPP_PUBLIC
1172+
void
1173+
remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler);
9691174

9701175
/// Remove a callback registered with `add_on_set_parameters_callback`.
9711176
/**
@@ -994,6 +1199,18 @@ class Node : public std::enable_shared_from_this<Node>
9941199
void
9951200
remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler);
9961201

1202+
/// Remove a callback registered with `add_post_set_parameters_callback`.
1203+
/**
1204+
* Delete a handler returned by `add_post_set_parameters_callback`.
1205+
*
1206+
* \param handler The callback handler to remove.
1207+
* \throws std::runtime_error if the handler was not created with `add_post_set_parameters_callback`,
1208+
* or if it has been removed before.
1209+
*/
1210+
RCLCPP_PUBLIC
1211+
void
1212+
remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler);
1213+
9971214
/// Get the fully-qualified names of all available nodes.
9981215
/**
9991216
* The fully-qualified name includes the local namespace and name of the node.

0 commit comments

Comments
 (0)