From 6419307fd59c35131caa01eb1d7f6a80b27bbf47 Mon Sep 17 00:00:00 2001 From: Carlos Orduno Date: Tue, 20 Aug 2019 17:31:20 -0700 Subject: [PATCH] Providing robot pose through transforms --- nav2_system_tests/CMakeLists.txt | 2 + nav2_system_tests/package.xml | 2 + .../src/planning/planner_tester.cpp | 49 +++++++------------ .../src/planning/planner_tester.hpp | 13 ++--- .../src/planning/test_planner_node.cpp | 7 --- 5 files changed, 29 insertions(+), 44 deletions(-) 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();