Providing robot pose through transforms

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
 )
 

nav2_system_tests/package.xml

@@ -22,6 +22,7 @@
   <build_depend>launch_testing</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>gazebo_ros_pkgs</build_depend>
   <build_depend>launch_ros</build_depend>
   <build_depend>launch_testing</build_depend>
@@ -38,6 +39,7 @@
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav2_amcl</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf2_geometry_msgs</exec_depend>
   <exec_depend>gazebo_ros_pkgs</exec_depend>
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>launch_testing</exec_depend>

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<nav_msgs::msg::OccupancyGrid>("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<tf2_msgs::msg::TFMessage>("/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<rmw_request_id_t>/*request_header*/,
-    const std::shared_ptr<nav2_msgs::srv::GetRobotPose::Request>/*request*/,
-    std::shared_ptr<nav2_msgs::srv::GetRobotPose::Response> response) -> void
-    {
-      std::lock_guard<std::mutex> 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<nav2_msgs::srv::GetRobotPose>(
-    "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<std::mutex> 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;
   }
 

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<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_server_;
-  rclcpp::Service<nav2_msgs::srv::GetRobotPose>::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<tf2_msgs::msg::TFMessage>::SharedPtr transform_publisher_;
+
   void updateRobotPosition(const geometry_msgs::msg::Point & position);
 
   // Occupancy grid publisher for visualization

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();