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;
}
Asked by Cipek on 2019-06-19 06:01:08 UTC
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.
Asked by gvdhoorn on 2019-06-19 06:43:56 UTC
Could you support me with some links or example for this statemachine pattern?
Asked by Cipek on 2019-06-19 07:11:01 UTC