diff --git a/gz_ros2_control/include/gz_ros2_control/gz_ros2_control_plugin.hpp b/gz_ros2_control/include/gz_ros2_control/gz_ros2_control_plugin.hpp index 43d71a20..ad8abe5a 100644 --- a/gz_ros2_control/include/gz_ros2_control/gz_ros2_control_plugin.hpp +++ b/gz_ros2_control/include/gz_ros2_control/gz_ros2_control_plugin.hpp @@ -29,7 +29,8 @@ class GazeboSimROS2ControlPlugin : public sim::System, public sim::ISystemConfigure, public sim::ISystemPreUpdate, - public sim::ISystemPostUpdate + public sim::ISystemPostUpdate, + public sim::ISystemReset { public: /// \brief Constructor @@ -54,6 +55,10 @@ class GazeboSimROS2ControlPlugin const sim::UpdateInfo & _info, const sim::EntityComponentManager & _ecm) override; + void Reset( + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) override; + private: /// \brief Private data pointer. std::unique_ptr dataPtr; diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index 7e1bcb01..dc0c626b 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -263,6 +263,16 @@ GazeboSimROS2ControlPlugin::~GazeboSimROS2ControlPlugin() this->dataPtr->thread_executor_spin_.join(); } +////////////////////////////////////////////////// +void GazeboSimROS2ControlPlugin::Reset( + const gz::sim::UpdateInfo &/*_info*/, + gz::sim::EntityComponentManager &/*_ecm*/) +{ + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), + "Resetting gz_ros2_control plugin"); +} + ////////////////////////////////////////////////// void GazeboSimROS2ControlPlugin::Configure( const sim::Entity & _entity, @@ -541,4 +551,5 @@ GZ_ADD_PLUGIN( gz::sim::System, gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemConfigure, gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPreUpdate, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemReset, gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPostUpdate)