Skip to content
Snippets Groups Projects
Unverified Commit fe404dfe authored by Sarthak Mittal's avatar Sarthak Mittal Committed by GitHub
Browse files

Add BehaviorTreeTestFixture for nav2_behavior_tree tests (#1812)


* Add GoalReachedCondition test and move around headers

Signed-off-by: default avatarSarthak Mittal <sarthakmittal2608@gmail.com>

* Rename to BehaviorTreeTestFixture

Signed-off-by: default avatarSarthak Mittal <sarthakmittal2608@gmail.com>

* Remove GoalReachedCondition test

Signed-off-by: default avatarSarthak Mittal <sarthakmittal2608@gmail.com>
parent dc597153
No related branches found
No related tags found
No related merge requests found
Showing
with 205 additions and 293 deletions
......@@ -13,8 +13,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
#include <string>
#include <memory>
......@@ -62,4 +62,4 @@ private:
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
......@@ -13,8 +13,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
#include <string>
......@@ -51,4 +51,4 @@ private:
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
......@@ -13,8 +13,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
#include <memory>
#include <string>
......@@ -63,4 +63,4 @@ private:
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
......@@ -13,8 +13,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__SPEED_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__SPEED_CONTROLLER_HPP_
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
#include <memory>
#include <string>
......@@ -97,4 +97,4 @@ private:
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__SPEED_CONTROLLER_HPP_
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
......@@ -13,16 +13,13 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_
#include <string>
#include <memory>
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_behavior_tree/plugins/distance_traveled_condition.hpp"
#include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
namespace nav2_behavior_tree
{
......@@ -87,5 +84,3 @@ BT_REGISTER_NODES(factory)
{
factory.registerNodeType<nav2_behavior_tree::DistanceTraveledCondition>("DistanceTraveled");
}
#endif // NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_
......@@ -13,15 +13,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_
#include <string>
#include <memory>
#include "behaviortree_cpp_v3/condition_node.h"
#include "nav2_behavior_tree/plugins/time_expired_condition.hpp"
#include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp"
namespace nav2_behavior_tree
{
......@@ -65,5 +62,3 @@ BT_REGISTER_NODES(factory)
{
factory.registerNodeType<nav2_behavior_tree::TimeExpiredCondition>("TimeExpired");
}
#endif // NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_
......@@ -25,7 +25,7 @@
#include "behaviortree_cpp_v3/decorator_node.h"
#include "nav2_behavior_tree/plugins/distance_controller.hpp"
#include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
namespace nav2_behavior_tree
{
......
......@@ -18,7 +18,7 @@
#include "nav2_util/geometry_utils.hpp"
#include "nav2_behavior_tree/plugins/speed_controller.hpp"
#include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
namespace nav2_behavior_tree
{
......
ament_add_gtest(test_decorator_distance_controller
plugins/decorator/test_distance_controller.cpp
)
target_link_libraries(test_decorator_distance_controller
nav2_distance_controller_bt_node
)
ament_target_dependencies(test_decorator_distance_controller
${dependencies}
)
ament_add_gtest(test_decorator_speed_controller
plugins/decorator/test_speed_controller.cpp
)
target_link_libraries(test_decorator_speed_controller
nav2_speed_controller_bt_node
)
ament_target_dependencies(test_decorator_speed_controller
${dependencies}
)
ament_add_gtest(test_condition_distance_traveled
plugins/condition/test_distance_traveled.cpp
)
target_link_libraries(test_condition_distance_traveled
nav2_distance_traveled_condition_bt_node
)
ament_target_dependencies(test_condition_distance_traveled
${dependencies}
)
ament_add_gtest(test_condition_time_expired
plugins/condition/test_time_expired.cpp
)
target_link_libraries(test_condition_time_expired
nav2_time_expired_condition_bt_node
)
ament_target_dependencies(test_condition_time_expired
${dependencies}
)
add_subdirectory(plugins/condition)
add_subdirectory(plugins/decorator)
ament_add_gtest(test_condition_distance_traveled test_distance_traveled.cpp)
target_link_libraries(test_condition_distance_traveled nav2_distance_traveled_condition_bt_node)
ament_target_dependencies(test_condition_distance_traveled ${dependencies})
ament_add_gtest(test_condition_time_expired test_time_expired.cpp)
target_link_libraries(test_condition_time_expired nav2_time_expired_condition_bt_node)
ament_target_dependencies(test_condition_time_expired ${dependencies})
......@@ -14,81 +14,39 @@
// limitations under the License.
#include <gtest/gtest.h>
#include <cmath>
#include <memory>
#include <set>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_transform_handler.hpp"
#include "../../test_dummy_tree_node.hpp"
#include "nav2_behavior_tree/plugins/distance_traveled_condition.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
class DistanceTraveledConditionTestFixture : public ::testing::Test
class DistanceTraveledConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
static void SetUpTestCase()
{
transform_handler_ = new nav2_behavior_tree::TransformHandler();
config_ = new BT::NodeConfiguration();
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create();
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
rclcpp::Node::SharedPtr(transform_handler_));
config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
"tf_buffer",
transform_handler_->getBuffer());
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds(10));
config_->blackboard->set<bool>("path_updated", false);
config_->blackboard->set<bool>("initial_pose_received", false);
transform_handler_->activate();
transform_handler_->waitForTransform();
}
static void TearDownTestCase()
{
transform_handler_->deactivate();
delete transform_handler_;
delete config_;
transform_handler_ = nullptr;
config_ = nullptr;
}
void SetUp()
{
node_ = new nav2_behavior_tree::DistanceTraveledCondition("distance_traveled", *config_);
bt_node_ = std::make_shared<nav2_behavior_tree::DistanceTraveledCondition>(
"distance_traveled", *config_);
}
void TearDown()
{
node_ = nullptr;
bt_node_.reset();
}
protected:
static nav2_behavior_tree::TransformHandler * transform_handler_;
static BT::NodeConfiguration * config_;
static nav2_behavior_tree::DistanceTraveledCondition * node_;
static std::shared_ptr<nav2_behavior_tree::DistanceTraveledCondition> bt_node_;
};
nav2_behavior_tree::TransformHandler * DistanceTraveledConditionTestFixture::transform_handler_ =
nullptr;
BT::NodeConfiguration * DistanceTraveledConditionTestFixture::config_ = nullptr;
nav2_behavior_tree::DistanceTraveledCondition * DistanceTraveledConditionTestFixture::node_ =
nullptr;
std::shared_ptr<nav2_behavior_tree::DistanceTraveledCondition>
DistanceTraveledConditionTestFixture::bt_node_ = nullptr;
TEST_F(DistanceTraveledConditionTestFixture, test_behavior)
{
EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE);
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE);
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 0;
......@@ -111,9 +69,9 @@ TEST_F(DistanceTraveledConditionTestFixture, test_behavior)
}
if (i % 2) {
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
} else {
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
}
}
}
......
......@@ -18,84 +18,47 @@
#include <memory>
#include <set>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_transform_handler.hpp"
#include "nav2_behavior_tree/plugins/time_expired_condition.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
class TimeExpiredConditionTestFixture : public ::testing::Test
class TimeExpiredConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
static void SetUpTestCase()
{
transform_handler_ = new nav2_behavior_tree::TransformHandler();
config_ = new BT::NodeConfiguration();
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create();
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
rclcpp::Node::SharedPtr(transform_handler_));
config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
"tf_buffer",
transform_handler_->getBuffer());
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds(10));
config_->blackboard->set<bool>("path_updated", false);
config_->blackboard->set<bool>("initial_pose_received", false);
transform_handler_->activate();
transform_handler_->waitForTransform();
}
static void TearDownTestCase()
{
transform_handler_->deactivate();
delete transform_handler_;
delete config_;
transform_handler_ = nullptr;
config_ = nullptr;
}
void SetUp()
{
node_ = new nav2_behavior_tree::TimeExpiredCondition("time_expired", *config_);
bt_node_ = std::make_shared<nav2_behavior_tree::TimeExpiredCondition>(
"time_expired", *config_);
}
void TearDown()
{
node_ = nullptr;
bt_node_.reset();
}
protected:
static nav2_behavior_tree::TransformHandler * transform_handler_;
static BT::NodeConfiguration * config_;
static nav2_behavior_tree::TimeExpiredCondition * node_;
static std::shared_ptr<nav2_behavior_tree::TimeExpiredCondition> bt_node_;
};
nav2_behavior_tree::TransformHandler * TimeExpiredConditionTestFixture::transform_handler_ =
nullptr;
BT::NodeConfiguration * TimeExpiredConditionTestFixture::config_ = nullptr;
nav2_behavior_tree::TimeExpiredCondition * TimeExpiredConditionTestFixture::node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::TimeExpiredCondition>
TimeExpiredConditionTestFixture::bt_node_ = nullptr;
TEST_F(TimeExpiredConditionTestFixture, test_behavior)
{
EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE);
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE);
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
for (int i = 0; i < 20; ++i) {
rclcpp::sleep_for(500ms);
if (i % 2) {
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
} else {
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
}
}
}
......
ament_add_gtest(test_decorator_distance_controller test_distance_controller.cpp)
target_link_libraries(test_decorator_distance_controller nav2_distance_controller_bt_node)
ament_target_dependencies(test_decorator_distance_controller ${dependencies})
ament_add_gtest(test_decorator_speed_controller test_speed_controller.cpp)
target_link_libraries(test_decorator_speed_controller nav2_speed_controller_bt_node)
ament_target_dependencies(test_decorator_speed_controller ${dependencies})
......@@ -18,80 +18,46 @@
#include <memory>
#include <set>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_transform_handler.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "../../test_dummy_tree_node.hpp"
#include "nav2_behavior_tree/plugins/distance_controller.hpp"
#include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
class DistanceControllerTestFixture : public ::testing::Test
class DistanceControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
static void SetUpTestCase()
{
transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>();
config_ = std::make_shared<BT::NodeConfiguration>();
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create();
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
rclcpp::Node::SharedPtr(transform_handler_));
config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
"tf_buffer",
transform_handler_->getBuffer());
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds(10));
config_->blackboard->set<bool>("path_updated", false);
config_->blackboard->set<bool>("initial_pose_received", false);
transform_handler_->activate();
transform_handler_->waitForTransform();
}
static void TearDownTestCase()
{
transform_handler_->deactivate();
delete dummy_node_;
delete node_;
}
void SetUp()
{
node_ = new nav2_behavior_tree::DistanceController("distance_controller", *config_);
dummy_node_ = new nav2_behavior_tree::DummyNode();
node_->setChild(dummy_node_);
bt_node_ = std::make_shared<nav2_behavior_tree::DistanceController>(
"distance_controller", *config_);
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
bt_node_->setChild(dummy_node_.get());
}
void TearDown()
{
dummy_node_ = nullptr;
node_ = nullptr;
dummy_node_.reset();
bt_node_.reset();
}
protected:
static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
static std::shared_ptr<BT::NodeConfiguration> config_;
static nav2_behavior_tree::DistanceController * node_;
static nav2_behavior_tree::DummyNode * dummy_node_;
static std::shared_ptr<nav2_behavior_tree::DistanceController> bt_node_;
static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
};
std::shared_ptr<nav2_behavior_tree::TransformHandler>
DistanceControllerTestFixture::transform_handler_ = nullptr;
std::shared_ptr<BT::NodeConfiguration> DistanceControllerTestFixture::config_ = nullptr;
nav2_behavior_tree::DistanceController * DistanceControllerTestFixture::node_ = nullptr;
nav2_behavior_tree::DummyNode * DistanceControllerTestFixture::dummy_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DistanceController>
DistanceControllerTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
DistanceControllerTestFixture::dummy_node_ = nullptr;
TEST_F(DistanceControllerTestFixture, test_behavior)
{
EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE);
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
geometry_msgs::msg::PoseStamped pose;
......@@ -119,9 +85,9 @@ TEST_F(DistanceControllerTestFixture, test_behavior)
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
if (i % 2) {
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::RUNNING);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
} else {
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
}
}
......
......@@ -22,76 +22,41 @@
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_transform_handler.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "../../test_dummy_tree_node.hpp"
#include "nav2_behavior_tree/plugins/speed_controller.hpp"
#include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
class SpeedControllerTestFixture : public ::testing::Test
class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
static void SetUpTestCase()
void SetUp()
{
transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>();
config_ = std::make_shared<BT::NodeConfiguration>();
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create();
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
rclcpp::Node::SharedPtr(transform_handler_));
config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
"tf_buffer",
transform_handler_->getBuffer());
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds(10));
config_->blackboard->set<bool>("path_updated", false);
config_->blackboard->set<bool>("initial_pose_received", false);
geometry_msgs::msg::PoseStamped goal;
goal.header.stamp = transform_handler_->now();
goal.header.stamp = node_->now();
config_->blackboard->set("goal", goal);
transform_handler_->activate();
transform_handler_->waitForTransform();
}
static void TearDownTestCase()
{
transform_handler_->deactivate();
delete dummy_node_;
delete node_;
}
void SetUp()
{
node_ = new nav2_behavior_tree::SpeedController("speed_controller", *config_);
dummy_node_ = new nav2_behavior_tree::DummyNode();
node_->setChild(dummy_node_);
bt_node_ = std::make_shared<nav2_behavior_tree::SpeedController>("speed_controller", *config_);
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
bt_node_->setChild(dummy_node_.get());
}
void TearDown()
{
dummy_node_ = nullptr;
node_ = nullptr;
dummy_node_.reset();
bt_node_.reset();
}
protected:
static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
static std::shared_ptr<BT::NodeConfiguration> config_;
static nav2_behavior_tree::SpeedController * node_;
static nav2_behavior_tree::DummyNode * dummy_node_;
static std::shared_ptr<nav2_behavior_tree::SpeedController> bt_node_;
static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
};
std::shared_ptr<nav2_behavior_tree::TransformHandler>
SpeedControllerTestFixture::transform_handler_ = nullptr;
std::shared_ptr<BT::NodeConfiguration> SpeedControllerTestFixture::config_ = nullptr;
nav2_behavior_tree::SpeedController * SpeedControllerTestFixture::node_ = nullptr;
nav2_behavior_tree::DummyNode * SpeedControllerTestFixture::dummy_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::SpeedController>
SpeedControllerTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
SpeedControllerTestFixture::dummy_node_ = nullptr;
/*
* Test for speed controller behavior
......@@ -102,30 +67,30 @@ nav2_behavior_tree::DummyNode * SpeedControllerTestFixture::dummy_node_ = nullpt
*/
TEST_F(SpeedControllerTestFixture, test_behavior)
{
auto odom_pub = transform_handler_->create_publisher<nav_msgs::msg::Odometry>("odom", 1);
auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 1);
nav_msgs::msg::Odometry odom_msg;
auto time = transform_handler_->now();
auto time = node_->now();
odom_msg.header.stamp = time;
odom_msg.twist.twist.linear.x = 0.223;
odom_pub->publish(odom_msg);
EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE);
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
// after the first tick, period should be a default value of 1s
// first tick should return running since period has not exceeded
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::RUNNING);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
// set the child node to success so node can return success
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
// should return success since period has exceeded and new period should be set to ~2s
rclcpp::sleep_for(1s);
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
// send new velocity for update after the next period
odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.5);
......@@ -135,17 +100,17 @@ TEST_F(SpeedControllerTestFixture, test_behavior)
// Period should be set to ~2s based on the last speed of 0.223 m/s
rclcpp::sleep_for(1s);
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::RUNNING);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
rclcpp::sleep_for(1s);
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
// period should be set to ~10s based on the last speed of 0 m/s
// should return running for the first 9 seconds
for (int i = 0; i < 9; ++i) {
rclcpp::sleep_for(1s);
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::RUNNING);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
}
// set the child node to success so node can return success
......@@ -153,7 +118,7 @@ TEST_F(SpeedControllerTestFixture, test_behavior)
// should return success since period has exceeded
rclcpp::sleep_for(1s);
EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
}
int main(int argc, char ** argv)
......
// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_BEHAVIOR_TREE_FIXTURE_HPP_
#define TEST_BEHAVIOR_TREE_FIXTURE_HPP_
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "rclcpp/rclcpp.hpp"
#include "test_transform_handler.hpp"
#include "test_dummy_tree_node.hpp"
namespace nav2_behavior_tree
{
class BehaviorTreeTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase()
{
node_ = std::make_shared<rclcpp::Node>("test_behavior_tree_fixture");
transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>(node_);
factory_ = std::make_shared<BT::BehaviorTreeFactory>();
config_ = new BT::NodeConfiguration();
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create();
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_);
config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
"tf_buffer",
transform_handler_->getBuffer());
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds(10));
config_->blackboard->set<bool>("path_updated", false);
config_->blackboard->set<bool>("initial_pose_received", false);
transform_handler_->activate();
transform_handler_->waitForTransform();
}
static void TearDownTestCase()
{
transform_handler_->deactivate();
delete config_;
config_ = nullptr;
transform_handler_.reset();
node_.reset();
factory_.reset();
}
protected:
static rclcpp::Node::SharedPtr node_;
static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
};
} // namespace nav2_behavior_tree
rclcpp::Node::SharedPtr nav2_behavior_tree::BehaviorTreeTestFixture::node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::TransformHandler>
nav2_behavior_tree::BehaviorTreeTestFixture::transform_handler_ = nullptr;
BT::NodeConfiguration * nav2_behavior_tree::BehaviorTreeTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
nav2_behavior_tree::BehaviorTreeTestFixture::factory_ = nullptr;
#endif // TEST_BEHAVIOR_TREE_FIXTURE_HPP_
......@@ -37,16 +37,16 @@ using namespace std::chrono; // NOLINT
namespace nav2_behavior_tree
{
class TransformHandler : public rclcpp::Node
class TransformHandler
{
public:
TransformHandler()
: Node("TransformHandler"),
explicit TransformHandler(rclcpp::Node::SharedPtr & node)
: node_(node),
is_active_(false),
base_transform_(nullptr),
tf_broadcaster_(nullptr)
{
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
}
......@@ -66,7 +66,7 @@ public:
is_active_ = true;
// Launch a thread to process the messages for this node
spin_thread_ = std::make_unique<nav2_util::NodeThread>(this);
spin_thread_ = std::make_unique<nav2_util::NodeThread>(node_->get_node_base_interface());
startRobotTransform();
}
......@@ -94,7 +94,7 @@ public:
while (!tf_buffer_->canTransform("map", "base_link", rclcpp::Time(0))) {
std::this_thread::sleep_for(100ms);
}
RCLCPP_INFO(get_logger(), "Transforms are available now!");
RCLCPP_INFO(node_->get_logger(), "Transforms are available now!");
return;
}
throw std::runtime_error("Trying to deactivate while already inactive");
......@@ -116,14 +116,14 @@ public:
private:
void publishRobotTransform()
{
base_transform_->header.stamp = now();
base_transform_->header.stamp = node_->now();
tf_broadcaster_->sendTransform(*base_transform_);
}
void startRobotTransform()
{
// Provide the robot pose transform
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
if (!base_transform_) {
base_transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>();
......@@ -139,10 +139,12 @@ private:
updateRobotPose(robot_pose);
// Publish the transform periodically
transform_timer_ = create_wall_timer(
transform_timer_ = node_->create_wall_timer(
100ms, std::bind(&TransformHandler::publishRobotTransform, this));
}
rclcpp::Node::SharedPtr node_;
bool is_active_;
// A thread for spinning the ROS node
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment