Skip to content

Commit d36bdd1

Browse files
DavidG-DevelopDavidG-Develop
authored
Collision monitor toggle bt plugin (#5532)
* add Bt pluging for toggle collision monitor service Signed-off-by: David G <[email protected]> * Add test for btt plugin Signed-off-by: David G <[email protected]> * Clean up test Signed-off-by: David G <[email protected]> * Fix copyright in header Signed-off-by: David G <[email protected]> * uncrustify Signed-off-by: David G <[email protected]> * fix lint Signed-off-by: David G <[email protected]> * fix circle ci Signed-off-by: David G <[email protected]> --------- Signed-off-by: David G <[email protected]> Signed-off-by: DavidG-Develop <[email protected]>
1 parent 16e63a4 commit d36bdd1

File tree

6 files changed

+260
-0
lines changed

6 files changed

+260
-0
lines changed

‎nav2_behavior_tree/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -255,6 +255,9 @@ list(APPEND plugin_libs nav2_compute_and_track_route_bt_node)
255255
add_library(nav2_compute_route_bt_node SHARED plugins/action/compute_route_action.cpp)
256256
list(APPEND plugin_libs nav2_compute_route_bt_node)
257257

258+
add_library(nav2_toggle_collision_monitor_service_bt_node SHARED plugins/action/toggle_collision_monitor_service.cpp)
259+
list(APPEND plugin_libs nav2_toggle_collision_monitor_service_bt_node)
260+
258261
foreach(bt_plugin ${plugin_libs})
259262
target_include_directories(${bt_plugin}
260263
PRIVATE

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

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
// Copyright (c) 2025 David Grbac
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+
#pragma once
16+
17+
#include <string>
18+
19+
#include "nav2_behavior_tree/bt_service_node.hpp"
20+
#include "nav2_msgs/srv/toggle.hpp"
21+
22+
namespace nav2_behavior_tree
23+
{
24+
25+
/**
26+
* @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::Toggle
27+
* @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing.
28+
* It will re-initialize when halted.
29+
*/
30+
class ToggleCollisionMonitorService : public BtServiceNode<nav2_msgs::srv::Toggle>
31+
{
32+
public:
33+
/**
34+
* @brief A constructor for nav2_behavior_tree::ToggleCollisionMonitorService
35+
* @param service_node_name Service name this node creates a client for
36+
* @param conf BT node configuration
37+
*/
38+
ToggleCollisionMonitorService(
39+
const std::string & service_node_name,
40+
const BT::NodeConfiguration & conf);
41+
42+
/**
43+
* @brief The main override required by a BT service
44+
* @return BT::NodeStatus Status of tick execution
45+
*/
46+
void on_tick() override;
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+
BT::InputPort<bool>(
57+
"enable", true,
58+
"Whether to enable or disable collision monitoring"),
59+
});
60+
}
61+
};
62+
63+
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/nav2_tree_nodes.xml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -311,6 +311,12 @@
311311
<output_port name="pose">Stamped extracted pose</output_port>
312312
</Action>
313313

314+
<Action ID="ToggleCollisionMonitor">
315+
<input_port name="enable">Whether to enable collision monitoring</input_port>
316+
<input_port name="service_name">Service name</input_port>
317+
<input_port name="server_timeout">Server timeout</input_port>
318+
</Action>
319+
314320
<Action ID="GetCurrentPose">
315321
<input_port name="global_frame">Global reference frame</input_port>
316322
<input_port name="robot_base_frame">robot base frame</input_port>

‎nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.cpp‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
// Copyright (c) 2025 David Grbac
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+
#include <string>
16+
#include <memory>
17+
18+
#include "nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp"
19+
20+
namespace nav2_behavior_tree
21+
{
22+
23+
ToggleCollisionMonitorService::ToggleCollisionMonitorService(
24+
const std::string & service_node_name,
25+
const BT::NodeConfiguration & conf)
26+
: BtServiceNode<nav2_msgs::srv::Toggle>(service_node_name, conf)
27+
{
28+
}
29+
30+
void ToggleCollisionMonitorService::on_tick()
31+
{
32+
getInput("enable", request_->enable);
33+
}
34+
35+
} // namespace nav2_behavior_tree
36+
37+
#include "behaviortree_cpp/bt_factory.h"
38+
BT_REGISTER_NODES(factory)
39+
{
40+
factory.registerNodeType<nav2_behavior_tree::ToggleCollisionMonitorService>(
41+
"ToggleCollisionMonitor");
42+
}

‎nav2_behavior_tree/test/plugins/action/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -90,3 +90,7 @@ plugin_add_test(test_smoother_selector_node test_smoother_selector_node.cpp nav2
9090
plugin_add_test(test_goal_checker_selector_node test_goal_checker_selector_node.cpp nav2_goal_checker_selector_bt_node)
9191

9292
plugin_add_test(test_progress_checker_selector_node test_progress_checker_selector_node.cpp nav2_progress_checker_selector_bt_node)
93+
94+
plugin_add_test(test_toggle_collision_monitor_service
95+
test_toggle_collision_monitor_service.cpp
96+
nav2_toggle_collision_monitor_service_bt_node)

‎nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp‎

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
// Copyright (c) 2025 David Grbac
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+
#include <gtest/gtest.h>
16+
#include <memory>
17+
#include <set>
18+
#include <string>
19+
20+
#include "behaviortree_cpp/bt_factory.h"
21+
22+
#include "utils/test_service.hpp"
23+
#include "nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp"
24+
25+
class ToggleCollisionMonitorService : public TestService<nav2_msgs::srv::Toggle>
26+
{
27+
public:
28+
ToggleCollisionMonitorService()
29+
: TestService("toggle_collision_monitor")
30+
{}
31+
};
32+
33+
class ToggleCollisionMonitorTestFixture : public ::testing::Test
34+
{
35+
public:
36+
static void SetUpTestCase()
37+
{
38+
node_ = std::make_shared<nav2::LifecycleNode>("toggle_collision_monitor_test_fixture");
39+
factory_ = std::make_shared<BT::BehaviorTreeFactory>();
40+
41+
config_ = new BT::NodeConfiguration();
42+
43+
// Create the blackboard that will be shared by all of the nodes in the tree
44+
config_->blackboard = BT::Blackboard::create();
45+
// Put items on the blackboard
46+
config_->blackboard->set(
47+
"node",
48+
node_);
49+
config_->blackboard->set<std::chrono::milliseconds>(
50+
"server_timeout",
51+
std::chrono::milliseconds(20));
52+
config_->blackboard->set<std::chrono::milliseconds>(
53+
"bt_loop_duration",
54+
std::chrono::milliseconds(10));
55+
config_->blackboard->set<std::chrono::milliseconds>(
56+
"wait_for_service_timeout",
57+
std::chrono::milliseconds(1000));
58+
config_->blackboard->set("initial_pose_received", false);
59+
60+
factory_->registerNodeType<nav2_behavior_tree::ToggleCollisionMonitorService>(
61+
"ToggleCollisionMonitor");
62+
}
63+
64+
static void TearDownTestCase()
65+
{
66+
delete config_;
67+
config_ = nullptr;
68+
node_.reset();
69+
server_.reset();
70+
factory_.reset();
71+
}
72+
73+
void TearDown() override
74+
{
75+
tree_.reset();
76+
}
77+
78+
static std::shared_ptr<ToggleCollisionMonitorService> server_;
79+
80+
protected:
81+
static nav2::LifecycleNode::SharedPtr node_;
82+
static BT::NodeConfiguration * config_;
83+
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
84+
static std::shared_ptr<BT::Tree> tree_;
85+
};
86+
87+
nav2::LifecycleNode::SharedPtr ToggleCollisionMonitorTestFixture::node_ = nullptr;
88+
std::shared_ptr<ToggleCollisionMonitorService>
89+
ToggleCollisionMonitorTestFixture::server_ = nullptr;
90+
BT::NodeConfiguration * ToggleCollisionMonitorTestFixture::config_ = nullptr;
91+
std::shared_ptr<BT::BehaviorTreeFactory>
92+
ToggleCollisionMonitorTestFixture::factory_ = nullptr;
93+
std::shared_ptr<BT::Tree> ToggleCollisionMonitorTestFixture::tree_ = nullptr;
94+
95+
class ToggleParamTest
96+
: public ToggleCollisionMonitorTestFixture,
97+
public ::testing::WithParamInterface<bool> {};
98+
99+
TEST_P(ToggleParamTest, test_tick)
100+
{
101+
const bool enable = GetParam();
102+
103+
std::string xml_txt =
104+
std::string(
105+
R"(
106+
<root BTCPP_format="4">
107+
<BehaviorTree ID="MainTree">
108+
<ToggleCollisionMonitor service_name="toggle_collision_monitor" enable=")")
109+
+
110+
std::string(enable ? "true" : "false") +
111+
R"(" />
112+
</BehaviorTree>
113+
</root>)";
114+
115+
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
116+
EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
117+
}
118+
119+
INSTANTIATE_TEST_SUITE_P(EnableDisable, ToggleParamTest, ::testing::Values(true, false));
120+
121+
int main(int argc, char ** argv)
122+
{
123+
::testing::InitGoogleTest(&argc, argv);
124+
125+
// initialize ROS
126+
rclcpp::init(argc, argv);
127+
128+
// initialize service and spin on new thread
129+
ToggleCollisionMonitorTestFixture::server_ =
130+
std::make_shared<ToggleCollisionMonitorService>();
131+
std::thread server_thread([]() {
132+
rclcpp::spin(ToggleCollisionMonitorTestFixture::server_);
133+
});
134+
135+
int all_successful = RUN_ALL_TESTS();
136+
137+
// shutdown ROS
138+
rclcpp::shutdown();
139+
server_thread.join();
140+
141+
return all_successful;
142+
}

0 commit comments

Comments
 (0)