My node of ROS keeps dying. Subscribe a gazebo topic and publish in a ros topic
Hi, I have created a node in ROS so I can subscribe a topic of gazebo, analyse the information and then use it. I have a callback function for the subscription and I put the information in a certain structure. Then i use that structure to analyse that information and finally I publish the information in a ROS topic.
To keep the information updated I need to clear the structures where I am putting the information . (Probably I need some thread safe procedure)
In main function I have some initializations and the usual while(ros:ok()) cycle. Inside the while i have a gazebo::common::Time::MSleep(50); and then ros::spinOnce();
The error messages printed in the screen are:
terminate called after throwing an instance of 'std::bad_alloc' what(): std::bad_alloc Aborted (core dumped)
or:
(Segmentation fault)
I think this happens because at some point I try to access the structure right after i clear it in the callback function.
Does anyone have some clue of what is happening?
EDIT: my code is something like that:
gazebo_msgs::ContactsState contact_gz_ros_global;
gazebo_msgs::ContactsState updateContactData(){
gazebo_msgs::ContactsState result;
for(int i=0; i< contact_gz_ros_global.states.size() ; i++){
result.states.push_back(contact_gz_ros_global.states[i]);
}
return result;
}
void cb(ConstContactsPtr &msg){
//if there is contacts
//contact_gz_ros_global.clear();
//copy info from msg to contac_gz_ros_global structure
}
int main(int _argc, char **_argv)
{
// Load gazebo
gazebo::setupClient(_argc, _argv);
ros::init(_argc, _argv, "gz_subs_tf_pub");
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init();
gazebo::transport::SubscriberPtr sub = node->Subscribe( "~/physics/contacts" , cb);
ros::NodeHandle n;
ros::Publisher pub_gz_ros = n.advertise<icub_gazebo::TFVectors>("/gazebo/regrasp/positions_normals",1000);
ros::Rate loop_rate(1);
icub_gazebo::TFVectors msg_tf;
gazebo_msgs::ContactsState contact_gz_ros;
while (ros::ok()){
contact_gz_ros = updateContactData();
for(int contacts_count=0; contacts_count< contact_gz_ros.states.size();contacts_count++){
//access information of the contact_gz_ros
}
gazebo::common::Time::MSleep(50);
ros::spinOnce();
}
gazebo::shutdown();
}
I tried to put: static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; pthread_mutex_lock(&mutex); contact_gz_ros = updateContactData(); pthread_mutex_unlock(&mutex); and : gazebo::common::Time::MSleep(50); loop_rate.sleep(); ros::spinOnce(); but it does not work either.