Segmentation fault when subscriber is destroyed while publishing

edit

I am using two gazebo plugins, VictimPlugin and LifeBuoyPlugin. The VictimPlugin subscribes to a topic to get a bool that is used by timerThreadFunction, which sleeps for a predefined amount of time and when it awakes if this bool is true it deletes the gazebo model associated to the plugin. The LifeBuoyPlugin publishes this bool which indicates if the model should be deleted or not.

The problem is that sometimes when LifeBuoyPlugin is trying to publish and VictimPlugin is deleted a segmentation fault happens, the gdb log is:

#0  0x00007ffff5dbcfa0 in __GI___pthread_mutex_lock (mutex=0x110) at ../nptl/pthread_mutex_lock.c:65
#1  0x00007fffbc942fa0 in ros::Publication::hasSubscribers() () at /opt/ros/melodic/lib/libroscpp.so
#2  0x00007fffbc933296 in ros::TopicManager::publish(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, boost::function<ros::SerializedMessage ()> const&, ros::SerializedMessage&) () at /opt/ros/melodic/lib/libroscpp.so
#3  0x00007ffeb85d92e5 in void ros::Publisher::publish<std_msgs::Bool_<std::allocator<void> > >(std_msgs::Bool_<std::allocator<void> > const&) const ()
    at /home/gus/Documents/git/jason_ros_ws/devel/lib/liblifebuoy.so
#4  0x00007ffeb85d653c in gazebo::LifeBuoyPlugin::OnRosMsg(boost::shared_ptr<gazebo_msgs::ModelStates_<std::allocator<void> > const> const&) () at /home/gus/Documents/git/jason_ros_ws/devel/lib/liblifebuoy.so

VictimPlugin code:

#include <thread>
#include <chrono>
#include <mutex>

#include <gazebo/transport/Node.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/Model.hh>
#include <ignition/math/Pose3.hh>

#include "ros/ros.h"
#include "std_msgs/Bool.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"

namespace gazebo{
  class VictimPlugin : public ModelPlugin{
  private:
    gazebo::transport::NodePtr node;
    std::unique_ptr<ros::NodeHandle> rosNode;

    ros::Subscriber rosSub;
    ros::CallbackQueue rosQueue;
    std::thread rosQueueThread;

    bool drowning = true;

    physics::ModelPtr model;

    int drowning_time_ = 0;
    std::thread timer_thread_;
    std::mutex mtx;

    void timerThreadFunction(){
      std::this_thread::sleep_for(
        std::chrono::milliseconds(this->drowning_time_));

      std::lock_guard<std::mutex> guard(mtx);

      if(this->drowning){
        gazebo::transport::PublisherPtr request_pub = this->node->Advertise<msgs::Request>("~/request");
        msgs::Request *msg = msgs::CreateRequest("entity_delete", this->model->GetName());
        request_pub->Publish(*msg);
        delete msg;
      }
    }

    void QueueThread(){
      static const double timeout = 0.01;
      while (this->rosNode->ok())
      {
        this->rosQueue.callAvailable(ros::WallDuration(timeout));
      }
    }

  public:
    VictimPlugin() : ModelPlugin(){}
    ~VictimPlugin(){
      rosQueue.clear();
      rosQueue.disable();
      rosNode->shutdown();
      rosQueueThread.join();
      timer_thread_.join();
    }

    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
      model = _model;

      this->node = boost::make_shared<gazebo::transport::Node>();
      this->node->Init();

      if (!_sdf->HasElement("drowningTime")){
        this->drowning_time_ = 5000;
        ROS_DEBUG("Victim plugin missing <drowningTime>, default to 5000ms");
      }else{
        this->drowning_time_ = _sdf->Get<int>("drowningTime");
      }

      if (!ros::isInitialized()){
        ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
          << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
        return;
      }
      this->rosNode.reset(new ros::NodeHandle("victim"));

      ros::SubscribeOptions so =
        ros::SubscribeOptions::create<std_msgs::Bool>(
          "/victim/"+this->model->GetName(),
          1,
          boost::bind(&VictimPlugin::OnRosMsg, this, _1),
          ros::VoidPtr(), &this->rosQueue);

      this->rosSub = this->rosNode->subscribe(so);

      this->rosQueueThread =
        std::thread(std::bind(&VictimPlugin::QueueThread, this));

      this->timer_thread_ =
        std::thread(std::bind(&VictimPlugin::timerThreadFunction, this));
    }

    void OnRosMsg(const std_msgs::BoolConstPtr &_msg){
      std::lock_guard<std::mutex> guard(mtx);
      this->drowning = _msg->data;
    }
  };
  GZ_REGISTER_MODEL_PLUGIN(VictimPlugin)
}

LifeBuoyPlugin code:

#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include <thread>
#include <ignition/math/Pose3.hh>
#include "gazebo/common/common.hh"
#include "gazebo/gazebo.hh"

#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "gazebo_msgs ...