From d02cb4f64139543f5eb954babbb2d31d192ae69e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 17 Feb 2026 21:17:28 +0100 Subject: [PATCH] Fix null pointer check for trajectory publisher Happens when setting "publish_optimal_trajectory" dynamically Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 7ad1e5976e9..d507b2b76fa 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -114,7 +114,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration); #endif - if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) { + if (publish_optimal_trajectory_ && opt_traj_pub_ && opt_traj_pub_->get_subscription_count() > 0) { std_msgs::msg::Header trajectory_header; trajectory_header.stamp = cmd.header.stamp; trajectory_header.frame_id = costmap_ros_->getGlobalFrameID();