Segmentation fault when subscriber is destroyed while publishing
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 ...