Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Segmentation fault when publishing to a topic while the ModelPlugin which subscribes to this topic is deleted

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/ModelStates.h"
#include "gazebo_msgs/DeleteModel.h"
#include "geometry_msgs/Pose.h"
// #include "std_msgs/String.h"
#include "std_msgs/Bool.h"

#include <iostream>
#include <math.h>
#include <unistd.h>

double euclideanDistance(geometry_msgs::Pose &buoyPose, ignition::math::Pose3d &modelPose){
  return  pow(pow(buoyPose.position.x - modelPose.Pos().X(), 2) +
              pow(buoyPose.position.y - modelPose.Pos().Y(), 2) +
              pow(buoyPose.position.z - modelPose.Pos().Z(), 2), 0.5);
}

namespace gazebo{
  class LifeBuoyPlugin : public ModelPlugin{
  private:
    physics::ModelPtr model;

    std::unique_ptr<ros::NodeHandle> rosNode;

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

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

  public:
    LifeBuoyPlugin() : ModelPlugin(){}

    ~LifeBuoyPlugin(){
      rosQueue.clear();
      rosQueue.disable();
      rosNode->shutdown();                     // This MUST BE CALLED before thread join()!!
      rosQueueThread.join();
    }

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

      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("buoy"));

      ros::SubscribeOptions so =
        ros::SubscribeOptions::create<gazebo_msgs::ModelStates>(
          "/gazebo/model_states",
          1,
          boost::bind(&LifeBuoyPlugin::OnRosMsg, this, _1),
          ros::VoidPtr(), &this->rosQueue);

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

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

    void OnRosMsg(const gazebo_msgs::ModelStatesConstPtr &_msg){
      for (size_t i = 0; i < _msg->name.size(); i++) {
        if(_msg->name.at(i).compare(0, 6,"victim") == 0){
          geometry_msgs::Pose victimPose = _msg->pose.at(i);
          ignition::math::Pose3d buoyPose = model->WorldPose();

          double distance = euclideanDistance(victimPose, buoyPose);
          if(distance < 2.5 & ros::ok()){
            std_msgs::Bool pubMsg;
            pubMsg.data = false;

            ros::Publisher victimPub;
            victimPub = rosNode->advertise<std_msgs::Bool>("/victim/"+_msg->name.at(i), 1);
            victimPub.publish(pubMsg);

            i = _msg->name.size();
          }
        }
      }
    }

  };
  GZ_REGISTER_MODEL_PLUGIN(LifeBuoyPlugin)
}