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


//ROS-TOPICS
ros::Publisher pub_array("apm/data_raw",&data_raw);
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", motor_cb);

void setup() {
 //I2C        

 // VAR INITIALIZATION
    encoder0_cnt = 0;
    encoder1_cnt = 0;
    data_raw.data_length = DATA_LENGTH; // The number of output message entries
    data_raw.data = (float *)malloc(sizeof(float)*data_raw.data_length);
 //MOTORS           
    md.init();
 //ROS        
    nh.initNode();
    nh.subscribe(sub_motors);
    nh.subscribe(sub_srv_clapper);
    nh.subscribe(sub_srv_carpet);
    nh.advertise(pub_array);
}

void loop()
{  
    if ( (millis()-publish_timer) > 20) {
        MagnetometerScaled scaled = compass.ReadScaledAxis();      
 //IMU
        data_raw.data[IMU_ANG_VX] = ToD((float)(imu.GyroX()));
