diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 3b7a65cda37..2b85723e562 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -131,6 +131,7 @@ controller_server: gamma: 0.015 motion_model: "DiffDrive" visualize: true + publish_optimal_trajectory: true regenerate_noises: true TrajectoryVisualizer: trajectory_step: 5 diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 0810664d85f..3b11644e19f 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -58,6 +58,8 @@ This process is then repeated a number of times and returns a converged solution | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | + | publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. | + #### Trajectory Visualizer | Parameter | Type | Definition | diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 0816fcc32e6..c2fc9b5cc9e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -105,16 +105,20 @@ class MPPIController : public nav2_core::Controller /** * @brief Visualize trajectories * @param transformed_plan Transformed input plan + * @param cmd_stamp Command stamp + * @param optimal_trajectory Optimal trajectory, if already computed */ void visualize( nav_msgs::msg::Path transformed_plan, - const builtin_interfaces::msg::Time & cmd_stamp); + const builtin_interfaces::msg::Time & cmd_stamp, + const Eigen::ArrayXXf & optimal_trajectory); std::string name_; rclcpp_lifecycle::LifecycleNode::WeakPtr parent_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; std::shared_ptr costmap_ros_; std::shared_ptr tf_buffer_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr opt_traj_pub_; std::unique_ptr parameters_handler_; Optimizer optimizer_; @@ -122,6 +126,7 @@ class MPPIController : public nav2_core::Controller TrajectoryVisualizer trajectory_visualizer_; bool visualize_; + bool publish_optimal_trajectory_; }; } // namespace nav2_mppi_controller diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 93fa74f7f38..3115cc4e0a1 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2025 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index c84ef98e946..e0821e548d6 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -105,6 +105,12 @@ class Optimizer */ Eigen::ArrayXXf getOptimizedTrajectory(); + /** + * @brief Get the optimal control sequence for a cycle for visualization + * @return Optimal control sequence + */ + const models::ControlSequence & getOptimalControlSequence(); + /** * @brief Set the maximum speed based on the speed limits callback * @param speed_limit Limit of the speed for use @@ -117,6 +123,15 @@ class Optimizer */ void reset(); + /** + * @brief Get the motion model time step + * @return Time step of the model + */ + const models::OptimizerSettings & getSettings() const + { + return settings_; + } + protected: /** * @brief Main function to generate, score, and return trajectories diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 5e52d6468e7..05b82ab20a8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -32,6 +32,7 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/trajectory.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include "rclcpp/rclcpp.hpp" @@ -170,6 +171,34 @@ inline geometry_msgs::msg::TwistStamped toTwistStamped( return twist; } +inline std::unique_ptr toTrajectoryMsg( + const Eigen::ArrayXXf & trajectory, + const models::ControlSequence & control_sequence, + const double & model_dt, + const std_msgs::msg::Header & header) +{ + auto trajectory_msg = std::make_unique(); + trajectory_msg->header = header; + trajectory_msg->points.resize(trajectory.rows()); + + for (int i = 0; i < trajectory.rows(); ++i) { + auto & curr_pt = trajectory_msg->points[i]; + curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt); + curr_pt.pose.position.x = trajectory(i, 0); + curr_pt.pose.position.y = trajectory(i, 1); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, trajectory(i, 2)); + curr_pt.pose.orientation = tf2::toMsg(quat); + curr_pt.velocity.linear.x = control_sequence.vx(i); + curr_pt.velocity.angular.z = control_sequence.wz(i); + if (control_sequence.vy.size() > 0) { + curr_pt.velocity.linear.y = control_sequence.vy(i); + } + } + + return trajectory_msg; +} + /** * @brief Convert path to a tensor * @param path Path to convert diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index c9ff6e9e0f0..57effc952ee 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -38,6 +38,8 @@ void MPPIController::configure( auto getParam = parameters_handler_->getParamGetter(name_); getParam(visualize_, "visualize", false); + getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false); + // Configure composed objects optimizer_.initialize(parent_, name_, costmap_ros_, parameters_handler_.get()); path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); @@ -45,6 +47,11 @@ void MPPIController::configure( parent_, name_, costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); + if (publish_optimal_trajectory_) { + opt_traj_pub_ = node->create_publisher( + "~/optimal_trajectory", rclcpp::SystemDefaultsQoS()); + } + RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } @@ -53,19 +60,27 @@ void MPPIController::cleanup() optimizer_.shutdown(); trajectory_visualizer_.on_cleanup(); parameters_handler_.reset(); + opt_traj_pub_.reset(); RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str()); } void MPPIController::activate() { + auto node = parent_.lock(); trajectory_visualizer_.on_activate(); parameters_handler_->start(); + if (opt_traj_pub_) { + opt_traj_pub_->on_activate(); + } RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); } void MPPIController::deactivate() { trajectory_visualizer_.on_deactivate(); + if (opt_traj_pub_) { + opt_traj_pub_->on_deactivate(); + } RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } @@ -100,8 +115,19 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration); #endif + Eigen::ArrayXXf optimal_trajectory; + if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) { + optimal_trajectory = optimizer_.getOptimizedTrajectory(); + auto trajectory_msg = utils::toTrajectoryMsg( + optimal_trajectory, + optimizer_.getOptimalControlSequence(), + optimizer_.getSettings().model_dt, + cmd.header); + opt_traj_pub_->publish(std::move(trajectory_msg)); + } + if (visualize_) { - visualize(std::move(transformed_plan), cmd.header.stamp); + visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); } return cmd; @@ -109,10 +135,16 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( void MPPIController::visualize( nav_msgs::msg::Path transformed_plan, - const builtin_interfaces::msg::Time & cmd_stamp) + const builtin_interfaces::msg::Time & cmd_stamp, + const Eigen::ArrayXXf & optimal_trajectory) { trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); - trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp); + if (optimal_trajectory.size() > 0) { + trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + } else { + trajectory_visualizer_.add( + optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp); + } trajectory_visualizer_.visualize(std::move(transformed_plan)); } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 493141daaaa..d4f70373e23 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2025 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -426,6 +427,11 @@ Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() return trajectories; } +const models::ControlSequence & Optimizer::getOptimalControlSequence() +{ + return control_sequence_; +} + void Optimizer::updateControlSequence() { const bool is_holo = isHolonomic(); diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 0a6b2a4d47e..de4f2249fb9 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -26,9 +26,10 @@ void TrajectoryVisualizer::on_configure( logger_ = node->get_logger(); frame_id_ = frame_id; trajectories_publisher_ = - node->create_publisher("trajectories", 1); - transformed_path_pub_ = node->create_publisher("transformed_global_plan", 1); - optimal_path_pub_ = node->create_publisher("optimal_trajectory", 1); + node->create_publisher("~/candidate_trajectories", 1); + transformed_path_pub_ = node->create_publisher( + "~/transformed_global_plan", 1); + optimal_path_pub_ = node->create_publisher("~/optimal_path", 1); parameters_handler_ = parameters_handler; auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer"); diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index 5cb66a27aff..ef32775a5b6 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -38,6 +38,7 @@ TEST(ControllerStateTransitionTest, ControllerNotFail) options.parameter_overrides(params); auto node = getDummyNode(options); + node->declare_parameter("publish_optimal_trajectory", true); auto tf_buffer = std::make_shared(node->get_clock()); auto costmap_ros = getDummyCostmapRos(costmap_settings); costmap_ros->setRobotFootprint(getDummySquareFootprint(0.01)); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 1e8cdd31f65..26eb2fff83a 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -650,6 +650,26 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) } } +TEST(OptimizerTests, TestGetters) +{ + OptimizerTester optimizer_tester; + optimizer_tester.fillOptimizerWithGarbage(); + + auto control_seq = optimizer_tester.getOptimalControlSequence(); + EXPECT_EQ(control_seq.vx(0), 342.0); + EXPECT_EQ(control_seq.vx.rows(), 30); + + auto node = std::make_shared("my_node"); + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); + EXPECT_EQ(optimizer_tester.getSettings().model_dt, 0.05f); +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index fd172c7b445..a69c78dbf98 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -45,7 +45,7 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) pub_path.poses.resize(5); auto my_sub = node->create_subscription( - "transformed_global_plan", 10, + "~/transformed_global_plan", 10, [&](const nav_msgs::msg::Path msg) {received_path = msg;}); TrajectoryVisualizer vis; @@ -65,7 +65,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( - "trajectories", 10, + "~/candidate_trajectories", 10, [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); // optimal_trajectory empty, should fail to publish @@ -127,7 +127,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( - "trajectories", 10, + "~/candidate_trajectories", 10, [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); models::Trajectories candidate_trajectories; @@ -157,7 +157,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) nav_msgs::msg::Path received_path; auto my_sub = node->create_subscription( - "optimal_trajectory", 10, + "~/optimal_path", 10, [&](const nav_msgs::msg::Path msg) {received_path = msg;}); // optimal_trajectory empty, should fail to publish diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index cc45443c7e0..a9d805c8253 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -569,6 +569,65 @@ TEST(UtilsTests, NormalizeYawsBetweenPointsTest) EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3); } +TEST(UtilsTests, toTrajectoryMsgTest) +{ + Eigen::ArrayXXf trajectory(5, 3); + trajectory << + 0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, + 3.0, 3.0, 3.0, + 4.0, 4.0, 4.0; + + models::ControlSequence control_sequence; + control_sequence.vx = Eigen::ArrayXf::Ones(5); + control_sequence.wz = Eigen::ArrayXf::Ones(5); + control_sequence.vy = Eigen::ArrayXf::Zero(5); + + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = rclcpp::Time(100, 0, RCL_ROS_TIME); + + auto trajectory_msg = utils::toTrajectoryMsg( + trajectory, control_sequence, 1.0, header); + + EXPECT_EQ(trajectory_msg->header.frame_id, "map"); + EXPECT_EQ(trajectory_msg->header.stamp, header.stamp); + EXPECT_EQ(trajectory_msg->points.size(), 5u); + EXPECT_EQ(trajectory_msg->points[0].pose.position.x, 0.0); + EXPECT_EQ(trajectory_msg->points[0].pose.position.y, 0.0); + EXPECT_EQ(trajectory_msg->points[1].pose.position.x, 1.0); + EXPECT_EQ(trajectory_msg->points[1].pose.position.y, 1.0); + EXPECT_EQ(trajectory_msg->points[2].pose.position.x, 2.0); + EXPECT_EQ(trajectory_msg->points[2].pose.position.y, 2.0); + EXPECT_EQ(trajectory_msg->points[3].pose.position.x, 3.0); + EXPECT_EQ(trajectory_msg->points[3].pose.position.y, 3.0); + EXPECT_EQ(trajectory_msg->points[4].pose.position.x, 4.0); + EXPECT_EQ(trajectory_msg->points[4].pose.position.y, 4.0); + + EXPECT_EQ(trajectory_msg->points[0].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[0].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[0].velocity.angular.z, 1.0); + EXPECT_EQ(trajectory_msg->points[1].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[1].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[1].velocity.angular.z, 1.0); + EXPECT_EQ(trajectory_msg->points[2].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[2].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[2].velocity.angular.z, 1.0); + EXPECT_EQ(trajectory_msg->points[3].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[3].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[3].velocity.angular.z, 1.0); + EXPECT_EQ(trajectory_msg->points[4].velocity.linear.x, 1.0); + EXPECT_EQ(trajectory_msg->points[4].velocity.linear.y, 0.0); + EXPECT_EQ(trajectory_msg->points[4].velocity.angular.z, 1.0); + + EXPECT_EQ(trajectory_msg->points[0].time_from_start, rclcpp::Duration(0, 0)); + EXPECT_EQ(trajectory_msg->points[1].time_from_start, rclcpp::Duration(1, 0)); + EXPECT_EQ(trajectory_msg->points[2].time_from_start, rclcpp::Duration(2, 0)); + EXPECT_EQ(trajectory_msg->points[3].time_from_start, rclcpp::Duration(3, 0)); + EXPECT_EQ(trajectory_msg->points[4].time_from_start, rclcpp::Duration(4, 0)); +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 1e546801f52..fbe05cf3ecc 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -32,6 +32,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" + "msg/Trajectory.msg" + "msg/TrajectoryPoint.msg" "srv/GetCosts.srv" "srv/GetCostmap.srv" "srv/IsPathValid.srv" diff --git a/nav2_msgs/msg/Trajectory.msg b/nav2_msgs/msg/Trajectory.msg new file mode 100644 index 00000000000..808173d71e2 --- /dev/null +++ b/nav2_msgs/msg/Trajectory.msg @@ -0,0 +1,7 @@ +# An array of trajectory points that represents a trajectory for a robot to follow. + +# Indicates the frame_id of the trajectory. +std_msgs/Header header + +# Array of trajectory points to follow. +TrajectoryPoint[] points diff --git a/nav2_msgs/msg/TrajectoryPoint.msg b/nav2_msgs/msg/TrajectoryPoint.msg new file mode 100644 index 00000000000..afe672a6a5c --- /dev/null +++ b/nav2_msgs/msg/TrajectoryPoint.msg @@ -0,0 +1,16 @@ +# Trajectory point state + +# Desired time from the trajectory start to arrive at this trajectory sample. +builtin_interfaces/Duration time_from_start + +# Pose of the trajectory sample. +geometry_msgs/Pose pose + +# Velocity of the trajectory sample. +geometry_msgs/Twist velocity + +# Acceleration of the trajectory (optional). +geometry_msgs/Accel acceleration + +# Force/Torque to apply at trajectory sample (optional). +geometry_msgs/Wrench effort