Callback function not executed arduino code
Hello all I have a simple Arduino code to control a servo. I used the following code for my teensy board. I just find I cannot publish the command from ros and send it to the teensy board. The communication between the teensy board and the servo is correct as I can directly send commands to the servo by seting some constant commands in the Arduino code.
Then I used a dummy topic to test whether the callback function to subscribe the command from ros is called or not (shown in the following code). The value of the dummy topic "truck/manual/throttle" is always zero. Clearly, spinonce() function doesn't call the callback function to get the command I sent otherwise the value should be 10. The following is the code. Can anyone help with this
#include <ros.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/Imu.h>
#include <std_srvs/SetBool.h>
#include <std_srvs/Empty.h>
#include <Encoder.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Servo.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include "TeensyThreads.h"
#define USE_TEENSY_HW_SERIAL
/****PIN CONNECTION MACROS****/
#define _SERVO 1
#define _ESC 3
/*******Reciever PWM range macros*********/
#define _REC_ESC_MIN 1065
#define _REC_ESC_MAX 2006
#define _ESC_NEUTRAL 1514
#define _SERVO_NEUTRAL 1610
/************STRUCT DEFINTIONS************/
std_msgs::Float32 manualThrottle;
std_msgs::Float32 cmdThrottle;
/*********PROTOTYPES**************/
void cmdThrottleCb(const std_msgs::Float32 &msg);
/************VARIABLES********************/
Servo esc;
Servo steer;
ros::NodeHandle nh;
ros::Publisher pub_manual_throttle_command("truck/manual/throttle", &manualThrottle);
ros::Subscriber<std_msgs::Float32> sub_cmdThrottle("truck/cmd/throttle", &cmdThrottleCb);
/************************************************/
int Flag = 0;
int Cmd_esc = 0;
void setup(){
esc.attach(_ESC);
steer.attach(_SERVO);
steer.writeMicroseconds(_SERVO_NEUTRAL); //send neutral commands for safety
esc.writeMicroseconds(_ESC_NEUTRAL);
nh.initNode();
nh.advertise(pub_manual_throttle_command);
nh.subscribe(sub_cmdThrottle); //control command
delay(20);
}
void loop()
{
manualThrottle.data=Flag; // publish this data to test callback function
pub_manual_throttle_command.publish(&manualThrottle);
esc.writeMicroseconds(Cmd_esc);
nh.spinOnce();
delay(10);
}
void cmdThrottleCb(const std_msgs::Float32 &msg){
Cmd_esc=msg.data;
Flag=10;// test if this function is called
}
Just two quick observations. You send a float but Cmd_esc is of type int. I have seen issues where arduino basic types do not match "ros"/PC types(there were different "double" types I think), causing trouble/errors. Iam not sure how well implicit casting works here. Way back I also seen that declaring a function below void loop() didnt work for some code. I dont get why there is a prototype of cmdThrottleCb at the top and the implementation at the bottom. Get rid of the prototype?
Thanks. I revised my code, just change "Flag" to float type. The "cmdThrottleCb" function is used to create the publisher:
But this still doesn't work, the teensy 4.0 microcontroller cannot receive the command and the servo doesn't rotate correspondingly.
I would still try to implement the cmdthrottlecb() up where the prototype is. All your linked code does it this way as well. I would also build a led on/off trigger into the callback. I checked some of my old code and this should work. What does your rosserial launchfile look like on the PC side? Also regarding "#define USE_TEENSY_HW_SERIAL", does your libraries etc comply with this and is 4.0 adapted? How do you connect to the PC? Did any other rosserial scetch work on this teensy 4.0 for you?
Thanks. I tried what you said. It still doesn't work. I tried to publish command from the terminal for the topic truck/cmd/throttle as
The terminal then displays the following info
If I didn't Ctr+c, the info is always there.
You could "rostopic info" and "rostopic hz" the topics to at least see whats going on the PC side. But whats with my questions about teensy serial setup? I had issues with it way back. Did you use the LED to confirm that anything ever reaches the teensy via serial? How do you connect USB? Did this setup ever work with rosserial?