add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,41 @@
|
||||
ament_add_gtest_executable(test_lifecycle_gtest
|
||||
test_lifecycle_manager.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(test_lifecycle_gtest
|
||||
${library_name}
|
||||
)
|
||||
|
||||
ament_target_dependencies(test_lifecycle_gtest
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
ament_add_test(test_lifecycle
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_lifecycle_test.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 20
|
||||
ENV
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:test_lifecycle_gtest>
|
||||
)
|
||||
|
||||
ament_add_gtest_executable(test_bond_gtest
|
||||
test_bond.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(test_bond_gtest
|
||||
${library_name}
|
||||
)
|
||||
|
||||
ament_target_dependencies(test_bond_gtest
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
ament_add_test(test_bond
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_bond_test.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 20
|
||||
ENV
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:test_bond_gtest>
|
||||
)
|
||||
@@ -0,0 +1,57 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright (c) 2020 Samsung Research America
|
||||
#
|
||||
# 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.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import ExecuteProcess
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_test',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': False},
|
||||
{'autostart': False},
|
||||
{'node_names': ['bond_tester']}]),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable],
|
||||
name='test_bond_gtest',
|
||||
output='screen')
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,58 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright (c) 2020 Shivang Patel
|
||||
#
|
||||
# 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.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import ExecuteProcess
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_test',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': False},
|
||||
{'autostart': False},
|
||||
{'bond_timeout': 0.0},
|
||||
{'node_names': ['lifecycle_node_test']}]),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable],
|
||||
name='test_lifecycle_node_gtest',
|
||||
output='screen')
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,199 @@
|
||||
// Copyright (c) 2020 Samsung Research America
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <chrono>
|
||||
#include <string>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_util/node_thread.hpp"
|
||||
#include "nav2_lifecycle_manager/lifecycle_manager.hpp"
|
||||
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
|
||||
|
||||
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||
|
||||
// minimal lifecycle node implementing bond as in rest of navigation servers
|
||||
class TestLifecycleNode : public nav2_util::LifecycleNode
|
||||
{
|
||||
public:
|
||||
TestLifecycleNode(bool bond, std::string name)
|
||||
: nav2_util::LifecycleNode(name)
|
||||
{
|
||||
state = "";
|
||||
enable_bond = bond;
|
||||
}
|
||||
|
||||
CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Configured!");
|
||||
state = "configured";
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Activated!");
|
||||
state = "activated";
|
||||
if (enable_bond) {
|
||||
createBond();
|
||||
}
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Deactivated!");
|
||||
state = "deactivated";
|
||||
if (enable_bond) {
|
||||
destroyBond();
|
||||
}
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_cleanup(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Cleanup!");
|
||||
state = "cleaned up";
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Shutdown!");
|
||||
state = "shut down";
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is encountered an error!");
|
||||
state = "errored";
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
bool bondAllocated()
|
||||
{
|
||||
return bond_ ? true : false;
|
||||
}
|
||||
|
||||
void breakBond()
|
||||
{
|
||||
bond_->breakBond();
|
||||
}
|
||||
|
||||
std::string getState()
|
||||
{
|
||||
return state;
|
||||
}
|
||||
|
||||
bool isBondEnabled()
|
||||
{
|
||||
return enable_bond;
|
||||
}
|
||||
|
||||
bool isBondConnected()
|
||||
{
|
||||
return bondAllocated() ? !bond_->isBroken() : false;
|
||||
}
|
||||
|
||||
std::string state;
|
||||
bool enable_bond;
|
||||
};
|
||||
|
||||
class TestFixture
|
||||
{
|
||||
public:
|
||||
TestFixture(bool bond, std::string node_name)
|
||||
{
|
||||
lf_node_ = std::make_shared<TestLifecycleNode>(bond, node_name);
|
||||
lf_thread_ = std::make_unique<nav2_util::NodeThread>(lf_node_->get_node_base_interface());
|
||||
}
|
||||
|
||||
std::shared_ptr<TestLifecycleNode> lf_node_;
|
||||
std::unique_ptr<nav2_util::NodeThread> lf_thread_;
|
||||
};
|
||||
|
||||
TEST(LifecycleBondTest, POSITIVE)
|
||||
{
|
||||
// let the lifecycle server come up
|
||||
rclcpp::Rate(1).sleep();
|
||||
|
||||
auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
|
||||
nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
|
||||
|
||||
// create node, should be up now
|
||||
auto fixture = TestFixture(true, "bond_tester");
|
||||
auto bond_tester = fixture.lf_node_;
|
||||
|
||||
EXPECT_TRUE(client.startup());
|
||||
|
||||
// check if bond is connected after being activated
|
||||
rclcpp::Rate(5).sleep();
|
||||
EXPECT_TRUE(bond_tester->isBondConnected());
|
||||
EXPECT_EQ(bond_tester->getState(), "activated");
|
||||
|
||||
bond_tester->breakBond();
|
||||
|
||||
// bond should be disconnected now and lifecycle manager should know and react to reset
|
||||
rclcpp::Rate(5).sleep();
|
||||
EXPECT_EQ(
|
||||
nav2_lifecycle_manager::SystemStatus::INACTIVE,
|
||||
client.is_active(std::chrono::nanoseconds(1000000000)));
|
||||
EXPECT_FALSE(bond_tester->isBondConnected());
|
||||
EXPECT_EQ(bond_tester->getState(), "cleaned up");
|
||||
|
||||
// check that bringing up again is OK
|
||||
EXPECT_TRUE(client.startup());
|
||||
EXPECT_EQ(bond_tester->getState(), "activated");
|
||||
EXPECT_TRUE(bond_tester->isBondConnected());
|
||||
EXPECT_EQ(
|
||||
nav2_lifecycle_manager::SystemStatus::ACTIVE,
|
||||
client.is_active(std::chrono::nanoseconds(1000000000)));
|
||||
|
||||
// clean state for next test.
|
||||
EXPECT_TRUE(client.reset());
|
||||
EXPECT_FALSE(bond_tester->isBondConnected());
|
||||
EXPECT_EQ(bond_tester->getState(), "cleaned up");
|
||||
}
|
||||
|
||||
TEST(LifecycleBondTest, NEGATIVE)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
|
||||
nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
|
||||
|
||||
// create node, now without bond setup to connect to. Should fail because no bond
|
||||
auto fixture = TestFixture(false, "bond_tester");
|
||||
auto bond_tester = fixture.lf_node_;
|
||||
EXPECT_FALSE(client.startup());
|
||||
EXPECT_FALSE(bond_tester->isBondEnabled());
|
||||
EXPECT_EQ(
|
||||
nav2_lifecycle_manager::SystemStatus::INACTIVE,
|
||||
client.is_active(std::chrono::nanoseconds(1000000000)));
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,128 @@
|
||||
// Copyright (c) 2020 Shivang Patel
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <chrono>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "nav2_util/node_thread.hpp"
|
||||
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
|
||||
|
||||
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||
|
||||
class LifecycleNodeTest : public rclcpp_lifecycle::LifecycleNode
|
||||
{
|
||||
public:
|
||||
LifecycleNodeTest()
|
||||
: rclcpp_lifecycle::LifecycleNode("lifecycle_node_test") {}
|
||||
|
||||
CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Configured!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Activated!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Deactivated!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_cleanup(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Cleanup!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is Shutdown!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) override
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Lifecycle Test node is encountered an error!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
class LifecycleClientTestFixture
|
||||
{
|
||||
public:
|
||||
LifecycleClientTestFixture()
|
||||
{
|
||||
lf_node_ = std::make_shared<LifecycleNodeTest>();
|
||||
lf_thread_ = std::make_unique<nav2_util::NodeThread>(lf_node_->get_node_base_interface());
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<LifecycleNodeTest> lf_node_;
|
||||
std::unique_ptr<nav2_util::NodeThread> lf_thread_;
|
||||
};
|
||||
|
||||
TEST(LifecycleClientTest, BasicTest)
|
||||
{
|
||||
LifecycleClientTestFixture fix;
|
||||
auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
|
||||
nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
|
||||
EXPECT_EQ(
|
||||
nav2_lifecycle_manager::SystemStatus::TIMEOUT,
|
||||
client.is_active(std::chrono::nanoseconds(1000)));
|
||||
EXPECT_TRUE(client.startup());
|
||||
EXPECT_EQ(
|
||||
nav2_lifecycle_manager::SystemStatus::ACTIVE,
|
||||
client.is_active(std::chrono::nanoseconds(1000000000)));
|
||||
EXPECT_EQ(
|
||||
nav2_lifecycle_manager::SystemStatus::ACTIVE,
|
||||
client.is_active());
|
||||
EXPECT_TRUE(client.pause());
|
||||
EXPECT_EQ(
|
||||
nav2_lifecycle_manager::SystemStatus::INACTIVE,
|
||||
client.is_active(std::chrono::nanoseconds(1000000000)));
|
||||
EXPECT_TRUE(client.resume());
|
||||
EXPECT_TRUE(client.reset());
|
||||
EXPECT_TRUE(client.shutdown());
|
||||
}
|
||||
|
||||
TEST(LifecycleClientTest, WithoutFixture)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_test_service_client");
|
||||
nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node);
|
||||
EXPECT_EQ(
|
||||
nav2_lifecycle_manager::SystemStatus::TIMEOUT,
|
||||
client.is_active(std::chrono::nanoseconds(1000)));
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
Reference in New Issue
Block a user