 Servo port_thruster;
 Servo stbd_thruster;
 
-const float ANGLE_CONVERSION = 4096.0/360.0;
-
 void setup()
 {
   nh.initNode();
   nh.advertise(state_pub);
+  state.layout.data_offset = 0;
+  state.layout.dim[0].label = "command";
+  state.layout.dim[0].size = 2;
+  state.layout.dim[0].stride = 2;
 
   ax12Init(1000000);
   aft_thruster.attach(AFT_THRUSTER);

    stbd_thruster.writeMicroseconds(ZERO_THRUST);
    aft_thruster.writeMicroseconds(ZERO_THRUST);
  }
  state[0] = port_servo_pos;
  state[1] = stbd_servo_pos;
  state[2] = GetSpeed(PORT_SERVO);
  state[2] = GetSpeed(STBD_SERVO);
  state[4] = GetLoad(PORT_SERVO);
  state[5] = GetLoad(STBD_SERVO);
  state_pub.publish(&state);
  delay(33);
}

#include <ros.h>
#include <std_msgs/Int32MultiArray.h>

#include "ax12.h"
#include <Servo.h>
