Skip to content

Commit

Permalink
Fix smoother server tests (ros-navigation#3663)
Browse files Browse the repository at this point in the history
* Fix smoother server tests

* Update test_smoother_server.cpp

Signed-off-by: enricosutera <enricosutera@outlook.com>
  • Loading branch information
SteveMacenski authored and enricosutera committed May 19, 2024
1 parent 7479b40 commit 23eaf3c
Showing 1 changed file with 18 additions and 20 deletions.
38 changes: 18 additions & 20 deletions nav2_smoother/test/test_smoother_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -223,21 +223,16 @@ class DummySmootherServer : public nav2_smoother::SmootherServer
};

// Define a test class to hold the context for the tests

class SmootherConfigTest : public ::testing::Test
{
};

class SmootherTest : public ::testing::Test
{
protected:
SmootherTest() {SetUp();}
public:
SmootherTest() {}
~SmootherTest() {}

void SetUp()
void SetUp() override
{
node_lifecycle_ =
std::make_shared<rclcpp_lifecycle::LifecycleNode>(
node_ =
std::make_shared<rclcpp::Node>(
"LifecycleSmootherTestNode", rclcpp::NodeOptions());

smoother_server_ = std::make_shared<DummySmootherServer>();
Expand All @@ -252,10 +247,10 @@ class SmootherTest : public ::testing::Test
smoother_server_->activate();

client_ = rclcpp_action::create_client<SmoothAction>(
node_lifecycle_->get_node_base_interface(),
node_lifecycle_->get_node_graph_interface(),
node_lifecycle_->get_node_logging_interface(),
node_lifecycle_->get_node_waitables_interface(), "smooth_path");
node_->get_node_base_interface(),
node_->get_node_graph_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(), "smooth_path");
std::cout << "Setup complete." << std::endl;
}

Expand All @@ -264,6 +259,9 @@ class SmootherTest : public ::testing::Test
smoother_server_->deactivate();
smoother_server_->cleanup();
smoother_server_->shutdown();
smoother_server_.reset();
client_.reset();
node_.reset();
}

bool sendGoal(
Expand Down Expand Up @@ -291,7 +289,7 @@ class SmootherTest : public ::testing::Test

auto future_goal = client_->async_send_goal(goal);

if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) !=
if (rclcpp::spin_until_future_complete(node_, future_goal) !=
rclcpp::FutureReturnCode::SUCCESS)
{
std::cout << "failed sending goal" << std::endl;
Expand All @@ -315,12 +313,12 @@ class SmootherTest : public ::testing::Test
std::cout << "Getting async result..." << std::endl;
auto future_result = client_->async_get_result(goal_handle_);
std::cout << "Waiting on future..." << std::endl;
rclcpp::spin_until_future_complete(node_lifecycle_, future_result);
rclcpp::spin_until_future_complete(node_, future_result);
std::cout << "future received!" << std::endl;
return future_result.get();
}

std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_lifecycle_;
std::shared_ptr<rclcpp::Node> node_;
std::shared_ptr<DummySmootherServer> smoother_server_;
std::shared_ptr<rclcpp_action::Client<SmoothAction>> client_;
std::shared_ptr<rclcpp_action::ClientGoalHandle<SmoothAction>> goal_handle_;
Expand Down Expand Up @@ -387,7 +385,7 @@ TEST_F(SmootherTest, testingCollisionCheckDisabled)
SUCCEED();
}

TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin)
TEST(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin)
{
auto smoother_server = std::make_shared<DummySmootherServer>();
smoother_server->set_parameter(
Expand All @@ -402,7 +400,7 @@ TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin)
SUCCEED();
}

TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin)
TEST(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin)
{
auto smoother_server = std::make_shared<DummySmootherServer>();
smoother_server->set_parameter(
Expand All @@ -417,7 +415,7 @@ TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin)
SUCCEED();
}

TEST_F(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin)
TEST(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin)
{
auto smoother_server = std::make_shared<DummySmootherServer>();
auto state = smoother_server->configure();
Expand Down

0 comments on commit 23eaf3c

Please sign in to comment.