Fix null pointer check for trajectory publisher

[tonynajjar]

Crash happens when setting "publish_optimal_trajectory" dynamically:

[controller_server-75] Stack trace (most recent call last) in thread 138462:
[controller_server-75] #12   Object "/usr/lib/x86_64-linux-gnu/ld-linux-x86-64.so.2", at 0xffffffffffffffff, in 
[controller_server-75] #11   Source "../sysdeps/unix/sysv/linux/x86_64/clone3.S", line 78, in clone3 [0x72424d800c6b]
[controller_server-75] #10   Source "./nptl/pthread_create.c", line 447, in start_thread [0x72424d773aa3]
[controller_server-75] #9    Source "../../../../../src/libstdc++-v3/src/c++11/thread.cc", line 104, in execute_native_thread_routine [0x72424da03db3]
[controller_server-75] #8    Object "/opt/auto_ws/install/nav2_controller/lib/libcontroller_server_core.so", at 0x72424e15d0f7, in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::{lambda()#1}> >, void>::_M_run()
[controller_server-75] #7    Source "./nptl/pthread_once.c", line 116, in __pthread_once_slow [0x72424d778ed2]
[controller_server-75] #6    Object "/opt/auto_ws/install/nav2_controller/lib/libcontroller_server_core.so", at 0x72424e15239c, in std::__future_base::_State_baseV2::_M_do_set(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*)
[controller_server-75] #5    Object "/opt/auto_ws/install/nav2_controller/lib/libcontroller_server_core.so", at 0x72424e1b5673, in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::{lambda()#1}> >, void> >::_M_invoke(std::_Any_data const&)
[controller_server-75] #4    Object "/opt/auto_ws/install/nav2_controller/lib/libcontroller_server_core.so", at 0x72424e1b47d4, in nav2::SimpleActionServer<nav2_msgs::action::FollowPath>::work()
[controller_server-75] #3    Object "/opt/auto_ws/install/nav2_controller/lib/libcontroller_server_core.so", at 0x72424e13dd5d, in nav2_controller::ControllerServer::computeControl()
[controller_server-75] #2    Object "/opt/auto_ws/install/nav2_controller/lib/libcontroller_server_core.so", at 0x72424e13bde6, in nav2_controller::ControllerServer::computeAndPublishVelocity()
[controller_server-75] #1    Object "/opt/auto_ws/install/nav2_mppi_controller/lib/libmppi_controller.so", at 0x72418c097ea4, in nav2_mppi_controller::MPPIController::computeVelocityCommands(geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&, geometry_msgs::msg::Twist_<std::allocator<void> > const&, nav2_core::GoalChecker*, nav_msgs::msg::Path_<std::allocator<void> > const&, geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&)
[controller_server-75] #0    Object "/opt/ros/kilted/lib/librclcpp.so", at 0x72424dd62fa6, in rclcpp::PublisherBase::get_subscription_count() const
[controller_server-75] Segmentation fault (Address not mapped to object [0x38])