diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index a66b6d5ab8..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
)
diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml
index f7c29380d1..01c283ae1c 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
@@ -38,6 +39,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/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp
index fe5e381231..eda7b3167a 100644
--- a/nav2_system_tests/src/planning/planner_tester.cpp
+++ b/nav2_system_tests/src/planning/planner_tester.cpp
@@ -48,7 +48,7 @@ PlannerTester::PlannerTester()
this->get_node_waitables_interface(),
"ComputePathToPose");
- startRobotPoseServer();
+ startRobotPoseProvider();
// For visualization, we'll publish the map
map_pub_ = this->create_publisher("map");
@@ -163,28 +163,15 @@ void PlannerTester::loadSimpleCostmap(const TestCostmap & testCostmapType)
using_fake_costmap_ = true;
}
-void PlannerTester::startRobotPoseServer()
+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);
-
- auto robot_pose_service_callback = [this](
- const std::shared_ptr/*request_header*/,
- const std::shared_ptr/*request*/,
- std::shared_ptr response) -> void
- {
- std::lock_guard lock(update_robot_pose_);
- RCLCPP_DEBUG(this->get_logger(), "Incoming robot pose request");
- response->pose = robot_pose_;
- response->is_pose_valid = true;
- };
-
- // Create a service that will use the callback function to handle requests.
- get_robot_pose_server_ = create_service(
- "GetRobotPose", robot_pose_service_callback);
}
void PlannerTester::startCostmapServer()
@@ -360,17 +347,17 @@ bool PlannerTester::plannerTest(
void PlannerTester::updateRobotPosition(const geometry_msgs::msg::Point & position)
{
- std::lock_guard lock(update_robot_pose_);
-
- robot_pose_.header.frame_id = "map";
- robot_pose_.header.stamp = rclcpp::Time();
-
- robot_pose_.pose.position = position;
-
- robot_pose_.pose.orientation.x = 0.0;
- robot_pose_.pose.orientation.y = 0.0;
- robot_pose_.pose.orientation.z = 0.0;
- robot_pose_.pose.orientation.w = 1.0;
+ 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(
@@ -499,9 +486,9 @@ void PlannerTester::printPath(const ComputePathToPoseResult & path) const
auto ss = std::stringstream{};
for (auto pose : path.poses) {
- ss << " point #" << index << " with" <<
- " x: " << std::setprecision(3) << pose.position.x <<
- " y: " << std::setprecision(3) << pose.position.y << '\n';
+ ss << " point #" << index << " with" <<
+ " x: " << std::setprecision(3) << pose.position.x <<
+ " y: " << std::setprecision(3) << pose.position.y << '\n';
++index;
}
diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp
index 79e0b8a38f..c2d7902ef3 100644
--- a/nav2_system_tests/src/planning/planner_tester.hpp
+++ b/nav2_system_tests/src/planning/planner_tester.hpp
@@ -26,10 +26,11 @@
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_msgs/srv/get_costmap.hpp"
-#include "nav2_msgs/srv/get_robot_pose.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "nav2_util/costmap.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
{
@@ -84,7 +85,7 @@ class PlannerTester : public rclcpp::Node, public ::testing::Test
private:
void setCostmap();
- void startRobotPoseServer();
+ void startRobotPoseProvider();
void startCostmapServer();
TaskStatus sendRequest(
@@ -119,12 +120,12 @@ class PlannerTester : public rclcpp::Node, public ::testing::Test
std::string plannerName_;
void waitForPlanner();
- // The tester must provide these services
+ // The tester must provide the costmap service
rclcpp::Service::SharedPtr costmap_server_;
- rclcpp::Service::SharedPtr get_robot_pose_server_;
- geometry_msgs::msg::PoseStamped robot_pose_;
- mutable std::mutex update_robot_pose_;
+ // 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
diff --git a/nav2_system_tests/src/planning/test_planner_node.cpp b/nav2_system_tests/src/planning/test_planner_node.cpp
index 01c4c3eceb..d7d5af3929 100644
--- a/nav2_system_tests/src/planning/test_planner_node.cpp
+++ b/nav2_system_tests/src/planning/test_planner_node.cpp
@@ -56,13 +56,6 @@ TEST_F(PlannerTester, testSimpleCostmaps)
}
}
-TEST_F(PlannerTester, testWithOneFixedEndpoint)
-{
- loadDefaultMap();
- ComputePathToPoseResult result;
- EXPECT_EQ(true, defaultPlannerTest(result));
-}
-
TEST_F(PlannerTester, testWithHundredRandomEndPoints)
{
loadDefaultMap();