diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp index 6f8b05ece2b..ae041c1d32b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp @@ -79,7 +79,9 @@ class TrajectoryVisualizer * @brief Add an optimal trajectory to visualize * @param trajectory Optimal trajectory */ - void add(const xt::xtensor & trajectory, const std::string & marker_namespace); + void add( + const xt::xtensor & trajectory, const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp); /** * @brief Add candidate trajectories to visualize @@ -91,13 +93,15 @@ class TrajectoryVisualizer * @brief Publish the optimal trajectory in the form of a path message * @param trajectory Optimal trajectory */ - void publish_optimal_path(const builtin_interfaces::msg::Time & cmd_stamp); + void populate_optimal_path( + const xt::xtensor & optimal_traj, + const builtin_interfaces::msg::Time & cmd_stamp); /** * @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); /** * @brief Reset object @@ -111,7 +115,7 @@ class TrajectoryVisualizer std::shared_ptr> transformed_path_pub_; std::shared_ptr> optimal_path_pub_; - std::unique_ptr> optimal_traj_; + std::unique_ptr optimal_path_; std::unique_ptr points_; int marker_id_ = 0; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 238e05d4b7d..54eb1f57a08 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -110,8 +110,8 @@ void MPPIController::visualize( const builtin_interfaces::msg::Time & cmd_stamp) { trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); - trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory"); - trajectory_visualizer_.visualize(std::move(transformed_plan), cmd_stamp); + trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp); + trajectory_visualizer_.visualize(std::move(transformed_plan)); } void MPPIController::setPlan(const nav_msgs::msg::Path & path) diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 97d05cbf005..ed2d3e89b16 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -28,7 +28,7 @@ void TrajectoryVisualizer::on_configure( trajectories_publisher_ = node->create_publisher("/trajectories", 1); transformed_path_pub_ = node->create_publisher("transformed_global_plan", 1); - optimal_path_pub_ = node->create_publisher("/local_plan", 1); + optimal_path_pub_ = node->create_publisher("optimal_trajectory", 1); parameters_handler_ = parameters_handler; auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer"); @@ -61,7 +61,9 @@ void TrajectoryVisualizer::on_deactivate() } void TrajectoryVisualizer::add( - const xt::xtensor & trajectory, const std::string & marker_namespace) + const xt::xtensor & trajectory, + const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp) { auto & size = trajectory.shape()[0]; if (!size) { @@ -86,7 +88,7 @@ void TrajectoryVisualizer::add( add_marker(i); } - optimal_traj_ = std::make_unique>(trajectory); + populate_optimal_path(trajectory, cmd_stamp); } void TrajectoryVisualizer::add( @@ -117,19 +119,17 @@ void TrajectoryVisualizer::reset() { marker_id_ = 0; points_ = std::make_unique(); - optimal_traj_ = std::make_unique>(); + optimal_path_ = std::make_unique(); } -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) { if (trajectories_publisher_->get_subscription_count() > 0) { trajectories_publisher_->publish(std::move(points_)); } if (optimal_path_pub_->get_subscription_count() > 0) { - publish_optimal_path(cmd_stamp); + optimal_path_pub_->publish(std::move(optimal_path_)); } reset(); @@ -140,16 +140,17 @@ void TrajectoryVisualizer::visualize( } } -void TrajectoryVisualizer::publish_optimal_path(const builtin_interfaces::msg::Time & cmd_stamp) +void TrajectoryVisualizer::populate_optimal_path( + const xt::xtensor & optimal_traj, + const builtin_interfaces::msg::Time & cmd_stamp) { - auto & size = optimal_traj_->shape()[0]; + auto & size = optimal_traj.shape()[0]; if (size == 0) { return; } - nav_msgs::msg::Path optimal_path; - optimal_path.header.stamp = cmd_stamp; - optimal_path.header.frame_id = frame_id_; + optimal_path_->header.stamp = cmd_stamp; + optimal_path_->header.frame_id = frame_id_; for (size_t i = 0; i < size; i++) { // create new pose for the path @@ -157,16 +158,15 @@ void TrajectoryVisualizer::publish_optimal_path(const builtin_interfaces::msg::T pose_stamped.header.frame_id = frame_id_; // position & orientation - pose_stamped.pose = utils::createPose((*optimal_traj_)(i, 0), (*optimal_traj_)(i, 1), 0.0); + pose_stamped.pose = utils::createPose(optimal_traj(i, 0), optimal_traj(i, 1), 0.0); tf2::Quaternion quaternion_tf2; - quaternion_tf2.setRPY(0., 0., (*optimal_traj_)(i, 2)); + quaternion_tf2.setRPY(0., 0., optimal_traj(i, 2)); pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2); // add pose to the path - optimal_path.poses.push_back(pose_stamped); + optimal_path_->poses.push_back(pose_stamped); } - optimal_path_pub_->publish(optimal_path); } } // namespace mppi diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 3caddcc1a11..8b28eeb7ff4 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -59,8 +59,7 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "map", parameters_handler.get()); vis.on_activate(); - builtin_interfaces::msg::Time bogus_stamp; - vis.visualize(pub_path, bogus_stamp); + vis.visualize(pub_path); rclcpp::spin_some(node->get_node_base_interface()); EXPECT_EQ(recieved_path.poses.size(), 5u); @@ -82,18 +81,18 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) 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; builtin_interfaces::msg::Time bogus_stamp; - vis.visualize(bogus_path, bogus_stamp); + vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); + nav_msgs::msg::Path bogus_path; + vis.visualize(bogus_path); 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({20, 2}); - vis.add(optimal_trajectory, "Optimal Trajectory"); - vis.visualize(bogus_path, bogus_stamp); + vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); + vis.visualize(bogus_path); rclcpp::spin_some(node->get_node_base_interface()); @@ -149,8 +148,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) vis.on_activate(); vis.add(candidate_trajectories, "Candidate Trajectories"); nav_msgs::msg::Path bogus_path; - builtin_interfaces::msg::Time bogus_stamp; - vis.visualize(bogus_path, bogus_stamp); + vis.visualize(bogus_path); rclcpp::spin_some(node->get_node_base_interface()); // 40 * 4, for 5 trajectory steps + 3 point steps @@ -167,7 +165,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) nav_msgs::msg::Path recieved_path; auto my_sub = node->create_subscription( - "/local_plan", 10, + "optimal_trajectory", 10, [&](const nav_msgs::msg::Path msg) {recieved_path = msg;}); // optimal_trajectory empty, should fail to publish @@ -175,9 +173,9 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); - vis.add(optimal_trajectory, "Optimal Trajectory"); + vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); nav_msgs::msg::Path bogus_path; - vis.visualize(bogus_path, cmd_stamp); + vis.visualize(bogus_path); rclcpp::spin_some(node->get_node_base_interface()); EXPECT_EQ(recieved_path.poses.size(), 0u); @@ -188,8 +186,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) 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); + vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + vis.visualize(bogus_path); rclcpp::spin_some(node->get_node_base_interface());