diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index 54c3bb7b..5980e9d5 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -235,6 +235,9 @@ GazeboSimROS2ControlPlugin::GazeboSimROS2ControlPlugin() GazeboSimROS2ControlPlugin::~GazeboSimROS2ControlPlugin() { // Stop controller manager thread + if (!this->dataPtr->controller_manager_) { + return; + } this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_); this->dataPtr->executor_->cancel(); this->dataPtr->thread_executor_spin_.join(); @@ -514,6 +517,9 @@ void GazeboSimROS2ControlPlugin::PreUpdate( const sim::UpdateInfo & _info, sim::EntityComponentManager & /*_ecm*/) { + if (!this->dataPtr->controller_manager_) { + return; + } static bool warned{false}; if (!warned) { rclcpp::Duration gazebo_period(_info.dt); @@ -548,6 +554,9 @@ void GazeboSimROS2ControlPlugin::PostUpdate( const sim::UpdateInfo & _info, const sim::EntityComponentManager & /*_ecm*/) { + if (!this->dataPtr->controller_manager_) { + return; + } // Get the simulation time and period rclcpp::Time sim_time_ros(std::chrono::duration_cast( _info.simTime).count(), RCL_ROS_TIME);