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();
}
Originally posted by anasilva on ROS Answers with karma: 1 on 2015-09-09
Post score: 0
Original comments
Comment by anasilva on 2015-09-10:
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.