tests added for optimal path publication

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

Author: alyquantillion

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -106,7 +106,9 @@ class MPPIController : public nav2_core::Controller
     * @brief Visualize trajectories
     * @param transformed_plan Transformed input plan
     */
-  void visualize(nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp);
+  void visualize(
+    nav_msgs::msg::Path transformed_plan,
+    const builtin_interfaces::msg::Time & cmd_stamp);
 
   std::string name_;
   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

@@ -97,7 +97,7 @@ class TrajectoryVisualizer
     * @brief Visualize the plan
     * @param plan Plan to visualize
     */
-  void visualize(const nav_msgs::msg::Path & plan,const builtin_interfaces::msg::Time & cmd_stamp);
+  void visualize(const nav_msgs::msg::Path & plan, const builtin_interfaces::msg::Time & cmd_stamp);
 
   /**
     * @brief Reset object

nav2_mppi_controller/src/controller.cpp

@@ -105,7 +105,9 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   return cmd;
 }
 
-void MPPIController::visualize(nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp)
+void MPPIController::visualize(
+  nav_msgs::msg::Path transformed_plan,
+  const builtin_interfaces::msg::Time & cmd_stamp)
 {
   trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
   trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");

nav2_mppi_controller/src/trajectory_visualizer.cpp

@@ -120,7 +120,9 @@ void TrajectoryVisualizer::reset()
   optimal_traj_ = std::make_unique<xt::xtensor<float, 2>>();
 }
 
-void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan, const builtin_interfaces::msg::Time & cmd_stamp)
+void TrajectoryVisualizer::visualize(
+  const nav_msgs::msg::Path & plan,
+  const builtin_interfaces::msg::Time & cmd_stamp)
 {
   if (trajectories_publisher_->get_subscription_count() > 0) {
     trajectories_publisher_->publish(std::move(points_));

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);
+  }
+}