From 39ab506d865f830efdff01761a3ed9d681548d57 Mon Sep 17 00:00:00 2001 From: "Carlos A. Orduno" <39749557+orduno@users.noreply.github.com> Date: Wed, 28 Aug 2019 15:50:02 -0700 Subject: [PATCH] Enable global planner tester (#927) --- nav2_system_tests/CMakeLists.txt | 4 +- nav2_system_tests/package.xml | 2 + nav2_system_tests/src/planning/CMakeLists.txt | 2 +- .../src/planning/planner_tester.cpp | 207 +++++++++++------- .../src/planning/planner_tester.hpp | 60 +++-- .../src/planning/test_planner_launch.py | 20 +- .../src/planning/test_planner_node.cpp | 14 +- tools/run_test_suite.bash | 1 + 8 files changed, 190 insertions(+), 120 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index e34a365077..f1bc3bfdb5 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(gazebo_ros_pkgs REQUIRED) find_package(nav2_amcl REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) @@ -30,6 +31,7 @@ set(dependencies gazebo_ros_pkgs geometry_msgs std_msgs + tf2_geometry_msgs rclpy ) @@ -40,7 +42,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) - # add_subdirectory(src/planning) + add_subdirectory(src/planning) add_subdirectory(src/localization) add_subdirectory(src/system) add_subdirectory(src/updown) diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 4f8def6648..2cee9db1c6 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -22,6 +22,7 @@ launch_testing geometry_msgs std_msgs + tf2_geometry_msgs gazebo_ros_pkgs launch_ros launch_testing @@ -39,6 +40,7 @@ geometry_msgs nav2_amcl std_msgs + tf2_geometry_msgs gazebo_ros_pkgs launch_ros launch_testing diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt index bf240381fd..7ad2a7a468 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -7,7 +7,7 @@ ament_target_dependencies(test_planner_node ${dependencies} ) -ament_add_test(test_planner_node +ament_add_test(test_planner GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_planner_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 8c7ab8ecf2..7da49966ce 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include "planner_tester.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -25,6 +27,7 @@ #include "nav2_msgs/msg/costmap_meta_data.hpp" using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT using nav2_util::Costmap; using nav2_util::TestCostmap; @@ -35,19 +38,17 @@ PlannerTester::PlannerTester() : Node("PlannerTester"), map_publish_rate_(100s), map_set_(false), costmap_set_(false), using_fake_costmap_(true), costmap_server_running_(false), trinary_costmap_(true), track_unknown_space_(false), lethal_threshold_(100), unknown_cost_value_(-1), - testCostmapType_(TestCostmap::open_space), spin_thread_(nullptr), spinning_ok_(false) + testCostmapType_(TestCostmap::open_space), spin_thread_(nullptr) { // The client used to invoke the services of the global planner (ComputePathToPose) - auto temp_node = std::shared_ptr(this, [](rclcpp::Node *) {}); - planner_client_ = std::make_unique(temp_node); + planner_client_ = rclcpp_action::create_client( + this->get_node_base_interface(), + this->get_node_graph_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + "ComputePathToPose"); - if (!planner_client_->waitForServer(nav2_behavior_tree::defaultServerTimeout)) { - RCLCPP_ERROR(this->get_logger(), "Planner not running"); - throw std::runtime_error("Planner not running"); - } - - // Publisher of the faked current robot pose - pose_pub_ = this->create_publisher("amcl_pose"); + startRobotPoseProvider(); // For visualization, we'll publish the map map_pub_ = this->create_publisher("map"); @@ -55,16 +56,15 @@ PlannerTester::PlannerTester() // We start with a 10x10 grid with no obstacles loadSimpleCostmap(TestCostmap::open_space); - startCostmapServer("GetCostmap"); + startCostmapServer(); // Launch a thread to process the messages for this node - spinning_ok_ = true; spin_thread_ = new std::thread(&PlannerTester::spinThread, this); } PlannerTester::~PlannerTester() { - spinning_ok_ = false; + executor_.cancel(); spin_thread_->join(); delete spin_thread_; spin_thread_ = nullptr; @@ -72,10 +72,9 @@ PlannerTester::~PlannerTester() void PlannerTester::spinThread() { - while (spinning_ok_) { - rclcpp::spin_some(this->get_node_base_interface()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } + executor_.add_node(this->get_node_base_interface()); + executor_.spin(); + executor_.remove_node(this->get_node_base_interface()); } void PlannerTester::loadDefaultMap() @@ -164,7 +163,18 @@ void PlannerTester::loadSimpleCostmap(const TestCostmap & testCostmapType) using_fake_costmap_ = true; } -void PlannerTester::startCostmapServer(std::string serviceName) +void PlannerTester::startRobotPoseProvider() +{ + transform_publisher_ = create_publisher("/tf", rclcpp::QoS(100)); + + geometry_msgs::msg::Point robot_position; + robot_position.x = 0.0; + robot_position.y = 0.0; + + updateRobotPosition(robot_position); +} + +void PlannerTester::startCostmapServer() { if (!costmap_set_) { RCLCPP_ERROR(this->get_logger(), "Costmap must be set before starting the service"); @@ -177,18 +187,18 @@ void PlannerTester::startCostmapServer(std::string serviceName) std::shared_ptr response) -> void { RCLCPP_DEBUG(this->get_logger(), "Incoming costmap request"); - response->map = costmap_->getCostmap(request->specs); + response->map = costmap_->get_costmap(request->specs); }; // Create a service that will use the callback function to handle requests. costmap_server_ = create_service( - serviceName, costmap_service_callback); + "GetCostmap", costmap_service_callback); costmap_server_running_ = true; } bool PlannerTester::defaultPlannerTest( - nav2_behavior_tree::ComputePathToPoseResult::SharedPtr & path, + ComputePathToPoseResult & path, const double /*deviation_tolerance*/) { if (!costmap_set_) { @@ -196,9 +206,11 @@ bool PlannerTester::defaultPlannerTest( return false; } + waitForPlanner(); + // TODO(orduno) #443 Add support for planners that take into account robot orientation geometry_msgs::msg::Point robot_position; - auto goal = std::make_shared(); + ComputePathToPoseCommand goal; auto costmap_properties = costmap_->get_properties(); if (using_fake_costmap_) { @@ -207,8 +219,8 @@ bool PlannerTester::defaultPlannerTest( robot_position.x = 1.0; robot_position.y = 1.0; - goal->pose.position.x = 8.0; - goal->pose.position.y = 8.0; + goal.pose.position.x = 8.0; + goal.pose.position.y = 8.0; } else { RCLCPP_DEBUG(this->get_logger(), "Planning using the provided map"); @@ -218,8 +230,8 @@ bool PlannerTester::defaultPlannerTest( robot_position.x = 390.0; robot_position.y = 10.0; - goal->pose.position.x = 10.0; - goal->pose.position.y = 390.0; + goal.pose.position.x = 10.0; + goal.pose.position.y = 390.0; } // TODO(orduno): #443 On a default test, provide the reference path to compare with the planner @@ -243,11 +255,15 @@ bool PlannerTester::defaultPlannerRandomTests( return false; } + waitForPlanner(); + // Initialize random number generator std::random_device random_device; std::mt19937 generator(random_device()); - std::uniform_int_distribution<> distribution_x(0, costmap_->get_properties().size_x); - std::uniform_int_distribution<> distribution_y(0, costmap_->get_properties().size_y); + + // Obtain random positions within map + std::uniform_int_distribution<> distribution_x(1, costmap_->get_properties().size_x - 1); + std::uniform_int_distribution<> distribution_y(1, costmap_->get_properties().size_y - 1); auto generate_random = [&]() mutable -> std::pair { bool point_is_free = false; @@ -255,19 +271,20 @@ bool PlannerTester::defaultPlannerRandomTests( while (!point_is_free) { x = distribution_x(generator); y = distribution_y(generator); - point_is_free = costmap_->isFree(x, y); + point_is_free = costmap_->is_free(x, y); } return std::make_pair(x, y); }; // TODO(orduno) #443 Add support for planners that take into account robot orientation geometry_msgs::msg::Point robot_position; - auto goal = std::make_shared(); - auto path = std::make_shared(); + ComputePathToPoseCommand goal; + ComputePathToPoseResult path; unsigned int num_fail = 0; + auto start = high_resolution_clock::now(); for (unsigned int test_num = 0; test_num < number_tests; ++test_num) { - RCLCPP_INFO(this->get_logger(), "Running test #%u", test_num + 1); + RCLCPP_DEBUG(this->get_logger(), "Running test #%u", test_num + 1); // Compose the robot start position and goal using random numbers // Defined with respect to world coordinate system @@ -278,18 +295,21 @@ bool PlannerTester::defaultPlannerRandomTests( robot_position.y = vals.second; vals = generate_random(); - goal->pose.position.x = vals.first; - goal->pose.position.y = vals.second; + goal.pose.position.x = vals.first; + goal.pose.position.y = vals.second; if (!plannerTest(robot_position, goal, path)) { RCLCPP_WARN(this->get_logger(), "Failed with start at %0.2f, %0.2f and goal at %0.2f, %0.2f", - robot_position.x, robot_position.y, goal->pose.position.x, goal->pose.position.y); + robot_position.x, robot_position.y, goal.pose.position.x, goal.pose.position.y); ++num_fail; } } + auto end = high_resolution_clock::now(); + auto elapsed = duration_cast(end - start); RCLCPP_INFO(this->get_logger(), - "Tested with %u tests. Planner failed on %u", number_tests, num_fail); + "Tested with %u tests. Planner failed on %u. Test time %u ms", + number_tests, num_fail, elapsed.count()); if ((num_fail / number_tests) > acceptable_fail_ratio) { return false; @@ -300,13 +320,13 @@ bool PlannerTester::defaultPlannerRandomTests( bool PlannerTester::plannerTest( const geometry_msgs::msg::Point & robot_position, - const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr & goal, - nav2_behavior_tree::ComputePathToPoseResult::SharedPtr & path) + const ComputePathToPoseCommand & goal, + ComputePathToPoseResult & path) { RCLCPP_DEBUG(this->get_logger(), "Getting the path from the planner"); // First make available the current robot position for the planner to take as starting point - publishRobotPosition(robot_position); + updateRobotPosition(robot_position); // Then request to compute a path TaskStatus status = sendRequest(goal, path); @@ -319,47 +339,68 @@ bool PlannerTester::plannerTest( // TODO(orduno): #443 check why task may report success while planner returns a path of 0 points RCLCPP_DEBUG(this->get_logger(), "Got path, checking for possible collisions"); - return isCollisionFree(*path) && isWithinTolerance(robot_position, *goal, *path); + return isCollisionFree(path) && isWithinTolerance(robot_position, goal, path); } return false; } -void PlannerTester::publishRobotPosition(const geometry_msgs::msg::Point & position) const +void PlannerTester::updateRobotPosition(const geometry_msgs::msg::Point & position) { - geometry_msgs::msg::PoseWithCovarianceStamped p; - p.header.frame_id = "map"; - p.header.stamp = rclcpp::Time(); - p.pose.pose.position = position; - p.pose.pose.orientation.x = 0.0; - p.pose.pose.orientation.y = 0.0; - p.pose.pose.orientation.z = 0.0; - p.pose.pose.orientation.w = 1.0; - - for (int i = 0; i < 12; i++) { - p.pose.covariance[i] = 0.0; - } - - pose_pub_->publish(p); + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped.header.frame_id = "map"; + tf_stamped.header.stamp = now() + rclcpp::Duration(1.0); + tf_stamped.child_frame_id = "base_link"; + tf_stamped.transform.translation.x = position.x; + tf_stamped.transform.translation.y = position.y; + tf_stamped.transform.rotation.w = 1.0; + + tf2_msgs::msg::TFMessage tf_message; + tf_message.transforms.push_back(tf_stamped); + transform_publisher_->publish(tf_message); } TaskStatus PlannerTester::sendRequest( - const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr & goal, - nav2_behavior_tree::ComputePathToPoseResult::SharedPtr & path) + const ComputePathToPoseCommand & goal, + ComputePathToPoseResult & path) { - planner_client_->sendCommand(goal); + nav2_msgs::action::ComputePathToPose::Goal action_goal; + action_goal.pose = goal; + auto future_goal_handle = planner_client_->async_send_goal(action_goal); + + RCLCPP_DEBUG(this->get_logger(), "Waiting for goal acceptance"); + auto status_request = future_goal_handle.wait_for(seconds(5)); + if (status_request != std::future_status::ready) { + RCLCPP_ERROR(this->get_logger(), "Failed to send the goal"); + return TaskStatus::FAILED; + } + + auto goal_handle = future_goal_handle.get(); + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal rejected"); + return TaskStatus::FAILED; + } - // Loop until the subtask is completed - while (true) { - TaskStatus status = planner_client_->waitForResult(path, std::chrono::milliseconds(100)); + auto future_result = planner_client_->async_get_result(goal_handle); - if (status != TaskStatus::RUNNING) { - return status; - } + RCLCPP_DEBUG(this->get_logger(), "Wait for the server to be done with the action"); + auto status_result = future_result.wait_for(seconds(10)); + if (status_result != std::future_status::ready) { + RCLCPP_ERROR(this->get_logger(), "Failed to get a plan within the allowed time"); + return TaskStatus::FAILED; } + + auto result = future_result.get(); + if (result.code != rclcpp_action::ResultCode::SUCCEEDED) { + return TaskStatus::FAILED; + } + + path = result.result->path; + + return TaskStatus::SUCCEEDED; } -bool PlannerTester::isCollisionFree(const nav2_behavior_tree::ComputePathToPoseResult & path) +bool PlannerTester::isCollisionFree(const ComputePathToPoseResult & path) { // At each point of the path, check if the corresponding cell is free @@ -373,7 +414,7 @@ bool PlannerTester::isCollisionFree(const nav2_behavior_tree::ComputePathToPoseR bool collisionFree = true; for (auto pose : path.poses) { - collisionFree = costmap_->isFree( + collisionFree = costmap_->is_free( static_cast(std::round(pose.position.x)), static_cast(std::round(pose.position.y))); @@ -391,19 +432,19 @@ bool PlannerTester::isCollisionFree(const nav2_behavior_tree::ComputePathToPoseR bool PlannerTester::isWithinTolerance( const geometry_msgs::msg::Point & robot_position, - const nav2_behavior_tree::ComputePathToPoseCommand & goal, - const nav2_behavior_tree::ComputePathToPoseResult & path) const + const ComputePathToPoseCommand & goal, + const ComputePathToPoseResult & path) const { return isWithinTolerance( - robot_position, goal, path, 0.0, nav2_behavior_tree::ComputePathToPoseResult()); + robot_position, goal, path, 0.0, ComputePathToPoseResult()); } bool PlannerTester::isWithinTolerance( const geometry_msgs::msg::Point & robot_position, - const nav2_behavior_tree::ComputePathToPoseCommand & goal, - const nav2_behavior_tree::ComputePathToPoseResult & path, + const ComputePathToPoseCommand & goal, + const ComputePathToPoseResult & path, const double /*deviationTolerance*/, - const nav2_behavior_tree::ComputePathToPoseResult & /*reference_path*/) const + const ComputePathToPoseResult & /*reference_path*/) const { // TODO(orduno) #443 Work in progress, for now we only check that the path start matches the // robot start location and that the path end matches the goal. @@ -440,14 +481,28 @@ bool PlannerTester::sendCancel() return false; } -void PlannerTester::printPath(const nav2_behavior_tree::ComputePathToPoseResult & path) const +void PlannerTester::printPath(const ComputePathToPoseResult & path) const { - int index = 0; + auto index = 0; + auto ss = std::stringstream{}; + for (auto pose : path.poses) { - RCLCPP_INFO(get_logger(), " point %u x: %0.2f, y: %0.2f", - index, pose.position.x, pose.position.y); + ss << " point #" << index << " with" << + " x: " << std::setprecision(3) << pose.position.x << + " y: " << std::setprecision(3) << pose.position.y << '\n'; ++index; } + + RCLCPP_INFO(get_logger(), ss.str().c_str()); } +void PlannerTester::waitForPlanner() +{ + RCLCPP_DEBUG(this->get_logger(), "Waiting for ComputePathToPose action server"); + + if (!planner_client_ || !planner_client_->wait_for_action_server(10s)) { + RCLCPP_ERROR(this->get_logger(), "Planner not running"); + throw std::runtime_error("Planner not running"); + } +} } // namespace nav2_system_tests diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 75d8cef2ee..c2d7902ef3 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -21,20 +21,33 @@ #include #include "rclcpp/rclcpp.hpp" -#include "nav2_behavior_tree/compute_path_to_pose_task.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_msgs/action/compute_path_to_pose.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_msgs/msg/costmap.hpp" #include "nav2_msgs/srv/get_costmap.hpp" #include "visualization_msgs/msg/marker.hpp" #include "nav2_util/costmap.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_msgs/msg/tf_message.hpp" namespace nav2_system_tests { +enum class TaskStatus : int8_t +{ + SUCCEEDED = 1, + FAILED = 2, + RUNNING = 3, +}; + class PlannerTester : public rclcpp::Node, public ::testing::Test { public: + using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; + using ComputePathToPoseResult = nav2_msgs::msg::Path; + PlannerTester(); ~PlannerTester(); @@ -51,15 +64,15 @@ class PlannerTester : public rclcpp::Node, public ::testing::Test // TODO(orduno): #443 Assuming a robot the size of a costmap cell bool plannerTest( const geometry_msgs::msg::Point & robot_position, - const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr & goal, - nav2_behavior_tree::ComputePathToPoseResult::SharedPtr & path); + const ComputePathToPoseCommand & goal, + ComputePathToPoseResult & path); // Sends the request to the planner and gets the result. // Uses the default map or preloaded costmaps. // Success criteria is a collision free path and a deviation to a // reference path smaller than a tolerance. bool defaultPlannerTest( - nav2_behavior_tree::ComputePathToPoseResult::SharedPtr & path, + ComputePathToPoseResult & path, const double deviation_tolerance = 1.0); bool defaultPlannerRandomTests( @@ -72,28 +85,29 @@ class PlannerTester : public rclcpp::Node, public ::testing::Test private: void setCostmap(); - void startCostmapServer(std::string serviceName); + void startRobotPoseProvider(); + void startCostmapServer(); - nav2_behavior_tree::TaskStatus sendRequest( - const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr & goal, - nav2_behavior_tree::ComputePathToPoseResult::SharedPtr & path + TaskStatus sendRequest( + const ComputePathToPoseCommand & goal, + ComputePathToPoseResult & path ); - bool isCollisionFree(const nav2_behavior_tree::ComputePathToPoseResult & path); + bool isCollisionFree(const ComputePathToPoseResult & path); bool isWithinTolerance( const geometry_msgs::msg::Point & robot_position, - const nav2_behavior_tree::ComputePathToPoseCommand & goal, - const nav2_behavior_tree::ComputePathToPoseResult & path) const; + const ComputePathToPoseCommand & goal, + const ComputePathToPoseResult & path) const; bool isWithinTolerance( const geometry_msgs::msg::Point & robot_position, - const nav2_behavior_tree::ComputePathToPoseCommand & goal, - const nav2_behavior_tree::ComputePathToPoseResult & path, + const ComputePathToPoseCommand & goal, + const ComputePathToPoseResult & path, const double deviationTolerance, - const nav2_behavior_tree::ComputePathToPoseResult & reference_path) const; + const ComputePathToPoseResult & reference_path) const; - void printPath(const nav2_behavior_tree::ComputePathToPoseResult & path) const; + void printPath(const ComputePathToPoseResult & path) const; // The static map std::shared_ptr map_; @@ -102,15 +116,17 @@ class PlannerTester : public rclcpp::Node, public ::testing::Test std::unique_ptr costmap_; // The interface to the global planner - std::unique_ptr planner_client_; + std::shared_ptr> planner_client_; std::string plannerName_; + void waitForPlanner(); - // Server for providing a costmap + // The tester must provide the costmap service rclcpp::Service::SharedPtr costmap_server_; - // Publisher of the robot position - rclcpp::Publisher::SharedPtr pose_pub_; - void publishRobotPosition(const geometry_msgs::msg::Point & position) const; + // The tester must provide the robot pose through a transform + rclcpp::Publisher::SharedPtr transform_publisher_; + + void updateRobotPosition(const geometry_msgs::msg::Point & position); // Occupancy grid publisher for visualization rclcpp::Publisher::SharedPtr map_pub_; @@ -133,7 +149,7 @@ class PlannerTester : public rclcpp::Node, public ::testing::Test // A thread for spinning the ROS node void spinThread(); std::thread * spin_thread_; - std::atomic spinning_ok_; + rclcpp::executors::SingleThreadedExecutor executor_; }; } // namespace nav2_system_tests diff --git a/nav2_system_tests/src/planning/test_planner_launch.py b/nav2_system_tests/src/planning/test_planner_launch.py index 7ddfa9efb0..c73077e32a 100755 --- a/nav2_system_tests/src/planning/test_planner_launch.py +++ b/nav2_system_tests/src/planning/test_planner_launch.py @@ -29,28 +29,26 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') - ld = LaunchDescription([launch_ros.actions.Node( + run_navfn = launch_ros.actions.Node( package='nav2_navfn_planner', node_executable='navfn_planner', - output='screen'), - ]) - - test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_planner_node', - output='screen' - ) - + output='screen') run_lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', node_executable='lifecycle_manager', node_name='lifecycle_manager', output='screen', parameters=[{'node_names': ['navfn_planner']}, {'autostart': True}]) + ld = LaunchDescription([run_navfn, run_lifecycle_manager]) + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_planner_node', + output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) - lts.add_test_action(ld, run_lifecycle_manager) ls = LaunchService(argv=argv) ls.include_launch_description(ld) return lts.run(ls) diff --git a/nav2_system_tests/src/planning/test_planner_node.cpp b/nav2_system_tests/src/planning/test_planner_node.cpp index 5b3fb05ee6..d7d5af3929 100644 --- a/nav2_system_tests/src/planning/test_planner_node.cpp +++ b/nav2_system_tests/src/planning/test_planner_node.cpp @@ -20,9 +20,13 @@ #include "planner_tester.hpp" using namespace std::chrono_literals; + using nav2_system_tests::PlannerTester; using nav2_util::TestCostmap; +using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; +using ComputePathToPoseResult = nav2_msgs::msg::Path; + // rclcpp::init can only be called once per process, so this needs to be a global variable class RclCppFixture { @@ -44,7 +48,7 @@ TEST_F(PlannerTester, testSimpleCostmaps) TestCostmap::maze2 }; - auto result = std::make_shared(); + ComputePathToPoseResult result; for (auto costmap : costmaps) { loadSimpleCostmap(costmap); @@ -52,16 +56,8 @@ TEST_F(PlannerTester, testSimpleCostmaps) } } -TEST_F(PlannerTester, testWithOneFixedEndpoint) -{ - loadDefaultMap(); - auto result = std::make_shared(); - EXPECT_EQ(true, defaultPlannerTest(result)); -} - TEST_F(PlannerTester, testWithHundredRandomEndPoints) { loadDefaultMap(); - auto result = std::make_shared(); EXPECT_EQ(true, defaultPlannerRandomTests(100, 0.1)); } diff --git a/tools/run_test_suite.bash b/tools/run_test_suite.bash index ee0c68a801..434f69a2b5 100755 --- a/tools/run_test_suite.bash +++ b/tools/run_test_suite.bash @@ -23,5 +23,6 @@ colcon test --packages-select nav2_system_tests --ctest-args --exclude-regex "te colcon test-result --verbose $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_localization$ +$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner$ $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator$ $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator_with_dijkstra$