ros::spinOnce() blocks publisher
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;
}
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.
Could you support me with some links or example for this statemachine pattern?