Issues with ros_lib for Arduino

edit

Please, I need help to review the code at the bottom of the post, there is something very odd happening.

Github: https://github.com/renanmb/box_controller

https://github.com/renanmb/Ros_arduino

When I compile the code there is no errors, however when I upload the code on my Arduino Mega and Arduino Uno it does not connect with the :

rosrun rosserial_python serial_node.py

gives me the following error:

[ERROR] [1537656135.447846]: Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

I tried to reinstall rosserial, the arduino IDE, the ros_lib. I changed the wires, increased the baud_rate, I don't know what else to try.

What is driving me crazy is that it does connect when I run all the examples in ros_lib or when I run similar codes such as:

ros_pwm_test5

    #include <Wire.h>
    #include <ros.h>
    #include <geometry_msgs/Twist.h>
    #include <std_msgs/Int32.h>

    #include <PWM.h> 


    ros::NodeHandle  nh; 
    std_msgs::Int32 str_msg; 
    ros::Publisher chatter("debug", &str_msg);

    const int step_pin1 = 11;  
    const int dir_pin1 = 22;      
    const int step_pin2 = 12;     
    const int dir_pin2 = 24;  
    int32_t frequency1 = 0; //frequency (in Hz) 
    int32_t frequency2 = 0; //frequency (in Hz) 
    int32_t duty1 = 0; 
    int32_t duty2 = 0; 
    bool dir1 = true; 
    bool dir2 = true;

    void driveCallback ( const geometry_msgs::Twist&  twistMsg ){   
//need to pubish straigth the motor commands and do the dir calculations on the Arduino   
frequency1 = abs( twistMsg.linear.x ) ;   
frequency2 = abs( twistMsg.linear.y ) ;
SetPinFrequency(step_pin1, frequency1); 
SetPinFrequency(step_pin2, frequency2); 

      str_msg.data = frequency1;   
      chatter.publish(&str_msg);   
     if(frequency1 > 0){
        duty1 = 32768; 
        if(twistMsg.linear.x >= 0){
        digitalWrite(dir_pin1, HIGH);
        }
        else{
        digitalWrite(dir_pin1, LOW);
          }   
        }   
        else{
        duty1 = 0;   }

      if(frequency2 > 0){
        duty2 = 32768;
        if(twistMsg.linear.y >= 0){
        digitalWrite(dir_pin2, HIGH);
        }
        else{
        digitalWrite(dir_pin2, LOW);
           }   
        }   
        else{
        duty2 = 0;   } 
        }

    ros::Subscriber<geometry_msgs::Twist> driveSubscriber("comm_drive_robot", &driveCallback) ;

    void setup(){   //initialize all timers except for 0, to save time keeping functions   
       InitTimersSafe();  
       nh.initNode();

      // Subscribe to the steering and throttle messages   
       nh.subscribe(driveSubscriber);   
       nh.advertise(chatter);

      //=======================================================================================   /* ================ PWM Function ======================================================= */   
         pinMode(step_pin1, OUTPUT);   
         pinMode(dir_pin1, OUTPUT);
         //PWM + direction to the motors 2  
         pinMode(step_pin2, OUTPUT);   
         pinMode(dir_pin2, OUTPUT);
          }

    void loop(){   
      nh.spinOnce();

      pwmWriteHR(step_pin1, duty1);   
      pwmWriteHR(step_pin2, duty2);
         delay(1); }

Code with the issue:

box_controller_test3

#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float32.h>
#include <PWM.h> //PWM library for controlling the steppers

ros::NodeHandle  nh;

std_msgs::Float32 temp_msg;
ros::Publisher temp("temp", &temp_msg);

//use pin 11 on the Mega instead, otherwise there is a frequency cap at 31 Hz
const int cooler_pin = 9;     
const int LED_left_pin = 10;     
const int LED_right_pin = 11;
const int sensor1 = A0;

int32_t frequency1 = 0; //frequency (in Hz)
int32_t frequency2 = 0; //frequency (in Hz)
int32_t frequency3 = 0;


void boxCallback ( const geometry_msgs::Twist& twistMsg ){
  frequency1 = twistMsg.linear.x;
  frequency2 = twistMsg.linear.y;
  frequency3 = twistMsg.linear.z; 
}

ros::Subscriber<geometry_msgs::Twist> sub1("box", &boxCallback) ;

void setup(){
  //initialize all timers except for 0, to save time keeping functions
  InitTimersSafe(); 
  nh.initNode();

  // Subscribe 
  nh.subscribe(sub1);
  nh.advertise(temp);

  // =======================================================================================
  /* ================ PWM Function  ======================================================= */
  //PWM to cooler
  pinMode(cooler_pin ...