tests added for optimal path publication

Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io>

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,15 +84,16 @@ 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);
 
  // Now populated with content, should publish
  optimal_trajectory = xt::ones<float>({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<rclcpp_lifecycle::LifecycleNode>("my_node");
+ auto parameters_handler = std::make_unique<ParametersHandler>(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<nav_msgs::msg::Path>(
+ "/local_plan", 10,
+ [&](const nav_msgs::msg::Path msg) {recieved_path = msg;});
+
+ // optimal_trajectory empty, should fail to publish
+ xt::xtensor<float, 2> 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<float>(i);
+ optimal_trajectory(i, 1) = static_cast<float>(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<float>(i));
+ EXPECT_EQ(recieved_path.poses[i].pose.position.y, static_cast<float>(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);
+ }
+}