From ec7c72dc13ae853d863d52cda8648219febf2d49 Mon Sep 17 00:00:00 2001 From: Alyssa Agnissan Date: Fri, 23 Aug 2024 14:15:55 +0200 Subject: [PATCH] tests added for optimal path publication Signed-off-by: Alyssa Agnissan --- .../test/trajectory_visualizer_tests.cpp | 73 ++++++++++++++++++- 1 file changed, 69 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 7ebada2a6a2..bec2dea1c86 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -59,7 +59,8 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "map", parameters_handler.get()); vis.on_activate(); - vis.visualize(pub_path); + builtin_interfaces::msg::Time bogus_stamp; + vis.visualize(pub_path, bogus_stamp); rclcpp::spin_some(node->get_node_base_interface()); EXPECT_EQ(recieved_path.poses.size(), 5u); @@ -83,7 +84,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) vis.on_activate(); vis.add(optimal_trajectory, "Optimal Trajectory"); nav_msgs::msg::Path bogus_path; - vis.visualize(bogus_path); + builtin_interfaces::msg::Time bogus_stamp; + vis.visualize(bogus_path, bogus_stamp); rclcpp::spin_some(node->get_node_base_interface()); EXPECT_EQ(recieved_msg.markers.size(), 0u); @@ -91,7 +93,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) // Now populated with content, should publish optimal_trajectory = xt::ones({20, 2}); vis.add(optimal_trajectory, "Optimal Trajectory"); - vis.visualize(bogus_path); + vis.visualize(bogus_path, bogus_stamp); rclcpp::spin_some(node->get_node_base_interface()); @@ -147,9 +149,72 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) vis.on_activate(); vis.add(candidate_trajectories, "Candidate Trajectories"); nav_msgs::msg::Path bogus_path; - vis.visualize(bogus_path); + builtin_interfaces::msg::Time bogus_stamp; + vis.visualize(bogus_path, bogus_stamp); rclcpp::spin_some(node->get_node_base_interface()); // 40 * 4, for 5 trajectory steps + 3 point steps EXPECT_EQ(recieved_msg.markers.size(), 160u); } + +TEST(TrajectoryVisualizerTests, VisOptimalPath) +{ + auto node = std::make_shared("my_node"); + auto parameters_handler = std::make_unique(node); + builtin_interfaces::msg::Time cmd_stamp; + cmd_stamp.sec = 5; + cmd_stamp.nanosec = 10; + + nav_msgs::msg::Path recieved_path; + auto my_sub = node->create_subscription( + "/local_plan", 10, + [&](const nav_msgs::msg::Path msg) {recieved_path = msg;}); + + // optimal_trajectory empty, should fail to publish + xt::xtensor optimal_trajectory; + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + vis.add(optimal_trajectory, "Optimal Trajectory"); + nav_msgs::msg::Path bogus_path; + vis.visualize(bogus_path, cmd_stamp); + + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_EQ(recieved_path.poses.size(), 0u); + + // Now populated with content, should publish + optimal_trajectory.resize({20, 2}); + for (unsigned int i = 0; i != optimal_trajectory.shape()[0] - 1; i++) { + optimal_trajectory(i, 0) = static_cast(i); + optimal_trajectory(i, 1) = static_cast(i); + } + vis.add(optimal_trajectory, "Optimal Trajectory"); + vis.visualize(bogus_path, cmd_stamp); + + rclcpp::spin_some(node->get_node_base_interface()); + + // Should have a 20 points path in the map frame and with same stamp than velocity command + EXPECT_EQ(recieved_path.poses.size(), 20u); + EXPECT_EQ(recieved_path.header.frame_id, "fkmap"); + EXPECT_EQ(recieved_path.header.stamp.sec, cmd_stamp.sec); + EXPECT_EQ(recieved_path.header.stamp.nanosec, cmd_stamp.nanosec); + + tf2::Quaternion quat; + for (unsigned int i = 0; i != recieved_path.poses.size() - 1; i++) { + // Poses should be in map frame too + EXPECT_EQ(recieved_path.poses[i].header.frame_id, "fkmap"); + + // Check positions are correct + EXPECT_EQ(recieved_path.poses[i].pose.position.x, static_cast(i)); + EXPECT_EQ(recieved_path.poses[i].pose.position.y, static_cast(i)); + EXPECT_EQ(recieved_path.poses[i].pose.position.z, 0.0); + + // Check orientations are correct + quat.setRPY(0., 0., optimal_trajectory(i, 2)); + auto expected_orientation = tf2::toMsg(quat); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.x, expected_orientation.x); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.y, expected_orientation.y); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.z, expected_orientation.z); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.w, expected_orientation.w); + } +}