Skip to content

Commit f4b61f8

Browse files
SteveMacenskiRBT22
SteveMacenski
authored and
RBT22
committed
Manual Backport Route Server to Jazzy (ros-navigation#5517)
* Manual backport of Route Server to Jazzy Signed-off-by: Steve Macenski <[email protected]> * linting Signed-off-by: Steve Macenski <[email protected]> * Fix backport error Signed-off-by: Steve Macenski <[email protected]> * lint Signed-off-by: Steve Macenski <[email protected]> * Adding in Nav2 BT + Launch Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]>
1 parent 8ab5b1e commit f4b61f8

File tree

118 files changed

+16642
-0
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

118 files changed

+16642
-0
lines changed

‎nav2_behavior_tree/CMakeLists.txt‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -215,6 +215,15 @@ list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)
215215
add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
216216
list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)
217217

218+
add_library(nav2_compute_and_track_route_cancel_bt_node SHARED plugins/action/compute_and_track_route_cancel_node.cpp)
219+
list(APPEND plugin_libs nav2_compute_and_track_route_cancel_bt_node)
220+
221+
add_library(nav2_compute_and_track_route_bt_node SHARED plugins/action/compute_and_track_route_action.cpp)
222+
list(APPEND plugin_libs nav2_compute_and_track_route_bt_node)
223+
224+
add_library(nav2_compute_route_bt_node SHARED plugins/action/compute_route_action.cpp)
225+
list(APPEND plugin_libs nav2_compute_route_bt_node)
226+
218227
foreach(bt_plugin ${plugin_libs})
219228
ament_target_dependencies(${bt_plugin} ${dependencies})
220229
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp‎

Lines changed: 133 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,133 @@
1+
// Copyright (c) 2025 Open Navigation LLC
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_ACTION_HPP_
16+
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_ACTION_HPP_
17+
18+
#include <string>
19+
#include <memory>
20+
21+
#include "nav2_msgs/action/compute_and_track_route.hpp"
22+
#include "nav2_behavior_tree/bt_action_node.hpp"
23+
#include "nav2_util/lifecycle_node.hpp"
24+
25+
namespace nav2_behavior_tree
26+
{
27+
28+
/**
29+
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputeAndTrackRoute
30+
*/
31+
class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::ComputeAndTrackRoute>
32+
{
33+
using Action = nav2_msgs::action::ComputeAndTrackRoute;
34+
using ActionResult = Action::Result;
35+
36+
public:
37+
/**
38+
* @brief A constructor for nav2_behavior_tree::ComputeAndTrackRouteAction
39+
* @param xml_tag_name Name for the XML tag for this node
40+
* @param action_name Action name this node creates a client for
41+
* @param conf BT node configuration
42+
*/
43+
ComputeAndTrackRouteAction(
44+
const std::string & xml_tag_name,
45+
const std::string & action_name,
46+
const BT::NodeConfiguration & conf);
47+
48+
/**
49+
* @brief Function to perform some user-defined operation on tick
50+
*/
51+
void on_tick() override;
52+
53+
/**
54+
* @brief Function to perform some user-defined operation upon successful completion of the action
55+
*/
56+
BT::NodeStatus on_success() override;
57+
58+
/**
59+
* @brief Function to perform some user-defined operation upon abortion of the action
60+
*/
61+
BT::NodeStatus on_aborted() override;
62+
63+
/**
64+
* @brief Function to perform some user-defined operation upon cancellation of the action
65+
*/
66+
BT::NodeStatus on_cancelled() override;
67+
68+
/**
69+
* @brief Function to perform some user-defined operation after a timeout
70+
* waiting for a result that hasn't been received yet
71+
* @param feedback shared_ptr to latest feedback message
72+
*/
73+
void on_wait_for_result(
74+
std::shared_ptr<const Action::Feedback> feedback) override;
75+
76+
/**
77+
* @brief Function to set all feedbacks and output ports to be null values
78+
*/
79+
void resetFeedbackAndOutputPorts();
80+
81+
/**
82+
* @brief Creates list of BT ports
83+
* @return BT::PortsList Containing basic ports along with node-specific ports
84+
*/
85+
static BT::PortsList providedPorts()
86+
{
87+
return providedBasicPorts(
88+
{
89+
BT::InputPort<unsigned int>("start_id", "ID of the start node"),
90+
BT::InputPort<unsigned int>("goal_id", "ID of the goal node"),
91+
BT::InputPort<geometry_msgs::msg::PoseStamped>(
92+
"start",
93+
"Start pose of the path if overriding current robot pose and using poses over IDs"),
94+
BT::InputPort<geometry_msgs::msg::PoseStamped>(
95+
"goal", "Goal pose of the path if using poses over IDs"),
96+
BT::InputPort<bool>(
97+
"use_start", false,
98+
"Whether to use the start pose or the robot's current pose"),
99+
BT::InputPort<bool>(
100+
"use_poses", false, "Whether to use poses or IDs for start and goal"),
101+
BT::OutputPort<builtin_interfaces::msg::Duration>(
102+
"execution_duration",
103+
"Time taken to compute and track route"),
104+
BT::OutputPort<ActionResult::_error_code_type>(
105+
"error_code_id", "The compute route error code"),
106+
BT::OutputPort<uint16_t>(
107+
"last_node_id",
108+
"ID of the previous node"),
109+
BT::OutputPort<uint16_t>(
110+
"next_node_id",
111+
"ID of the next node"),
112+
BT::OutputPort<uint16_t>(
113+
"current_edge_id",
114+
"ID of current edge"),
115+
BT::OutputPort<nav2_msgs::msg::Route>(
116+
"route",
117+
"List of RouteNodes to go from start to end"),
118+
BT::OutputPort<nav_msgs::msg::Path>(
119+
"path",
120+
"Path created by ComputeAndTrackRoute node"),
121+
BT::OutputPort<bool>(
122+
"rerouted",
123+
"Whether the plan has rerouted"),
124+
});
125+
}
126+
127+
protected:
128+
Action::Feedback feedback_;
129+
};
130+
131+
} // namespace nav2_behavior_tree
132+
133+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_ACTION_HPP_

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_cancel_node.hpp‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
// Copyright (c) 2025 Open Navigation LLC
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_CANCEL_NODE_HPP_
16+
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_CANCEL_NODE_HPP_
17+
18+
#include <memory>
19+
#include <string>
20+
21+
#include "nav2_msgs/action/compute_and_track_route.hpp"
22+
23+
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
24+
#include "nav2_util/lifecycle_node.hpp"
25+
26+
namespace nav2_behavior_tree
27+
{
28+
29+
/**
30+
* @brief A nav2_behavior_tree::BtActionNode class
31+
* that wraps nav2_msgs::action::ComputeAndTrackRoute cancellation
32+
*/
33+
class ComputeAndTrackRouteCancel
34+
: public BtCancelActionNode<nav2_msgs::action::ComputeAndTrackRoute>
35+
{
36+
public:
37+
/**
38+
* @brief A constructor for nav2_behavior_tree::ComputeAndTrackRouteCancel
39+
* @param xml_tag_name Name for the XML tag for this node
40+
* @param action_name Action name this node creates a client for
41+
* @param conf BT node configuration
42+
*/
43+
ComputeAndTrackRouteCancel(
44+
const std::string & xml_tag_name,
45+
const std::string & action_name,
46+
const BT::NodeConfiguration & conf);
47+
48+
/**
49+
* @brief Creates list of BT ports
50+
* @return BT::PortsList Containing basic ports along with node-specific ports
51+
*/
52+
static BT::PortsList providedPorts()
53+
{
54+
return providedBasicPorts(
55+
{
56+
});
57+
}
58+
};
59+
60+
} // namespace nav2_behavior_tree
61+
62+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_AND_TRACK_ROUTE_CANCEL_NODE_HPP_

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp‎

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
// Copyright (c) 2025 Open Navigation LLC
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_ROUTE_ACTION_HPP_
16+
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_ROUTE_ACTION_HPP_
17+
18+
#include <string>
19+
20+
#include "nav2_msgs/action/compute_route.hpp"
21+
#include "nav_msgs/msg/path.hpp"
22+
#include "nav2_behavior_tree/bt_action_node.hpp"
23+
#include "nav2_util/lifecycle_node.hpp"
24+
25+
namespace nav2_behavior_tree
26+
{
27+
28+
/**
29+
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputeRoute
30+
*/
31+
class ComputeRouteAction : public BtActionNode<nav2_msgs::action::ComputeRoute>
32+
{
33+
using Action = nav2_msgs::action::ComputeRoute;
34+
using ActionResult = Action::Result;
35+
36+
public:
37+
/**
38+
* @brief A constructor for nav2_behavior_tree::ComputeRoute
39+
* @param xml_tag_name Name for the XML tag for this node
40+
* @param action_name Action name this node creates a client for
41+
* @param conf BT node configuration
42+
*/
43+
ComputeRouteAction(
44+
const std::string & xml_tag_name,
45+
const std::string & action_name,
46+
const BT::NodeConfiguration & conf);
47+
48+
/**
49+
* @brief Function to perform some user-defined operation on tick
50+
*/
51+
void on_tick() override;
52+
53+
/**
54+
* @brief Function to perform some user-defined operation upon successful completion of the action
55+
*/
56+
BT::NodeStatus on_success() override;
57+
58+
/**
59+
* @brief Function to perform some user-defined operation upon abortion of the action
60+
*/
61+
BT::NodeStatus on_aborted() override;
62+
63+
/**
64+
* @brief Function to perform some user-defined operation upon cancellation of the action
65+
*/
66+
BT::NodeStatus on_cancelled() override;
67+
68+
/**
69+
* \brief Override required by the a BT action. Cancel the action and set the path output
70+
*/
71+
void halt() override;
72+
73+
/**
74+
* @brief Reset output port values on failure
75+
*/
76+
void resetPorts();
77+
78+
/**
79+
* @brief Creates list of BT ports
80+
* @return BT::PortsList Containing basic ports along with node-specific ports
81+
*/
82+
static BT::PortsList providedPorts()
83+
{
84+
return providedBasicPorts(
85+
{
86+
BT::InputPort<unsigned int>("start_id", "ID of the start node"),
87+
BT::InputPort<unsigned int>("goal_id", "ID of the goal node"),
88+
BT::InputPort<geometry_msgs::msg::PoseStamped>(
89+
"start",
90+
"Start pose of the path if overriding current robot pose and using poses over IDs"),
91+
BT::InputPort<geometry_msgs::msg::PoseStamped>(
92+
"goal", "Goal pose of the path if using poses over IDs"),
93+
BT::InputPort<bool>(
94+
"use_start", false,
95+
"Whether to use the start pose or the robot's current pose"),
96+
BT::InputPort<bool>(
97+
"use_poses", false, "Whether to use poses or IDs for start and goal"),
98+
BT::OutputPort<ActionResult::_route_type>(
99+
"route", "The route computed by ComputeRoute node"),
100+
BT::OutputPort<builtin_interfaces::msg::Duration>("planning_time",
101+
"Time taken to compute route"),
102+
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputeRoute node"),
103+
BT::OutputPort<ActionResult::_error_code_type>(
104+
"error_code_id", "The compute route error code"),
105+
});
106+
}
107+
};
108+
109+
} // namespace nav2_behavior_tree
110+
111+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_ROUTE_ACTION_HPP_

0 commit comments

Comments
 (0)