Delete a model from within its own modelPlugin
I wrote a ModelPlugin to delete the model to which it is attached after a certain period of time. The problem is that when the model is deleted, gazebo stops working, it just freezes.
Plugin code:
#include <ignition/math/Pose3.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/Model.hh>
#include "gazebo_msgs/DeleteModel.h"
#include "ros/ros.h"
#include <thread>
#include <chrono>
namespace gazebo{
class VictimPlugin : public ModelPlugin{
private:
physics::ModelPtr model;
int drowning_time_ = 0;
std::thread timer_thread_;
void timerThreadFunction(){
std::this_thread::sleep_for(
std::chrono::milliseconds(this->drowning_time_));
gazebo_msgs::DeleteModel del_model;
del_model.request.model_name = this->model->GetName();
ros::service::call("/gazebo/delete_model", del_model);
}
public:
VictimPlugin() : ModelPlugin(){}
~VictimPlugin(){
timer_thread_.join();
}
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
model = _model;
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");
}
this->timer_thread_ =
std::thread(std::bind(&VictimPlugin::timerThreadFunction, this));
}
};
GZ_REGISTER_MODEL_PLUGIN(VictimPlugin)
}
I am adding the model to the world file like this:
<include>
<name>victim_randy</name>
<pose>7 17.5 0.1 -1.57 0 0</pose>
<uri>model://rescue_randy</uri>
<plugin name="victim" filename="libvictim.so">
<drowningTime>5000</drowningTime>
</plugin>
</include>
I am using gazebo version 9.13.1