Gazebo | Ignition | Community
Ask Your Question
0

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

asked 2020-07-14 12:20:17 -0500

rezenders gravatar image

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 ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-07-14 13:23:13 -0500

rezenders gravatar image

Setting so.transport_hints = ros::TransportHints().unreliable(); solved this

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

      so.transport_hints = ros::TransportHints().unreliable();
edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2020-07-14 12:20:17 -0500

Seen: 297 times

Last updated: Jul 14