Unstable Rosseial callback with Arduino Mega2560

edit

Hi guys! I'm working on a project which aims to control a wheel. And by accident I found that the callback function on Arduino won't be executed each time. That's say, ideally, if I send a message with type of TWIST to Arduino, the LED of pin 13 should have been changed once the data received; however, it doesn't happened every time. According to my test, this problem only occurs when the message is type of TWIST and the callback function failed to be executed 3~4 times out of 20 tests. For other types of messages, such as float32 or string, everything is fine.

The simplified Arduino sketch is shown below and it was modified based on Rosserial "Blink" example.

/*
 * rosserial Subscriber Example
 * Blinks an LED on callback
 */ 

#include <ros.h>
#include <geometry_msgs/Twist.h>

ros::NodeHandle nh;
geometry_msgs::Twist ros_feedbackSpeed;


void messageCb( const geometry_msgs::Twist & toggle_msg){
  digitalWrite(13, HIGH-digitalRead(13));   // blink the led

}

ros::Subscriber< geometry_msgs::Twist > sub("toggle_led", &messageCb );


volatile long newTime = 0;
volatile long preTime = 0;

void setup()
{
  pinMode(13, OUTPUT);
  nh.initNode();
  nh.subscribe(sub);

}

void loop()
{

  if((newTime = millis()) - preTime >= 50)
  {
    nh.spinOnce();
    preTime = newTime;  
  }      
}

In the callback function, the only thing to do is to change the state of LED. I can see that the Tx LED and the Rx LED(on Arduino) blinking when the callback works fine but once the callback function failed, the Rx LED blinks while the Tx LED stays in silence. NOTE THAT the failure rate is around 3~4 out of 20 tests, which means in most situations the callback function can be executed but such a high failure rate is hard to be accepted. BTW, the MEGA2560 I used is a cheap version which uses CH340G as an USB-Serial chip.

Till now I haven't read any threads about this problem. Did I make any MISTAKES in my sketch? Thanks for help.