   
   // Start ROS node
   nh.initNode();
-  nh.subscribe(s);
   
-  // initialize ultrasound sensor datatype
-  ultrasound.layout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension) * 2);
-  ultrasound.layout.dim[0].label = "cm";
-  ultrasound.layout.dim[0].size = 4;
-  ultrasound.layout.dim[0].stride = 4;
-  ultrasound.layout.data_offset = 0;
-  ultrasound.data = (float *) malloc(sizeof(float)*4);
-  ultrasound.data_length = 4;
-  
-  // Public data to topics
+  // Publish / Subscribe to topics
   nh.advertise(pub_ultrasound);
   nh.advertise(pub_encoder);
+  nh.subscribe(s);
 }
 
 

**************************************************************************/
int saturateMotor(int x) 
{
  if (x  == noAction ){
    return motor_neutral;
  }
  
  if (x  >  motor_max) {
    x = motor_max; 
  } 
  else if (x < motor_min) {

  // publish measurements
  if (dt > 50) {
    // publish encodeer measurement
    
    encoder.FL = FL_count;
    encoder.FR = FR_count;
    encoder.BL = 0;
    encoder.BR = 0;
    pub_encoder.publish(&encoder);
    
    // publish ultra-sound measurement
    ultrasound.front = us_fr.getRange();
    ultrasound.back = us_bk.getRange();
    ultrasound.left = us_lt.getRange();
    ultrasound.right = us_rt.getRange();
    pub_ultrasound.publish(&ultrasound);
    t0 = millis();
  }
  
  nh.spinOnce();
