Skip to content

Commit f12e811

Browse files
silanus23silanus23
committed
test file added
Signed-off-by: silanus23 <[email protected]>
1 parent 1146630 commit f12e811

File tree

1 file changed

+223
-0
lines changed

1 file changed

+223
-0
lines changed

‎nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp‎

Lines changed: 223 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,223 @@
1+
// Copyright (c) 2025 Berkan Tali
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+
#include <chrono>
20+
21+
#include "nav2_msgs/msg/tracking_feedback.hpp"
22+
23+
#include "utils/test_behavior_tree_fixture.hpp"
24+
#include "nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp"
25+
26+
class IsWithinPathTrackingBoundsConditionTestFixture : public ::testing::Test
27+
{
28+
public:
29+
static void SetUpTestCase()
30+
{
31+
node_ = std::make_shared<nav2::LifecycleNode>("test_is_within_path_tracking_bounds");
32+
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
33+
executor_->add_node(node_->get_node_base_interface());
34+
factory_ = std::make_shared<BT::BehaviorTreeFactory>();
35+
36+
config_ = new BT::NodeConfiguration();
37+
38+
// Create the blackboard that will be shared by all of the nodes in the tree
39+
config_->blackboard = BT::Blackboard::create();
40+
// Put items on the blackboard
41+
config_->blackboard->set(
42+
"node",
43+
node_);
44+
config_->blackboard->set<std::chrono::milliseconds>(
45+
"bt_loop_duration",
46+
std::chrono::milliseconds(10));
47+
48+
factory_->registerNodeType<nav2_behavior_tree::IsWithinPathTrackingBoundsCondition>(
49+
"IsWithinPathTrackingBounds");
50+
51+
tracking_feedback_pub_ = node_->create_publisher<nav2_msgs::msg::TrackingFeedback>(
52+
"/tracking_feedback",
53+
rclcpp::SystemDefaultsQoS());
54+
tracking_feedback_pub_->on_activate();
55+
}
56+
57+
static void TearDownTestCase()
58+
{
59+
delete config_;
60+
config_ = nullptr;
61+
tracking_feedback_pub_.reset();
62+
node_.reset();
63+
factory_.reset();
64+
executor_.reset();
65+
}
66+
67+
protected:
68+
static nav2::LifecycleNode::SharedPtr node_;
69+
static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
70+
static BT::NodeConfiguration * config_;
71+
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
72+
static nav2::Publisher<nav2_msgs::msg::TrackingFeedback>::SharedPtr tracking_feedback_pub_;
73+
};
74+
75+
nav2::LifecycleNode::SharedPtr IsWithinPathTrackingBoundsConditionTestFixture::node_ = nullptr;
76+
rclcpp::executors::SingleThreadedExecutor::SharedPtr
77+
IsWithinPathTrackingBoundsConditionTestFixture::executor_ = nullptr;
78+
BT::NodeConfiguration * IsWithinPathTrackingBoundsConditionTestFixture::config_ = nullptr;
79+
std::shared_ptr<BT::BehaviorTreeFactory> IsWithinPathTrackingBoundsConditionTestFixture::factory_ =
80+
nullptr;
81+
nav2::Publisher<nav2_msgs::msg::TrackingFeedback>::SharedPtr
82+
IsWithinPathTrackingBoundsConditionTestFixture::tracking_feedback_pub_ = nullptr;
83+
84+
TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_within_bounds)
85+
{
86+
std::string xml_txt =
87+
R"(
88+
<root BTCPP_format="4">
89+
<BehaviorTree ID="MainTree">
90+
<IsWithinPathTrackingBounds max_error="1.0"/>
91+
</BehaviorTree>
92+
</root>)";
93+
94+
auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
95+
96+
// Test case 1: Error is within bounds (should return SUCCESS)
97+
nav2_msgs::msg::TrackingFeedback tracking_feedback_msg;
98+
tracking_feedback_msg.tracking_error = 0.5;
99+
tracking_feedback_pub_->publish(tracking_feedback_msg);
100+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
101+
executor_->spin_some();
102+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
103+
104+
// Test case 2: Error is exactly at the boundary (should return SUCCESS)
105+
tracking_feedback_msg.tracking_error = 1.0;
106+
tracking_feedback_pub_->publish(tracking_feedback_msg);
107+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
108+
executor_->spin_some();
109+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
110+
111+
// Test case 3: Error exceeds bounds (should return FAILURE)
112+
tracking_feedback_msg.tracking_error = 1.5;
113+
tracking_feedback_pub_->publish(tracking_feedback_msg);
114+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
115+
executor_->spin_some();
116+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
117+
118+
// Test case 4: Zero error (should return SUCCESS)
119+
tracking_feedback_msg.tracking_error = 0.0;
120+
tracking_feedback_pub_->publish(tracking_feedback_msg);
121+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
122+
executor_->spin_some();
123+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
124+
}
125+
126+
TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_different_max_error)
127+
{
128+
std::string xml_txt =
129+
R"(
130+
<root BTCPP_format="4">
131+
<BehaviorTree ID="MainTree">
132+
<IsWithinPathTrackingBounds max_error="0.2"/>
133+
</BehaviorTree>
134+
</root>)";
135+
136+
auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
137+
138+
// Test case 1: Error is within smaller bounds (should return SUCCESS)
139+
nav2_msgs::msg::TrackingFeedback tracking_feedback_msg;
140+
tracking_feedback_msg.tracking_error = 0.1;
141+
tracking_feedback_pub_->publish(tracking_feedback_msg);
142+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
143+
executor_->spin_some();
144+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
145+
146+
// Test case 2: Error exceeds smaller bounds (should return FAILURE)
147+
tracking_feedback_msg.tracking_error = 0.3;
148+
tracking_feedback_pub_->publish(tracking_feedback_msg);
149+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
150+
executor_->spin_some();
151+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
152+
}
153+
154+
TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_large_error_values)
155+
{
156+
std::string xml_txt =
157+
R"(
158+
<root BTCPP_format="4">
159+
<BehaviorTree ID="MainTree">
160+
<IsWithinPathTrackingBounds max_error="5.0"/>
161+
</BehaviorTree>
162+
</root>)";
163+
164+
auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
165+
166+
// Test case 1: Large error within bounds (should return SUCCESS)
167+
nav2_msgs::msg::TrackingFeedback tracking_feedback_msg;
168+
tracking_feedback_msg.tracking_error = 4.9;
169+
tracking_feedback_pub_->publish(tracking_feedback_msg);
170+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
171+
executor_->spin_some();
172+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
173+
174+
// Test case 2: Very large error exceeding bounds (should return FAILURE)
175+
tracking_feedback_msg.tracking_error = 10.0;
176+
tracking_feedback_pub_->publish(tracking_feedback_msg);
177+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
178+
executor_->spin_some();
179+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
180+
}
181+
182+
TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_edge_cases)
183+
{
184+
std::string xml_txt =
185+
R"(
186+
<root BTCPP_format="4">
187+
<BehaviorTree ID="MainTree">
188+
<IsWithinPathTrackingBounds max_error="0.0"/>
189+
</BehaviorTree>
190+
</root>)";
191+
192+
auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
193+
194+
// Test case 1: Zero max_error with zero tracking error (should return SUCCESS)
195+
nav2_msgs::msg::TrackingFeedback tracking_feedback_msg;
196+
tracking_feedback_msg.tracking_error = 0.0;
197+
tracking_feedback_pub_->publish(tracking_feedback_msg);
198+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
199+
executor_->spin_some();
200+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
201+
202+
// Test case 2: Zero max_error with any positive tracking error (should return FAILURE)
203+
tracking_feedback_msg.tracking_error = 0.001;
204+
tracking_feedback_pub_->publish(tracking_feedback_msg);
205+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
206+
executor_->spin_some();
207+
EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
208+
}
209+
210+
int main(int argc, char ** argv)
211+
{
212+
::testing::InitGoogleTest(&argc, argv);
213+
214+
// initialize ROS
215+
rclcpp::init(argc, argv);
216+
217+
bool all_successful = RUN_ALL_TESTS();
218+
219+
// shutdown ROS
220+
rclcpp::shutdown();
221+
222+
return all_successful;
223+
}

0 commit comments

Comments
 (0)