[binary filter] change boolean ros params

[enricosutera]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3511
Primary OS tested on	Ubuntu 22.04, Rolling (debian)
Robotic platform tested on	None

---

Description of contribution in a few bullet points

• The binary filter is now able to change boolean ros parameters using a service. The parameters can be changed accordingly to filter binary value or opposite to it (this can be specified for each parameter).

Description of documentation updates required from your changes

• Added new parameters for the binary filters, documentation ([binary filter] change boolean ros params documentation docs.nav2.org#460)

Future work that may be required in bullet points

• Not required, but an extension or another filter could be developed to change also double parameters dynamically.

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[SteveMacenski]

I think also this entire thing could use a single https://docs.ros2.org/bouncy/api/rclcpp/classrclcpp_1_1_async_parameters_client.html, having the individual /set_parameter clients seems not great

[SteveMacenski]

What if we have a namespace applied?

[enricosutera]

I can removed the absolute slash ("/" +) to make it automatically handle namespaces, but I guess then there would be an issue with /local_costmap//global_costmap sub-namespace of the costmap, which - I think - is the same reason why you use /scan (instead of scan) in the obstacle layers (using a relative source, e.g. scan creates a subscription to /global_costmap/scan).

I mean, I kept it absolute to have more control. This way a user can add the namespace in the params file, while if the namespace would be handled automatically a user would need to do a remap.
What do you suggest?
I think something like this (from costmap_2d_ros would solve it:

     "map_topic", rclcpp::ParameterValue(
      (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map")));

But I don't have such information (parent_namespace_) at costmap filter level

[enricosutera]

To be honest I'm not sure about how I could solve this... As I said, the filter has not the access level to distinguish between the two namespaces, so only workarounds come to my mind (e.g. the only two sub_namespace I can image for these filter are global_costmap and local_costmap, so I could simply cut them from the service name), but I don't think that having earthbound reasoning is acceptable

[AlexeyMerzlyakov]

What will be happened, if you will just remain the

 auto change_parameters_client_ = node->create_client<rcl_interfaces::srv::SetParameters>(
      param.node_name + "/set_parameters");

without leading "/"?

• If param.node_name is specified in absolute path (e.g. /some_node) will be will applied as /some_node/set_parameters service.
• If param.node_name is specified in relative path (e.g. some_node), won't service will be created relative to current BinaryFilter namespace: some_node/set_parameters which will be treated as /my_namespace/some_node/set_parameters when operating with it in the run-time?

[SteveMacenski]

Perform wait for service here to make sure up before continuing

[enricosutera]

Did, I have to figure out what do if the service is not available (comment here)
496ce37

[SteveMacenski]

Remove wait for service here

[AlexeyMerzlyakov]

This wait for service cycle seems to be moved to the end of BinaryFilter::initializeFilter()

[enricosutera]

Also related to previous comment:

Perform wait for service here to make sure up before continuing

Do you mean that if any service goes timeout at the beginning we should not add it or just give an error/warning? What if the server is temporary offline (e.g. from a plugin)?
Also, a node could go offline at a certain point and since the service is checked only at initialization time, there would not be information for the user to understand that parameters are not updated.
I understand that adding the wait_for_server slows down the execution, but in my opinion it should be the user to keep attention to the selected parameters (e.g. too many "offline" servers).

[AlexeyMerzlyakov]

Once the BF is being configured, we need to check that services/nodes are available on that stage. Later on we are assuming that everything is OK, and we are working with bringed-up nodes. If something goes wrong (e.g. node became offline), service request should fail by timeout. For that, see Steve comment from above and please use spin_until_future_complete(future_result, change_parameter_timeout_) API to not have infinite loops. Anyway, loosing the service with the node - is extraordinary situation, which could be handled by other than each time checking for service availability way (performance for usual serivce calls in this way will be faster, and we still could handle error situations).

[enricosutera]

Hi there. I agree with using spin_until_future_complete(future_result, change_parameter_timeout_). However to me it's still not clear what should the filter do if a node is offline. I mean, service request fail by timeout and then? Should I throw an error? While log seems not much substantial, as I was saying this seems to be a little bit much. What if a node is not active yet or has just crashed and will be up again in a few seconds?
Anyway, it is only a matter of choices. If you want this to work only with nodes that are always up, it's ok!
By the way I could also add another parameter to somehow distinguish between "always-up" nodes and "discontinuous" ones (for instance plugin). Or adding a "global" timeout (not for the service request timeout but for the node in general, for instance 10 seconds timeout after the first service request timeout... this would implies some re-call logic, even when there's no binary state change). Or simply logging but also exposing error information for any third part diagnostic nodes... I really don't know, just shooting ideas

[AlexeyMerzlyakov]

There is no need to add global timeouts and try to re-send the service again. This should be a responsibility of DDS implementation, and if some node is not online for some reasons (e.g. bad network connection), DDS to re-try to deliver the request to the given node within a timeout. So, there is no need to do the same job twice.

Regarding throwing an exception, this is a good question. Changing the state of node - is a serious situation that might break full logic working mechanisms on the end-developer side. Imagine, if you have some handling "NodeA" that is having two sub-algorithms: robot local planning and hand manipulation. By moving into some marked area, the BinaryFilter decides to stop the robot and send a binary state to "NodeA" to switch algorithm from navigation -> to hand manipulation. If this signal won't be delivered to "NodeA", it will continue to working as previously, so the normal operation logic will break.
Additionally, this situation could be treated as ROS-parameter setting error, which is also always going with exceptions throwing.

So from this, I'd more inclined that we need to throw an error in service failure situation (and thus, stop normal BF execution).

[enricosutera]

I mean, I agree a log is not enough, while an exception may be too much in some cases. What do you think of adding a parameter for letting the user decide this one?

Each parameter can have a parameter for this: something like a boolean critical (default True).
Any suggestion on the name is appreciated! It could be reverse also, like is_optional (default False).
Configuration would be:

    ParamNamespace:
      node_name: "node"
      param_name: "param"
      default_state: True
      is_critical: False

Hence the code should look like this:

    if (!change_parameters_client->wait_for_service(
        std::chrono::milliseconds(change_parameter_timeout_)))
    {
      if (param.is_critical){
        throw std::runtime_error("BinaryFilter: Service " + 
          std::string(change_parameters_client->get_service_name()) + 
          "not available!");
      }
      RCLCPP_ERROR(
        logger_, "BinaryFilter:  service %s not available. Skipping ...",
        change_parameters_client->get_service_name());
    } else {
      RCLCPP_INFO(
        logger_, "BinaryFilter:  service %s available.",
        change_parameters_client->get_service_name());
    }
  }

We could also think of having just one global parameter for the filter, without giving the chance to specify the behavior for each parameter, but I don't see great advantages a part from having less parameters.

What do you think?

[AlexeyMerzlyakov]

What is the example when we have the parameter of some node not changed, but we are still normally operating? Is this worth to skip this node in the config file at all, which is not reliable and not necessary for us?
I doubt about such situation: imagine - you have some node which might be offline or online. And you can not guarantee it's parameters consistency. Is it real worth to support? In this case, I believe it is better to throw to developer an exception causing an error, allowing the developer to remove this unreliable node from config at all. Since the node is unreliable, it's binary param won't be guaranteed to be consistent, and thus I do not think, this is normal situation.

In other words, not-changing / not-setting of ROS parameters - is already a critical situation which throws and error in ROS2 API. For these cases, we could compare setting the parameter through a service request and thus it could be treated with exception.
Adding additional parameter to choose throw exceptions or not for the unreliable nodes, in my opinion - over-complicating the structure, since we are still could not rely on this flaky node parameter, so why don't throw error instead in all times?

[SteveMacenski]

Remove callback. Spin until future complete. Then check the value and throw something more substantial than a log if it failed. Just a log failure isn't good enough

[enricosutera]

I guess it depends on what you think about this

[AlexeyMerzlyakov]

The main code has been checked. Please check the first Steve review and these items. After the code will be more-less stabilized, we could advance to next step with testcases and documentation verification.

[AlexeyMerzlyakov]

This wait for service cycle seems to be moved to the end of BinaryFilter::initializeFilter()

[AlexeyMerzlyakov]

@enricosutera, this is kind ping, since there are no news more than one month. Are there any updates on this PR?

[enricosutera]

I think also this entire thing could use a single https://docs.ros2.org/bouncy/api/rclcpp/classrclcpp_1_1_async_parameters_client.html, having the individual /set_parameter clients seems not great

@SteveMacenski
Honestly I'm not entirely sure what you mean. I still have to create a client for each node I think (the client constructor needs the remote_node_name), so at most I could create each time I need to change parameters a client and then destroy it, but I don't see the advantage. This Am I losing any piece of information?

[SteveMacenski]

@enricosutera good point, let me look at this again - but you have not yet responded to the bulk of the comments in my last review

[SteveMacenski]

But generally looks good! Much better now

[AlexeyMerzlyakov]

@enricosutera, thank you for the update and taking care on this. Started the review, but I'm running out-of-time today. Will complete it at the beginning of next week.

[AlexeyMerzlyakov]

Adding the review comments for the main code. There are some items remaining to fix from this and previous reviews.

[AlexeyMerzlyakov]

Once the BF is being configured, we need to check that services/nodes are available on that stage. Later on we are assuming that everything is OK, and we are working with bringed-up nodes. If something goes wrong (e.g. node became offline), service request should fail by timeout. For that, see Steve comment from above and please use spin_until_future_complete(future_result, change_parameter_timeout_) API to not have infinite loops. Anyway, loosing the service with the node - is extraordinary situation, which could be handled by other than each time checking for service availability way (performance for usual serivce calls in this way will be faster, and we still could handle error situations).

[AlexeyMerzlyakov]

There are a few items remained:

• Throw an exception if service was failed (discussable)
• Remove unnecessary calling of wait_for_service from changeParameters()
• Allow to use relative namespaces in ROS-parameters ([binary filter] change boolean ros params #3796 (comment))
• Switch to use non-default ROS-parameters instead of empty strings
• Future destroyed after end-of-loop question ([binary filter] change boolean ros params #3796 (comment))
• Other minor changes / nitpicks from the items below

Test code coverage is >95% for binary_filter.cpp which is OK (some exceptions were not covered):

[enricosutera]

There are a few items remained:

* Throw an exception if service was failed (discussable)

* Remove unnecessary calling of `wait_for_service` from `changeParameters()`

* Allow to use relative namespaces in ROS-parameters ([[binary filter] change boolean ros params #3796 (comment)](https://github.com/ros-planning/navigation2/pull/3796#discussion_r1317581467))

* Switch to use non-default ROS-parameters instead of empty strings

* Future destroyed after end-of-loop question ([[binary filter] change boolean ros params #3796 (comment)](https://github.com/ros-planning/navigation2/pull/3796#discussion_r1372142376))

* Other minor changes / nitpicks from the items below

Test code coverage is >95% for binary_filter.cpp which is OK (some exceptions were not covered):

@AlexeyMerzlyakov All the above points are satisfied a part from these two:

1. Just waiting for an answer on this
2. Concerning this:

* Allow to use relative namespaces in ROS-parameters ([[binary filter] change boolean ros params #3796 (comment)](https://github.com/ros-planning/navigation2/pull/3796#discussion_r1317581467))

have you read my comment? Do you have any suggestion?

[AlexeyMerzlyakov]

@enricosutera, apologies for late respond: I was overkilled with other activities on prev. two weeks. Please refer to my comments on remaining two questions (exceptions and namespaces in service calls) from above

[codecov]

Codecov Report

Attention: Patch coverage is 73.77049% with 16 lines in your changes are missing coverage. Please review.

Project coverage is 89.84%. Comparing base (0be2f25) to head (aa40776).

Files	Patch %	Lines
...stmap_2d/plugins/costmap_filters/binary_filter.cpp	73.77%	16 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3796      +/-   ##
==========================================
+ Coverage   89.75%   89.84%   +0.09%     
==========================================
  Files         431      431              
  Lines       19571    19632      +61     
==========================================
+ Hits        17566    17639      +73     
+ Misses       2005     1993      -12     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[SteveMacenski]

@enricosutera is it your intent to finish up this PR? I generally don't like to leave things hanging like this indefinitely

[enricosutera]

Thank you for pinging. Yes I do. I will give a last shot asap

[codecov]

Codecov Report

Attention: Patch coverage is 90.45139% with 110 lines in your changes are missing coverage. Please review.

Files	Coverage Δ
nav2_amcl/src/pf/pf.c	91.66% <ø> (ø)
nav2_amcl/src/pf/pf_vector.c	100.00% <100.00%> (ø)
..._amcl/src/sensors/laser/likelihood_field_model.cpp	97.50% <100.00%> (ø)
...nclude/nav2_behavior_tree/behavior_tree_engine.hpp	100.00% <ø> (ø)
...tree/include/nav2_behavior_tree/bt_action_node.hpp	85.82% <100.00%> (ø)
...ee/include/nav2_behavior_tree/bt_action_server.hpp	83.33% <ø> (ø)
...clude/nav2_behavior_tree/bt_cancel_action_node.hpp	80.64% <100.00%> (ø)
...avior_tree/include/nav2_behavior_tree/bt_utils.hpp	100.00% <100.00%> (ø)
...ior_tree/plugins/action/assisted_teleop_action.hpp	100.00% <ø> (ø)
...v2_behavior_tree/plugins/action/back_up_action.hpp	100.00% <ø> (ø)
... and 144 more