ros::spinOnce() blocks publisher

asked 2019-06-19 06:01:08 -0500

Cipek gravatar image

updated 2019-06-19 06:02:19 -0500

I am working on controlling the AR Drone with c++ script. I added the subscriber for checking the state of the drone. However, it runs once, when the spinOnce() is called and then it doesn't update the state. What is more, it stacks in the while loop before taking off, whereas where there is no spinOnce() the drone takes off without any issues.

#include "ros/ros.h"
#include "std_msgs/Empty.h"
#include "std_msgs/String.h"
#include <iostream>
#include <ardrone_autonomy/Navdata.h>
#include <geometry_msgs/Twist.h>
using namespace std;
using namespace ros;
using namespace std_msgs;


ardrone_autonomy::Navdata data;
int state = -1;

void receiveNavdata(const ardrone_autonomy::Navdata& data)
{
    //Take in state of ardrone
    cout<<"\nState: "<< data.state<<endl;   
    state=data.state;   
}

int main(int argc, char **argv){

  init(argc, argv, "talker");
NodeHandle rospy;
Empty empty;

Subscriber navData = rospy.subscribe("/ardrone/navdata", 1, receiveNavdata);

while( state == -1){
    ros::spinOnce();
    ros::Duration(0.1).sleep();
}   

Publisher reset;
if(state == 0){
    reset = rospy.advertise<Empty>("/ardrone/reset", 1);

    while(reset.getNumSubscribers() < 1)
        ros::Duration(0.1).sleep();

    reset.publish(empty);
}

while(state == 0)
    ros::Duration(0.1).sleep();

Publisher takeoff = rospy.advertise<Empty>("/ardrone/takeoff", 1);

while(takeoff.getNumSubscribers() < 1){
    cout<<"\nIN while"<<endl;
    ros::Duration(0.1).sleep();
}

cout<<"\nTakeOff"<<endl;

takeoff.publish(empty);
ros::Duration(5).sleep();

Publisher land = rospy.advertise<Empty>("/ardrone/land", 1);

while(land.getNumSubscribers() < 1)
    ros::Duration(0.1).sleep();

cout<<"\nLand"<<endl;

land.publish(empty);

ros::Duration(1).sleep();

return 0;
}
edit retag flag offensive close merge delete

Comments

Can you clarify why you are creating all those publishers and subscribers right before you need them? I'd suggest to create all publishers and subscribers in the configuration phase of your program (ie: at the start) and then just regulate when messages are published.

Also: you may want to take a look at a statemachine pattern and/or behavior trees.

gvdhoorn gravatar image gvdhoorn  ( 2019-06-19 06:43:56 -0500 )edit

Could you support me with some links or example for this statemachine pattern?

Cipek gravatar image Cipek  ( 2019-06-19 07:11:01 -0500 )edit