[Binary Filter] make it able to change boolean ros parameters

[enricosutera]

Hi, what about having the costmap binary filter (or a new one) capable of setting ros parameters along with publishing on a topic? Some possible use cases:

• enabling or disabling an obstacle layer source marking or clearing in a specific environment (e.g. infrared proximity sensors in a black environment)
• enabling or disabling full costmap layers (e.g. when going from indoor to pedestrian area).

I realize this can easily be done creating a third part node that subscribes to the currently available topic and modifies the parameter itself or directly having the callback in the node to which the parameter belongs. However I think this would be more flexible and decoupled to have all this is a configuration file!

[SteveMacenski]

@AlexeyMerzlyakov

[AlexeyMerzlyakov]

From first sight, it better to be done from existing binary filter adding new param_name string filed, where the full parameter name to be specified. There could be two options: O1. We have the pointer to the Costmap2DROS node inside each plugin, so we could try to call node_->set_parameter("param_name", {true/false}) directly O2. Use rcl_interfaces::msg::SetParameters.srv service-call to set binary parameters for any node, we want.

I see second option is more universal from the perspective of further use-cases coverage.

---

Continuing the thoughts: maybe we want to cover not only boolean values, but also integer/doubles as well. Encoded in raw values mask data to be linearly converted into resulting value as: res = mul * raw + base. So, this might be useful to dynamically set integer/double ROS-parameters in a filter as well. However, I could not find any suitable use-case of it today. Any idea, do we really need this, or better to stop only on ROS2 boolean-parameters would be appreciated :wink:

[enricosutera]

I'd also go with O2 because there would be the flexibility to change parameters also outside the costmap2D.

For what concerns non boolean values, I agree it could be useful, for instance speed limit filter could be replaced as long as max speed is a parameter changeable dynamically. Anyway this could be done later on!

I can work on this btw

[AlexeyMerzlyakov]

@enricosutera, thank you for taking the interest in this topic! Agree with O2 strategy to use service name as ROS-param for the Binary Filter. The PR is welcoming :blush:

[SteveMacenski]

Any update? Its been about a month!

[enricosutera]

Hi! Unfortunately I haven't a solution ready. I had very little time and I have an issue with using the .../set_parameters service. Service calls fail (I did it with rclcpp first, but it's the same with ros2cli):

ros2 service call /global_costmap/global_costmap/set_parameters_atomically  rcl_interfaces/srv/SetParametersAtomically   '{parameters: [{name: "static_layer.enabled"}]}'
requester: making request: rcl_interfaces.srv.SetParametersAtomically_Request(parameters=[rcl_interfaces.msg.Parameter(name='static_layer.enabled', value=rcl_interfaces.msg.ParameterValue(type=0, bool_value=False, integer_value=0, double_value=0.0, string_value='', byte_array_value=[], bool_array_value=[], integer_array_value=[], double_array_value=[], string_array_value=[]))])

response:
rcl_interfaces.srv.SetParametersAtomically_Response(result=rcl_interfaces.msg.SetParametersResult(successful=False, reason='cannot undeclare a statically typed parameter'))

I have no idea why setting a parameter would require to undeclare it, anyway I think this is related to this old issue https://github.com/ros2/rclcpp/issues/1762

Also I've tried using rclcpp::SyncParametersClient as in the demos But again I encountered known issues.

Naturally It works fine using ros2 param so I'll probably take a look at how it operates

[AlexeyMerzlyakov]

@enricosutera, the reason of such issue is seems that you are not specifying the value of parameter, and this parameter is treated to be unset. As I understand, the intention is to make boolean True/False trigger support which could be specified directly via boolean type of ROS-parameter. Something like:

ros2 service call /global_costmap/global_costmap/set_parameters_atomically  rcl_interfaces/srv/SetParametersAtomically   '{parameters: [{name: "static_layer.enabled", value: {type: 1, bool_value: False}}]}'
ros2 service call /global_costmap/global_costmap/set_parameters_atomically  rcl_interfaces/srv/SetParametersAtomically   '{parameters: [{name: "static_layer.enabled", value: {type: 1, bool_value: True}}]}'

works for me

[enricosutera]

Thank you @AlexeyMerzlyakov , the issue was really that simple.

As for configuration, does something like this make sense for you? I'm not sure whether to handle parameters separately or grouped by threshold.

Option 1: custom default state and flip threshold for each parameter

      binary_filter:

        plugin: "nav2_costmap_2d::BinaryFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
        transform_tolerance: 0.1
        default_state: False
        binary_state_topic: "/binary_state"
        flip_threshold: 50.0
        parameters: Param0 Param1 Param2
        Param0:
          node_name: "node_0"
          param_name: "param.sub_param"
          default_state: False
          flip_threshold: 50.0
        Param1:
          node_name: "node_1"
          param_name: "param.sub_param"
          default_state: True
          flip_threshold: 25.0
        Param2:
          node_name: "node_2"
          param_name: "param.sub_param"
          default_state: False
          flip_threshold: 75.0

Option2: common flip_threshold (in this case one should define two different istances of the filter in case different threshold were desired)

      binary_filter:

        plugin: "nav2_costmap_2d::BinaryFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
        transform_tolerance: 0.1
        default_state: False
        binary_state_topic: "/binary_state"
        flip_threshold: 50.0
        parameters: Param0 Param1 Param2
        Param0:
          node_name: "node_0"
          param_name: "param.sub_param"
          default_state: False
        Param1:
          node_name: "node_1"
          param_name: "param.sub_param"
          default_state: True
        Param2:
          node_name: "node_2"
          param_name: "param.sub_param"
          default_state: False

An alternative could be grouoing by threshold (that could correspond to map "zones"):

      binary_filter:

        plugin: "nav2_costmap_2d::BinaryFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
        transform_tolerance: 0.1
        default_state: False
        binary_state_topic: "/binary_state"
        flip_threshold: 50.0
        parameters: Thresh0 Thresh1
        Thresh0:
          nodes:       [node1,   node1,    node2]
          parameters: [param1,param2,  param1]    
          default_state: False
          flip_threshold: 50.0    
        Thresh1:
          nodes:       [node1,   node2,    node2]
          parameters: [param3,param2,  param3]    
          default_state: False
          flip_threshold: 60.0    

[AlexeyMerzlyakov]

I initially thought about one parameter per one binary filter instance. However, it is reasonable to have many parameters changing simultaneously, e.g. for the case of many costmap layers disabling/enabling in some areas on maps.

By its meaning, Binary Filter is having one threshold and to flip its state (and will flip parameters state as well). However, I do not see any reason or use-case to have multiple threshold in one Binary Filter. Otherwise, if we still need two parameters, each is switching by its own threshold, I think, using two instances of Binary Filters (each one tuned on its own threshold) on one filter mask will be more reasonable. Binary Filter - by its design is something simple, that flipping to the true/false when mask value is above or below this threshold. We could allow to think about different default states per each flipped parameter (e.g. when one layer is enabled, and another is disabled at the same time). But I'd like to consider the threshold to be something as an integral part of whole Binary Filter instance (otherwise, we will loose the understanding why do we need global BF's threshold, if each of sub-parameter has it own, and thus can not use binary state published by BF, say for debugging purposes).

The parameters could be just listed in parameters array, so the situation will be look like follows:

    binary_filter:
      plugin: "nav2_costmap_2d::BinaryFilter"
      enabled: True
      filter_info_topic: "/costmap_filter_info"
      transform_tolerance: 0.1
      default_state: False
      binary_state_topic: "/binary_state"
      flip_threshold: 50.0
+     binary_parameters: ["param1", "param2"]
+     param1:
+       node_name: "node0"
+       param_name: "name.of.param1"
+       default_state: True  # Optional parameter. If not set - using BF's default_state
+     param2:
+       node_name: "node1"
+       param_name: "param2"

[enricosutera]

i agree that it should stay simple.

For what concerns tests, what do you suggest? I'd add a a simple test in which the binary filter changes its own params. The rest of the logic should be changed. Anything else?

Btw here's the branch

[AlexeyMerzlyakov]

I've checked the local branch. In overall, the approach seems OK for me.

Regarding the testcases, I would make the configuration with {binary_filter + node1 + node2}, when binary_filter changes node1.binary_param and node2.binary_param binary parameters. The first node parameter default value might be missed and thus, be equal to filter' default value. Second parameter's default value - to set explicitly (e.g. binary_filter_->setParam("param2.default_state", False)). Then check that parameters for these nodes were set correctly. Also, I would like to add the testcase for the situation, when one of the nodes is not running, or not created - to check the wait_for_service() verification logic; and the testcase when one of the parameters has no declared (say, send node1.incorrect_param change request) - to check errors handling situation.

That is my point of view, so you don't have to strictly follow it, and could propose your scenario of the testcases. The main intention here - is to cover added functionality and logic by new testcases.

[enricosutera]

I'm ok with what you said:

1. basic flow: change two params from two nodes
2. basic flow 2: test that filter default value is used if no param default is given
3. error control: one node is offline and the other is not -> skip the offline and change the online one
4. error control: target nodes exist but the parameter does not. I expect this to give an error (?)

To be honest you raised a nice "question". One might have made a mistake with params rather than the target node being offline. What do you suggest to do in that case? I think best option is to just log it once, at the beginning to easily let find any error.

[SteveMacenski]

What's the story here? Personally, I think the 3rd party node to take the binary flip to set a parameter makes sense to me. By the time you're setting a bunch of thresholds, that's not really a "binary" filter anymore. It might make sense to simply create a new costmap filter layer to handle param setting on value triggers.

[enricosutera]

In the end we have only one threshold for the filter, however other than publishing the state in a topic it can change some configured boolean parameters. Maybe I was not clear? Generally I don't see great advantages in having another filter just to bridge the boolean topic into services. The only case in which I could find that useful is if there's a third part boolean topic, maybe not based on localization (but I don't really have an example in mind)

[AlexeyMerzlyakov]

What do you suggest to do in that case? I think best option is to just log it once, at the beginning to easily let find any error.

In both cases: when the target node off-line or when the node has no such parameter declared, the service will return back some kind of failure response. I think, the filter is needed to log an error and continue its work, since the target node and/or parameter might be dynamically set later.

In the end we have only one threshold for the filter, however other than publishing the state in a topic it can change some configured boolean parameters. Maybe I was not clear?

I think, one threshold per a filter should be enough (moreover, it is conceptually correct for the Binary Filter essence). If you want to have more than one thresholds, each one per each parameter, you can run multiple instance of binary filters. Each instance of BF is set it corresponding binary parameter(s) with necessary threshold.

Generally I don't see great advantages in having another filter just to bridge the boolean topic into services. The only case in which I could find that useful is if there's a third part boolean topic, maybe not based on localization (but I don't really have an example in mind)

I am not sure that understood your intention well. What kind of bridge do you mean? We might have another instance of binary filter, if we want to, trigger another binary parameter with another threshold. Or I could imagine, if we want to use second instance of BF to add the second threshold to the same parameter. Let's say:

[False ... 0.1 ... True ... 0.5 ... False]
            ^                ^
            |                |_ Handled by first BF instance 
            |_ Handled by second BF instance

But I am not aware of real applications or use-cases of such scheme.

[enricosutera]

I think, one threshold per a filter should be enough (moreover, it is conceptually correct for the Binary Filter essence). If you want to have more than one thresholds, each one per each parameter, you can run multiple instance of binary filters. Each instance of BF is set it corresponding binary parameter(s) with necessary threshold.

I'm ok to have just one threshold, I was answering to Steve.

I am not sure that understood your intention well. What kind of bridge do you mean?

I was referring to the 3rd parts node Steve mentioned.

Generally speaking I think the version we've come up with makes sense and it's simple:

• Only one threshold per filter
• Each parameter can have a configurable default value
• Unreachable service or call failures (e.g. wrong param name) lead to log errors but the filter keeps working and will try again later.

[AlexeyMerzlyakov]

I was referring to the 3rd parts node Steve mentioned.

I think Steve talked about 3rd party node, as a some other node where we set the variable, in agreement with what was suggested.

So, we are should be on the same page, if I understood the intention correctly.

[enricosutera]

Hello there,

In the end I have these tests:

• TEST_F(TestNode, testUnchangedBinaryParams) No flip of binary state happens, I check the parameters have not changed.

• TEST_F(TestNode, testChangedBinaryParams) Flip of binary state happens, I check the parameters have changed.

• TEST_F(TestNode, testUnsetDefaultBinaryParams) Specific default state is not set for target parameters. Flip of binary state happens, I check the parameters have changed accordingly to filter default state.

• TEST_F(TestNode, testWrongBinaryParamNode) Wrong node name is provided for one of the parameter to be changed. Flip of binary state happens, I check the parameter of the node with the wrong name has not change, while the other has. Shall I had some check on time elapsed here? Or is the CmakeList.txt timeout enough?

• TEST_F(TestNode, testWrongBinaryParamName) Same as above but instead of the name of the node, the param name itself is wrong.

The test set is here

If that's ok I'm going to create the PR and update the docs.

[AlexeyMerzlyakov]

I've checked local branch code, and left my 0-review comments at the below. Please check them and after that, create a new PR: overall approach seems to be good.

The review comments:

1. Please follow the Nav2 formatting rules (you can run colcon test --packages-select nav2_costmap_2d and check the cpplint/uncrustify results). Also, clear extra blank lines, spaces, etc...

2. Please check the residual and unused code, e.g:

   • std::vector<std::string> BINARY_PARAMETERS is unused
   • bool use_parameters_{false}; is unused
   • private: empty and thus not needed for ParamsNode class
   • params_executor_* is also seems to be non needed

3. I think, createNodeWithParams should be renamed to reflect more its meaning: create two separate nodes. Maybe, createParameterNodes() or createNodesWithParams(). Following the same logic, I believe addBinaryParams() is an addBinaryParam()?

4. addBinaryParams() logic is not clear. If the Param1/2 is a namespace, it better to be parametrized as const, same as NODE_NAME_* and PARAM_NAME_*. It should be ordered as second argument, after node name, since we have the following logic: /node_name/param_namespace/param_name. In this case, values are also should be equal to something like namespace1/2 instead of Param1/2 which leads to a confusion.

5. How about to rename testUnsetDefaultBinaryParams -> testDefaultChangedBinaryParams for better clarity? Word "Unset" looks like parameter was intentionally unset, but this is not how this testcase works.

Shall I had some check on time elapsed here? Or is the CmakeList.txt timeout enough?

I am not sure, what do you mean about CMakeLists.txt timeout? The testcase in its normal operation shouldn't operate in this timeouts (around 180 seconds, as I recall). This is not a normal operation.

Isn't the testWrongBinaryParamNode testcase will work around 100ms used to wait in verifyBinaryParams(). Or testcase time will be conditioned by BinaryFilter process() time, which will be related to change_parameter_timeout (10 seconds default). If so, please set this parameter in testcase before, to lesser values (say, 500ms) in order to get whole test to operate fast enough.

7. In testWrongBinaryParamNode, rename wrong_name -> wrong_node_name

[AlexeyMerzlyakov]

@enricosutera, are there any updates on this activity?

[enricosutera]

Not really, I've done little progress due to lack of time. I hope to give it a last shot in these days

[enricosutera]

I made the corrections!

Concerning the timeout thing, just a note: change_parameter_timeout default is 10ms, not 10s, So it should be ok. I can increase a little bit the timeout if it's too low.

Shall I open the PR?

[AlexeyMerzlyakov]

Yes, please proceed with a new PR to check the concrete code to be reviewed before the merge

[dines1111]

hello, is it possible to run all binary, keepout and speed filters together simultaneously?