Ask Your Question
0

My node of ROS keeps dying. Subscribe a gazebo topic and publish in a ros topic

asked 2015-09-09 18:18:05 -0600

anasilva gravatar image

updated 2015-09-10 09:01:18 -0600

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();
}
edit retag flag offensive close merge delete

Comments

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.

anasilva gravatar imageanasilva ( 2015-09-10 09:08:32 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-09-09 19:30:00 -0600

ahendrix gravatar image

(You've given a vague question, so the best I can do is a vague answer)

You have a bad memory access somewhere in your program.

It sounds like you have multiple threads, and you're modifying variables from both threads. You should protect any accesses to shared variables with a mutex or some other thread locking mechanism.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2015-09-09 18:18:05 -0600

Seen: 809 times

Last updated: Sep 10 '15