Callback function not executed arduino code

edit

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
}