supporting multiple goal checkers #2108
Conversation
| @@ -90,7 +90,8 @@ controller_server: | |||
| min_y_velocity_threshold: 0.5 | |||
There was a problem hiding this comment.
before anything else, any API changes need to have update to the docs/parameters/params.md file for descriptions and such. Also those same updates should be added to the navigation.ros.org repo's configuration pages for the modified packages.
| @@ -90,7 +90,8 @@ controller_server: | |||
| min_y_velocity_threshold: 0.5 | |||
| min_theta_velocity_threshold: 0.001 | |||
| progress_checker_plugin: "progress_checker" | |||
| goal_checker_plugin: "goal_checker" | |||
| goal_checker_plugins: ["goal_checker", "precise_goal_checker"] | |||
| current_goal_checker: "goal_checker" # it his param is not specified "goal_checker" is selected by default | |||
There was a problem hiding this comment.
default_ should be a more appropriate prefix than current, also please replace the comment.
There was a problem hiding this comment.
Well, this pull request was an initial contact to show how it works (we can change many things).
Concerning replacing "current" with "default", I would agree if it was the "default goal checker", but in fact, that parameter is being used following the idea of "dynamic reconfigure server". That parameter could be updated at runtime. So that it is used to select the "current goal checker (it is not just the initial default goal checker).
Maybe you disagree with the idea of chaning the goal checker at runtime via ros2 parameters.What do you think about that idea?
There was a problem hiding this comment.
Lets discuss this in the comment below. When I made that request, I did not realize you were trying to use the parameter system dynamically. I don't think there's anything technically wrong about that strategy, but philosophically I don't think its going to have the best outcome for your needs.
Also, ROS2 has a concept of parameter update event callbacks. In this situation, you'll polling for parameter updates. If we were to go in this direction (which I'm not sure is the right move), it should be event based.
| @@ -104,6 +105,11 @@ controller_server: | |||
| xy_goal_tolerance: 0.25 | |||
| yaw_goal_tolerance: 0.25 | |||
| stateful: True | |||
| precise_goal_checker: | |||
There was a problem hiding this comment.
I'm not sure if we should make these changes to the main branch (adding another goal checker that's not used).
There was a problem hiding this comment.
I agree, this was just to show the idea how two different goal checkers could be changed.
There was a problem hiding this comment.
I think that's valuable in a tutorial or docs once we get this up and running
| RCLCPP_INFO(get_logger(), "instanciating goal checkers if not declared"); | ||
| for (size_t i = 0; i != goal_checker_ids_.size(); i++) { | ||
| try { | ||
| RCLCPP_INFO_STREAM(get_logger(), "goal checker id "<< i<<": "<< goal_checker_ids_[i]); |
There was a problem hiding this comment.
Remove excess logging please
| @@ -279,6 +342,16 @@ void ControllerServer::computeControl() | |||
| action_server_->terminate_current(); | |||
| return; | |||
| } | |||
|
|
|||
| std::string gc_name ; | |||
| get_parameter("current_goal_checker", gc_name); | |||
There was a problem hiding this comment.
Never get parameters at run time, use the stored default type. This logic also fails to communicate goal checker IDs from the action request to even utilize non-default types.
There was a problem hiding this comment.
Would be okay for you if I move this "selected goal checker" step to the "parameters update callback" and we check there if the requested goal checker exist there?
BTW, I am using the same logic used for the controller_id to check if the goal_checker exists.
checkout findGoalCheckerId.
If the goal checker does not exist nothing is updated, the previous selected "plugin" (maybe the default one) remains selected.
There was a problem hiding this comment.
Sorry, took me a few days to gather my thoughts on this. I think this is a very fragile way of handling multiple goal checkers that won't meet the needs of most uses of the feature. If you had, lets say 5 controllers, where 3 are typical point-to-point navigation path followers for specific situations, 1 is a docking controller, and 1 is a person following controller.
The 3 point to point navigation controllers would likely use the same goal checker, and the other 2 unique situations would use 2 unique goal checkers as well. In this case, the use of a specific goal checker is generally mapped to the use of a specific controller plugin. Changing the "parameter" to use would require an external agent to mark the new parameter outside of the context of the navigation task for each controller, which would be difficult.
Imagining another situation where the goal checker to use is not relative to the controller only but as a component of it (maybe some situation where you want a rough and another precise) but still have a few different controller plugins. How would you know when to switch them and what that active controller is at the point you change the parameter? This external server that's changing the parameter would need a great deal of insight into the current state of the controller server and the behavior tree to time that correctly. If this logic occurred in the BT itself, that would make a little more sense, but not very sensible to change parameters then because you're inside the BT itself to send some information directly to the controller server.
So in light of that, can you highlight the situation you would like to change the goal checker during? If its mapped to specific controller IDs, then I think this is better served as a field in the controller namespace to select the name of the goal checker to use. If its changing based on some environmental conditions, then it should be baked into the controller update request.
There was a problem hiding this comment.
Hello I have been away for a couple of months.
I just noticed this answer you made in December.
Let me answer your points.
So in light of that, can you highlight the situation you would like to change the goal checker during? If its mapped to specific controller IDs, then I think this is better served as a field in the controller namespace to select the name of the goal checker to use. If its changing based on some environmental conditions, then it should be baked into the controller update request.
In our cases we want/need to control the way the robot moves at any moment depending on the task the robot is doing. From my point of view, this decision is something that is made at "application level", not at "navigation executive level". For our cases, navigation is a "service" we want to configure and use, but it is not the center of the application.
Answering to your specific question, IMHO, goal checkers could be reused or not depending on the case. they could even change for the same controller: I could need sometimes make more priority the precision in the goal meanwhile in others I could need to prioritize the speed. When I use different controllers probably the configuration would differ, but they could also be shared (after all, the parameters of the simple_goal_checker are quite simple conceptually)
Another possible alternative would just be being able to update at runtime "the goal checker" parameters at runtime, at least for the simple goal checker.
I think this is a very fragile way of handling multiple goal checkers that won't meet the needs of most uses of the feature.
Changing the "parameter" to use would require an external agent to mark the new parameter outside of the context of the navigation task for each controller, which would be difficult.
The solution we made has pros and cons. The discussion about the details can be long.
However, my point in this discussion is not how "this should be done". It is just a feature request: "being able to configure externally some essential aspects of the navigation stack".
So in light of that, can you highlight the situation you would like to change the goal checker during? If its mapped to specific controller IDs, then I think this is better served as a field in the controller namespace to select the name of the goal checker to use. If its changing based on some environmental conditions, then it should be baked into the controller update request.
I am open to change the namespace if that is more convenient.
This external server that's changing the parameter would need a great deal of insight into the current state of the controller server and the behavior tree to time that correctly. If this logic occurred in the BT itself, that would make a little more sense, but not very sensible to change parameters then because you're inside the BT itself to send some information directly to the controller server.
I think this you are proposing is related with this other issue I recently asked about but I still find challenges on that approach:How can a behavior action node communicate with the ros network?
For our requirements, the "specific question" if the goal checker configuration is something that belong to the controller_node, the bt_navigator_node or to other node is not important, we can adapt. For example, for the navigation2 stack, the goal checking parameters were located in the local_planner, but in this fork we did we interact with the goal_checker of the bt_navigator node. All of these approaches may have cons and pros depending on the point of view and priorities of everyone.
I've been trying to identify what is the most convenient approach from the authors of the nav2 stack point of view. But, to be sincere, I do not know that yet.
In resume: I have a requirement that I feel it cannot be easily satisfied by the nav2 at the current moment: "Being able to control the navigation configuration of the goal checking externally", also being able to change the "controller" or the "planner".
I would need to know what is the way I can configure the navigation stack in runtime externally. I do not understand very well why this feature was available in the ROS1 navigation stack and not now.
Nonetheless, I insist, at the current moment the fork (ref) we have in the navigation2 stack work for our requirements. Maybe it does not fit on authors nav2 stack software design point of view, but at least it is something.
How can we solve this issue?
I am open to any point of view to implement these features.
There was a problem hiding this comment.
Hi,
So its been awhile since I've looked at this so I've had a chance to look at it with fresh eyes. I think there's something we could do here very analog to the controller ID selection. So part of the action definition we have the goal checker ID to use as well. Then if none is provided, it uses the single one if only a single one is around (else complain and reject the request like the controller does). That lets us have things like "precise" and "coarse" be high level BT navigator options that remaps down to specific goal checker instances with the finite specifics as to what "precise" and "coarse" means!!
So you get the ability to have variable precision navigation goals and also simplify that from the perspective of the application/BT designer only working with the given goal checker IDs (like we do with controller IDs to abstract out the specific algorithms behind its role)
So in responding to your comments: In our cases we want/need to control the way the robot moves at any moment depending on the task the robot is doing. since this is "task level", I think having this option as part of the action request directly meets that requirement! It also doesn't require a separate external server to set the parameter, now its just part of the behavior tree request (e.g. have subtrees for precise tasks and coarse tasks, or have a selector node for changing between them if there's not a clear behavior tree structured way of separating them -- this is application specific so there's no "right" answer).
"Being able to control the navigation configuration of the goal checking externally", also being able to change the "controller" or the "planner".
We should chat about this more, can you PM me on Slack and we discuss this a little more synchronously? Or does this request described above fully fill your need?
I'd really like to be able to homologate your needs with the architecture here so that you don't need to have a separate fork to do what you need to do. Changing navigation request precision seems like a very reasonable feature we should find a way to better support.
But changing the controller / planner algorithm is incredibly easy, all you do is change the planner_id or the controller_id port in your BT and "poof" a new algorithm is used. If you had different tasks, then your custom BT would be able to have calls to these BT nodes with different algorithms in their unique contexts!
There was a problem hiding this comment.
Hello Steve.
Thanks for your answer.
So part of the action definition we have the goal checker ID to use as well. Then if none is provided, it uses the single one if only a single one is around (else complain and reject the request like the controller does). That lets us have things like "precise" and "coarse" be high level BT navigator options that remaps down to specific goal checker instances with the finite specifics as to what "precise" and "coarse" means!!
So in responding to your comments: In our cases we want/need to control the way the robot moves at any moment depending on the task the robot is doing. since this is "task level", I think having this option as part of the action request directly meets that requirement! It also doesn't require a separate external server to set the parameter, now its just part of the behavior tree request (e.g. have subtrees for precise tasks and coarse tasks, or have a selector node for changing between them if there's not a clear behavior tree structured way of separating them -- this is application specific so there's no "right" answer).
IMHO that approach is convenient and clean. That makes the goal_checker challenge the same the planner_selector issue is.
We should chat about this more, can you PM me on Slack and we discuss this a little more synchronously? Or does this request described above fully fill your need?
Sure, lets keep in contact via slack.
I'd really like to be able to homologate your needs with the architecture here so that you don't need to have a separate fork to do what you need to do. Changing navigation request precision seems like a very reasonable feature we should find a way to better support.
That is great. It is not our goal to make forks from the navigation stack. It is just a way of progressing in some concepts or ideas. It is easier to talk if there is something done or there is some progress on the issue.
We really want to use the nav2 in the way is designed (we know there are many people thinking about its design). And we want to use the nav2 stack the most adapting to its design, but something we cannot do is discarding the whole task-level design we have from an external ros2 node. So we are here trying to figure out the tradeoff solution to solve this.
But changing the controller / planner algorithm is incredibly easy, all you do is change the planner_id or the controller_id port in your BT and "poof" a new algorithm is used. If you had different tasks, then your custom BT would be able to have calls to these BT nodes with different algorithms in their unique contexts!
I understand how you can select a planner or controller from the behavior tree. For example setting the ComputePathToPose's planner_id input port value.
However, my requirement is that the information must come from the external ROS network (in some way or another) from a "task decision node". I know that this could be done directly inside the nav-tree (totally redesigning or discarding our approach) but we cannot do that and we still think that making the task decision level in an external node is legit. (maybe here we disagree and we can chat about that in slack)
This means we need to make the bt_navigator ros2 node some kind of "server" that handles (some service callback or some action callback or some topic subscription callback).
In some of the issues we discussed you showed me how the bt_navigator rclcpp::Node was accesible anywhere in the nav-bt application. That helps because I can create action servers or services servers. But I still find an issue: I think using a BTNode is a problem because they have a limited life-time (they are good to be clients but not severs). In other words; they are good to pull information from the ros network, but they are not good to received pushed information from the ros network.
At this point of the discussion I am not sure if we are still aligned or you have a totally different design in mind.
Continuing the argument, the question for me now is: what about the lifetime of the BTNode? I need to handle any request at any moment of the execution (independently that the planner_id value may be used in the next planning iteration in "a typical" nav-btree). From my understanding that means that the BTNode should be alive for the whole execution.
That is why in the experimental implementation in the fork I used a decorator that wrapped the whole nav-btree.
I still keep that question unsolved if this must be done with a BTActionNode
Now, How many things we disagree? :-) (I cover my eyes with my hands)
There was a problem hiding this comment.
I think we covered the lifetime topic in slack, so I think this is mostly on the same page about adding this to the FollowPath action msg / BT port so that the same mechanics can apply to a selector node. You should be able to set from your external server the values for the inputs to the selector nodes via a topic / service!
The external node method is "legit" but I also want to offer both ways of doing things, for those that want to build a little more reactive intelligence into their BT (so selector nodes for external servers to set, and ports for statically set algorithms in unique subtree contexts for people using conditionals to steer the conditions to set algorithms in context)
|
Any progress or discussion here? |
|
Hello, |
|
@pabloinigoblasco you're also welcome to add the goal checker BT plugin here as well if you like |
Basic Info
Supporting multiple goal checkers in controller_server and switch between them dynamically.
Motivation; at the current moment there isnt (AFAIK) any mechanism to update dynamically the goal checker or the goal checker parameters at runtime.
This pull request for main is a correction of the same pull request made for foxy previously (#2098)
Description of contribution in a few bullet points
This is a basic functionality to support multiple goal checkers in the controller_server (in the same way it is supported multiple controllers )
This initial proposal uses a controller_server parameter to store the "current_goal_checker" (alternatively it could be in the FollowPath action request like it is the controller_id).
It is provided with a "nav_params.yaml configuration" with a second "goal checker" to show how it is used. But that could be removed final pull request.
Description of documentation updates required from your changes
No documentation change should be required at (least in the c++ code)
Future work that may be required in bullet points