   
   // 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);
 }
 
 

 
// include libraries
#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <geometry_msgs/Vector3.h>
#include <Servo.h>
#include "Maxbotix.h"


volatile unsigned long dt;
volatile unsigned long t0;

ros::NodeHandle nh;

// define global message variables
// Encoder, Electronic Control Unit, Ultrasound
barc::Ultrasound ultrasound;
barc::ECU ecu;
barc::Encoder encoder;

ros::Publisher pub_encoder("encoder", &encoder);
ros::Publisher pub_ultrasound("ultrasound", &ultrasound);


/**************************************************************************
ESC COMMAND {MOTOR, SERVO} CALLBACK
**************************************************************************/
void messageCb(const barc::ECU& ecu){
  // deconstruct esc message
  motorCMD = saturateMotor( int(ecu.motor_pwm) );
  servoCMD = saturateServo( int(ecu.servo_pwm) );
  
  // apply commands to motor and servo
  motor.write( motorCMD );   
  steering.write(  servoCMD );
}
// ECU := Engine Control Unit
ros::Subscriber<barc::ECU> s("ecu", messageCb);

// Set up ultrasound sensors
Maxbotix us_fr(14, Maxbotix::PW, Maxbotix::LV); // front
