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$