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.

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

Add preliminary support for parameters #332

Merged
merged 83 commits into from
Oct 27, 2023

Conversation

luca-della-vedova
Copy link
Collaborator

@luca-della-vedova luca-della-vedova commented Oct 5, 2023

Description

This PR brings preliminary support for parameters to rclrs. It's fairly complex and some choices have been made for ergonomics and safety that don't necessarily match what other client libraries do. At this stage feedback on whether the behavior and those deviations are OK is probably more productive that going too deep into code review.

There are extensive unit tests implemented in parameter.rs. These can be a good example of what the experience of using the library would be like.

Note: This PR is currently built on top of #325 since it refactors some of its usage of parameters, depending on the preferred order we can either look at this after or I can rebase this on top of main and look at the time source PR later, but they are still somewhat interconnected.

What's implemented

  • Parameter getting and setting API, as well as parsing of command line overrides (this was present but not accessible).
  • Both declared and non declared parameters.

What's not implemented

  • Parameter services.
  • Parameter callbacks, such as on set, pre set and post set callbacks.
  • Automatic parameter declaration from overrides.

API Overview

The main API that users are expected to interact with is node.declare_parameter(...) which returns a parameter builder object that can be used to set its properties before finalizing it and declaring the parameter.
The lifetime of the parameter is tied to the object returned by the declaration. A parameter can only be declared once and once the returned object is destroyed it will be automatically undeclared.
The returned object depends on the type of parameter and offers different APIs.

Three types of parameters have been implemented:

  1. Mandatory, built through node.declare_parameter(name, default_value).[options].mandatory(). This is a parameter that must always have its value set. Its setter must provide a value and its getter will always return a value.
  2. Optional, built through .optional() builder function. Differs from the mandatory since it accepts an Option<T> as a default value and can be set to None. The getter as a consequence will return an Option<T> as well.
  3. ReadOnly, built through .read_only() builder function. Cannot be changed and only offers a get API, making it actually impossible to edit.

Main features

Parameter typing and interaction

The main way users are expected to do operations on parameters is through the object returned by the builder and its associated getters and setters. This allows better compile time control over the type and less error handling to be done by the user, for example:

let int_param = node.declare_parameter("int_param", 10).mandatory();
// Getter will always return an i64
assert_eq!(int_param.get(), 10);
// Setter only accepts an i64
assert!(int_param.set(100).is_ok());

This is opposed to the parameter API in the other libraries that leaves the user to make sure the type returned by the getter is the expected type.

Of course there must be support for dynamic typing, but this still works with the proposed API where now users declare a parameter of enum type, like so:

let dynamic_param = node.declare_parameter("dynamic_int", ParameterValue::Integer(10)).mandatory();
// Setter now accepts any object of ParameterValue type
assert!(dynamic_param.set(ParameterValue::Double(1.0)).is_ok());

The dynamic nature of the parameter is implied from the user setting its value as an enum rather than a matching primitive type. This is in contrast to other client libraries that use a boolean flag for this purpose.

Parameter lifetime and declaration

As mentioned before parameters will be declared and undeclared automatically based on the lifetime of the object returned by the declaration.

Undeclared parameter API

Users can still access all parameters, declared or undeclared, through their name and value. However this is hidden behind a node.use_undeclared_parameters() that returns an interface that has the more familiar .set(name, value) and .get(name).
The idea is that users must explicitly express their intent to allow setting undeclared parameters in their code. When services are implemented they will behave accordingly when trying to set a non existing parameter.

No ParameterDescriptor argument

Other libraries such as rclcpp, use a ParameterDescriptor object as an input to the declaration function. However this is non ideal in several ways:

  • The message contains a type and a name. However these properties are already included in the declare_parameter function signature so they are unnecessary and silently ignored.
  • read_only is a boolean parameter which is saved as a parameter property. Every time a parameter is set the code needs to check it and return an error if it is read only. This is not allowed with the proposed API that doesn't offer a set function in the first place.
  • The descriptor contains both an int_range and a float_range field. This is unnecessary information and also a bit confusing if for example we are declaring a statically typed string parameter.

For such reason a builder type API has been implemented. The read_only and dynamic properties of the descriptor don't exist anymore and are embedded in the type as described in the previous paragraphs. The others are arguments of the builder. For example:

let range = ParameterRange {
    lower: Some(0),
    upper: Some(100),
    step: Some(10),
}

let int_param = node.declare_parameter("percentage", 0)
                    .range(range)
                    .description("Describes a percentage, only multiple of 10 are accepted")
                    .constraints("If the robot is of model X only values above 50 are acceptable");

Depending on the parameter type only a certain type of range can be set:

  • Integer param: the range(..) function will take an integer range as an argument.
  • Double param: the range(..) function will take a double range.
  • Dynamic typing parameter, the input will be ParameterRanges struct that contains both a float and an integer range.
  • Other types currently have an empty range function that does nothing.

More control over ranges

As shown partially above, ranges now are only specified when the correct parameter type applies. In addition, a few changes are present that might differ from how they work in other libraries.

There is an InvalidRange error that will be thrown if the user specifies an invalid range, like lower > upper or step <= 0. These are all allowed in rclcpp where:

  • If lower > upper, the only acceptable parameter values are equal to either lower or upper.
  • If step == 0 it will be interpreted as no step (we use Option for this).
  • If step < 0 it will be still used.

In my opinion these cases, especially the first one, are really not very intuitive and for this reason the proposal is to introduce an error that flags them out.

Furthermore, in the ROS interface, if a range is specified its upper and lower bounds must both be specified, while the step is optional (where step = 0 means no step specified).
The proposal here is to also allow users to specify only one of lower or upper bound, these can be trivially translated to the interface messages by using the float infinity or integer max value.

Easily clonable ParameterValue

The ParameterValue variants that were not wrapping primitive types and needed heap allocations have been wrapped by an Arc<[]> type. This allows both less expensive parameter.get() calls, since they return a cloned value, and guarantee immutability outside of the parameter.set(value) functions.
Extra care has been put to make the parameter declaration behavior as ergonomic as possible and avoid forcing users to add Arc::from(type) or .into() in all their calls.

Extensive and partially configurable error reporting

Parameter declaration is a complex subject and each library has some discretion over what is acceptable and what is not. In this implementation the following always fail:

  • Declaring a parameter with a range that is not valid (as described above).
  • Declaring a parameter that was already declared.
  • Declaring a parameter of a certain type but its override was of a different type.
  • Declaring a parameter with a certain range but its override is out of range.
  • Declaring a parameter with a range and a default value but its default value is out of range.

In addition, we use an explicitly different storage for declared and undeclared parameters, upon declaration if there was already an undeclared value the following could happen:

  • Previously undeclared value is of a different type than what declared.
  • Previously undeclared value is out of the declared range.

This API defaults to ignoring the value set through the undeclared interface if any of these happens. The idea is that at the declaration level the user knows exactly what their parameter should contain. Furthermore if declaration failed it could be tricky for users to reset the parameter.
Still, there is an API to make these cases an explicit error rather than a silently drop behavior, as such:

// Note the wrong type
node.use_undeclared_parameters().set("float_param", "foo");
// This returns a type mismatch error.
assert!(node.declare_parameter("float_param", 1.0).tentative().mandatory().is_err());
// This silently overrides the type and returns a parameter with current value = 1.0
let float_param = node.declare_parameter("float_param", 1.0).mandatory().unwrap();

luca-della-vedova and others added 30 commits July 31, 2023 16:16
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Co-authored-by: jhdcs <48914066+jhdcs@users.noreply.github.com>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Copy link
Collaborator

@maspe36 maspe36 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not the most familiar person with ROS 2 parameters, but I really like the ergonomics of this impl.

Thanks for the great work!

@nnmm
Copy link
Contributor

nnmm commented Oct 15, 2023

However, the ParameterDescriptor message that is returned from the /describe_parameters service has a range field so we need to be able to translate whatever constraint the user set to a range object that can be returned to the service call. From my understanding this translation is immediate if Range is a separate object but would be trickier if it is just an object that implements a trait and we share a bunch of Box<dyn ParameterConstraint> objects.

My understanding is that in rclcpp, the parameter callbacks are validators, and they don't fit the ParameterDescriptor message either, so it would be fine to have more general validation. My impression (without researching it really) is that this field is for informational purposes, for humans.

/// Describes the parameter's type. Similar to `ParameterValue` but also includes a `Dynamic`
/// variant for dynamic parameters.
#[derive(Clone, Debug, PartialEq)]
pub enum ParameterKind {
Copy link
Contributor

@nnmm nnmm Oct 15, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The need for this comes (1) from the case where the parameter is an DeclaredValue::Optional, because it can be derived from the ParameterValue itself in that case, and (2) from it being the source of truth for identifying dynamic parameters, right?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That's correct. Unset optional parameters and dynamic parameters are two cases where the type constraint cannot be inferred from just looking at what variant is active in the ParameterValue enum.

///
/// Returns a `Parameter` struct that can be used to get and set all parameters.
pub fn use_undeclared_parameters(&self) -> Parameters {
self._parameter.allow_undeclared();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where does this get reset? In fact, is it used at all?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In the current API, once undeclared parameters are allowed, they're allowed for the rest of the node's lifespan. It's hard for me to imagine a use case for undoing that allowance or even what the behavior should be to undo it.

In rclcpp this decision is set in stone at initialization, so I think it's reasonable for us to not offer a way to reverse it.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But I don't see the allow_undeclared atomic being read anywhere.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It has been added in anticipation of parameter services. When parameter services are implemented on top of this in the future, this flag will determine whether to reject parameter service requests on undeclared parameters.

Until then it's true that it doesn't have any effect, simply because we've designed the rclrs API in a way that makes it unnecessary to check this for rclrs API calls.

#[derive(Debug)]
enum ParameterStorage {
Declared(DeclaredStorage),
Undeclared(Arc<RwLock<ParameterValue>>),
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a really different usage of the words "declared" and "undeclared" than rclcpp uses imo. In rclcpp, there is no internal distinction between a declared and an undeclared parameter - some parameter either exists or it doesn't. The allow_undeclared_parameters flag just causes all parameter overrides to create/declare parameters automatically, instead of acting only as defaults for declare_parameter(). By contrast, you have a user-facing distinction between the two and do not implement allow_undeclared_parameters, right?

The PR in rclcpp that introduced the "declaration" concept was ros2/rclcpp#495.

To reply to @mxgrey's comment on the rationale for the distinction:

The gist is that declared parameters carry a lot more information with them. When a parameter is declared it will have some type (we say kind in the implementation to avoid keyword collision) and may have a range, description, or other properties. An undeclared parameter does not have any of that.

I imagine in rclcpp they probably just have "sensible default" values for all that extra information to store declared and undeclared parameters the same way. That makes sense in C++ where tagged unions are ... not pleasant (std::variant has terrible ergonomics). In this implementation we're taking an approach of "don't do/say more than what is explicitly needed". An undeclared parameter cannot have ranges, constraints, or descriptions because the user never provided any of those, so we should not pretend to have any of them in our implementation.

You could also have one parameter map, with parameter's fields for user-provided metadata being optionals, right?

rclrs/src/parameter.rs Outdated Show resolved Hide resolved
Comment on lines 273 to 281
impl ParameterVariant for ParameterValue {
type Range = ParameterRanges;
fn maybe_from(value: ParameterValue) -> Option<Self> {
Some(value)
}

fn kind() -> ParameterKind {
ParameterKind::Dynamic
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are there good use cases for dynamic parameters? This functionality always seemed like an antipattern to me, so I left it out from my prototype to keep complexity smaller, at least initially.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I... Honestly don't know of any real use case but I thought I would add the functionality since it is present in the other libraries.
It is also part of ParameterDescriptor and I thought we should have an implementation for all the fields in the message so when we implement services and users call /describe_parameters we have an implementation for all the features in the message fields.
The nice aspect of the proposed implementation is that it is just treated as a different enum value and not as a boolean flag that we need to check all over the place so it doesn't really clutter the code or introduce potential errors too much. If there is consensus to remove it I'm happy to do so though

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think dynamic types should be kept. I can imagine dynamic types being used to represent variants, especially if a future PR introduces the "parameter struct" concept that was discussed in the last WG meeting. Dynamic parameters would theoretically allow us to support enum parameter structs. That allows json/yaml structures with schemas to be converted into ROS parameters more completely.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I feel that if the other client libraries are including dynamic parameters, we probably should as well. At least initially. Especially since this doesn't seem to add too many issues at first glance.

In future, we can probably gather some more data to see whether or not this is a useful feature for the client libraries in general.

But I will agree that dynamic parameters feel a tad spooky to me as well. And if it weren't for the fact that the other libraries have them, I'd leave them out as well. But, for now, I think we should leave this in.

type Range: Into<ParameterRanges> + Default + Clone;
/// Attempts to convert `value` into the requested type.
/// Returns `Some(Self)` if the conversion was successful, `None` otherwise.
// TODO(luca) should we use try_from?
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, we should

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I gave it a try but I struggled a fair bit with the error type and it ended up being a lot more verbose without really adding much (I could only get it to work by converting the errors to options through ok()) 25cd2a8. The main part I struggled with is the fact that the trait TryFrom can have any type as its Error, so all the code that uses try_from(_) needs to be able to deal with this unless we constrain the type through a quite verbose bound.
Happy to refactor / revert it, or add the trait bounds, I find them a bit tough to get right.

rclrs/src/node.rs Outdated Show resolved Hide resolved
@mxgrey
Copy link
Collaborator

mxgrey commented Oct 15, 2023

You could also have one parameter map, with parameter's fields for user-provided metadata being optionals, right?

It's true that there are many valid ways to structure the internals of the parameter map. The question really boils down to what reads most clearly to the maintainers (the perspective of users doesn't even matter since this is just implementation detail that they can't touch).

Our feeling was that a declared/undeclared split is the most clear because it allows a clear path to upgrade an undeclared parameter into a declared parameter if the user asks for it. There are two important differences between declared and undeclared parameters that make this relevant:

  • The user holds a concrete reference to each declared parameter whereas undeclared are stored entirely in the parameter map.
  • You cannot redeclare a parameter that has already been declared (unless the original declaration has been dropped). On the other hand there is nothing wrong with upgrading an existing undeclared parameter to declared.

If the implementation does not have a clear distinction between which parameters are declared or undeclared then implementing those behaviors becomes less obvious (but certainly not impossible).

We are also able to do a slight optimization for undeclared parameters because they do not need to be shared or modified from outside of the parameter map, but I don't think that minor optimization is a strong argument for sticking to the current implementation if another arrangement can improve the clarity of the implementation.

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
@luca-della-vedova
Copy link
Collaborator Author

However, the ParameterDescriptor message that is returned from the /describe_parameters service has a range field so we need to be able to translate whatever constraint the user set to a range object that can be returned to the service call. From my understanding this translation is immediate if Range is a separate object but would be trickier if it is just an object that implements a trait and we share a bunch of Box<dyn ParameterConstraint> objects.

My understanding is that in rclcpp, the parameter callbacks are validators, and they don't fit the ParameterDescriptor message either, so it would be fine to have more general validation. My impression (without researching it really) is that this field is for informational purposes, for humans.

I suspect the field you are referring to is additional_constraints here, which indeed is just a human readable string to describe complex constraints. I was referring to the ranges at the end of the file.

There are different types of validation happening in rclcpp and other libraries.
There are parameter callbacks that, as you say, are general purpose and don't fit the ParameterDescriptor message. I agree we should implement them but we can do it in a followup PR.
Parameter ranges present in the descriptor message, these are separate from the user provided callbacks and set at declaration level when passing the ParameterDescriptor argument to declare_parameter. When a parameter is set (or declared) the range check is made here and only if it is successful the user provided callback is called afterwards.

I agree that in general having a single place for constraints and unifying both ranged and callbacks into a single callback would be cleaner, but that would make it impossible for the /describe_callbacks to distinguish the two.

luca-della-vedova and others added 5 commits October 17, 2023 00:48
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Michael X. Grey <grey@openrobotics.org>
Signed-off-by: Michael X. Grey <grey@openrobotics.org>
@mxgrey
Copy link
Collaborator

mxgrey commented Oct 18, 2023

I just pushed some substantial changes, some of which address feedback we've received and others which tie up some loose ends. Here's a summary of the changes.

One entry point for building parameters

We previously had Node::declare_parameter, Node::declare_optional_parameter, and a few other signatures that can be used to kick off parameter declaration in different ways depending on how you want to pass in (or not pass in) a default value. All these different function signatures have been merged into a single Node::declare_parameter signature where the default value is set with the builder itself.

Now setting a default value is always optional, but .mandatory() and .read_only() will give errors if there is no available initial value, i.e. there is no default value, override value, or previously set value to use as an initial value for the parameter.

Initial value discriminator

For any declaration there are up to three possible values the parameter could be initialized to:

  • default value
  • override value
  • prior value (set while the parameter was undeclared)

Initially we thought we would provide a variety of flags to the builder to determine which value to use as the initial value in any given declaration. For example, .ignore_override() would prevent the override value from being used. We could provide another flag to prevent a prior value from being used. Then another flag could say that prior values should be preferred over default values but less than override values. Then ... on and on until we have too many flags to comprehend.

Instead we have settled on allowing users to provide a custom discriminator function. The discriminator would have a signature FnOnce(AvailableValues<T>) -> Option<T> where

pub struct AvailableValues<T> {
    /// The value given to the parameter builder as the default value.
    pub default_value: Option<T>,
    /// The value given as an override value, usually as a command line argument.
    pub override_value: Option<T>,
    /// A prior value that the parameter was set to before it was declared.
    pub prior_value: Option<T>,
    /// The valid ranges for the parameter value.
    pub range: ParameterRanges,
}

There is a default discriminator whose behavior is to prefer prior_value over override_value and prefer override_value over default_value.

We could provide a variety of out-of-the-box discriminators for whatever uses cases we expect will be relatively common if anyone has opinions on what those should look like.

We still provide .ignore_override() for the sake of convenience and familiarity. When .ignore_override() is used, AvailableValues::override_value will always be set to None.

Arc<str>

Parameter names are now stored as Arc<str> which will be much more memory and CPU efficient. Since parameter names are immutable there was never a benefit to storing them as String.

luca-della-vedova and others added 7 commits October 19, 2023 05:42
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Michael X. Grey <grey@openrobotics.org>
…-vedova/ros2_rust into luca/parameter_services
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
@esteve
Copy link
Collaborator

esteve commented Oct 25, 2023

@luca-della-vedova thanks for addressing my feedback. I'll have another look, but I'm leaning towards just merging this PR and iterate.

@esteve
Copy link
Collaborator

esteve commented Oct 26, 2023

@jhdcs @nnmm any more feedback? Unless there's a huge blocker, I'd like to merge this before the week ends. Any improvements can go in follow-up PRs.

Copy link
Collaborator

@jhdcs jhdcs left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have no further comments.

@esteve esteve merged commit 7d11dc7 into ros2-rust:main Oct 27, 2023
3 checks passed
@esteve
Copy link
Collaborator

esteve commented Oct 27, 2023

@luca-della-vedova @mxgrey thank you so much for all the hard work and @jhdcs @maspe36 @nnmm thank you for the thorough reviews

@esteve esteve mentioned this pull request Nov 7, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

7 participants