Hi, I have a simple arduino code that takes in inputs from my joystick where a button on my joystick would act as a switch ( pressing once will turn 'on' and pressing it again turns it 'off'). My code basically publish the output as true or false and it works. However the problem arises when I add another subscriber to the cmd_vel topic. My output doesn't change and later a lost sync error occurs.
#include
#include
#include
#include
#include
#include
ros::NodeHandle nh;
float tempx;
float tempz;
float joy_button;
boolean last_reading;
boolean currentReading;
boolean flag;
boolean published;
void overrideCallback(const sensor_msgs::Joy& joy_msg){
joy_button = joy_msg.buttons[4];
}
/*
void joyCallback(const geometry_msgs::Twist& cmd_msg){
tempx = cmd_msg.linear.x;
tempz = cmd_msg.angular.z;
}*/
//ros::Subscriber joy_sub("cmd_vel", &joyCallback );
ros::Subscriber override_sub("joy", &overrideCallback);
std_msgs::Float32 override_msg;
ros::Publisher override_pub("override_status", &override_msg);
std_msgs::Bool pushed_msg;
ros::Publisher pub_button("pushed", &pushed_msg);
void setup(){
nh.initNode();
nh.subscribe(override_sub);
//nh.subscribe(joy_sub);
nh.advertise(override_pub);
nh.advertise(pub_button);
last_reading = false;
currentReading = false;
flag=true;
}
void loop(){
override_msg.data = joy_button;
if (override_msg.data == 0.0){
currentReading = false;
}else currentReading = true;
if (currentReading && !last_reading) {
flag=!flag;
if (flag) {
published = true;
}
else {
published = false;
}
}
last_reading = currentReading;
pushed_msg.data = published;
pub_button.publish(&pushed_msg);
override_pub.publish(&override_msg);
nh.spinOnce();
delay(5);
}
The above code works without the commented lines. Any help would be appreciated. Thank you.
↧