commit c1426757626c59452c6bd8a0dad1d23430b56fae
Author: Tom Mullins <tmullins@andrew.cmu.edu>
Date:   Thu May 3 20:18:26 2012 -0400

    Adding some files to arduino so it compiles
    
    These files are generated by rosserial, but they don't need to be
    regenerated by everyone, and I don't remember now how to generate them
    anyway.

diff --git a/quad2/arduino/src/ros_lib/ArduinoHardware-new.h b/quad2/arduino/src/ros_lib/ArduinoHardware-new.h
new file mode 100644
index 0000000..89f75b6
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ArduinoHardware-new.h
@@ -0,0 +1,87 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ *
+ * This is a modified version of ArduinoHardware.h to use Serial_ instead of
+ * HardwareSerial for the Leonardo
+ *
+ */
+
+#ifndef ROS_ARDUINO_HARDWARE_H_
+#define ROS_ARDUINO_HARDWARE_H_
+
+#include "WProgram.h"
+#include <HardwareSerial.h>
+
+class ArduinoHardware {
+  public:
+    ArduinoHardware(Serial_* io , long baud= 57600){
+      iostream = io;
+      baud_ = baud;
+    }
+    ArduinoHardware()
+    {
+      iostream = &Serial;
+      baud_ = 57600;
+    }
+    ArduinoHardware(ArduinoHardware& h){
+      this->iostream = iostream;
+      this->baud_ = h.baud_;
+    }
+  
+    void setBaud(long baud){
+      this->baud_= baud;
+    }
+  
+    int getBaud(){return baud_;}
+
+    void init(){
+      iostream->begin(baud_);
+    }
+
+    int read(){return iostream->read();};
+    void write(uint8_t* data, int length){
+      for(int i=0; i<length; i++)
+        iostream->write(data[i]);
+    }
+
+    unsigned long time(){return millis();}
+
+  protected:
+    Serial_* iostream;
+    long baud_;
+};
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/ArduinoHardware-org.h b/quad2/arduino/src/ros_lib/ArduinoHardware-org.h
new file mode 100644
index 0000000..a3efe09
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ArduinoHardware-org.h
@@ -0,0 +1,80 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROS_ARDUINO_HARDWARE_H_
+#define ROS_ARDUINO_HARDWARE_H_
+
+#include "WProgram.h"
+#include <HardwareSerial.h>
+
+class ArduinoHardware {
+  public:
+    ArduinoHardware(HardwareSerial* io , long baud= 57600){
+      iostream = io;
+      baud_ = baud;
+    }
+    ArduinoHardware()
+    {
+      iostream = &Serial;
+      baud_ = 57600;
+    }
+    ArduinoHardware(ArduinoHardware& h){
+      this->iostream = iostream;
+      this->baud_ = h.baud_;
+    }
+  
+    void setBaud(long baud){
+      this->baud_= baud;
+    }
+  
+    int getBaud(){return baud_;}
+
+    void init(){
+      iostream->begin(baud_);
+    }
+
+    int read(){return iostream->read();};
+    void write(uint8_t* data, int length){
+      for(int i=0; i<length; i++)
+        iostream->write(data[i]);
+    }
+
+    unsigned long time(){return millis();}
+
+  protected:
+    HardwareSerial* iostream;
+    long baud_;
+};
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/ArduinoHardware.h b/quad2/arduino/src/ros_lib/ArduinoHardware.h
new file mode 100644
index 0000000..a3efe09
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ArduinoHardware.h
@@ -0,0 +1,80 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROS_ARDUINO_HARDWARE_H_
+#define ROS_ARDUINO_HARDWARE_H_
+
+#include "WProgram.h"
+#include <HardwareSerial.h>
+
+class ArduinoHardware {
+  public:
+    ArduinoHardware(HardwareSerial* io , long baud= 57600){
+      iostream = io;
+      baud_ = baud;
+    }
+    ArduinoHardware()
+    {
+      iostream = &Serial;
+      baud_ = 57600;
+    }
+    ArduinoHardware(ArduinoHardware& h){
+      this->iostream = iostream;
+      this->baud_ = h.baud_;
+    }
+  
+    void setBaud(long baud){
+      this->baud_= baud;
+    }
+  
+    int getBaud(){return baud_;}
+
+    void init(){
+      iostream->begin(baud_);
+    }
+
+    int read(){return iostream->read();};
+    void write(uint8_t* data, int length){
+      for(int i=0; i<length; i++)
+        iostream->write(data[i]);
+    }
+
+    unsigned long time(){return millis();}
+
+  protected:
+    HardwareSerial* iostream;
+    long baud_;
+};
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/duration.cpp b/quad2/arduino/src/ros_lib/duration.cpp
new file mode 100644
index 0000000..7a55efd
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/duration.cpp
@@ -0,0 +1,81 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <math.h>
+#include "ros/duration.h"
+
+namespace ros
+{
+  void normalizeSecNSecSigned(long &sec, long &nsec)
+  {
+    long nsec_part = nsec;
+    long sec_part = sec;
+     
+    while (nsec_part > 1000000000L)
+    {
+      nsec_part -= 1000000000L;
+      ++sec_part;
+    }
+    while (nsec_part < 0)
+    {
+      nsec_part += 1000000000L;
+      --sec_part;
+    }
+    sec = sec_part;
+    nsec = nsec_part;
+  }
+
+  Duration& Duration::operator+=(const Duration &rhs)
+  {
+    sec += rhs.sec;
+    nsec += rhs.nsec;
+    normalizeSecNSecSigned(sec, nsec);
+    return *this;
+  }
+
+  Duration& Duration::operator-=(const Duration &rhs){
+    sec += -rhs.sec;
+    nsec += -rhs.nsec;
+    normalizeSecNSecSigned(sec, nsec);
+    return *this;
+  }
+
+  Duration& Duration::operator*=(double scale){
+    sec *= scale;
+    nsec *= scale;
+    normalizeSecNSecSigned(sec, nsec);
+    return *this;
+  }
+
+}
diff --git a/quad2/arduino/src/ros_lib/examples/ADC/ADC.pde b/quad2/arduino/src/ros_lib/examples/ADC/ADC.pde
new file mode 100644
index 0000000..22546aa
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/ADC/ADC.pde
@@ -0,0 +1,48 @@
+/* 
+ * rosserial ADC Example
+ * 
+ * This is a poor man's Oscilloscope.  It does not have the sampling 
+ * rate or accuracy of a commerical scope, but it is great to get
+ * an analog value into ROS in a pinch.
+ */
+
+#include <WProgram.h>
+#include <ros.h>
+#include <rosserial_arduino/Adc.h>
+
+ros::NodeHandle nh;
+
+rosserial_arduino::Adc adc_msg;
+ros::Publisher p("adc", &adc_msg);
+
+void setup()
+{ 
+  pinMode(13, OUTPUT);
+  nh.initNode();
+
+  nh.advertise(p);
+}
+
+//We average the analog reading to elminate some of the noise
+int averageAnalog(int pin){
+  int v=0;
+  for(int i=0; i<4; i++) v+= analogRead(pin);
+  return v/4;
+}
+
+long adc_timer;
+
+void loop()
+{
+  adc_msg.adc0 = averageAnalog(0);
+  adc_msg.adc1 = averageAnalog(1);
+  adc_msg.adc2 = averageAnalog(2);
+  adc_msg.adc3 = averageAnalog(3);
+  adc_msg.adc4 = averageAnalog(4);
+  adc_msg.adc5 = averageAnalog(5);
+    
+  p.publish(&adc_msg);
+
+  nh.spinOnce();
+}
+
diff --git a/quad2/arduino/src/ros_lib/examples/Blink/Blink.pde b/quad2/arduino/src/ros_lib/examples/Blink/Blink.pde
new file mode 100644
index 0000000..4e9e185
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/Blink/Blink.pde
@@ -0,0 +1,29 @@
+/* 
+ * rosserial Subscriber Example
+ * Blinks an LED on callback
+ */
+
+#include <ros.h>
+#include <std_msgs/Empty.h>
+
+ros::NodeHandle  nh;
+
+void messageCb( const std_msgs::Empty& toggle_msg){
+  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
+}
+
+ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );
+
+void setup()
+{ 
+  pinMode(13, OUTPUT);
+  nh.initNode();
+  nh.subscribe(sub);
+}
+
+void loop()
+{  
+  nh.spinOnce();
+  delay(1);
+}
+
diff --git a/quad2/arduino/src/ros_lib/examples/BlinkM/BlinkM.pde b/quad2/arduino/src/ros_lib/examples/BlinkM/BlinkM.pde
new file mode 100644
index 0000000..f4f3e89
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/BlinkM/BlinkM.pde
@@ -0,0 +1,163 @@
+/*
+*  RosSerial BlinkM Example
+*  This program shows how to control a blinkm
+*  from an arduino using RosSerial
+*/
+
+#include "WProgram.h" //include the Arduino library
+#include <stdlib.h>
+
+
+#include <ros.h>
+#include <std_msgs/String.h>
+
+
+//include Wire/ twi for the BlinkM
+#include <Wire.h>
+extern "C" { 
+#include "utility/twi.h" 
+}
+
+#include "BlinkM_funcs.h"
+const byte blinkm_addr = 0x09; //default blinkm address
+
+
+void setLED( bool solid,  char color)
+{
+
+       	if (solid)
+	{
+           switch (color)
+		{
+
+		case 'w':  // white
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0xff);  
+			break;
+			
+		case 'r': //RED
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0);  
+			break;
+
+		case 'g':// Green
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0);
+			break;
+
+		case 'b':// Blue
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0,0,0xff);
+			break;
+
+		case 'c':// Cyan
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0xff);
+			break;
+
+		case 'm': // Magenta
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0xff);
+			break;
+
+		case 'y': // yellow
+                        BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0);
+			break;
+
+		default: // Black
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_fadeToRGB( blinkm_addr, 0,0,0);
+			break;
+		}
+	}
+
+
+	else
+	{
+               	switch (color)
+		{
+		case 'r':  // Blink Red
+                        BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 3,0,0 );
+			break;
+		case 'w':  // Blink white
+                        BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 2,0,0 );
+			break;
+		case 'g':  // Blink Green
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 4,0,0 );
+			break;
+
+		case 'b': // Blink Blue
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 5,0,0 );
+			break;
+
+		case 'c': //Blink Cyan
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 6,0,0 );
+			break;
+
+		case 'm': //Blink Magenta
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 7,0,0 );
+			break;
+
+		case 'y': //Blink Yellow
+                        BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 8,0,0 );
+			break;
+
+		default: //OFF
+			BlinkM_stopScript( blinkm_addr );
+			BlinkM_playScript( blinkm_addr, 9,0,0 );
+			break;
+		}
+
+	}
+}
+
+void light_cb( const std_msgs::String& light_cmd){
+        bool solid =false;
+        char color; 
+        if (strlen( (const char* ) light_cmd.data) ==2 ){
+          solid  = (light_cmd.data[0] == 'S') || (light_cmd.data[0] == 's');
+          color = light_cmd.data[1];
+        }
+        else{
+          solid=  false;
+          color = light_cmd.data[0];
+        } 
+        
+	setLED(solid, color);
+}
+
+
+
+ros::NodeHandle  nh;
+ros::Subscriber<std_msgs::String> sub("blinkm" , light_cb);
+
+
+void setup()
+{
+   
+    pinMode(13, OUTPUT); //set up the LED
+
+	BlinkM_beginWithPower();
+	delay(100);
+	BlinkM_stopScript(blinkm_addr);  // turn off startup script
+	setLED(false, 0); //turn off the led
+        
+        nh.initNode();
+        nh.subscribe(sub);
+
+}
+
+void loop()
+{
+ nh.spinOnce();
+ delay(1);
+}
+
diff --git a/quad2/arduino/src/ros_lib/examples/BlinkM/BlinkM_funcs.h b/quad2/arduino/src/ros_lib/examples/BlinkM/BlinkM_funcs.h
new file mode 100644
index 0000000..d997d9c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/BlinkM/BlinkM_funcs.h
@@ -0,0 +1,442 @@
+/*
+ * BlinkM_funcs.h -- Arduino 'library' to control BlinkM
+ * --------------
+ *
+ *
+ * Note: original version of this file lives with the BlinkMTester sketch
+ *
+ * Note: all the functions are declared 'static' because 
+ *       it saves about 1.5 kbyte in code space in final compiled sketch.  
+ *       A C++ library of this costs a 1kB more.
+ *
+ * 2007-8, Tod E. Kurt, ThingM, http://thingm.com/
+ *
+ * version: 20081101
+ *
+ * history:
+ *  20080101 - initial release
+ *  20080203 - added setStartupParam(), bugfix receiveBytes() from Dan Julio
+ *  20081101 - fixed to work with Arduino-0012, added MaxM commands,
+ *             added test script read/write functions, cleaned up some functions
+ *  20090121 - added I2C bus scan functions, has dependencies on private 
+ *             functions inside Wire library, so might break in the future
+ *  20100420 - added BlinkM_startPower and _stopPower
+ *
+ */
+
+#include "WProgram.h"
+#include "wiring.h"
+#include <Wire.h>
+
+extern "C" { 
+#include "utility/twi.h"  // from Wire library, so we can do bus scanning
+}
+
+
+// format of light script lines: duration, command, arg1,arg2,arg3
+typedef struct _blinkm_script_line {
+  uint8_t dur;
+  uint8_t cmd[4];    // cmd,arg1,arg2,arg3
+} blinkm_script_line;
+
+
+// Call this first (when powering BlinkM from a power supply)
+static void BlinkM_begin()
+{
+  Wire.begin();                // join i2c bus (address optional for master)
+}
+
+/*
+ * actually can't do this either, because twi_init() has THREE callocs in it too
+ *
+static void BlinkM_reset()
+{
+  twi_init();  // can't just call Wire.begin() again because of calloc()s there
+}
+*/
+
+//
+// each call to twi_writeTo() should return 0 if device is there
+// or other value (usually 2) if nothing is at that address
+// 
+static void BlinkM_scanI2CBus(byte from, byte to, 
+                              void(*callback)(byte add, byte result) ) 
+{
+  byte rc;
+  byte data = 0; // not used, just an address to feed to twi_writeTo()
+  for( byte addr = from; addr <= to; addr++ ) {
+    rc = twi_writeTo(addr, &data, 0, 1);
+    callback( addr, rc );
+  }
+}
+
+//
+//
+static int8_t BlinkM_findFirstI2CDevice() 
+{
+  byte rc;
+  byte data = 0; // not used, just an address to feed to twi_writeTo()
+  for( byte addr=1; addr < 120; addr++ ) {  // only scan addrs 1-120
+    rc = twi_writeTo(addr, &data, 0, 1);
+    if( rc == 0 ) return addr; // found an address
+  }
+  return -1; // no device found in range given
+}
+
+// FIXME: make this more Arduino-like
+static void BlinkM_startPowerWithPins(byte pwrpin, byte gndpin)
+{
+  DDRC |= _BV(pwrpin) | _BV(gndpin);  // make outputs
+  PORTC &=~ _BV(gndpin);
+  PORTC |=  _BV(pwrpin);
+}
+
+// FIXME: make this more Arduino-like
+static void BlinkM_stopPowerWithPins(byte pwrpin, byte gndpin)
+{
+  DDRC &=~ (_BV(pwrpin) | _BV(gndpin));
+}
+
+//
+static void BlinkM_startPower()
+{
+  BlinkM_startPowerWithPins( PORTC3, PORTC2 );
+}
+
+//
+static void BlinkM_stopPower()
+{
+  BlinkM_stopPowerWithPins( PORTC3, PORTC2 );
+}
+
+// General version of BlinkM_beginWithPower().
+// Call this first when BlinkM is plugged directly into Arduino
+static void BlinkM_beginWithPowerPins(byte pwrpin, byte gndpin)
+{
+  BlinkM_startPowerWithPins(pwrpin,gndpin);
+  delay(100);  // wait for things to stabilize
+  Wire.begin();
+}
+
+// Call this first when BlinkM is plugged directly into Arduino
+// FIXME: make this more Arduino-like
+static void BlinkM_beginWithPower()
+{
+  BlinkM_beginWithPowerPins( PORTC3, PORTC2 );
+}
+
+// sends a generic command
+static void BlinkM_sendCmd(byte addr, byte* cmd, int cmdlen)
+{
+  Wire.beginTransmission(addr);
+  for( byte i=0; i<cmdlen; i++) 
+    Wire.send(cmd[i]);
+  Wire.endTransmission();
+}
+
+// receives generic data
+// returns 0 on success, and -1 if no data available
+// note: responsiblity of caller to know how many bytes to expect
+static int BlinkM_receiveBytes(byte addr, byte* resp, byte len)
+{
+  Wire.requestFrom(addr, len);
+  if( Wire.available() ) {
+    for( int i=0; i<len; i++) 
+      resp[i] = Wire.receive();
+    return 0;
+  }
+  return -1;
+}
+
+// Sets the I2C address of the BlinkM.  
+// Uses "general call" broadcast address
+static void BlinkM_setAddress(byte newaddress)
+{
+  Wire.beginTransmission(0x00);  // general call (broadcast address)
+  Wire.send('A');
+  Wire.send(newaddress);
+  Wire.send(0xD0);
+  Wire.send(0x0D);  // dood!
+  Wire.send(newaddress);
+  Wire.endTransmission();
+  delay(50); // just in case
+}
+
+
+// Gets the I2C address of the BlinKM
+// Kind of redundant when sent to a specific address
+// but uses to verify BlinkM communication
+static int BlinkM_getAddress(byte addr)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('a');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)1);  // general call
+  if( Wire.available() ) {
+    byte b = Wire.receive();
+    return b;
+  }
+  return -1;
+}
+
+// Gets the BlinkM firmware version
+static int BlinkM_getVersion(byte addr)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('Z');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)2);
+  if( Wire.available() ) {
+    byte major_ver = Wire.receive();
+    byte minor_ver = Wire.receive();
+    return (major_ver<<8) + minor_ver;
+  }
+  return -1;
+}
+
+// Demonstrates how to verify you're talking to a BlinkM 
+// and that you know its address
+static int BlinkM_checkAddress(byte addr)
+{
+  //Serial.print("Checking BlinkM address...");
+  int b = BlinkM_getAddress(addr);
+  if( b==-1 ) {
+    //Serial.println("No response, that's not good");
+    return -1;  // no response
+  } 
+  //Serial.print("received addr: 0x");
+  //Serial.print(b,HEX);
+  if( b != addr )
+    return 1; // error, addr mismatch 
+  else 
+    return 0; // match, everything okay
+}
+
+// Sets the speed of fading between colors.  
+// Higher numbers means faster fading, 255 == instantaneous fading
+static void BlinkM_setFadeSpeed(byte addr, byte fadespeed)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('f');
+  Wire.send(fadespeed);
+  Wire.endTransmission();  
+}
+
+// Sets the light script playback time adjust
+// The timeadj argument is signed, and is an additive value to all
+// durations in a light script. Set to zero to turn off time adjust.
+static void BlinkM_setTimeAdj(byte addr, byte timeadj)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('t');
+  Wire.send(timeadj);
+  Wire.endTransmission();  
+}
+
+// Fades to an RGB color
+static void BlinkM_fadeToRGB(byte addr, byte red, byte grn, byte blu)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('c');
+  Wire.send(red);
+  Wire.send(grn);
+  Wire.send(blu);
+  Wire.endTransmission();
+}
+
+// Fades to an HSB color
+static void BlinkM_fadeToHSB(byte addr, byte hue, byte saturation, byte brightness)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('h');
+  Wire.send(hue);
+  Wire.send(saturation);
+  Wire.send(brightness);
+  Wire.endTransmission();
+}
+
+// Sets an RGB color immediately
+static void BlinkM_setRGB(byte addr, byte red, byte grn, byte blu)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('n');
+  Wire.send(red);
+  Wire.send(grn);
+  Wire.send(blu);
+  Wire.endTransmission();
+}
+
+// Fades to a random RGB color
+static void BlinkM_fadeToRandomRGB(byte addr, byte rrnd, byte grnd, byte brnd)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('C');
+  Wire.send(rrnd);
+  Wire.send(grnd);
+  Wire.send(brnd);
+  Wire.endTransmission();
+}
+// Fades to a random HSB color
+static void BlinkM_fadeToRandomHSB(byte addr, byte hrnd, byte srnd, byte brnd)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('H');
+  Wire.send(hrnd);
+  Wire.send(srnd);
+  Wire.send(brnd);
+  Wire.endTransmission();
+}
+
+//
+static void BlinkM_getRGBColor(byte addr, byte* r, byte* g, byte* b)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('g');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)3);
+  if( Wire.available() ) {
+    *r = Wire.receive();
+    *g = Wire.receive();
+    *b = Wire.receive();
+  }
+}
+
+//
+static void BlinkM_playScript(byte addr, byte script_id, byte reps, byte pos)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('p');
+  Wire.send(script_id);
+  Wire.send(reps);
+  Wire.send(pos);
+  Wire.endTransmission();
+}
+
+//
+static void BlinkM_stopScript(byte addr)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('o');
+  Wire.endTransmission();
+}
+
+//
+static void BlinkM_setScriptLengthReps(byte addr, byte script_id, 
+                                       byte len, byte reps)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('L');
+  Wire.send(script_id);
+  Wire.send(len);
+  Wire.send(reps);
+  Wire.endTransmission();
+}
+
+// Fill up script_line with data from a script line
+// currently only script_id = 0 works (eeprom script)
+static void BlinkM_readScriptLine(byte addr, byte script_id, 
+                                  byte pos, blinkm_script_line* script_line)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('R');
+  Wire.send(script_id);
+  Wire.send(pos);
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)5);
+  while( Wire.available() < 5 ) ; // FIXME: wait until we get 7 bytes
+  script_line->dur    = Wire.receive();
+  script_line->cmd[0] = Wire.receive();
+  script_line->cmd[1] = Wire.receive();
+  script_line->cmd[2] = Wire.receive();
+  script_line->cmd[3] = Wire.receive();
+}
+
+//
+static void BlinkM_writeScriptLine(byte addr, byte script_id, 
+                                   byte pos, byte dur,
+                                   byte cmd, byte arg1, byte arg2, byte arg3)
+{
+#ifdef BLINKM_FUNCS_DEBUG
+  Serial.print("writing line:");  Serial.print(pos,DEC);
+  Serial.print(" with cmd:"); Serial.print(cmd); 
+  Serial.print(" arg1:"); Serial.println(arg1,HEX);
+#endif
+  Wire.beginTransmission(addr);
+  Wire.send('W');
+  Wire.send(script_id);
+  Wire.send(pos);
+  Wire.send(dur);
+  Wire.send(cmd);
+  Wire.send(arg1);
+  Wire.send(arg2);
+  Wire.send(arg3);
+  Wire.endTransmission();
+
+}
+
+//
+static void BlinkM_writeScript(byte addr, byte script_id, 
+                               byte len, byte reps,
+                               blinkm_script_line* lines)
+{
+#ifdef BLINKM_FUNCS_DEBUG
+  Serial.print("writing script to addr:"); Serial.print(addr,DEC);
+  Serial.print(", script_id:"); Serial.println(script_id,DEC);
+#endif
+  for(byte i=0; i < len; i++) {
+    blinkm_script_line l = lines[i];
+    BlinkM_writeScriptLine( addr, script_id, i, l.dur,
+                            l.cmd[0], l.cmd[1], l.cmd[2], l.cmd[3]);
+    delay(20); // must wait for EEPROM to be programmed
+  }
+  BlinkM_setScriptLengthReps(addr, script_id, len, reps);
+}
+
+//
+static void BlinkM_setStartupParams(byte addr, byte mode, byte script_id,
+                                    byte reps, byte fadespeed, byte timeadj)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('B');
+  Wire.send(mode);             // default 0x01 == Play script
+  Wire.send(script_id);        // default 0x00 == script #0
+  Wire.send(reps);             // default 0x00 == repeat infinitely
+  Wire.send(fadespeed);        // default 0x08 == usually overridden by sketch 
+  Wire.send(timeadj);          // default 0x00 == sometimes overridden by sketch
+  Wire.endTransmission();
+} 
+
+
+// Gets digital inputs of the BlinkM
+// returns -1 on failure
+static int BlinkM_getInputsO(byte addr)
+{
+  Wire.beginTransmission(addr);
+  Wire.send('i');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)1);
+  if( Wire.available() ) {
+    byte b = Wire.receive();
+    return b; 
+  }
+  return -1;
+}
+
+// Gets digital inputs of the BlinkM
+// stores them in passed in array
+// returns -1 on failure
+static int BlinkM_getInputs(byte addr, byte inputs[])
+{
+  Wire.beginTransmission(addr);
+  Wire.send('i');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)4);
+  while( Wire.available() < 4 ) ; // FIXME: wait until we get 4 bytes
+    
+  inputs[0] = Wire.receive();
+  inputs[1] = Wire.receive();
+  inputs[2] = Wire.receive();
+  inputs[3] = Wire.receive();
+
+  return 0;
+}
diff --git a/quad2/arduino/src/ros_lib/examples/Clapper/Clapper.pde b/quad2/arduino/src/ros_lib/examples/Clapper/Clapper.pde
new file mode 100644
index 0000000..2444358
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/Clapper/Clapper.pde
@@ -0,0 +1,91 @@
+/* 
+ * rosserial Clapper Example
+ * 
+ * This code is a very simple example of the kinds of 
+ * custom sensors that you can easily set up with rosserial
+ * and Arduino.  This code uses a microphone attached to 
+ * analog pin 5 detect two claps (2 loud sounds).
+ * You can use this clapper, for example, to command a robot 
+ * in the area to come do your bidding.
+ */
+
+#include <WProgram.h>
+
+#include <ros.h>
+#include <std_msgs/Empty.h>
+
+ros::NodeHandle  nh;
+
+std_msgs::Empty clap_msg;
+ros::Publisher p("clap", &clap_msg);
+
+enum clapper_state { clap1, clap_one_waiting,  pause, clap2};
+clapper_state clap;
+
+int volume_thresh = 200;  //a clap sound needs to be: 
+                          //abs(clap_volume) > average noise + volume_thresh
+int mic_pin = 5;
+int adc_ave=0;
+
+void setup()
+{ 
+  pinMode(13, OUTPUT);
+  nh.initNode();
+
+  nh.advertise(p);
+
+  //measure the average volume of the noise in the area
+  for (int i =0; i<10;i++) adc_ave += analogRead(mic_pin);
+  adc_ave /= 10;
+}
+
+long event_timer;
+
+void loop()
+{
+  int mic_val = 0;
+  for(int i=0; i<4; i++) mic_val += analogRead(mic_pin);
+  
+  mic_val = mic_val/4-adc_ave;
+  
+  switch(clap){
+   case clap1:
+    if (abs(mic_val) > volume_thresh){
+       clap = clap_one_waiting; 
+       event_timer = millis();
+    }
+    break;
+    case clap_one_waiting:
+      if ( (abs(mic_val) < 30) && ( (millis() - event_timer) > 20 ) )  
+      {
+        clap= pause;
+        event_timer = millis();
+
+      }
+      break;
+    case pause: // make sure there is a pause between 
+                // the loud sounds
+      if ( mic_val > volume_thresh) 
+      {
+        clap = clap1;
+
+      }
+      else if ( (millis()-event_timer)> 60)  {
+        clap = clap2;
+        event_timer = millis();
+
+      }
+      break;
+     case clap2:
+        if (abs(mic_val) > volume_thresh){ //we have got a double clap!
+            clap = clap1;
+            p.publish(&clap_msg);
+        }
+        else if ( (millis()-event_timer)> 200) {
+          clap= clap1; // no clap detected, reset state machine
+        }
+        
+        break;   
+  }
+  nh.spinOnce();
+}
diff --git a/quad2/arduino/src/ros_lib/examples/HelloWorld/HelloWorld.pde b/quad2/arduino/src/ros_lib/examples/HelloWorld/HelloWorld.pde
new file mode 100644
index 0000000..2474413
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/HelloWorld/HelloWorld.pde
@@ -0,0 +1,28 @@
+/*
+ * rosserial Publisher Example
+ * Prints "hello world!"
+ */
+
+#include <ros.h>
+#include <std_msgs/String.h>
+
+ros::NodeHandle  nh;
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+char hello[13] = "hello world!";
+
+void setup()
+{
+  nh.initNode();
+  nh.advertise(chatter);
+}
+
+void loop()
+{
+  str_msg.data = hello;
+  chatter.publish( &str_msg );
+  nh.spinOnce();
+  delay(1000);
+}
diff --git a/quad2/arduino/src/ros_lib/examples/IrRanger/IrRanger.pde b/quad2/arduino/src/ros_lib/examples/IrRanger/IrRanger.pde
new file mode 100644
index 0000000..240b5a1
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/IrRanger/IrRanger.pde
@@ -0,0 +1,64 @@
+/* 
+ * rosserial IR Ranger Example  
+ * 
+ * This example is calibrated for the Sharp GP2D120XJ00F.
+ */
+
+#include <ros.h>
+#include <ros/time.h>
+#include <sensor_msgs/Range.h>
+
+ros::NodeHandle  nh;
+
+
+sensor_msgs::Range range_msg;
+ros::Publisher pub_range( "range_data", &range_msg);
+
+const int analog_pin = 0;
+unsigned long range_timer;
+
+/*
+ * getRange() - samples the analog input from the ranger
+ * and converts it into meters.  
+ */
+float getRange(int pin_num){
+    int sample;
+    // Get data
+    sample = analogRead(pin_num)/4;
+    // if the ADC reading is too low, 
+    //   then we are really far away from anything
+    if(sample < 10)
+        return 254;     // max range
+    // Magic numbers to get cm
+    sample= 1309/(sample-3);
+    return (sample - 1)/100; //convert to meters
+}
+
+char frameid[] = "/ir_ranger";
+
+void setup()
+{
+  nh.initNode();
+  nh.advertise(pub_range);
+  
+  range_msg.radiation_type = sensor_msgs::Range::INFRARED;
+  range_msg.header.frame_id =  frameid;
+  range_msg.field_of_view = 0.01;
+  range_msg.min_range = 0.03;
+  range_msg.max_range = 0.4;
+  
+}
+
+void loop()
+{
+  // publish the range value every 50 milliseconds
+  //   since it takes that long for the sensor to stabilize
+  if ( (millis()-range_timer) > 50){
+    range_msg.range = getRange(analog_pin);
+    range_msg.header.stamp = nh.now();
+    pub_range.publish(&range_msg);
+    range_timer =  millis() + 50;
+  }
+  nh.spinOnce();
+}
+
diff --git a/quad2/arduino/src/ros_lib/examples/Logging/Logging.pde b/quad2/arduino/src/ros_lib/examples/Logging/Logging.pde
new file mode 100644
index 0000000..400a9cd
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/Logging/Logging.pde
@@ -0,0 +1,45 @@
+/*
+ * rosserial PubSub Example
+ * Prints "hello world!" and toggles led
+ */
+
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Empty.h>
+
+ros::NodeHandle  nh;
+
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+char hello[13] = "hello world!";
+
+
+char debug[]= "debug statements";
+char info[] = "infos";
+char warn[] = "warnings";
+char error[] = "errors";
+char fatal[] = "fatalities";
+
+void setup()
+{
+  pinMode(13, OUTPUT);
+  nh.initNode();
+  nh.advertise(chatter);
+}
+
+void loop()
+{
+  str_msg.data = hello;
+  chatter.publish( &str_msg );
+  
+  nh.logdebug(debug);
+  nh.loginfo(info);
+  nh.logwarn(warn);
+  nh.logerror(error);
+  nh.logfatal(fatal);
+  
+  nh.spinOnce();
+  delay(500);
+}
diff --git a/quad2/arduino/src/ros_lib/examples/Odom/Odom.pde b/quad2/arduino/src/ros_lib/examples/Odom/Odom.pde
new file mode 100644
index 0000000..5841020
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/Odom/Odom.pde
@@ -0,0 +1,53 @@
+/* 
+ * rosserial Planar Odometry Example
+ */
+
+#include <ros.h>
+#include <ros/time.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+
+ros::NodeHandle  nh;
+
+geometry_msgs::TransformStamped t;
+tf::TransformBroadcaster broadcaster;
+
+double x = 1.0;
+double y = 0.0;
+double theta = 1.57;
+
+char base_link[] = "/base_link";
+char odom[] = "/odom";
+
+void setup()
+{
+  nh.initNode();
+  broadcaster.init(nh);
+}
+
+void loop()
+{  
+  // drive in a circle
+  double dx = 0.2;
+  double dtheta = 0.18;
+  x += cos(theta)*dx*0.1;
+  y += sin(theta)*dx*0.1;
+  theta += dtheta*0.1;
+  if(theta > 3.14)
+    theta=-3.14;
+    
+  // tf odom->base_link
+  t.header.frame_id = odom;
+  t.child_frame_id = base_link;
+  
+  t.transform.translation.x = x;
+  t.transform.translation.y = y;
+  
+  t.transform.rotation = tf::createQuaternionFromYaw(theta);
+  t.header.stamp = nh.now();
+  
+  broadcaster.sendTransform(t);
+  nh.spinOnce();
+  
+  delay(10);
+}
diff --git a/quad2/arduino/src/ros_lib/examples/ServiceClient/ServiceClient.pde b/quad2/arduino/src/ros_lib/examples/ServiceClient/ServiceClient.pde
new file mode 100644
index 0000000..75093a9
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/ServiceClient/ServiceClient.pde
@@ -0,0 +1,38 @@
+/*
+ * rosserial Service Client
+ */
+
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <rosserial_arduino/Test.h>
+
+ros::NodeHandle  nh;
+using rosserial_arduino::Test;
+
+ros::ServiceClient<Test::Request, Test::Response> client("test_srv");
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+char hello[13] = "hello world!";
+
+void setup()
+{
+  nh.initNode();
+  nh.serviceClient(client);
+  nh.advertise(chatter);
+  while(!nh.connected()) nh.spinOnce();
+  nh.loginfo("Startup complete");
+}
+
+void loop()
+{
+  Test::Request req;
+  Test::Response res;
+  req.input = hello;
+  client.call(req, res);
+  str_msg.data = res.output;
+  chatter.publish( &str_msg );
+  nh.spinOnce();
+  delay(100);
+}
diff --git a/quad2/arduino/src/ros_lib/examples/ServiceClient/client.py b/quad2/arduino/src/ros_lib/examples/ServiceClient/client.py
new file mode 100755
index 0000000..3b27bd5
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/ServiceClient/client.py
@@ -0,0 +1,20 @@
+#!/usr/bin/env python
+
+""" 
+Sample code to use with ServiceClient.pde
+"""
+
+import roslib; roslib.load_manifest("rosserial_arduino")
+import rospy
+
+from rosserial_arduino.srv import *
+
+def callback(req):
+    print "The arduino is calling! Please send it a message:"
+    t = TestResponse()
+    t.output = raw_input()
+    return t
+
+rospy.init_node("service_client_test")
+rospy.Service("test_srv", Test, callback)
+rospy.spin()
diff --git a/quad2/arduino/src/ros_lib/examples/ServiceClient/client.pyc b/quad2/arduino/src/ros_lib/examples/ServiceClient/client.pyc
new file mode 100644
index 0000000..b7a945a
Binary files /dev/null and b/quad2/arduino/src/ros_lib/examples/ServiceClient/client.pyc differ
diff --git a/quad2/arduino/src/ros_lib/examples/ServiceServer/ServiceServer.pde b/quad2/arduino/src/ros_lib/examples/ServiceServer/ServiceServer.pde
new file mode 100644
index 0000000..2d3fd70
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/ServiceServer/ServiceServer.pde
@@ -0,0 +1,40 @@
+/*
+ * rosserial Service Server
+ */
+
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <rosserial_arduino/Test.h>
+
+ros::NodeHandle  nh;
+using rosserial_arduino::Test;
+
+int i;
+void callback(const Test::Request & req, Test::Response & res){
+  if((i++)%2)
+    res.output = "hello";
+  else
+    res.output = "world";
+}
+
+ros::ServiceServer<Test::Request, Test::Response> server("test_srv",&callback);
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+char hello[13] = "hello world!";
+
+void setup()
+{
+  nh.initNode();
+  nh.advertiseService(server);
+  nh.advertise(chatter);
+}
+
+void loop()
+{
+  str_msg.data = hello;
+  chatter.publish( &str_msg );
+  nh.spinOnce();
+  delay(10);
+}
diff --git a/quad2/arduino/src/ros_lib/examples/ServoControl/ServoControl.pde b/quad2/arduino/src/ros_lib/examples/ServoControl/ServoControl.pde
new file mode 100644
index 0000000..12063f0
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/ServoControl/ServoControl.pde
@@ -0,0 +1,45 @@
+/*
+ * rosserial Servo Control Example
+ *
+ * This sketch demonstrates the control of hobby R/C servos
+ * using ROS and the arduiono
+ * 
+ * For the full tutorial write up, visit
+ * www.ros.org/wiki/rosserial_arduino_demos
+ *
+ * For more information on the Arduino Servo Library
+ * Checkout :
+ * http://www.arduino.cc/en/Reference/Servo
+ */
+
+#include <WProgram.h>
+
+#include <Servo.h> 
+#include <ros.h>
+#include <std_msgs/UInt16.h>
+
+ros::NodeHandle  nh;
+
+Servo servo;
+
+void servo_cb( const std_msgs::UInt16& cmd_msg){
+  servo.write(cmd_msg.data); //set servo angle, should be from 0-180  
+  digitalWrite(13, HIGH-digitalRead(13));  //toggle led  
+}
+
+
+ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);
+
+void setup(){
+  pinMode(13, OUTPUT);
+
+  nh.initNode();
+  nh.subscribe(sub);
+  
+  servo.attach(9); //attach it to pin 9
+}
+
+void loop(){
+  nh.spinOnce();
+  delay(1);
+}
diff --git a/quad2/arduino/src/ros_lib/examples/Temperature/Temperature.pde b/quad2/arduino/src/ros_lib/examples/Temperature/Temperature.pde
new file mode 100644
index 0000000..013b543
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/Temperature/Temperature.pde
@@ -0,0 +1,73 @@
+/*
+ * rosserial Temperature Sensor Example
+ * 
+ * This tutorial demonstrates the usage of the
+ * Sparkfun TMP102 Digital Temperature Breakout board
+ * http://www.sparkfun.com/products/9418
+ * 
+ * Source Code Based off of:
+ * http://wiring.org.co/learning/libraries/tmp102sparkfun.html
+ */
+
+#include <WProgram.h>
+#include <Wire.h>
+#include <ros.h>
+#include <std_msgs/Float32.h>
+
+ros::NodeHandle  nh;
+
+
+std_msgs::Float32 temp_msg;
+ros::Publisher pub_temp("temperature", &temp_msg);
+
+
+// From the datasheet the BMP module address LSB distinguishes
+// between read (1) and write (0) operations, corresponding to 
+// address 0x91 (read) and 0x90 (write).
+// shift the address 1 bit right (0x91 or 0x90), the Wire library only needs the 7
+// most significant bits for the address 0x91 >> 1 = 0x48
+// 0x90 >> 1 = 0x48 (72)
+
+int sensorAddress = 0x91 >> 1;  // From datasheet sensor address is 0x91
+                                // shift the address 1 bit right, the Wire library only needs the 7
+                                // most significant bits for the address
+
+
+void setup()
+{
+  Wire.begin();        // join i2c bus (address optional for master) 
+  
+  nh.initNode();
+  nh.advertise(pub_temp);
+  
+}
+
+long publisher_timer;
+
+void loop()
+{
+  
+  if (millis() > publisher_timer) {
+  // step 1: request reading from sensor 
+    Wire.requestFrom(sensorAddress,2); 
+    delay(10);
+    if (2 <= Wire.available())  // if two bytes were received 
+    {
+      byte msb;
+      byte lsb;
+      int temperature;
+      
+      msb = Wire.receive();  // receive high byte (full degrees)
+      lsb = Wire.receive();  // receive low byte (fraction degrees) 
+      temperature = ((msb) << 4);  // MSB
+      temperature |= (lsb >> 4);   // LSB
+
+      temp_msg.data = temperature*0.0625;
+      pub_temp.publish(&temp_msg);
+    }
+  
+  publisher_timer = millis() + 1000;
+  }
+  
+  nh.spinOnce();
+}
diff --git a/quad2/arduino/src/ros_lib/examples/TimeTF/TimeTF.pde b/quad2/arduino/src/ros_lib/examples/TimeTF/TimeTF.pde
new file mode 100644
index 0000000..16fbb70
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/TimeTF/TimeTF.pde
@@ -0,0 +1,37 @@
+/* 
+ * rosserial Time and TF Example
+ * Publishes a transform at current time
+ */
+
+#include <ros.h>
+#include <ros/time.h>
+#include <tf/transform_broadcaster.h>
+
+ros::NodeHandle  nh;
+
+geometry_msgs::TransformStamped t;
+tf::TransformBroadcaster broadcaster;
+
+char base_link[] = "/base_link";
+char odom[] = "/odom";
+
+void setup()
+{
+  nh.initNode();
+  broadcaster.init(nh);
+}
+
+void loop()
+{  
+  t.header.frame_id = odom;
+  t.child_frame_id = base_link;
+  t.transform.translation.x = 1.0; 
+  t.transform.rotation.x = 0.0;
+  t.transform.rotation.y = 0.0; 
+  t.transform.rotation.z = 0.0; 
+  t.transform.rotation.w = 1.0;  
+  t.header.stamp = nh.now();
+  broadcaster.sendTransform(t);
+  nh.spinOnce();
+  delay(10);
+}
diff --git a/quad2/arduino/src/ros_lib/examples/Ultrasound/Ultrasound.pde b/quad2/arduino/src/ros_lib/examples/Ultrasound/Ultrasound.pde
new file mode 100644
index 0000000..d5870cb
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/Ultrasound/Ultrasound.pde
@@ -0,0 +1,61 @@
+/* 
+ * rosserial Ultrasound Example
+ * 
+ * This example is for the Maxbotix Ultrasound rangers.
+ */
+
+#include <ros.h>
+#include <ros/time.h>
+#include <sensor_msgs/Range.h>
+
+ros::NodeHandle  nh;
+
+sensor_msgs::Range range_msg;
+ros::Publisher pub_range( "/ultrasound", &range_msg);
+
+const int adc_pin = 0;
+
+char frameid[] = "/ultrasound";
+
+float getRange_Ultrasound(int pin_num){
+  int val = 0;
+  for(int i=0; i<4; i++) val += analogRead(pin_num);
+  float range =  val;
+  return range /322.519685;   // (0.0124023437 /4) ; //cvt to meters
+}
+
+void setup()
+{
+  nh.initNode();
+  nh.advertise(pub_range);
+  
+  
+  range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
+  range_msg.header.frame_id =  frameid;
+  range_msg.field_of_view = 0.1;  // fake
+  range_msg.min_range = 0.0;
+  range_msg.max_range = 6.47;
+  
+  pinMode(8,OUTPUT);
+  digitalWrite(8, LOW);
+}
+
+
+long range_time;
+
+void loop()
+{
+  
+  //publish the adc value every 50 milliseconds
+  //since it takes that long for the sensor to stablize
+  if ( millis() >= range_time ){
+    int r =0;
+
+    range_msg.range = getRange_Ultrasound(5);
+    range_msg.header.stamp = nh.now();
+    pub_range.publish(&range_msg);
+    range_time =  millis() + 50;
+  }
+  
+  nh.spinOnce();
+}
diff --git a/quad2/arduino/src/ros_lib/examples/button_example/button_example.pde b/quad2/arduino/src/ros_lib/examples/button_example/button_example.pde
new file mode 100644
index 0000000..26eed6d
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/button_example/button_example.pde
@@ -0,0 +1,61 @@
+/* 
+ * Button Example for Rosserial
+ */
+
+#include <ros.h>
+#include <std_msgs/Bool.h>
+
+
+ros::NodeHandle nh;
+
+std_msgs::Bool pushed_msg;
+ros::Publisher pub_button("pushed", &pushed_msg);
+
+const int button_pin = 7;
+const int led_pin = 13;
+
+bool last_reading;
+long last_debounce_time=0;
+long debounce_delay=50;
+bool published = true;
+
+void setup()
+{
+  nh.initNode();
+  nh.advertise(pub_button);
+  
+  //initialize an LED output pin 
+  //and a input pin for our push button
+  pinMode(led_pin, OUTPUT);
+  pinMode(button_pin, INPUT);
+  
+  //Enable the pullup resistor on the button
+  PORTD |= (1<<PD7);
+  
+  //The button is a normally button
+  last_reading = ! digitalRead(button_pin);
+ 
+}
+
+void loop()
+{
+  
+  bool reading = ! digitalRead(button_pin);
+  
+  if (last_reading!= reading){
+      last_debounce_time = millis();
+      published = false;
+  }
+  
+  //if the button value has not changed for the debounce delay, we know its stable
+  if ( !published && (millis() - last_debounce_time)  > debounce_delay) {
+    digitalWrite(led_pin, reading);
+    pushed_msg.data = reading;
+    pub_button.publish(&pushed_msg);
+    published = true;
+  }
+
+  last_reading = reading;
+  
+  nh.spinOnce();
+}
diff --git a/quad2/arduino/src/ros_lib/examples/pubsub/pubsub.pde b/quad2/arduino/src/ros_lib/examples/pubsub/pubsub.pde
new file mode 100644
index 0000000..753d8ed
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/examples/pubsub/pubsub.pde
@@ -0,0 +1,40 @@
+/*
+ * rosserial PubSub Example
+ * Prints "hello world!" and toggles led
+ */
+
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Empty.h>
+
+ros::NodeHandle  nh;
+
+
+void messageCb( const std_msgs::Empty& toggle_msg){
+  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
+}
+
+ros::Subscriber<std_msgs::Empty> sub("toggle_led", messageCb );
+
+
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+char hello[13] = "hello world!";
+
+void setup()
+{
+  pinMode(13, OUTPUT);
+  nh.initNode();
+  nh.advertise(chatter);
+  nh.subscribe(sub);
+}
+
+void loop()
+{
+  str_msg.data = hello;
+  chatter.publish( &str_msg );
+  nh.spinOnce();
+  delay(500);
+}
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Point.h b/quad2/arduino/src/ros_lib/geometry_msgs/Point.h
new file mode 100644
index 0000000..bc8dce7
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Point.h
@@ -0,0 +1,112 @@
+#ifndef _ROS_geometry_msgs_Point_h
+#define _ROS_geometry_msgs_Point_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace geometry_msgs
+{
+
+  class Point : public ros::Msg
+  {
+    public:
+      float x;
+      float y;
+      float z;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      int32_t * val_x = (long *) &(this->x);
+      int32_t exp_x = (((*val_x)>>23)&255);
+      if(exp_x != 0)
+        exp_x += 1023-127;
+      int32_t sig_x = *val_x;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_x<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_x>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_x>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_x>>4) & 0x7F;
+      if(this->x < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_y = (long *) &(this->y);
+      int32_t exp_y = (((*val_y)>>23)&255);
+      if(exp_y != 0)
+        exp_y += 1023-127;
+      int32_t sig_y = *val_y;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_y<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_y>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_y>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_y>>4) & 0x7F;
+      if(this->y < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_z = (long *) &(this->z);
+      int32_t exp_z = (((*val_z)>>23)&255);
+      if(exp_z != 0)
+        exp_z += 1023-127;
+      int32_t sig_z = *val_z;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_z<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_z>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_z>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_z<<4) & 0xF0) | ((sig_z>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_z>>4) & 0x7F;
+      if(this->z < 0) *(outbuffer + offset -1) |= 0x80;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t * val_x = (uint32_t*) &(this->x);
+      offset += 3;
+      *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_x |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_x !=0)
+        *val_x |= ((exp_x)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
+      uint32_t * val_y = (uint32_t*) &(this->y);
+      offset += 3;
+      *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_y |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_y !=0)
+        *val_y |= ((exp_y)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
+      uint32_t * val_z = (uint32_t*) &(this->z);
+      offset += 3;
+      *val_z = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_z |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_z = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_z |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_z !=0)
+        *val_z |= ((exp_z)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Point"; };
+    const char * getMD5(){ return "4a842b65f413084dc2b10fb484ea7f17"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Point32.h b/quad2/arduino/src/ros_lib/geometry_msgs/Point32.h
new file mode 100644
index 0000000..92c8b22
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Point32.h
@@ -0,0 +1,100 @@
+#ifndef _ROS_geometry_msgs_Point32_h
+#define _ROS_geometry_msgs_Point32_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace geometry_msgs
+{
+
+  class Point32 : public ros::Msg
+  {
+    public:
+      float x;
+      float y;
+      float z;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        float real;
+        uint32_t base;
+      } u_x;
+      u_x.real = this->x;
+      *(outbuffer + offset + 0) = (u_x.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_x.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_x.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_x.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->x);
+      union {
+        float real;
+        uint32_t base;
+      } u_y;
+      u_y.real = this->y;
+      *(outbuffer + offset + 0) = (u_y.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_y.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_y.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_y.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->y);
+      union {
+        float real;
+        uint32_t base;
+      } u_z;
+      u_z.real = this->z;
+      *(outbuffer + offset + 0) = (u_z.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_z.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_z.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_z.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->z);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        float real;
+        uint32_t base;
+      } u_x;
+      u_x.base = 0;
+      u_x.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_x.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_x.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_x.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->x = u_x.real;
+      offset += sizeof(this->x);
+      union {
+        float real;
+        uint32_t base;
+      } u_y;
+      u_y.base = 0;
+      u_y.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_y.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_y.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_y.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->y = u_y.real;
+      offset += sizeof(this->y);
+      union {
+        float real;
+        uint32_t base;
+      } u_z;
+      u_z.base = 0;
+      u_z.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_z.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_z.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_z.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->z = u_z.real;
+      offset += sizeof(this->z);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Point32"; };
+    const char * getMD5(){ return "cc153912f1453b708d221682bc23d9ac"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/PointStamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/PointStamped.h
new file mode 100644
index 0000000..f6cd226
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/PointStamped.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_PointStamped_h
+#define _ROS_geometry_msgs_PointStamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Point.h"
+
+namespace geometry_msgs
+{
+
+  class PointStamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::Point point;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->point.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->point.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/PointStamped"; };
+    const char * getMD5(){ return "c63aecb41bfdfd6b7e1fac37c7cbe7bf"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Polygon.h b/quad2/arduino/src/ros_lib/geometry_msgs/Polygon.h
new file mode 100644
index 0000000..5987b5f
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Polygon.h
@@ -0,0 +1,54 @@
+#ifndef _ROS_geometry_msgs_Polygon_h
+#define _ROS_geometry_msgs_Polygon_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/Point32.h"
+
+namespace geometry_msgs
+{
+
+  class Polygon : public ros::Msg
+  {
+    public:
+      uint8_t points_length;
+      geometry_msgs::Point32 st_points;
+      geometry_msgs::Point32 * points;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset++) = points_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < points_length; i++){
+      offset += this->points[i].serialize(outbuffer + offset);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint8_t points_lengthT = *(inbuffer + offset++);
+      if(points_lengthT > points_length)
+        this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32));
+      offset += 3;
+      points_length = points_lengthT;
+      for( uint8_t i = 0; i < points_length; i++){
+      offset += this->st_points.deserialize(inbuffer + offset);
+        memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Polygon"; };
+    const char * getMD5(){ return "cd60a26494a087f577976f0329fa120e"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/PolygonStamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/PolygonStamped.h
new file mode 100644
index 0000000..5cf90e5
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/PolygonStamped.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_PolygonStamped_h
+#define _ROS_geometry_msgs_PolygonStamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Polygon.h"
+
+namespace geometry_msgs
+{
+
+  class PolygonStamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::Polygon polygon;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->polygon.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->polygon.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/PolygonStamped"; };
+    const char * getMD5(){ return "c6be8f7dc3bee7fe9e8d296070f53340"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Pose.h b/quad2/arduino/src/ros_lib/geometry_msgs/Pose.h
new file mode 100644
index 0000000..d67ce17
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Pose.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_Pose_h
+#define _ROS_geometry_msgs_Pose_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/Point.h"
+#include "geometry_msgs/Quaternion.h"
+
+namespace geometry_msgs
+{
+
+  class Pose : public ros::Msg
+  {
+    public:
+      geometry_msgs::Point position;
+      geometry_msgs::Quaternion orientation;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->position.serialize(outbuffer + offset);
+      offset += this->orientation.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->position.deserialize(inbuffer + offset);
+      offset += this->orientation.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Pose"; };
+    const char * getMD5(){ return "e45d45a5a1ce597b249e23fb30fc871f"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Pose2D.h b/quad2/arduino/src/ros_lib/geometry_msgs/Pose2D.h
new file mode 100644
index 0000000..298349d
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Pose2D.h
@@ -0,0 +1,112 @@
+#ifndef _ROS_geometry_msgs_Pose2D_h
+#define _ROS_geometry_msgs_Pose2D_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace geometry_msgs
+{
+
+  class Pose2D : public ros::Msg
+  {
+    public:
+      float x;
+      float y;
+      float theta;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      int32_t * val_x = (long *) &(this->x);
+      int32_t exp_x = (((*val_x)>>23)&255);
+      if(exp_x != 0)
+        exp_x += 1023-127;
+      int32_t sig_x = *val_x;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_x<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_x>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_x>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_x>>4) & 0x7F;
+      if(this->x < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_y = (long *) &(this->y);
+      int32_t exp_y = (((*val_y)>>23)&255);
+      if(exp_y != 0)
+        exp_y += 1023-127;
+      int32_t sig_y = *val_y;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_y<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_y>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_y>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_y>>4) & 0x7F;
+      if(this->y < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_theta = (long *) &(this->theta);
+      int32_t exp_theta = (((*val_theta)>>23)&255);
+      if(exp_theta != 0)
+        exp_theta += 1023-127;
+      int32_t sig_theta = *val_theta;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_theta<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_theta>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_theta>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_theta<<4) & 0xF0) | ((sig_theta>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_theta>>4) & 0x7F;
+      if(this->theta < 0) *(outbuffer + offset -1) |= 0x80;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t * val_x = (uint32_t*) &(this->x);
+      offset += 3;
+      *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_x |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_x !=0)
+        *val_x |= ((exp_x)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
+      uint32_t * val_y = (uint32_t*) &(this->y);
+      offset += 3;
+      *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_y |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_y !=0)
+        *val_y |= ((exp_y)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
+      uint32_t * val_theta = (uint32_t*) &(this->theta);
+      offset += 3;
+      *val_theta = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_theta |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_theta |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_theta |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_theta = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_theta |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_theta !=0)
+        *val_theta |= ((exp_theta)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->theta = -this->theta;
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Pose2D"; };
+    const char * getMD5(){ return "938fa65709584ad8e77d238529be13b8"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/PoseArray.h b/quad2/arduino/src/ros_lib/geometry_msgs/PoseArray.h
new file mode 100644
index 0000000..7ae7de4
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/PoseArray.h
@@ -0,0 +1,58 @@
+#ifndef _ROS_geometry_msgs_PoseArray_h
+#define _ROS_geometry_msgs_PoseArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Pose.h"
+
+namespace geometry_msgs
+{
+
+  class PoseArray : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      uint8_t poses_length;
+      geometry_msgs::Pose st_poses;
+      geometry_msgs::Pose * poses;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = poses_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < poses_length; i++){
+      offset += this->poses[i].serialize(outbuffer + offset);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      uint8_t poses_lengthT = *(inbuffer + offset++);
+      if(poses_lengthT > poses_length)
+        this->poses = (geometry_msgs::Pose*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::Pose));
+      offset += 3;
+      poses_length = poses_lengthT;
+      for( uint8_t i = 0; i < poses_length; i++){
+      offset += this->st_poses.deserialize(inbuffer + offset);
+        memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::Pose));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/PoseArray"; };
+    const char * getMD5(){ return "916c28c5764443f268b296bb671b9d97"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/PoseStamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/PoseStamped.h
new file mode 100644
index 0000000..b0ef4a3
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/PoseStamped.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_PoseStamped_h
+#define _ROS_geometry_msgs_PoseStamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Pose.h"
+
+namespace geometry_msgs
+{
+
+  class PoseStamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::Pose pose;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->pose.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->pose.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/PoseStamped"; };
+    const char * getMD5(){ return "d3812c3cbc69362b77dc0b19b345f8f5"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/PoseWithCovariance.h b/quad2/arduino/src/ros_lib/geometry_msgs/PoseWithCovariance.h
new file mode 100644
index 0000000..2d4a5de
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/PoseWithCovariance.h
@@ -0,0 +1,70 @@
+#ifndef _ROS_geometry_msgs_PoseWithCovariance_h
+#define _ROS_geometry_msgs_PoseWithCovariance_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/Pose.h"
+
+namespace geometry_msgs
+{
+
+  class PoseWithCovariance : public ros::Msg
+  {
+    public:
+      geometry_msgs::Pose pose;
+      float covariance[36];
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->pose.serialize(outbuffer + offset);
+      unsigned char * covariance_val = (unsigned char *) this->covariance;
+      for( uint8_t i = 0; i < 36; i++){
+      int32_t * val_covariancei = (long *) &(this->covariance[i]);
+      int32_t exp_covariancei = (((*val_covariancei)>>23)&255);
+      if(exp_covariancei != 0)
+        exp_covariancei += 1023-127;
+      int32_t sig_covariancei = *val_covariancei;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_covariancei<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_covariancei>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_covariancei>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_covariancei<<4) & 0xF0) | ((sig_covariancei>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_covariancei>>4) & 0x7F;
+      if(this->covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->pose.deserialize(inbuffer + offset);
+      uint8_t * covariance_val = (uint8_t*) this->covariance;
+      for( uint8_t i = 0; i < 36; i++){
+      uint32_t * val_covariancei = (uint32_t*) &(this->covariance[i]);
+      offset += 3;
+      *val_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_covariancei !=0)
+        *val_covariancei |= ((exp_covariancei)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->covariance[i] = -this->covariance[i];
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/PoseWithCovariance"; };
+    const char * getMD5(){ return "c23e848cf1b7533a8d7c259073a97e6f"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h
new file mode 100644
index 0000000..92023cf
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_PoseWithCovarianceStamped_h
+#define _ROS_geometry_msgs_PoseWithCovarianceStamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/PoseWithCovariance.h"
+
+namespace geometry_msgs
+{
+
+  class PoseWithCovarianceStamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::PoseWithCovariance pose;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->pose.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->pose.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/PoseWithCovarianceStamped"; };
+    const char * getMD5(){ return "953b798c0f514ff060a53a3498ce6246"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Quaternion.h b/quad2/arduino/src/ros_lib/geometry_msgs/Quaternion.h
new file mode 100644
index 0000000..42cd473
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Quaternion.h
@@ -0,0 +1,138 @@
+#ifndef _ROS_geometry_msgs_Quaternion_h
+#define _ROS_geometry_msgs_Quaternion_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace geometry_msgs
+{
+
+  class Quaternion : public ros::Msg
+  {
+    public:
+      float x;
+      float y;
+      float z;
+      float w;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      int32_t * val_x = (long *) &(this->x);
+      int32_t exp_x = (((*val_x)>>23)&255);
+      if(exp_x != 0)
+        exp_x += 1023-127;
+      int32_t sig_x = *val_x;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_x<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_x>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_x>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_x>>4) & 0x7F;
+      if(this->x < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_y = (long *) &(this->y);
+      int32_t exp_y = (((*val_y)>>23)&255);
+      if(exp_y != 0)
+        exp_y += 1023-127;
+      int32_t sig_y = *val_y;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_y<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_y>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_y>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_y>>4) & 0x7F;
+      if(this->y < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_z = (long *) &(this->z);
+      int32_t exp_z = (((*val_z)>>23)&255);
+      if(exp_z != 0)
+        exp_z += 1023-127;
+      int32_t sig_z = *val_z;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_z<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_z>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_z>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_z<<4) & 0xF0) | ((sig_z>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_z>>4) & 0x7F;
+      if(this->z < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_w = (long *) &(this->w);
+      int32_t exp_w = (((*val_w)>>23)&255);
+      if(exp_w != 0)
+        exp_w += 1023-127;
+      int32_t sig_w = *val_w;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_w<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_w>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_w>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_w<<4) & 0xF0) | ((sig_w>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_w>>4) & 0x7F;
+      if(this->w < 0) *(outbuffer + offset -1) |= 0x80;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t * val_x = (uint32_t*) &(this->x);
+      offset += 3;
+      *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_x |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_x !=0)
+        *val_x |= ((exp_x)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
+      uint32_t * val_y = (uint32_t*) &(this->y);
+      offset += 3;
+      *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_y |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_y !=0)
+        *val_y |= ((exp_y)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
+      uint32_t * val_z = (uint32_t*) &(this->z);
+      offset += 3;
+      *val_z = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_z |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_z = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_z |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_z !=0)
+        *val_z |= ((exp_z)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
+      uint32_t * val_w = (uint32_t*) &(this->w);
+      offset += 3;
+      *val_w = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_w |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_w |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_w |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_w = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_w |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_w !=0)
+        *val_w |= ((exp_w)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->w = -this->w;
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Quaternion"; };
+    const char * getMD5(){ return "a779879fadf0160734f906b8c19c7004"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/QuaternionStamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/QuaternionStamped.h
new file mode 100644
index 0000000..e4e5606
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/QuaternionStamped.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_QuaternionStamped_h
+#define _ROS_geometry_msgs_QuaternionStamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Quaternion.h"
+
+namespace geometry_msgs
+{
+
+  class QuaternionStamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::Quaternion quaternion;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->quaternion.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->quaternion.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/QuaternionStamped"; };
+    const char * getMD5(){ return "e57f1e547e0e1fd13504588ffc8334e2"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Transform.h b/quad2/arduino/src/ros_lib/geometry_msgs/Transform.h
new file mode 100644
index 0000000..307620f
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Transform.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_Transform_h
+#define _ROS_geometry_msgs_Transform_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/Vector3.h"
+#include "geometry_msgs/Quaternion.h"
+
+namespace geometry_msgs
+{
+
+  class Transform : public ros::Msg
+  {
+    public:
+      geometry_msgs::Vector3 translation;
+      geometry_msgs::Quaternion rotation;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->translation.serialize(outbuffer + offset);
+      offset += this->rotation.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->translation.deserialize(inbuffer + offset);
+      offset += this->rotation.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Transform"; };
+    const char * getMD5(){ return "ac9eff44abf714214112b05d54a3cf9b"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/TransformStamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/TransformStamped.h
new file mode 100644
index 0000000..4aa5282
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/TransformStamped.h
@@ -0,0 +1,56 @@
+#ifndef _ROS_geometry_msgs_TransformStamped_h
+#define _ROS_geometry_msgs_TransformStamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Transform.h"
+
+namespace geometry_msgs
+{
+
+  class TransformStamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      char * child_frame_id;
+      geometry_msgs::Transform transform;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      uint32_t * length_child_frame_id = (uint32_t *)(outbuffer + offset);
+      *length_child_frame_id = strlen( (const char*) this->child_frame_id);
+      offset += 4;
+      memcpy(outbuffer + offset, this->child_frame_id, *length_child_frame_id);
+      offset += *length_child_frame_id;
+      offset += this->transform.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      uint32_t length_child_frame_id = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_child_frame_id; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_child_frame_id-1]=0;
+      this->child_frame_id = (char *)(inbuffer + offset-1);
+      offset += length_child_frame_id;
+      offset += this->transform.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/TransformStamped"; };
+    const char * getMD5(){ return "b5764a33bfeb3588febc2682852579b0"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Twist.h b/quad2/arduino/src/ros_lib/geometry_msgs/Twist.h
new file mode 100644
index 0000000..ca2b7b9
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Twist.h
@@ -0,0 +1,41 @@
+#ifndef _ROS_geometry_msgs_Twist_h
+#define _ROS_geometry_msgs_Twist_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/Vector3.h"
+
+namespace geometry_msgs
+{
+
+  class Twist : public ros::Msg
+  {
+    public:
+      geometry_msgs::Vector3 linear;
+      geometry_msgs::Vector3 angular;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->linear.serialize(outbuffer + offset);
+      offset += this->angular.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->linear.deserialize(inbuffer + offset);
+      offset += this->angular.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Twist"; };
+    const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/TwistStamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/TwistStamped.h
new file mode 100644
index 0000000..d3cf0c1
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/TwistStamped.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_TwistStamped_h
+#define _ROS_geometry_msgs_TwistStamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Twist.h"
+
+namespace geometry_msgs
+{
+
+  class TwistStamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::Twist twist;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->twist.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->twist.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/TwistStamped"; };
+    const char * getMD5(){ return "98d34b0043a2093cf9d9345ab6eef12e"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/TwistWithCovariance.h b/quad2/arduino/src/ros_lib/geometry_msgs/TwistWithCovariance.h
new file mode 100644
index 0000000..1e85fad
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/TwistWithCovariance.h
@@ -0,0 +1,70 @@
+#ifndef _ROS_geometry_msgs_TwistWithCovariance_h
+#define _ROS_geometry_msgs_TwistWithCovariance_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/Twist.h"
+
+namespace geometry_msgs
+{
+
+  class TwistWithCovariance : public ros::Msg
+  {
+    public:
+      geometry_msgs::Twist twist;
+      float covariance[36];
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->twist.serialize(outbuffer + offset);
+      unsigned char * covariance_val = (unsigned char *) this->covariance;
+      for( uint8_t i = 0; i < 36; i++){
+      int32_t * val_covariancei = (long *) &(this->covariance[i]);
+      int32_t exp_covariancei = (((*val_covariancei)>>23)&255);
+      if(exp_covariancei != 0)
+        exp_covariancei += 1023-127;
+      int32_t sig_covariancei = *val_covariancei;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_covariancei<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_covariancei>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_covariancei>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_covariancei<<4) & 0xF0) | ((sig_covariancei>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_covariancei>>4) & 0x7F;
+      if(this->covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->twist.deserialize(inbuffer + offset);
+      uint8_t * covariance_val = (uint8_t*) this->covariance;
+      for( uint8_t i = 0; i < 36; i++){
+      uint32_t * val_covariancei = (uint32_t*) &(this->covariance[i]);
+      offset += 3;
+      *val_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_covariancei !=0)
+        *val_covariancei |= ((exp_covariancei)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->covariance[i] = -this->covariance[i];
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/TwistWithCovariance"; };
+    const char * getMD5(){ return "1fe8a28e6890a4cc3ae4c3ca5c7d82e6"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h
new file mode 100644
index 0000000..d362ad9
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_TwistWithCovarianceStamped_h
+#define _ROS_geometry_msgs_TwistWithCovarianceStamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/TwistWithCovariance.h"
+
+namespace geometry_msgs
+{
+
+  class TwistWithCovarianceStamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::TwistWithCovariance twist;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->twist.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->twist.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/TwistWithCovarianceStamped"; };
+    const char * getMD5(){ return "8927a1a12fb2607ceea095b2dc440a96"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Vector3.h b/quad2/arduino/src/ros_lib/geometry_msgs/Vector3.h
new file mode 100644
index 0000000..586f62c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Vector3.h
@@ -0,0 +1,112 @@
+#ifndef _ROS_geometry_msgs_Vector3_h
+#define _ROS_geometry_msgs_Vector3_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace geometry_msgs
+{
+
+  class Vector3 : public ros::Msg
+  {
+    public:
+      float x;
+      float y;
+      float z;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      int32_t * val_x = (long *) &(this->x);
+      int32_t exp_x = (((*val_x)>>23)&255);
+      if(exp_x != 0)
+        exp_x += 1023-127;
+      int32_t sig_x = *val_x;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_x<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_x>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_x>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_x>>4) & 0x7F;
+      if(this->x < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_y = (long *) &(this->y);
+      int32_t exp_y = (((*val_y)>>23)&255);
+      if(exp_y != 0)
+        exp_y += 1023-127;
+      int32_t sig_y = *val_y;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_y<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_y>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_y>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_y>>4) & 0x7F;
+      if(this->y < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_z = (long *) &(this->z);
+      int32_t exp_z = (((*val_z)>>23)&255);
+      if(exp_z != 0)
+        exp_z += 1023-127;
+      int32_t sig_z = *val_z;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_z<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_z>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_z>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_z<<4) & 0xF0) | ((sig_z>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_z>>4) & 0x7F;
+      if(this->z < 0) *(outbuffer + offset -1) |= 0x80;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t * val_x = (uint32_t*) &(this->x);
+      offset += 3;
+      *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_x |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_x !=0)
+        *val_x |= ((exp_x)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
+      uint32_t * val_y = (uint32_t*) &(this->y);
+      offset += 3;
+      *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_y |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_y !=0)
+        *val_y |= ((exp_y)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
+      uint32_t * val_z = (uint32_t*) &(this->z);
+      offset += 3;
+      *val_z = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_z |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_z = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_z |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_z !=0)
+        *val_z |= ((exp_z)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Vector3"; };
+    const char * getMD5(){ return "4a842b65f413084dc2b10fb484ea7f17"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Vector3Stamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/Vector3Stamped.h
new file mode 100644
index 0000000..9062d60
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Vector3Stamped.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_Vector3Stamped_h
+#define _ROS_geometry_msgs_Vector3Stamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Vector3.h"
+
+namespace geometry_msgs
+{
+
+  class Vector3Stamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::Vector3 vector;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->vector.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->vector.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Vector3Stamped"; };
+    const char * getMD5(){ return "7b324c7325e683bf02a9b14b01090ec7"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/Wrench.h b/quad2/arduino/src/ros_lib/geometry_msgs/Wrench.h
new file mode 100644
index 0000000..58a4f0d
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/Wrench.h
@@ -0,0 +1,41 @@
+#ifndef _ROS_geometry_msgs_Wrench_h
+#define _ROS_geometry_msgs_Wrench_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/Vector3.h"
+
+namespace geometry_msgs
+{
+
+  class Wrench : public ros::Msg
+  {
+    public:
+      geometry_msgs::Vector3 force;
+      geometry_msgs::Vector3 torque;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->force.serialize(outbuffer + offset);
+      offset += this->torque.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->force.deserialize(inbuffer + offset);
+      offset += this->torque.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/Wrench"; };
+    const char * getMD5(){ return "4f539cf138b23283b520fd271b567936"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/geometry_msgs/WrenchStamped.h b/quad2/arduino/src/ros_lib/geometry_msgs/WrenchStamped.h
new file mode 100644
index 0000000..3c9f74c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/geometry_msgs/WrenchStamped.h
@@ -0,0 +1,42 @@
+#ifndef _ROS_geometry_msgs_WrenchStamped_h
+#define _ROS_geometry_msgs_WrenchStamped_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Wrench.h"
+
+namespace geometry_msgs
+{
+
+  class WrenchStamped : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::Wrench wrench;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->wrench.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->wrench.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "geometry_msgs/WrenchStamped"; };
+    const char * getMD5(){ return "d78d3cb249ce23087ade7e7d0c40cfa7"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/nav_msgs/GetMap.h b/quad2/arduino/src/ros_lib/nav_msgs/GetMap.h
new file mode 100644
index 0000000..f8c38c8
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/nav_msgs/GetMap.h
@@ -0,0 +1,66 @@
+#ifndef _ROS_SERVICE_GetMap_h
+#define _ROS_SERVICE_GetMap_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "nav_msgs/OccupancyGrid.h"
+
+namespace nav_msgs
+{
+
+static const char GETMAP[] = "nav_msgs/GetMap";
+
+  class GetMapRequest : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return GETMAP; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class GetMapResponse : public ros::Msg
+  {
+    public:
+      nav_msgs::OccupancyGrid map;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->map.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->map.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return GETMAP; };
+    const char * getMD5(){ return "6cdd0a18e0aff5b0a3ca2326a89b54ff"; };
+
+  };
+
+  class GetMap {
+    public:
+    typedef GetMapRequest Request;
+    typedef GetMapResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/nav_msgs/GetPlan.h b/quad2/arduino/src/ros_lib/nav_msgs/GetPlan.h
new file mode 100644
index 0000000..b64c74b
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/nav_msgs/GetPlan.h
@@ -0,0 +1,95 @@
+#ifndef _ROS_SERVICE_GetPlan_h
+#define _ROS_SERVICE_GetPlan_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/PoseStamped.h"
+#include "nav_msgs/Path.h"
+
+namespace nav_msgs
+{
+
+static const char GETPLAN[] = "nav_msgs/GetPlan";
+
+  class GetPlanRequest : public ros::Msg
+  {
+    public:
+      geometry_msgs::PoseStamped start;
+      geometry_msgs::PoseStamped goal;
+      float tolerance;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->start.serialize(outbuffer + offset);
+      offset += this->goal.serialize(outbuffer + offset);
+      union {
+        float real;
+        uint32_t base;
+      } u_tolerance;
+      u_tolerance.real = this->tolerance;
+      *(outbuffer + offset + 0) = (u_tolerance.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_tolerance.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_tolerance.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_tolerance.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->tolerance);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->start.deserialize(inbuffer + offset);
+      offset += this->goal.deserialize(inbuffer + offset);
+      union {
+        float real;
+        uint32_t base;
+      } u_tolerance;
+      u_tolerance.base = 0;
+      u_tolerance.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_tolerance.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_tolerance.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_tolerance.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->tolerance = u_tolerance.real;
+      offset += sizeof(this->tolerance);
+     return offset;
+    }
+
+    const char * getType(){ return GETPLAN; };
+    const char * getMD5(){ return "e25a43e0752bcca599a8c2eef8282df8"; };
+
+  };
+
+  class GetPlanResponse : public ros::Msg
+  {
+    public:
+      nav_msgs::Path plan;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->plan.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->plan.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return GETPLAN; };
+    const char * getMD5(){ return "0002bc113c0259d71f6cf8cbc9430e18"; };
+
+  };
+
+  class GetPlan {
+    public:
+    typedef GetPlanRequest Request;
+    typedef GetPlanResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/nav_msgs/GridCells.h b/quad2/arduino/src/ros_lib/nav_msgs/GridCells.h
new file mode 100644
index 0000000..1b5b86d
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/nav_msgs/GridCells.h
@@ -0,0 +1,102 @@
+#ifndef _ROS_nav_msgs_GridCells_h
+#define _ROS_nav_msgs_GridCells_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Point.h"
+
+namespace nav_msgs
+{
+
+  class GridCells : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      float cell_width;
+      float cell_height;
+      uint8_t cells_length;
+      geometry_msgs::Point st_cells;
+      geometry_msgs::Point * cells;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      union {
+        float real;
+        uint32_t base;
+      } u_cell_width;
+      u_cell_width.real = this->cell_width;
+      *(outbuffer + offset + 0) = (u_cell_width.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_cell_width.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_cell_width.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_cell_width.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->cell_width);
+      union {
+        float real;
+        uint32_t base;
+      } u_cell_height;
+      u_cell_height.real = this->cell_height;
+      *(outbuffer + offset + 0) = (u_cell_height.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_cell_height.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_cell_height.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_cell_height.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->cell_height);
+      *(outbuffer + offset++) = cells_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < cells_length; i++){
+      offset += this->cells[i].serialize(outbuffer + offset);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      union {
+        float real;
+        uint32_t base;
+      } u_cell_width;
+      u_cell_width.base = 0;
+      u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_cell_width.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->cell_width = u_cell_width.real;
+      offset += sizeof(this->cell_width);
+      union {
+        float real;
+        uint32_t base;
+      } u_cell_height;
+      u_cell_height.base = 0;
+      u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_cell_height.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->cell_height = u_cell_height.real;
+      offset += sizeof(this->cell_height);
+      uint8_t cells_lengthT = *(inbuffer + offset++);
+      if(cells_lengthT > cells_length)
+        this->cells = (geometry_msgs::Point*)realloc(this->cells, cells_lengthT * sizeof(geometry_msgs::Point));
+      offset += 3;
+      cells_length = cells_lengthT;
+      for( uint8_t i = 0; i < cells_length; i++){
+      offset += this->st_cells.deserialize(inbuffer + offset);
+        memcpy( &(this->cells[i]), &(this->st_cells), sizeof(geometry_msgs::Point));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "nav_msgs/GridCells"; };
+    const char * getMD5(){ return "b9e4f5df6d28e272ebde00a3994830f5"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/nav_msgs/MapMetaData.h b/quad2/arduino/src/ros_lib/nav_msgs/MapMetaData.h
new file mode 100644
index 0000000..1996b52
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/nav_msgs/MapMetaData.h
@@ -0,0 +1,104 @@
+#ifndef _ROS_nav_msgs_MapMetaData_h
+#define _ROS_nav_msgs_MapMetaData_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "ros/time.h"
+#include "geometry_msgs/Pose.h"
+
+namespace nav_msgs
+{
+
+  class MapMetaData : public ros::Msg
+  {
+    public:
+      ros::Time map_load_time;
+      float resolution;
+      uint32_t width;
+      uint32_t height;
+      geometry_msgs::Pose origin;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->map_load_time.sec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->map_load_time.sec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->map_load_time.sec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->map_load_time.sec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->map_load_time.sec);
+      *(outbuffer + offset + 0) = (this->map_load_time.nsec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->map_load_time.nsec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->map_load_time.nsec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->map_load_time.nsec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->map_load_time.nsec);
+      union {
+        float real;
+        uint32_t base;
+      } u_resolution;
+      u_resolution.real = this->resolution;
+      *(outbuffer + offset + 0) = (u_resolution.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_resolution.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_resolution.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_resolution.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->resolution);
+      *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->width);
+      *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->height);
+      offset += this->origin.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->map_load_time.sec);
+      this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->map_load_time.nsec);
+      union {
+        float real;
+        uint32_t base;
+      } u_resolution;
+      u_resolution.base = 0;
+      u_resolution.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_resolution.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_resolution.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_resolution.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->resolution = u_resolution.real;
+      offset += sizeof(this->resolution);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->width);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->height);
+      offset += this->origin.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "nav_msgs/MapMetaData"; };
+    const char * getMD5(){ return "10cfc8a2818024d3248802c00c95f11b"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/nav_msgs/OccupancyGrid.h b/quad2/arduino/src/ros_lib/nav_msgs/OccupancyGrid.h
new file mode 100644
index 0000000..be2ac65
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/nav_msgs/OccupancyGrid.h
@@ -0,0 +1,74 @@
+#ifndef _ROS_nav_msgs_OccupancyGrid_h
+#define _ROS_nav_msgs_OccupancyGrid_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "nav_msgs/MapMetaData.h"
+
+namespace nav_msgs
+{
+
+  class OccupancyGrid : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      nav_msgs::MapMetaData info;
+      uint8_t data_length;
+      int8_t st_data;
+      int8_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->info.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int8_t real;
+        uint8_t base;
+      } u_datai;
+      u_datai.real = this->data[i];
+      *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->info.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (int8_t*)realloc(this->data, data_lengthT * sizeof(int8_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int8_t real;
+        uint8_t base;
+      } u_st_data;
+      u_st_data.base = 0;
+      u_st_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->st_data = u_st_data.real;
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(int8_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "nav_msgs/OccupancyGrid"; };
+    const char * getMD5(){ return "3381f2d731d4076ec5c71b0759edbe4e"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/nav_msgs/Odometry.h b/quad2/arduino/src/ros_lib/nav_msgs/Odometry.h
new file mode 100644
index 0000000..d058c38
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/nav_msgs/Odometry.h
@@ -0,0 +1,60 @@
+#ifndef _ROS_nav_msgs_Odometry_h
+#define _ROS_nav_msgs_Odometry_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/PoseWithCovariance.h"
+#include "geometry_msgs/TwistWithCovariance.h"
+
+namespace nav_msgs
+{
+
+  class Odometry : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      char * child_frame_id;
+      geometry_msgs::PoseWithCovariance pose;
+      geometry_msgs::TwistWithCovariance twist;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      uint32_t * length_child_frame_id = (uint32_t *)(outbuffer + offset);
+      *length_child_frame_id = strlen( (const char*) this->child_frame_id);
+      offset += 4;
+      memcpy(outbuffer + offset, this->child_frame_id, *length_child_frame_id);
+      offset += *length_child_frame_id;
+      offset += this->pose.serialize(outbuffer + offset);
+      offset += this->twist.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      uint32_t length_child_frame_id = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_child_frame_id; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_child_frame_id-1]=0;
+      this->child_frame_id = (char *)(inbuffer + offset-1);
+      offset += length_child_frame_id;
+      offset += this->pose.deserialize(inbuffer + offset);
+      offset += this->twist.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "nav_msgs/Odometry"; };
+    const char * getMD5(){ return "cd5e73d190d741a2f92e81eda573aca7"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/nav_msgs/Path.h b/quad2/arduino/src/ros_lib/nav_msgs/Path.h
new file mode 100644
index 0000000..f643237
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/nav_msgs/Path.h
@@ -0,0 +1,58 @@
+#ifndef _ROS_nav_msgs_Path_h
+#define _ROS_nav_msgs_Path_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/PoseStamped.h"
+
+namespace nav_msgs
+{
+
+  class Path : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      uint8_t poses_length;
+      geometry_msgs::PoseStamped st_poses;
+      geometry_msgs::PoseStamped * poses;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = poses_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < poses_length; i++){
+      offset += this->poses[i].serialize(outbuffer + offset);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      uint8_t poses_lengthT = *(inbuffer + offset++);
+      if(poses_lengthT > poses_length)
+        this->poses = (geometry_msgs::PoseStamped*)realloc(this->poses, poses_lengthT * sizeof(geometry_msgs::PoseStamped));
+      offset += 3;
+      poses_length = poses_lengthT;
+      for( uint8_t i = 0; i < poses_length; i++){
+      offset += this->st_poses.deserialize(inbuffer + offset);
+        memcpy( &(this->poses[i]), &(this->st_poses), sizeof(geometry_msgs::PoseStamped));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "nav_msgs/Path"; };
+    const char * getMD5(){ return "6227e2b7e9cce15051f669a5e197bbf7"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/ros.h b/quad2/arduino/src/ros_lib/ros.h
new file mode 100644
index 0000000..447cf32
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ros.h
@@ -0,0 +1,58 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _ROS_H_
+#define _ROS_H_
+
+#include "ros/node_handle.h"
+#include "ArduinoHardware.h"
+
+namespace ros
+{
+#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega168__)
+  /* downsize our buffers */
+  typedef NodeHandle_<ArduinoHardware, 6, 6, 150, 150> NodeHandle;
+
+#elif defined(__AVR_ATmega328P__)
+
+  typedef NodeHandle_<ArduinoHardware, 25, 25, 280, 280> NodeHandle;
+
+#else
+
+  typedef NodeHandle_<ArduinoHardware> NodeHandle;
+
+#endif   
+}
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/ros/duration.h b/quad2/arduino/src/ros_lib/ros/duration.h
new file mode 100644
index 0000000..61b1293
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ros/duration.h
@@ -0,0 +1,61 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _ROS_DURATION_H_
+#define _ROS_DURATION_H_
+
+namespace ros {
+
+  void normalizeSecNSecSigned(long& sec, long& nsec);
+
+  class Duration
+  {
+    public:
+      long sec, nsec; 
+      
+      Duration() : sec(0), nsec(0) {}
+      Duration(long _sec, long _nsec) : sec(_sec), nsec(_nsec)
+      {
+        normalizeSecNSecSigned(sec, nsec);
+      }
+
+      Duration& operator+=(const Duration &rhs);
+      Duration& operator-=(const Duration &rhs);
+      Duration& operator*=(double scale);
+  };
+
+}
+
+#endif
+
diff --git a/quad2/arduino/src/ros_lib/ros/msg.h b/quad2/arduino/src/ros_lib/ros/msg.h
new file mode 100644
index 0000000..41991c6
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ros/msg.h
@@ -0,0 +1,52 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _ROS_MSG_H_
+#define _ROS_MSG_H_
+
+namespace ros {
+
+  /* Base Message Type */
+  class Msg
+  {
+    public:
+      virtual int serialize(unsigned char *outbuffer) const = 0;
+	  virtual int deserialize(unsigned char *data) = 0;
+      virtual const char * getType() = 0;
+      virtual const char * getMD5() = 0;
+  };
+
+}
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/ros/node_handle.h b/quad2/arduino/src/ros_lib/ros/node_handle.h
new file mode 100644
index 0000000..b73229c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ros/node_handle.h
@@ -0,0 +1,480 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROS_NODE_HANDLE_H_
+#define ROS_NODE_HANDLE_H_
+
+#include "std_msgs/Time.h"
+#include "rosserial_msgs/TopicInfo.h"
+#include "rosserial_msgs/Log.h"
+#include "rosserial_msgs/RequestParam.h"
+
+#define SYNC_SECONDS        5
+
+#define MODE_FIRST_FF       0
+#define MODE_SECOND_FF      1
+#define MODE_TOPIC_L        2   // waiting for topic id
+#define MODE_TOPIC_H        3
+#define MODE_SIZE_L         4   // waiting for message size
+#define MODE_SIZE_H         5
+#define MODE_MESSAGE        6
+#define MODE_CHECKSUM       7
+
+#define MSG_TIMEOUT 20  //20 milliseconds to recieve all of message data
+
+#include "msg.h"
+
+namespace ros {
+
+  class NodeHandleBase_{
+    public:
+      virtual int publish(int id, const Msg* msg)=0;
+      virtual int spinOnce()=0;
+      virtual bool connected()=0;
+    };
+
+}
+
+#include "publisher.h"
+#include "subscriber.h"
+#include "service_server.h"
+#include "service_client.h"
+
+namespace ros {
+
+  using rosserial_msgs::TopicInfo;
+
+  /* Node Handle */
+  template<class Hardware,
+           int MAX_SUBSCRIBERS=25,
+           int MAX_PUBLISHERS=25,
+           int INPUT_SIZE=512,
+           int OUTPUT_SIZE=512>
+  class NodeHandle_ : public NodeHandleBase_
+  {
+    protected:
+      Hardware hardware_;
+
+      /* time used for syncing */
+      unsigned long rt_time;
+
+      /* used for computing current time */
+      unsigned long sec_offset, nsec_offset;
+
+      unsigned char message_in[INPUT_SIZE];
+      unsigned char message_out[OUTPUT_SIZE];
+
+      Publisher * publishers[MAX_PUBLISHERS];
+      Subscriber_ * subscribers[MAX_SUBSCRIBERS];
+
+      /*
+       * Setup Functions
+       */
+    public:
+      NodeHandle_() : configured_(false) {}
+      
+      Hardware* getHardware(){
+        return &hardware_;
+      }
+
+      /* Start serial, initialize buffers */
+      void initNode(){
+        hardware_.init();
+        mode_ = 0;
+        bytes_ = 0;
+        index_ = 0;
+        topic_ = 0;
+      };
+
+    protected:
+      //State machine variables for spinOnce
+      int mode_;
+      int bytes_;
+      int topic_;
+      int index_;
+      int checksum_;
+
+      bool configured_;
+
+      /* used for syncing the time */
+      unsigned long last_sync_time;
+      unsigned long last_sync_receive_time;
+      unsigned long last_msg_timeout_time;
+
+    public:
+      /* This function goes in your loop() function, it handles
+       *  serial input and callbacks for subscribers.
+       */
+
+      virtual int spinOnce(){
+
+        /* restart if timed out */
+        unsigned long c_time = hardware_.time();
+        if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){
+            configured_ = false;
+         }
+         
+        /* reset if message has timed out */
+        if ( mode_ != MODE_FIRST_FF){ 
+          if (c_time > last_msg_timeout_time){
+            mode_ = MODE_FIRST_FF;
+          }
+        }
+
+        /* while available buffer, read data */
+        while( true )
+        {
+          int data = hardware_.read();
+          if( data < 0 )
+            break;
+          checksum_ += data;
+          if( mode_ == MODE_MESSAGE ){        /* message data being recieved */
+            message_in[index_++] = data;
+            bytes_--;
+            if(bytes_ == 0)                   /* is message complete? if so, checksum */
+              mode_ = MODE_CHECKSUM;
+          }else if( mode_ == MODE_FIRST_FF ){
+            if(data == 0xff){
+              mode_++;
+              last_msg_timeout_time = c_time + MSG_TIMEOUT;
+            }
+          }else if( mode_ == MODE_SECOND_FF ){
+            if(data == 0xff){
+              mode_++;
+            }else{
+              mode_ = MODE_FIRST_FF;
+            }
+          }else if( mode_ == MODE_TOPIC_L ){  /* bottom half of topic id */
+            topic_ = data;
+            mode_++;
+            checksum_ = data;                 /* first byte included in checksum */
+          }else if( mode_ == MODE_TOPIC_H ){  /* top half of topic id */
+            topic_ += data<<8;
+            mode_++;
+          }else if( mode_ == MODE_SIZE_L ){   /* bottom half of message size */
+            bytes_ = data;
+            index_ = 0;
+            mode_++;
+          }else if( mode_ == MODE_SIZE_H ){   /* top half of message size */
+            bytes_ += data<<8;
+            mode_ = MODE_MESSAGE;
+            if(bytes_ == 0)
+              mode_ = MODE_CHECKSUM;
+          }else if( mode_ == MODE_CHECKSUM ){ /* do checksum */
+            mode_ = MODE_FIRST_FF;
+            if( (checksum_%256) == 255){
+              if(topic_ == TopicInfo::ID_PUBLISHER){
+                requestSyncTime();
+                negotiateTopics();
+                last_sync_time = c_time;
+                last_sync_receive_time = c_time;
+                return -1;
+              }else if(topic_ == TopicInfo::ID_TIME){
+                syncTime(message_in);
+              }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){
+                  req_param_resp.deserialize(message_in);
+                  param_recieved= true;
+              }else{
+                if(subscribers[topic_-100])
+                  subscribers[topic_-100]->callback( message_in );
+              }
+            }
+          }
+        }
+
+        /* occasionally sync time */
+        if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){
+          requestSyncTime();
+          last_sync_time = c_time;
+        }
+
+        return 0;
+      }
+
+      /* Are we connected to the PC? */
+      virtual bool connected() {
+        return configured_;
+      };
+
+      /********************************************************************
+       * Time functions
+       */
+
+      void requestSyncTime()
+      {
+        std_msgs::Time t;
+        publish(TopicInfo::ID_TIME, &t);
+        rt_time = hardware_.time();
+      }
+
+      void syncTime( unsigned char * data )
+      {
+        std_msgs::Time t;
+        unsigned long offset = hardware_.time() - rt_time;
+
+        t.deserialize(data);
+        t.data.sec += offset/1000;
+        t.data.nsec += (offset%1000)*1000000UL;
+
+        this->setNow(t.data);
+        last_sync_receive_time = hardware_.time();
+      }
+
+      Time now(){
+        unsigned long ms = hardware_.time();
+        Time current_time;
+        current_time.sec = ms/1000 + sec_offset;
+        current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
+        normalizeSecNSec(current_time.sec, current_time.nsec);
+        return current_time;
+      }
+
+      void setNow( Time & new_now )
+      {
+        unsigned long ms = hardware_.time();
+        sec_offset = new_now.sec - ms/1000 - 1;
+        nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
+        normalizeSecNSec(sec_offset, nsec_offset);
+      }
+
+      /********************************************************************
+       * Topic Management 
+       */
+
+      /* Register a new publisher */    
+      bool advertise(Publisher & p)
+      {
+        for(int i = 0; i < MAX_PUBLISHERS; i++){
+          if(publishers[i] == 0){ // empty slot
+            publishers[i] = &p;
+            p.id_ = i+100+MAX_SUBSCRIBERS;
+            p.nh_ = this;
+            return true;
+          }
+        }
+        return false;
+      }
+
+      /* Register a new subscriber */
+      template<typename MsgT>
+      bool subscribe(Subscriber< MsgT> & s){
+        for(int i = 0; i < MAX_SUBSCRIBERS; i++){
+          if(subscribers[i] == 0){ // empty slot
+            subscribers[i] = (Subscriber_*) &s;
+            s.id_ = i+100;
+            return true;
+          }
+        }
+        return false;
+      }
+
+      /* Register a new Service Server */
+      template<typename MReq, typename MRes>
+      bool advertiseService(ServiceServer<MReq,MRes>& srv){
+        bool v = advertise(srv.pub);
+        for(int i = 0; i < MAX_SUBSCRIBERS; i++){
+          if(subscribers[i] == 0){ // empty slot
+            subscribers[i] = (Subscriber_*) &srv;
+            srv.id_ = i+100;
+            return v;
+          }
+        }
+        return false;
+      }
+
+      /* Register a new Service Client */
+      template<typename MReq, typename MRes>
+      bool serviceClient(ServiceClient<MReq, MRes>& srv){
+        bool v = advertise(srv.pub);
+        for(int i = 0; i < MAX_SUBSCRIBERS; i++){
+          if(subscribers[i] == 0){ // empty slot
+            subscribers[i] = (Subscriber_*) &srv;
+            srv.id_ = i+100;
+            return v;
+          }
+        }
+        return false;
+      }
+
+      void negotiateTopics()
+      {
+        configured_ = true;
+
+        rosserial_msgs::TopicInfo ti;
+        int i;
+        for(i = 0; i < MAX_PUBLISHERS; i++)
+        {
+          if(publishers[i] != 0) // non-empty slot
+          {
+            ti.topic_id = publishers[i]->id_;
+            ti.topic_name = (char *) publishers[i]->topic_;
+            ti.message_type = (char *) publishers[i]->msg_->getType();
+            ti.md5sum = (char *) publishers[i]->msg_->getMD5();
+            ti.buffer_size = OUTPUT_SIZE;
+            publish( publishers[i]->getEndpointType(), &ti );
+          }
+        }
+        for(i = 0; i < MAX_SUBSCRIBERS; i++)
+        {
+          if(subscribers[i] != 0) // non-empty slot
+          {
+            ti.topic_id = subscribers[i]->id_;
+            ti.topic_name = (char *) subscribers[i]->topic_;
+            ti.message_type = (char *) subscribers[i]->getMsgType();
+            ti.md5sum = (char *) subscribers[i]->getMsgMD5();
+            ti.buffer_size = INPUT_SIZE;
+            publish( subscribers[i]->getEndpointType(), &ti );
+          }
+        }
+      }
+
+      virtual int publish(int id, const Msg * msg)
+      {
+        if(!configured_) return 0;
+
+        /* serialize message */
+        int l = msg->serialize(message_out+6);
+
+        /* setup the header */
+        message_out[0] = 0xff;
+        message_out[1] = 0xff;
+        message_out[2] = (unsigned char) id&255;
+        message_out[3] = (unsigned char) id>>8;
+        message_out[4] = (unsigned char) l&255;
+        message_out[5] = ((unsigned char) l>>8);
+
+        /* calculate checksum */
+        int chk = 0;
+        for(int i =2; i<l+6; i++)
+          chk += message_out[i];
+        l += 6;
+        message_out[l++] = 255 - (chk%256);
+
+        if( l <= OUTPUT_SIZE ){
+          hardware_.write(message_out, l);
+          return l;
+        }else{
+          logerror("Message from device dropped: message larger than buffer.");
+        }
+      }
+
+      /********************************************************************
+       * Logging
+       */
+
+    private:
+      void log(char byte, const char * msg){
+        rosserial_msgs::Log l;
+        l.level= byte;
+        l.msg = (char*)msg;
+        publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
+      }
+
+    public:
+      void logdebug(const char* msg){
+        log(rosserial_msgs::Log::DEBUG, msg);
+      }
+      void loginfo(const char * msg){
+        log(rosserial_msgs::Log::INFO, msg);
+      }
+      void logwarn(const char *msg){
+        log(rosserial_msgs::Log::WARN, msg);
+      }
+      void logerror(const char*msg){
+        log(rosserial_msgs::Log::ERROR, msg);
+      }
+      void logfatal(const char*msg){
+        log(rosserial_msgs::Log::FATAL, msg);
+      }
+
+      /********************************************************************
+       * Parameters
+       */
+
+    private:
+      bool param_recieved;
+      rosserial_msgs::RequestParamResponse req_param_resp;
+
+      bool requestParam(const char * name, int time_out =  1000){
+        param_recieved = false;
+        rosserial_msgs::RequestParamRequest req;
+        req.name  = (char*)name;
+        publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
+        int end_time = hardware_.time();
+        while(!param_recieved ){
+          spinOnce();
+          if (end_time > hardware_.time()) return false;
+        }
+        return true;
+      }
+
+    public:
+      bool getParam(const char* name, int* param, int length =1){
+        if (requestParam(name) ){
+          if (length == req_param_resp.ints_length){
+            //copy it over
+            for(int i=0; i<length; i++)
+              param[i] = req_param_resp.ints[i];
+            return true;
+          }
+        }
+        return false;
+      }
+      bool getParam(const char* name, float* param, int length=1){
+        if (requestParam(name) ){
+          if (length == req_param_resp.floats_length){
+            //copy it over
+            for(int i=0; i<length; i++) 
+              param[i] = req_param_resp.floats[i];
+            return true;
+          }
+        }
+        return false;
+      }
+      bool getParam(const char* name, char** param, int length=1){
+        if (requestParam(name) ){
+          if (length == req_param_resp.strings_length){
+            //copy it over
+            for(int i=0; i<length; i++)
+              strcpy(param[i],req_param_resp.strings[i]);
+            return true;
+          }
+        }
+        return false;
+      }  
+  };
+
+}
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/ros/publisher.h b/quad2/arduino/src/ros_lib/ros/publisher.h
new file mode 100644
index 0000000..1217362
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ros/publisher.h
@@ -0,0 +1,67 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _ROS_PUBLISHER_H_
+#define _ROS_PUBLISHER_H_
+
+#include "rosserial_msgs/TopicInfo.h"
+#include "node_handle.h"
+
+namespace ros {
+
+  /* Generic Publisher */
+  class Publisher
+  {
+    public:
+      Publisher( const char * topic_name, Msg * msg, int endpoint=rosserial_msgs::TopicInfo::ID_PUBLISHER) :
+        topic_(topic_name), 
+        msg_(msg),
+        endpoint_(endpoint) {};
+
+      int publish( const Msg * msg ) { return nh_->publish(id_, msg); };
+      int getEndpointType(){ return endpoint_; }
+
+      const char * topic_;
+      Msg *msg_;
+      // id_ and no_ are set by NodeHandle when we advertise 
+      int id_;
+      NodeHandleBase_* nh_;
+
+    private:
+      int endpoint_;
+  };
+
+}
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/ros/service_client.h b/quad2/arduino/src/ros_lib/ros/service_client.h
new file mode 100644
index 0000000..06522f2
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ros/service_client.h
@@ -0,0 +1,83 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _ROS_SERVICE_CLIENT_H_
+#define _ROS_SERVICE_CLIENT_H_
+
+#include "rosserial_msgs/TopicInfo.h"
+
+#include "publisher.h"
+#include "subscriber.h"
+
+namespace ros {
+
+  template<typename MReq , typename MRes>
+  class ServiceClient : public Subscriber_  {
+    public:
+      ServiceClient(const char* topic_name) : 
+        pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER)
+      {
+        this->topic_ = topic_name;
+        this->waiting = true;
+      }
+
+      virtual void call(const MReq & request, MRes & response)
+      {
+        if(!pub.nh_->connected()) return;
+        ret = &response;
+        waiting = true;
+        pub.publish(&request);
+        while(waiting && pub.nh_->connected())
+          if(pub.nh_->spinOnce() < 0) break;
+      }
+
+      // these refer to the subscriber
+      virtual void callback(unsigned char *data){
+        ret->deserialize(data);
+        waiting = false;
+      }
+      virtual const char * getMsgType(){ return this->resp.getType(); }
+      virtual const char * getMsgMD5(){ return this->resp.getMD5(); }
+      virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; }
+
+      MReq req;
+      MRes resp;
+      MRes * ret;
+      bool waiting;
+      Publisher pub;
+  };
+
+}
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/ros/service_server.h b/quad2/arduino/src/ros_lib/ros/service_server.h
new file mode 100644
index 0000000..67a3e0a
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ros/service_server.h
@@ -0,0 +1,76 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _ROS_SERVICE_SERVER_H_
+#define _ROS_SERVICE_SERVER_H_
+
+#include "rosserial_msgs/TopicInfo.h"
+
+#include "publisher.h"
+#include "subscriber.h"
+
+namespace ros {
+
+  template<typename MReq , typename MRes>
+  class ServiceServer : public Subscriber_ {
+    public:
+      typedef void(*CallbackT)(const MReq&,  MRes&);
+
+      ServiceServer(const char* topic_name, CallbackT cb) :
+        pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER)
+      {
+        this->topic_ = topic_name;
+        this->cb_ = cb;
+      }
+
+      // these refer to the subscriber
+      virtual void callback(unsigned char *data){
+        req.deserialize(data);
+        cb_(req,resp);
+        pub.publish(&resp);
+      }
+      virtual const char * getMsgType(){ return this->req.getType(); }
+      virtual const char * getMsgMD5(){ return this->req.getMD5(); }
+      virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; }
+
+      MReq req;
+      MRes resp;
+      Publisher pub;
+    private:
+      CallbackT cb_;
+  };
+
+}
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/ros/subscriber.h b/quad2/arduino/src/ros_lib/ros/subscriber.h
new file mode 100644
index 0000000..5464646
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ros/subscriber.h
@@ -0,0 +1,88 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROS_SUBSCRIBER_H_
+#define ROS_SUBSCRIBER_H_
+
+#include "rosserial_msgs/TopicInfo.h"
+
+namespace ros {
+
+  /* Base class for objects subscribers. */
+  class Subscriber_
+  {
+    public:
+      virtual void callback(unsigned char *data)=0;
+      virtual int getEndpointType()=0;
+
+      // id_ is set by NodeHandle when we advertise 
+      int id_;
+
+      virtual const char * getMsgType()=0;
+      virtual const char * getMsgMD5()=0;
+      const char * topic_;
+  };
+
+
+  /* Actual subscriber, templated on message type. */
+  template<typename MsgT>
+  class Subscriber: public Subscriber_{
+    public:
+      typedef void(*CallbackT)(const MsgT&);
+      MsgT msg;
+
+      Subscriber(const char * topic_name, CallbackT cb, int endpoint=rosserial_msgs::TopicInfo::ID_SUBSCRIBER) :
+        cb_(cb),
+        endpoint_(endpoint)
+      {
+        topic_ = topic_name;
+      };
+
+      virtual void callback(unsigned char* data){
+        msg.deserialize(data);
+        this->cb_(msg);
+      }
+
+      virtual const char * getMsgType(){ return this->msg.getType(); }
+      virtual const char * getMsgMD5(){ return this->msg.getMD5(); }
+      virtual int getEndpointType(){ return endpoint_; }
+
+    private:
+      CallbackT cb_;
+      int endpoint_;
+  };
+
+}
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/ros/time.h b/quad2/arduino/src/ros_lib/ros/time.h
new file mode 100644
index 0000000..602fbdb
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/ros/time.h
@@ -0,0 +1,71 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROS_TIME_H_
+#define ROS_TIME_H_
+
+#include <ros/duration.h>
+#include <math.h>
+
+namespace ros
+{
+  void normalizeSecNSec(unsigned long &sec, unsigned long &nsec);
+
+  class Time
+  {
+    public:
+      unsigned long sec, nsec;
+
+      Time() : sec(0), nsec(0) {}
+      Time(unsigned long _sec, unsigned long _nsec) : sec(_sec), nsec(_nsec)
+      {
+        normalizeSecNSec(sec, nsec);  
+      } 
+        
+      double toSec() const { return (double)sec + 1e-9*(double)nsec; };
+      void fromSec(double t) { sec = (unsigned long) floor(t); nsec = (unsigned long) round((t-sec) * 1e9); };
+
+      unsigned long toNsec() { return (unsigned long)sec*1000000000ull + (unsigned long)nsec; };
+      Time& fromNSec(long t);
+
+      Time& operator +=(const Duration &rhs);
+      Time& operator -=(const Duration &rhs);
+
+      static Time now();
+      static void setNow( Time & new_now);
+  };
+
+}
+
+#endif
diff --git a/quad2/arduino/src/ros_lib/roscpp/Empty.h b/quad2/arduino/src/ros_lib/roscpp/Empty.h
new file mode 100644
index 0000000..0e51679
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/roscpp/Empty.h
@@ -0,0 +1,62 @@
+#ifndef _ROS_SERVICE_Empty_h
+#define _ROS_SERVICE_Empty_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace roscpp
+{
+
+static const char EMPTY[] = "roscpp/Empty";
+
+  class EmptyRequest : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return EMPTY; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class EmptyResponse : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return EMPTY; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class Empty {
+    public:
+    typedef EmptyRequest Request;
+    typedef EmptyResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/roscpp/GetLoggers.h b/quad2/arduino/src/ros_lib/roscpp/GetLoggers.h
new file mode 100644
index 0000000..37ce600
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/roscpp/GetLoggers.h
@@ -0,0 +1,82 @@
+#ifndef _ROS_SERVICE_GetLoggers_h
+#define _ROS_SERVICE_GetLoggers_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "roscpp/Logger.h"
+
+namespace roscpp
+{
+
+static const char GETLOGGERS[] = "roscpp/GetLoggers";
+
+  class GetLoggersRequest : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return GETLOGGERS; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class GetLoggersResponse : public ros::Msg
+  {
+    public:
+      uint8_t loggers_length;
+      roscpp::Logger st_loggers;
+      roscpp::Logger * loggers;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset++) = loggers_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < loggers_length; i++){
+      offset += this->loggers[i].serialize(outbuffer + offset);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint8_t loggers_lengthT = *(inbuffer + offset++);
+      if(loggers_lengthT > loggers_length)
+        this->loggers = (roscpp::Logger*)realloc(this->loggers, loggers_lengthT * sizeof(roscpp::Logger));
+      offset += 3;
+      loggers_length = loggers_lengthT;
+      for( uint8_t i = 0; i < loggers_length; i++){
+      offset += this->st_loggers.deserialize(inbuffer + offset);
+        memcpy( &(this->loggers[i]), &(this->st_loggers), sizeof(roscpp::Logger));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return GETLOGGERS; };
+    const char * getMD5(){ return "32e97e85527d4678a8f9279894bb64b0"; };
+
+  };
+
+  class GetLoggers {
+    public:
+    typedef GetLoggersRequest Request;
+    typedef GetLoggersResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/roscpp/Logger.h b/quad2/arduino/src/ros_lib/roscpp/Logger.h
new file mode 100644
index 0000000..20cd93b
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/roscpp/Logger.h
@@ -0,0 +1,62 @@
+#ifndef _ROS_roscpp_Logger_h
+#define _ROS_roscpp_Logger_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace roscpp
+{
+
+  class Logger : public ros::Msg
+  {
+    public:
+      char * name;
+      char * level;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_name = (uint32_t *)(outbuffer + offset);
+      *length_name = strlen( (const char*) this->name);
+      offset += 4;
+      memcpy(outbuffer + offset, this->name, *length_name);
+      offset += *length_name;
+      uint32_t * length_level = (uint32_t *)(outbuffer + offset);
+      *length_level = strlen( (const char*) this->level);
+      offset += 4;
+      memcpy(outbuffer + offset, this->level, *length_level);
+      offset += *length_level;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_name = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_name; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_name-1]=0;
+      this->name = (char *)(inbuffer + offset-1);
+      offset += length_name;
+      uint32_t length_level = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_level; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_level-1]=0;
+      this->level = (char *)(inbuffer + offset-1);
+      offset += length_level;
+     return offset;
+    }
+
+    const char * getType(){ return "roscpp/Logger"; };
+    const char * getMD5(){ return "a6069a2ff40db7bd32143dd66e1f408e"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/roscpp/SetLoggerLevel.h b/quad2/arduino/src/ros_lib/roscpp/SetLoggerLevel.h
new file mode 100644
index 0000000..c009a2c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/roscpp/SetLoggerLevel.h
@@ -0,0 +1,90 @@
+#ifndef _ROS_SERVICE_SetLoggerLevel_h
+#define _ROS_SERVICE_SetLoggerLevel_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace roscpp
+{
+
+static const char SETLOGGERLEVEL[] = "roscpp/SetLoggerLevel";
+
+  class SetLoggerLevelRequest : public ros::Msg
+  {
+    public:
+      char * logger;
+      char * level;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_logger = (uint32_t *)(outbuffer + offset);
+      *length_logger = strlen( (const char*) this->logger);
+      offset += 4;
+      memcpy(outbuffer + offset, this->logger, *length_logger);
+      offset += *length_logger;
+      uint32_t * length_level = (uint32_t *)(outbuffer + offset);
+      *length_level = strlen( (const char*) this->level);
+      offset += 4;
+      memcpy(outbuffer + offset, this->level, *length_level);
+      offset += *length_level;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_logger = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_logger; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_logger-1]=0;
+      this->logger = (char *)(inbuffer + offset-1);
+      offset += length_logger;
+      uint32_t length_level = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_level; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_level-1]=0;
+      this->level = (char *)(inbuffer + offset-1);
+      offset += length_level;
+     return offset;
+    }
+
+    const char * getType(){ return SETLOGGERLEVEL; };
+    const char * getMD5(){ return "51da076440d78ca1684d36c868df61ea"; };
+
+  };
+
+  class SetLoggerLevelResponse : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return SETLOGGERLEVEL; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class SetLoggerLevel {
+    public:
+    typedef SetLoggerLevelRequest Request;
+    typedef SetLoggerLevelResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/rosgraph_msgs/Clock.h b/quad2/arduino/src/ros_lib/rosgraph_msgs/Clock.h
new file mode 100644
index 0000000..b995423
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/rosgraph_msgs/Clock.h
@@ -0,0 +1,56 @@
+#ifndef _ROS_rosgraph_msgs_Clock_h
+#define _ROS_rosgraph_msgs_Clock_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "ros/time.h"
+
+namespace rosgraph_msgs
+{
+
+  class Clock : public ros::Msg
+  {
+    public:
+      ros::Time clock;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->clock.sec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->clock.sec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->clock.sec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->clock.sec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->clock.sec);
+      *(outbuffer + offset + 0) = (this->clock.nsec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->clock.nsec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->clock.nsec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->clock.nsec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->clock.nsec);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->clock.sec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->clock.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->clock.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->clock.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->clock.sec);
+      this->clock.nsec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->clock.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->clock.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->clock.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->clock.nsec);
+     return offset;
+    }
+
+    const char * getType(){ return "rosgraph_msgs/Clock"; };
+    const char * getMD5(){ return "a9c97c1d230cfc112e270351a944ee47"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/rosgraph_msgs/Log.h b/quad2/arduino/src/ros_lib/rosgraph_msgs/Log.h
new file mode 100644
index 0000000..50b1ae9
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/rosgraph_msgs/Log.h
@@ -0,0 +1,144 @@
+#ifndef _ROS_rosgraph_msgs_Log_h
+#define _ROS_rosgraph_msgs_Log_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "rosgraph_msgs/byte.h"
+
+namespace rosgraph_msgs
+{
+
+  class Log : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      rosgraph_msgs::byte level;
+      char * name;
+      char * msg;
+      char * file;
+      char * function;
+      uint32_t line;
+      uint8_t topics_length;
+      char* st_topics;
+      char* * topics;
+      enum { DEBUG = 1  };
+      enum { INFO = 2   };
+      enum { WARN = 4   };
+      enum { ERROR = 8  };
+      enum { FATAL = 16  };
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->level.serialize(outbuffer + offset);
+      uint32_t * length_name = (uint32_t *)(outbuffer + offset);
+      *length_name = strlen( (const char*) this->name);
+      offset += 4;
+      memcpy(outbuffer + offset, this->name, *length_name);
+      offset += *length_name;
+      uint32_t * length_msg = (uint32_t *)(outbuffer + offset);
+      *length_msg = strlen( (const char*) this->msg);
+      offset += 4;
+      memcpy(outbuffer + offset, this->msg, *length_msg);
+      offset += *length_msg;
+      uint32_t * length_file = (uint32_t *)(outbuffer + offset);
+      *length_file = strlen( (const char*) this->file);
+      offset += 4;
+      memcpy(outbuffer + offset, this->file, *length_file);
+      offset += *length_file;
+      uint32_t * length_function = (uint32_t *)(outbuffer + offset);
+      *length_function = strlen( (const char*) this->function);
+      offset += 4;
+      memcpy(outbuffer + offset, this->function, *length_function);
+      offset += *length_function;
+      *(outbuffer + offset + 0) = (this->line >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->line >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->line >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->line >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->line);
+      *(outbuffer + offset++) = topics_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < topics_length; i++){
+      uint32_t * length_topicsi = (uint32_t *)(outbuffer + offset);
+      *length_topicsi = strlen( (const char*) this->topics[i]);
+      offset += 4;
+      memcpy(outbuffer + offset, this->topics[i], *length_topicsi);
+      offset += *length_topicsi;
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->level.deserialize(inbuffer + offset);
+      uint32_t length_name = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_name; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_name-1]=0;
+      this->name = (char *)(inbuffer + offset-1);
+      offset += length_name;
+      uint32_t length_msg = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_msg; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_msg-1]=0;
+      this->msg = (char *)(inbuffer + offset-1);
+      offset += length_msg;
+      uint32_t length_file = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_file; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_file-1]=0;
+      this->file = (char *)(inbuffer + offset-1);
+      offset += length_file;
+      uint32_t length_function = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_function; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_function-1]=0;
+      this->function = (char *)(inbuffer + offset-1);
+      offset += length_function;
+      this->line |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->line |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->line |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->line |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->line);
+      uint8_t topics_lengthT = *(inbuffer + offset++);
+      if(topics_lengthT > topics_length)
+        this->topics = (char**)realloc(this->topics, topics_lengthT * sizeof(char*));
+      offset += 3;
+      topics_length = topics_lengthT;
+      for( uint8_t i = 0; i < topics_length; i++){
+      uint32_t length_st_topics = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_st_topics; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_st_topics-1]=0;
+      this->st_topics = (char *)(inbuffer + offset-1);
+      offset += length_st_topics;
+        memcpy( &(this->topics[i]), &(this->st_topics), sizeof(char*));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "rosgraph_msgs/Log"; };
+    const char * getMD5(){ return "acffd30cd6b6de30f120938c17c593fb"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/rosserial_arduino/Adc.h b/quad2/arduino/src/ros_lib/rosserial_arduino/Adc.h
new file mode 100644
index 0000000..1e6aa2c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/rosserial_arduino/Adc.h
@@ -0,0 +1,76 @@
+#ifndef _ROS_rosserial_arduino_Adc_h
+#define _ROS_rosserial_arduino_Adc_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace rosserial_arduino
+{
+
+  class Adc : public ros::Msg
+  {
+    public:
+      uint16_t adc0;
+      uint16_t adc1;
+      uint16_t adc2;
+      uint16_t adc3;
+      uint16_t adc4;
+      uint16_t adc5;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->adc0 >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->adc0 >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->adc0);
+      *(outbuffer + offset + 0) = (this->adc1 >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->adc1 >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->adc1);
+      *(outbuffer + offset + 0) = (this->adc2 >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->adc2 >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->adc2);
+      *(outbuffer + offset + 0) = (this->adc3 >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->adc3 >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->adc3);
+      *(outbuffer + offset + 0) = (this->adc4 >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->adc4 >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->adc4);
+      *(outbuffer + offset + 0) = (this->adc5 >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->adc5 >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->adc5);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->adc0 |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->adc0 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->adc0);
+      this->adc1 |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->adc1 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->adc1);
+      this->adc2 |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->adc2 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->adc2);
+      this->adc3 |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->adc3 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->adc3);
+      this->adc4 |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->adc4 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->adc4);
+      this->adc5 |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->adc5 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->adc5);
+     return offset;
+    }
+
+    const char * getType(){ return "rosserial_arduino/Adc"; };
+    const char * getMD5(){ return "6d7853a614e2e821319068311f2af25b"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/rosserial_arduino/Test.h b/quad2/arduino/src/ros_lib/rosserial_arduino/Test.h
new file mode 100644
index 0000000..2285e09
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/rosserial_arduino/Test.h
@@ -0,0 +1,90 @@
+#ifndef _ROS_SERVICE_Test_h
+#define _ROS_SERVICE_Test_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace rosserial_arduino
+{
+
+static const char TEST[] = "rosserial_arduino/Test";
+
+  class TestRequest : public ros::Msg
+  {
+    public:
+      char * input;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_input = (uint32_t *)(outbuffer + offset);
+      *length_input = strlen( (const char*) this->input);
+      offset += 4;
+      memcpy(outbuffer + offset, this->input, *length_input);
+      offset += *length_input;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_input = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_input; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_input-1]=0;
+      this->input = (char *)(inbuffer + offset-1);
+      offset += length_input;
+     return offset;
+    }
+
+    const char * getType(){ return TEST; };
+    const char * getMD5(){ return "39e92f1778057359c64c7b8a7d7b19de"; };
+
+  };
+
+  class TestResponse : public ros::Msg
+  {
+    public:
+      char * output;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_output = (uint32_t *)(outbuffer + offset);
+      *length_output = strlen( (const char*) this->output);
+      offset += 4;
+      memcpy(outbuffer + offset, this->output, *length_output);
+      offset += *length_output;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_output = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_output; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_output-1]=0;
+      this->output = (char *)(inbuffer + offset-1);
+      offset += length_output;
+     return offset;
+    }
+
+    const char * getType(){ return TEST; };
+    const char * getMD5(){ return "0825d95fdfa2c8f4bbb4e9c74bccd3fd"; };
+
+  };
+
+  class Test {
+    public:
+    typedef TestRequest Request;
+    typedef TestResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/rosserial_msgs/Log.h b/quad2/arduino/src/ros_lib/rosserial_msgs/Log.h
new file mode 100644
index 0000000..cfe5c71
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/rosserial_msgs/Log.h
@@ -0,0 +1,58 @@
+#ifndef _ROS_rosserial_msgs_Log_h
+#define _ROS_rosserial_msgs_Log_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace rosserial_msgs
+{
+
+  class Log : public ros::Msg
+  {
+    public:
+      uint8_t level;
+      char * msg;
+      enum { DEBUG = 0 };
+      enum { INFO = 1 };
+      enum { WARN = 2 };
+      enum { ERROR = 3 };
+      enum { FATAL = 4 };
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->level);
+      uint32_t * length_msg = (uint32_t *)(outbuffer + offset);
+      *length_msg = strlen( (const char*) this->msg);
+      offset += 4;
+      memcpy(outbuffer + offset, this->msg, *length_msg);
+      offset += *length_msg;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->level |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->level);
+      uint32_t length_msg = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_msg; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_msg-1]=0;
+      this->msg = (char *)(inbuffer + offset-1);
+      offset += length_msg;
+     return offset;
+    }
+
+    const char * getType(){ return "rosserial_msgs/Log"; };
+    const char * getMD5(){ return "7170d5aec999754ba0d9f762bf49b913"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/rosserial_msgs/RequestParam.h b/quad2/arduino/src/ros_lib/rosserial_msgs/RequestParam.h
new file mode 100644
index 0000000..b001abd
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/rosserial_msgs/RequestParam.h
@@ -0,0 +1,182 @@
+#ifndef _ROS_SERVICE_RequestParam_h
+#define _ROS_SERVICE_RequestParam_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace rosserial_msgs
+{
+
+static const char REQUESTPARAM[] = "rosserial_msgs/RequestParam";
+
+  class RequestParamRequest : public ros::Msg
+  {
+    public:
+      char * name;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_name = (uint32_t *)(outbuffer + offset);
+      *length_name = strlen( (const char*) this->name);
+      offset += 4;
+      memcpy(outbuffer + offset, this->name, *length_name);
+      offset += *length_name;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_name = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_name; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_name-1]=0;
+      this->name = (char *)(inbuffer + offset-1);
+      offset += length_name;
+     return offset;
+    }
+
+    const char * getType(){ return REQUESTPARAM; };
+    const char * getMD5(){ return "c1f3d28f1b044c871e6eff2e9fc3c667"; };
+
+  };
+
+  class RequestParamResponse : public ros::Msg
+  {
+    public:
+      uint8_t ints_length;
+      int32_t st_ints;
+      int32_t * ints;
+      uint8_t floats_length;
+      float st_floats;
+      float * floats;
+      uint8_t strings_length;
+      char* st_strings;
+      char* * strings;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset++) = ints_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < ints_length; i++){
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_intsi;
+      u_intsi.real = this->ints[i];
+      *(outbuffer + offset + 0) = (u_intsi.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_intsi.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_intsi.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_intsi.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->ints[i]);
+      }
+      *(outbuffer + offset++) = floats_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < floats_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_floatsi;
+      u_floatsi.real = this->floats[i];
+      *(outbuffer + offset + 0) = (u_floatsi.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_floatsi.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_floatsi.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_floatsi.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->floats[i]);
+      }
+      *(outbuffer + offset++) = strings_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < strings_length; i++){
+      uint32_t * length_stringsi = (uint32_t *)(outbuffer + offset);
+      *length_stringsi = strlen( (const char*) this->strings[i]);
+      offset += 4;
+      memcpy(outbuffer + offset, this->strings[i], *length_stringsi);
+      offset += *length_stringsi;
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint8_t ints_lengthT = *(inbuffer + offset++);
+      if(ints_lengthT > ints_length)
+        this->ints = (int32_t*)realloc(this->ints, ints_lengthT * sizeof(int32_t));
+      offset += 3;
+      ints_length = ints_lengthT;
+      for( uint8_t i = 0; i < ints_length; i++){
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_st_ints;
+      u_st_ints.base = 0;
+      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_ints = u_st_ints.real;
+      offset += sizeof(this->st_ints);
+        memcpy( &(this->ints[i]), &(this->st_ints), sizeof(int32_t));
+      }
+      uint8_t floats_lengthT = *(inbuffer + offset++);
+      if(floats_lengthT > floats_length)
+        this->floats = (float*)realloc(this->floats, floats_lengthT * sizeof(float));
+      offset += 3;
+      floats_length = floats_lengthT;
+      for( uint8_t i = 0; i < floats_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_st_floats;
+      u_st_floats.base = 0;
+      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_floats = u_st_floats.real;
+      offset += sizeof(this->st_floats);
+        memcpy( &(this->floats[i]), &(this->st_floats), sizeof(float));
+      }
+      uint8_t strings_lengthT = *(inbuffer + offset++);
+      if(strings_lengthT > strings_length)
+        this->strings = (char**)realloc(this->strings, strings_lengthT * sizeof(char*));
+      offset += 3;
+      strings_length = strings_lengthT;
+      for( uint8_t i = 0; i < strings_length; i++){
+      uint32_t length_st_strings = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_st_strings; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_st_strings-1]=0;
+      this->st_strings = (char *)(inbuffer + offset-1);
+      offset += length_st_strings;
+        memcpy( &(this->strings[i]), &(this->st_strings), sizeof(char*));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return REQUESTPARAM; };
+    const char * getMD5(){ return "9f0e98bda65981986ddf53afa7a40e49"; };
+
+  };
+
+  class RequestParam {
+    public:
+    typedef RequestParamRequest Request;
+    typedef RequestParamResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/rosserial_msgs/TopicInfo.h b/quad2/arduino/src/ros_lib/rosserial_msgs/TopicInfo.h
new file mode 100644
index 0000000..ed3285b
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/rosserial_msgs/TopicInfo.h
@@ -0,0 +1,112 @@
+#ifndef _ROS_rosserial_msgs_TopicInfo_h
+#define _ROS_rosserial_msgs_TopicInfo_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace rosserial_msgs
+{
+
+  class TopicInfo : public ros::Msg
+  {
+    public:
+      uint16_t topic_id;
+      char * topic_name;
+      char * message_type;
+      char * md5sum;
+      int32_t buffer_size;
+      enum { ID_PUBLISHER = 0 };
+      enum { ID_SUBSCRIBER = 1 };
+      enum { ID_SERVICE_SERVER = 2 };
+      enum { ID_SERVICE_CLIENT = 4 };
+      enum { ID_PARAMETER_REQUEST = 6 };
+      enum { ID_LOG = 7 };
+      enum { ID_TIME = 10 };
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->topic_id >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->topic_id >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->topic_id);
+      uint32_t * length_topic_name = (uint32_t *)(outbuffer + offset);
+      *length_topic_name = strlen( (const char*) this->topic_name);
+      offset += 4;
+      memcpy(outbuffer + offset, this->topic_name, *length_topic_name);
+      offset += *length_topic_name;
+      uint32_t * length_message_type = (uint32_t *)(outbuffer + offset);
+      *length_message_type = strlen( (const char*) this->message_type);
+      offset += 4;
+      memcpy(outbuffer + offset, this->message_type, *length_message_type);
+      offset += *length_message_type;
+      uint32_t * length_md5sum = (uint32_t *)(outbuffer + offset);
+      *length_md5sum = strlen( (const char*) this->md5sum);
+      offset += 4;
+      memcpy(outbuffer + offset, this->md5sum, *length_md5sum);
+      offset += *length_md5sum;
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_buffer_size;
+      u_buffer_size.real = this->buffer_size;
+      *(outbuffer + offset + 0) = (u_buffer_size.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_buffer_size.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_buffer_size.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_buffer_size.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->buffer_size);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->topic_id |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->topic_id |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->topic_id);
+      uint32_t length_topic_name = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_topic_name; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_topic_name-1]=0;
+      this->topic_name = (char *)(inbuffer + offset-1);
+      offset += length_topic_name;
+      uint32_t length_message_type = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_message_type; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_message_type-1]=0;
+      this->message_type = (char *)(inbuffer + offset-1);
+      offset += length_message_type;
+      uint32_t length_md5sum = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_md5sum; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_md5sum-1]=0;
+      this->md5sum = (char *)(inbuffer + offset-1);
+      offset += length_md5sum;
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_buffer_size;
+      u_buffer_size.base = 0;
+      u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->buffer_size = u_buffer_size.real;
+      offset += sizeof(this->buffer_size);
+     return offset;
+    }
+
+    const char * getType(){ return "rosserial_msgs/TopicInfo"; };
+    const char * getMD5(){ return "63aa5e8f1bdd6f35c69fe1a1b9d28e9f"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/CameraInfo.h b/quad2/arduino/src/ros_lib/sensor_msgs/CameraInfo.h
new file mode 100644
index 0000000..3e85acb
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/CameraInfo.h
@@ -0,0 +1,238 @@
+#ifndef _ROS_sensor_msgs_CameraInfo_h
+#define _ROS_sensor_msgs_CameraInfo_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "sensor_msgs/RegionOfInterest.h"
+
+namespace sensor_msgs
+{
+
+  class CameraInfo : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      uint32_t height;
+      uint32_t width;
+      char * distortion_model;
+      uint8_t D_length;
+      float st_D;
+      float * D;
+      float K[9];
+      float R[9];
+      float P[12];
+      uint32_t binning_x;
+      uint32_t binning_y;
+      sensor_msgs::RegionOfInterest roi;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->height);
+      *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->width);
+      uint32_t * length_distortion_model = (uint32_t *)(outbuffer + offset);
+      *length_distortion_model = strlen( (const char*) this->distortion_model);
+      offset += 4;
+      memcpy(outbuffer + offset, this->distortion_model, *length_distortion_model);
+      offset += *length_distortion_model;
+      *(outbuffer + offset++) = D_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < D_length; i++){
+      int32_t * val_Di = (long *) &(this->D[i]);
+      int32_t exp_Di = (((*val_Di)>>23)&255);
+      if(exp_Di != 0)
+        exp_Di += 1023-127;
+      int32_t sig_Di = *val_Di;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_Di<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_Di>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_Di>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_Di<<4) & 0xF0) | ((sig_Di>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_Di>>4) & 0x7F;
+      if(this->D[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      unsigned char * K_val = (unsigned char *) this->K;
+      for( uint8_t i = 0; i < 9; i++){
+      int32_t * val_Ki = (long *) &(this->K[i]);
+      int32_t exp_Ki = (((*val_Ki)>>23)&255);
+      if(exp_Ki != 0)
+        exp_Ki += 1023-127;
+      int32_t sig_Ki = *val_Ki;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_Ki<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_Ki>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_Ki>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_Ki<<4) & 0xF0) | ((sig_Ki>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_Ki>>4) & 0x7F;
+      if(this->K[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      unsigned char * R_val = (unsigned char *) this->R;
+      for( uint8_t i = 0; i < 9; i++){
+      int32_t * val_Ri = (long *) &(this->R[i]);
+      int32_t exp_Ri = (((*val_Ri)>>23)&255);
+      if(exp_Ri != 0)
+        exp_Ri += 1023-127;
+      int32_t sig_Ri = *val_Ri;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_Ri<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_Ri>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_Ri>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_Ri<<4) & 0xF0) | ((sig_Ri>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_Ri>>4) & 0x7F;
+      if(this->R[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      unsigned char * P_val = (unsigned char *) this->P;
+      for( uint8_t i = 0; i < 12; i++){
+      int32_t * val_Pi = (long *) &(this->P[i]);
+      int32_t exp_Pi = (((*val_Pi)>>23)&255);
+      if(exp_Pi != 0)
+        exp_Pi += 1023-127;
+      int32_t sig_Pi = *val_Pi;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_Pi<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_Pi>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_Pi>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_Pi<<4) & 0xF0) | ((sig_Pi>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_Pi>>4) & 0x7F;
+      if(this->P[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      *(outbuffer + offset + 0) = (this->binning_x >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->binning_x >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->binning_x >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->binning_x >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->binning_x);
+      *(outbuffer + offset + 0) = (this->binning_y >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->binning_y >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->binning_y >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->binning_y >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->binning_y);
+      offset += this->roi.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->height);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->width);
+      uint32_t length_distortion_model = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_distortion_model; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_distortion_model-1]=0;
+      this->distortion_model = (char *)(inbuffer + offset-1);
+      offset += length_distortion_model;
+      uint8_t D_lengthT = *(inbuffer + offset++);
+      if(D_lengthT > D_length)
+        this->D = (float*)realloc(this->D, D_lengthT * sizeof(float));
+      offset += 3;
+      D_length = D_lengthT;
+      for( uint8_t i = 0; i < D_length; i++){
+      uint32_t * val_st_D = (uint32_t*) &(this->st_D);
+      offset += 3;
+      *val_st_D = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_st_D |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_st_D |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_st_D |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_st_D = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_st_D |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_st_D !=0)
+        *val_st_D |= ((exp_st_D)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_D = -this->st_D;
+        memcpy( &(this->D[i]), &(this->st_D), sizeof(float));
+      }
+      uint8_t * K_val = (uint8_t*) this->K;
+      for( uint8_t i = 0; i < 9; i++){
+      uint32_t * val_Ki = (uint32_t*) &(this->K[i]);
+      offset += 3;
+      *val_Ki = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_Ki |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_Ki |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_Ki |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_Ki = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_Ki |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_Ki !=0)
+        *val_Ki |= ((exp_Ki)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->K[i] = -this->K[i];
+      }
+      uint8_t * R_val = (uint8_t*) this->R;
+      for( uint8_t i = 0; i < 9; i++){
+      uint32_t * val_Ri = (uint32_t*) &(this->R[i]);
+      offset += 3;
+      *val_Ri = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_Ri |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_Ri |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_Ri |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_Ri = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_Ri |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_Ri !=0)
+        *val_Ri |= ((exp_Ri)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->R[i] = -this->R[i];
+      }
+      uint8_t * P_val = (uint8_t*) this->P;
+      for( uint8_t i = 0; i < 12; i++){
+      uint32_t * val_Pi = (uint32_t*) &(this->P[i]);
+      offset += 3;
+      *val_Pi = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_Pi |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_Pi |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_Pi |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_Pi = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_Pi |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_Pi !=0)
+        *val_Pi |= ((exp_Pi)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->P[i] = -this->P[i];
+      }
+      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->binning_x |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->binning_x);
+      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->binning_y |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->binning_y);
+      offset += this->roi.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/CameraInfo"; };
+    const char * getMD5(){ return "c9a58c1b0b154e0e6da7578cb991d214"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/ChannelFloat32.h b/quad2/arduino/src/ros_lib/sensor_msgs/ChannelFloat32.h
new file mode 100644
index 0000000..617b97c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/ChannelFloat32.h
@@ -0,0 +1,86 @@
+#ifndef _ROS_sensor_msgs_ChannelFloat32_h
+#define _ROS_sensor_msgs_ChannelFloat32_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace sensor_msgs
+{
+
+  class ChannelFloat32 : public ros::Msg
+  {
+    public:
+      char * name;
+      uint8_t values_length;
+      float st_values;
+      float * values;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_name = (uint32_t *)(outbuffer + offset);
+      *length_name = strlen( (const char*) this->name);
+      offset += 4;
+      memcpy(outbuffer + offset, this->name, *length_name);
+      offset += *length_name;
+      *(outbuffer + offset++) = values_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < values_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_valuesi;
+      u_valuesi.real = this->values[i];
+      *(outbuffer + offset + 0) = (u_valuesi.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_valuesi.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_valuesi.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_valuesi.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->values[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_name = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_name; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_name-1]=0;
+      this->name = (char *)(inbuffer + offset-1);
+      offset += length_name;
+      uint8_t values_lengthT = *(inbuffer + offset++);
+      if(values_lengthT > values_length)
+        this->values = (float*)realloc(this->values, values_lengthT * sizeof(float));
+      offset += 3;
+      values_length = values_lengthT;
+      for( uint8_t i = 0; i < values_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_st_values;
+      u_st_values.base = 0;
+      u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_values.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_values = u_st_values.real;
+      offset += sizeof(this->st_values);
+        memcpy( &(this->values[i]), &(this->st_values), sizeof(float));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/ChannelFloat32"; };
+    const char * getMD5(){ return "3d40139cdd33dfedcb71ffeeeb42ae7f"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/CompressedImage.h b/quad2/arduino/src/ros_lib/sensor_msgs/CompressedImage.h
new file mode 100644
index 0000000..ff04c90
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/CompressedImage.h
@@ -0,0 +1,73 @@
+#ifndef _ROS_sensor_msgs_CompressedImage_h
+#define _ROS_sensor_msgs_CompressedImage_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+
+namespace sensor_msgs
+{
+
+  class CompressedImage : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      char * format;
+      uint8_t data_length;
+      uint8_t st_data;
+      uint8_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      uint32_t * length_format = (uint32_t *)(outbuffer + offset);
+      *length_format = strlen( (const char*) this->format);
+      offset += 4;
+      memcpy(outbuffer + offset, this->format, *length_format);
+      offset += *length_format;
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      uint32_t length_format = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_format; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_format-1]=0;
+      this->format = (char *)(inbuffer + offset-1);
+      offset += length_format;
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      this->st_data |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/CompressedImage"; };
+    const char * getMD5(){ return "8f7a12909da2c9d3332d540a0977563f"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/Image.h b/quad2/arduino/src/ros_lib/sensor_msgs/Image.h
new file mode 100644
index 0000000..40bac60
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/Image.h
@@ -0,0 +1,111 @@
+#ifndef _ROS_sensor_msgs_Image_h
+#define _ROS_sensor_msgs_Image_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+
+namespace sensor_msgs
+{
+
+  class Image : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      uint32_t height;
+      uint32_t width;
+      char * encoding;
+      uint8_t is_bigendian;
+      uint32_t step;
+      uint8_t data_length;
+      uint8_t st_data;
+      uint8_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->height);
+      *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->width);
+      uint32_t * length_encoding = (uint32_t *)(outbuffer + offset);
+      *length_encoding = strlen( (const char*) this->encoding);
+      offset += 4;
+      memcpy(outbuffer + offset, this->encoding, *length_encoding);
+      offset += *length_encoding;
+      *(outbuffer + offset + 0) = (this->is_bigendian >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->is_bigendian);
+      *(outbuffer + offset + 0) = (this->step >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->step >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->step >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->step >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->step);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->height);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->width);
+      uint32_t length_encoding = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_encoding; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_encoding-1]=0;
+      this->encoding = (char *)(inbuffer + offset-1);
+      offset += length_encoding;
+      this->is_bigendian |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->is_bigendian);
+      this->step |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->step);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      this->st_data |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/Image"; };
+    const char * getMD5(){ return "060021388200f6f0f447d0fcd9c64743"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/Imu.h b/quad2/arduino/src/ros_lib/sensor_msgs/Imu.h
new file mode 100644
index 0000000..37ff026
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/Imu.h
@@ -0,0 +1,145 @@
+#ifndef _ROS_sensor_msgs_Imu_h
+#define _ROS_sensor_msgs_Imu_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Quaternion.h"
+#include "geometry_msgs/Vector3.h"
+
+namespace sensor_msgs
+{
+
+  class Imu : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      geometry_msgs::Quaternion orientation;
+      float orientation_covariance[9];
+      geometry_msgs::Vector3 angular_velocity;
+      float angular_velocity_covariance[9];
+      geometry_msgs::Vector3 linear_acceleration;
+      float linear_acceleration_covariance[9];
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->orientation.serialize(outbuffer + offset);
+      unsigned char * orientation_covariance_val = (unsigned char *) this->orientation_covariance;
+      for( uint8_t i = 0; i < 9; i++){
+      int32_t * val_orientation_covariancei = (long *) &(this->orientation_covariance[i]);
+      int32_t exp_orientation_covariancei = (((*val_orientation_covariancei)>>23)&255);
+      if(exp_orientation_covariancei != 0)
+        exp_orientation_covariancei += 1023-127;
+      int32_t sig_orientation_covariancei = *val_orientation_covariancei;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_orientation_covariancei<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_orientation_covariancei>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_orientation_covariancei>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_orientation_covariancei<<4) & 0xF0) | ((sig_orientation_covariancei>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_orientation_covariancei>>4) & 0x7F;
+      if(this->orientation_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      offset += this->angular_velocity.serialize(outbuffer + offset);
+      unsigned char * angular_velocity_covariance_val = (unsigned char *) this->angular_velocity_covariance;
+      for( uint8_t i = 0; i < 9; i++){
+      int32_t * val_angular_velocity_covariancei = (long *) &(this->angular_velocity_covariance[i]);
+      int32_t exp_angular_velocity_covariancei = (((*val_angular_velocity_covariancei)>>23)&255);
+      if(exp_angular_velocity_covariancei != 0)
+        exp_angular_velocity_covariancei += 1023-127;
+      int32_t sig_angular_velocity_covariancei = *val_angular_velocity_covariancei;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_angular_velocity_covariancei<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_angular_velocity_covariancei<<4) & 0xF0) | ((sig_angular_velocity_covariancei>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_angular_velocity_covariancei>>4) & 0x7F;
+      if(this->angular_velocity_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      offset += this->linear_acceleration.serialize(outbuffer + offset);
+      unsigned char * linear_acceleration_covariance_val = (unsigned char *) this->linear_acceleration_covariance;
+      for( uint8_t i = 0; i < 9; i++){
+      int32_t * val_linear_acceleration_covariancei = (long *) &(this->linear_acceleration_covariance[i]);
+      int32_t exp_linear_acceleration_covariancei = (((*val_linear_acceleration_covariancei)>>23)&255);
+      if(exp_linear_acceleration_covariancei != 0)
+        exp_linear_acceleration_covariancei += 1023-127;
+      int32_t sig_linear_acceleration_covariancei = *val_linear_acceleration_covariancei;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_linear_acceleration_covariancei<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_linear_acceleration_covariancei<<4) & 0xF0) | ((sig_linear_acceleration_covariancei>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_linear_acceleration_covariancei>>4) & 0x7F;
+      if(this->linear_acceleration_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->orientation.deserialize(inbuffer + offset);
+      uint8_t * orientation_covariance_val = (uint8_t*) this->orientation_covariance;
+      for( uint8_t i = 0; i < 9; i++){
+      uint32_t * val_orientation_covariancei = (uint32_t*) &(this->orientation_covariance[i]);
+      offset += 3;
+      *val_orientation_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_orientation_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_orientation_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_orientation_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_orientation_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_orientation_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_orientation_covariancei !=0)
+        *val_orientation_covariancei |= ((exp_orientation_covariancei)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->orientation_covariance[i] = -this->orientation_covariance[i];
+      }
+      offset += this->angular_velocity.deserialize(inbuffer + offset);
+      uint8_t * angular_velocity_covariance_val = (uint8_t*) this->angular_velocity_covariance;
+      for( uint8_t i = 0; i < 9; i++){
+      uint32_t * val_angular_velocity_covariancei = (uint32_t*) &(this->angular_velocity_covariance[i]);
+      offset += 3;
+      *val_angular_velocity_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_angular_velocity_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_angular_velocity_covariancei !=0)
+        *val_angular_velocity_covariancei |= ((exp_angular_velocity_covariancei)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->angular_velocity_covariance[i] = -this->angular_velocity_covariance[i];
+      }
+      offset += this->linear_acceleration.deserialize(inbuffer + offset);
+      uint8_t * linear_acceleration_covariance_val = (uint8_t*) this->linear_acceleration_covariance;
+      for( uint8_t i = 0; i < 9; i++){
+      uint32_t * val_linear_acceleration_covariancei = (uint32_t*) &(this->linear_acceleration_covariance[i]);
+      offset += 3;
+      *val_linear_acceleration_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_linear_acceleration_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_linear_acceleration_covariancei !=0)
+        *val_linear_acceleration_covariancei |= ((exp_linear_acceleration_covariancei)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->linear_acceleration_covariance[i] = -this->linear_acceleration_covariance[i];
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/Imu"; };
+    const char * getMD5(){ return "6a62c6daae103f4ff57a132d6f95cec2"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/JointState.h b/quad2/arduino/src/ros_lib/sensor_msgs/JointState.h
new file mode 100644
index 0000000..02e62d0
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/JointState.h
@@ -0,0 +1,194 @@
+#ifndef _ROS_sensor_msgs_JointState_h
+#define _ROS_sensor_msgs_JointState_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+
+namespace sensor_msgs
+{
+
+  class JointState : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      uint8_t name_length;
+      char* st_name;
+      char* * name;
+      uint8_t position_length;
+      float st_position;
+      float * position;
+      uint8_t velocity_length;
+      float st_velocity;
+      float * velocity;
+      uint8_t effort_length;
+      float st_effort;
+      float * effort;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = name_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < name_length; i++){
+      uint32_t * length_namei = (uint32_t *)(outbuffer + offset);
+      *length_namei = strlen( (const char*) this->name[i]);
+      offset += 4;
+      memcpy(outbuffer + offset, this->name[i], *length_namei);
+      offset += *length_namei;
+      }
+      *(outbuffer + offset++) = position_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < position_length; i++){
+      int32_t * val_positioni = (long *) &(this->position[i]);
+      int32_t exp_positioni = (((*val_positioni)>>23)&255);
+      if(exp_positioni != 0)
+        exp_positioni += 1023-127;
+      int32_t sig_positioni = *val_positioni;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_positioni<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_positioni>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_positioni>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_positioni<<4) & 0xF0) | ((sig_positioni>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_positioni>>4) & 0x7F;
+      if(this->position[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      *(outbuffer + offset++) = velocity_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < velocity_length; i++){
+      int32_t * val_velocityi = (long *) &(this->velocity[i]);
+      int32_t exp_velocityi = (((*val_velocityi)>>23)&255);
+      if(exp_velocityi != 0)
+        exp_velocityi += 1023-127;
+      int32_t sig_velocityi = *val_velocityi;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_velocityi<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_velocityi>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_velocityi>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_velocityi<<4) & 0xF0) | ((sig_velocityi>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_velocityi>>4) & 0x7F;
+      if(this->velocity[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      *(outbuffer + offset++) = effort_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < effort_length; i++){
+      int32_t * val_efforti = (long *) &(this->effort[i]);
+      int32_t exp_efforti = (((*val_efforti)>>23)&255);
+      if(exp_efforti != 0)
+        exp_efforti += 1023-127;
+      int32_t sig_efforti = *val_efforti;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_efforti<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_efforti>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_efforti>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_efforti<<4) & 0xF0) | ((sig_efforti>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_efforti>>4) & 0x7F;
+      if(this->effort[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      uint8_t name_lengthT = *(inbuffer + offset++);
+      if(name_lengthT > name_length)
+        this->name = (char**)realloc(this->name, name_lengthT * sizeof(char*));
+      offset += 3;
+      name_length = name_lengthT;
+      for( uint8_t i = 0; i < name_length; i++){
+      uint32_t length_st_name = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_st_name; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_st_name-1]=0;
+      this->st_name = (char *)(inbuffer + offset-1);
+      offset += length_st_name;
+        memcpy( &(this->name[i]), &(this->st_name), sizeof(char*));
+      }
+      uint8_t position_lengthT = *(inbuffer + offset++);
+      if(position_lengthT > position_length)
+        this->position = (float*)realloc(this->position, position_lengthT * sizeof(float));
+      offset += 3;
+      position_length = position_lengthT;
+      for( uint8_t i = 0; i < position_length; i++){
+      uint32_t * val_st_position = (uint32_t*) &(this->st_position);
+      offset += 3;
+      *val_st_position = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_st_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_st_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_st_position |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_st_position = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_st_position |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_st_position !=0)
+        *val_st_position |= ((exp_st_position)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_position = -this->st_position;
+        memcpy( &(this->position[i]), &(this->st_position), sizeof(float));
+      }
+      uint8_t velocity_lengthT = *(inbuffer + offset++);
+      if(velocity_lengthT > velocity_length)
+        this->velocity = (float*)realloc(this->velocity, velocity_lengthT * sizeof(float));
+      offset += 3;
+      velocity_length = velocity_lengthT;
+      for( uint8_t i = 0; i < velocity_length; i++){
+      uint32_t * val_st_velocity = (uint32_t*) &(this->st_velocity);
+      offset += 3;
+      *val_st_velocity = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_st_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_st_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_st_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_st_velocity = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_st_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_st_velocity !=0)
+        *val_st_velocity |= ((exp_st_velocity)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_velocity = -this->st_velocity;
+        memcpy( &(this->velocity[i]), &(this->st_velocity), sizeof(float));
+      }
+      uint8_t effort_lengthT = *(inbuffer + offset++);
+      if(effort_lengthT > effort_length)
+        this->effort = (float*)realloc(this->effort, effort_lengthT * sizeof(float));
+      offset += 3;
+      effort_length = effort_lengthT;
+      for( uint8_t i = 0; i < effort_length; i++){
+      uint32_t * val_st_effort = (uint32_t*) &(this->st_effort);
+      offset += 3;
+      *val_st_effort = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_st_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_st_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_st_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_st_effort = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_st_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_st_effort !=0)
+        *val_st_effort |= ((exp_st_effort)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_effort = -this->st_effort;
+        memcpy( &(this->effort[i]), &(this->st_effort), sizeof(float));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/JointState"; };
+    const char * getMD5(){ return "3066dcd76a6cfaef579bd0f34173e9fd"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/Joy.h b/quad2/arduino/src/ros_lib/sensor_msgs/Joy.h
new file mode 100644
index 0000000..8ec12c5
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/Joy.h
@@ -0,0 +1,114 @@
+#ifndef _ROS_sensor_msgs_Joy_h
+#define _ROS_sensor_msgs_Joy_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+
+namespace sensor_msgs
+{
+
+  class Joy : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      uint8_t axes_length;
+      float st_axes;
+      float * axes;
+      uint8_t buttons_length;
+      int32_t st_buttons;
+      int32_t * buttons;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = axes_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < axes_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_axesi;
+      u_axesi.real = this->axes[i];
+      *(outbuffer + offset + 0) = (u_axesi.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_axesi.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_axesi.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_axesi.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->axes[i]);
+      }
+      *(outbuffer + offset++) = buttons_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < buttons_length; i++){
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_buttonsi;
+      u_buttonsi.real = this->buttons[i];
+      *(outbuffer + offset + 0) = (u_buttonsi.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_buttonsi.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_buttonsi.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_buttonsi.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->buttons[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      uint8_t axes_lengthT = *(inbuffer + offset++);
+      if(axes_lengthT > axes_length)
+        this->axes = (float*)realloc(this->axes, axes_lengthT * sizeof(float));
+      offset += 3;
+      axes_length = axes_lengthT;
+      for( uint8_t i = 0; i < axes_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_st_axes;
+      u_st_axes.base = 0;
+      u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_axes.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_axes = u_st_axes.real;
+      offset += sizeof(this->st_axes);
+        memcpy( &(this->axes[i]), &(this->st_axes), sizeof(float));
+      }
+      uint8_t buttons_lengthT = *(inbuffer + offset++);
+      if(buttons_lengthT > buttons_length)
+        this->buttons = (int32_t*)realloc(this->buttons, buttons_lengthT * sizeof(int32_t));
+      offset += 3;
+      buttons_length = buttons_lengthT;
+      for( uint8_t i = 0; i < buttons_length; i++){
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_st_buttons;
+      u_st_buttons.base = 0;
+      u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_buttons.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_buttons = u_st_buttons.real;
+      offset += sizeof(this->st_buttons);
+        memcpy( &(this->buttons[i]), &(this->st_buttons), sizeof(int32_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/Joy"; };
+    const char * getMD5(){ return "5a9ea5f83505693b71e785041e67a8bb"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/JoyFeedback.h b/quad2/arduino/src/ros_lib/sensor_msgs/JoyFeedback.h
new file mode 100644
index 0000000..81e0106
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/JoyFeedback.h
@@ -0,0 +1,69 @@
+#ifndef _ROS_sensor_msgs_JoyFeedback_h
+#define _ROS_sensor_msgs_JoyFeedback_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace sensor_msgs
+{
+
+  class JoyFeedback : public ros::Msg
+  {
+    public:
+      uint8_t type;
+      uint8_t id;
+      float intensity;
+      enum { TYPE_LED =  0 };
+      enum { TYPE_RUMBLE =  1 };
+      enum { TYPE_BUZZER =  2 };
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->type >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->type);
+      *(outbuffer + offset + 0) = (this->id >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->id);
+      union {
+        float real;
+        uint32_t base;
+      } u_intensity;
+      u_intensity.real = this->intensity;
+      *(outbuffer + offset + 0) = (u_intensity.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_intensity.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_intensity.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_intensity.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->intensity);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->type |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->type);
+      this->id |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->id);
+      union {
+        float real;
+        uint32_t base;
+      } u_intensity;
+      u_intensity.base = 0;
+      u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_intensity.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->intensity = u_intensity.real;
+      offset += sizeof(this->intensity);
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/JoyFeedback"; };
+    const char * getMD5(){ return "f4dcd73460360d98f36e55ee7f2e46f1"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/JoyFeedbackArray.h b/quad2/arduino/src/ros_lib/sensor_msgs/JoyFeedbackArray.h
new file mode 100644
index 0000000..c022020
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/JoyFeedbackArray.h
@@ -0,0 +1,54 @@
+#ifndef _ROS_sensor_msgs_JoyFeedbackArray_h
+#define _ROS_sensor_msgs_JoyFeedbackArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "sensor_msgs/JoyFeedback.h"
+
+namespace sensor_msgs
+{
+
+  class JoyFeedbackArray : public ros::Msg
+  {
+    public:
+      uint8_t array_length;
+      sensor_msgs::JoyFeedback st_array;
+      sensor_msgs::JoyFeedback * array;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset++) = array_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < array_length; i++){
+      offset += this->array[i].serialize(outbuffer + offset);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint8_t array_lengthT = *(inbuffer + offset++);
+      if(array_lengthT > array_length)
+        this->array = (sensor_msgs::JoyFeedback*)realloc(this->array, array_lengthT * sizeof(sensor_msgs::JoyFeedback));
+      offset += 3;
+      array_length = array_lengthT;
+      for( uint8_t i = 0; i < array_length; i++){
+      offset += this->st_array.deserialize(inbuffer + offset);
+        memcpy( &(this->array[i]), &(this->st_array), sizeof(sensor_msgs::JoyFeedback));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/JoyFeedbackArray"; };
+    const char * getMD5(){ return "cde5730a895b1fc4dee6f91b754b213d"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/LaserScan.h b/quad2/arduino/src/ros_lib/sensor_msgs/LaserScan.h
new file mode 100644
index 0000000..2e09c95
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/LaserScan.h
@@ -0,0 +1,268 @@
+#ifndef _ROS_sensor_msgs_LaserScan_h
+#define _ROS_sensor_msgs_LaserScan_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+
+namespace sensor_msgs
+{
+
+  class LaserScan : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      float angle_min;
+      float angle_max;
+      float angle_increment;
+      float time_increment;
+      float scan_time;
+      float range_min;
+      float range_max;
+      uint8_t ranges_length;
+      float st_ranges;
+      float * ranges;
+      uint8_t intensities_length;
+      float st_intensities;
+      float * intensities;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      union {
+        float real;
+        uint32_t base;
+      } u_angle_min;
+      u_angle_min.real = this->angle_min;
+      *(outbuffer + offset + 0) = (u_angle_min.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_angle_min.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_angle_min.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_angle_min.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->angle_min);
+      union {
+        float real;
+        uint32_t base;
+      } u_angle_max;
+      u_angle_max.real = this->angle_max;
+      *(outbuffer + offset + 0) = (u_angle_max.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_angle_max.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_angle_max.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_angle_max.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->angle_max);
+      union {
+        float real;
+        uint32_t base;
+      } u_angle_increment;
+      u_angle_increment.real = this->angle_increment;
+      *(outbuffer + offset + 0) = (u_angle_increment.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_angle_increment.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_angle_increment.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_angle_increment.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->angle_increment);
+      union {
+        float real;
+        uint32_t base;
+      } u_time_increment;
+      u_time_increment.real = this->time_increment;
+      *(outbuffer + offset + 0) = (u_time_increment.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_time_increment.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_time_increment.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_time_increment.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->time_increment);
+      union {
+        float real;
+        uint32_t base;
+      } u_scan_time;
+      u_scan_time.real = this->scan_time;
+      *(outbuffer + offset + 0) = (u_scan_time.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_scan_time.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_scan_time.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_scan_time.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->scan_time);
+      union {
+        float real;
+        uint32_t base;
+      } u_range_min;
+      u_range_min.real = this->range_min;
+      *(outbuffer + offset + 0) = (u_range_min.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_range_min.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_range_min.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_range_min.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->range_min);
+      union {
+        float real;
+        uint32_t base;
+      } u_range_max;
+      u_range_max.real = this->range_max;
+      *(outbuffer + offset + 0) = (u_range_max.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_range_max.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_range_max.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_range_max.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->range_max);
+      *(outbuffer + offset++) = ranges_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < ranges_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_rangesi;
+      u_rangesi.real = this->ranges[i];
+      *(outbuffer + offset + 0) = (u_rangesi.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_rangesi.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_rangesi.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_rangesi.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->ranges[i]);
+      }
+      *(outbuffer + offset++) = intensities_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < intensities_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_intensitiesi;
+      u_intensitiesi.real = this->intensities[i];
+      *(outbuffer + offset + 0) = (u_intensitiesi.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_intensitiesi.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_intensitiesi.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_intensitiesi.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->intensities[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      union {
+        float real;
+        uint32_t base;
+      } u_angle_min;
+      u_angle_min.base = 0;
+      u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->angle_min = u_angle_min.real;
+      offset += sizeof(this->angle_min);
+      union {
+        float real;
+        uint32_t base;
+      } u_angle_max;
+      u_angle_max.base = 0;
+      u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->angle_max = u_angle_max.real;
+      offset += sizeof(this->angle_max);
+      union {
+        float real;
+        uint32_t base;
+      } u_angle_increment;
+      u_angle_increment.base = 0;
+      u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->angle_increment = u_angle_increment.real;
+      offset += sizeof(this->angle_increment);
+      union {
+        float real;
+        uint32_t base;
+      } u_time_increment;
+      u_time_increment.base = 0;
+      u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->time_increment = u_time_increment.real;
+      offset += sizeof(this->time_increment);
+      union {
+        float real;
+        uint32_t base;
+      } u_scan_time;
+      u_scan_time.base = 0;
+      u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->scan_time = u_scan_time.real;
+      offset += sizeof(this->scan_time);
+      union {
+        float real;
+        uint32_t base;
+      } u_range_min;
+      u_range_min.base = 0;
+      u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->range_min = u_range_min.real;
+      offset += sizeof(this->range_min);
+      union {
+        float real;
+        uint32_t base;
+      } u_range_max;
+      u_range_max.base = 0;
+      u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->range_max = u_range_max.real;
+      offset += sizeof(this->range_max);
+      uint8_t ranges_lengthT = *(inbuffer + offset++);
+      if(ranges_lengthT > ranges_length)
+        this->ranges = (float*)realloc(this->ranges, ranges_lengthT * sizeof(float));
+      offset += 3;
+      ranges_length = ranges_lengthT;
+      for( uint8_t i = 0; i < ranges_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_st_ranges;
+      u_st_ranges.base = 0;
+      u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_ranges = u_st_ranges.real;
+      offset += sizeof(this->st_ranges);
+        memcpy( &(this->ranges[i]), &(this->st_ranges), sizeof(float));
+      }
+      uint8_t intensities_lengthT = *(inbuffer + offset++);
+      if(intensities_lengthT > intensities_length)
+        this->intensities = (float*)realloc(this->intensities, intensities_lengthT * sizeof(float));
+      offset += 3;
+      intensities_length = intensities_lengthT;
+      for( uint8_t i = 0; i < intensities_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_st_intensities;
+      u_st_intensities.base = 0;
+      u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_intensities = u_st_intensities.real;
+      offset += sizeof(this->st_intensities);
+        memcpy( &(this->intensities[i]), &(this->st_intensities), sizeof(float));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/LaserScan"; };
+    const char * getMD5(){ return "90c7ef2dc6895d81024acba2ac42f369"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/NavSatFix.h b/quad2/arduino/src/ros_lib/sensor_msgs/NavSatFix.h
new file mode 100644
index 0000000..9c675fc
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/NavSatFix.h
@@ -0,0 +1,161 @@
+#ifndef _ROS_sensor_msgs_NavSatFix_h
+#define _ROS_sensor_msgs_NavSatFix_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "sensor_msgs/NavSatStatus.h"
+
+namespace sensor_msgs
+{
+
+  class NavSatFix : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      sensor_msgs::NavSatStatus status;
+      float latitude;
+      float longitude;
+      float altitude;
+      float position_covariance[9];
+      uint8_t position_covariance_type;
+      enum { COVARIANCE_TYPE_UNKNOWN =  0 };
+      enum { COVARIANCE_TYPE_APPROXIMATED =  1 };
+      enum { COVARIANCE_TYPE_DIAGONAL_KNOWN =  2 };
+      enum { COVARIANCE_TYPE_KNOWN =  3 };
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->status.serialize(outbuffer + offset);
+      int32_t * val_latitude = (long *) &(this->latitude);
+      int32_t exp_latitude = (((*val_latitude)>>23)&255);
+      if(exp_latitude != 0)
+        exp_latitude += 1023-127;
+      int32_t sig_latitude = *val_latitude;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_latitude<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_latitude>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_latitude>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_latitude<<4) & 0xF0) | ((sig_latitude>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_latitude>>4) & 0x7F;
+      if(this->latitude < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_longitude = (long *) &(this->longitude);
+      int32_t exp_longitude = (((*val_longitude)>>23)&255);
+      if(exp_longitude != 0)
+        exp_longitude += 1023-127;
+      int32_t sig_longitude = *val_longitude;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_longitude<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_longitude>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_longitude>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_longitude<<4) & 0xF0) | ((sig_longitude>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_longitude>>4) & 0x7F;
+      if(this->longitude < 0) *(outbuffer + offset -1) |= 0x80;
+      int32_t * val_altitude = (long *) &(this->altitude);
+      int32_t exp_altitude = (((*val_altitude)>>23)&255);
+      if(exp_altitude != 0)
+        exp_altitude += 1023-127;
+      int32_t sig_altitude = *val_altitude;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_altitude<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_altitude>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_altitude>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_altitude<<4) & 0xF0) | ((sig_altitude>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_altitude>>4) & 0x7F;
+      if(this->altitude < 0) *(outbuffer + offset -1) |= 0x80;
+      unsigned char * position_covariance_val = (unsigned char *) this->position_covariance;
+      for( uint8_t i = 0; i < 9; i++){
+      int32_t * val_position_covariancei = (long *) &(this->position_covariance[i]);
+      int32_t exp_position_covariancei = (((*val_position_covariancei)>>23)&255);
+      if(exp_position_covariancei != 0)
+        exp_position_covariancei += 1023-127;
+      int32_t sig_position_covariancei = *val_position_covariancei;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_position_covariancei<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_position_covariancei>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_position_covariancei>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_position_covariancei<<4) & 0xF0) | ((sig_position_covariancei>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_position_covariancei>>4) & 0x7F;
+      if(this->position_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      *(outbuffer + offset + 0) = (this->position_covariance_type >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->position_covariance_type);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->status.deserialize(inbuffer + offset);
+      uint32_t * val_latitude = (uint32_t*) &(this->latitude);
+      offset += 3;
+      *val_latitude = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_latitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_latitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_latitude |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_latitude = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_latitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_latitude !=0)
+        *val_latitude |= ((exp_latitude)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->latitude = -this->latitude;
+      uint32_t * val_longitude = (uint32_t*) &(this->longitude);
+      offset += 3;
+      *val_longitude = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_longitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_longitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_longitude |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_longitude = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_longitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_longitude !=0)
+        *val_longitude |= ((exp_longitude)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->longitude = -this->longitude;
+      uint32_t * val_altitude = (uint32_t*) &(this->altitude);
+      offset += 3;
+      *val_altitude = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_altitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_altitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_altitude |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_altitude = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_altitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_altitude !=0)
+        *val_altitude |= ((exp_altitude)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->altitude = -this->altitude;
+      uint8_t * position_covariance_val = (uint8_t*) this->position_covariance;
+      for( uint8_t i = 0; i < 9; i++){
+      uint32_t * val_position_covariancei = (uint32_t*) &(this->position_covariance[i]);
+      offset += 3;
+      *val_position_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_position_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_position_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_position_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_position_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_position_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_position_covariancei !=0)
+        *val_position_covariancei |= ((exp_position_covariancei)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->position_covariance[i] = -this->position_covariance[i];
+      }
+      this->position_covariance_type |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->position_covariance_type);
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/NavSatFix"; };
+    const char * getMD5(){ return "2d3a8cd499b9b4a0249fb98fd05cfa48"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/NavSatStatus.h b/quad2/arduino/src/ros_lib/sensor_msgs/NavSatStatus.h
new file mode 100644
index 0000000..3008018
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/NavSatStatus.h
@@ -0,0 +1,65 @@
+#ifndef _ROS_sensor_msgs_NavSatStatus_h
+#define _ROS_sensor_msgs_NavSatStatus_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace sensor_msgs
+{
+
+  class NavSatStatus : public ros::Msg
+  {
+    public:
+      int8_t status;
+      uint16_t service;
+      enum { STATUS_NO_FIX =   -1         };
+      enum { STATUS_FIX =       0         };
+      enum { STATUS_SBAS_FIX =  1         };
+      enum { STATUS_GBAS_FIX =  2         };
+      enum { SERVICE_GPS =      1 };
+      enum { SERVICE_GLONASS =  2 };
+      enum { SERVICE_COMPASS =  4       };
+      enum { SERVICE_GALILEO =  8 };
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        int8_t real;
+        uint8_t base;
+      } u_status;
+      u_status.real = this->status;
+      *(outbuffer + offset + 0) = (u_status.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->status);
+      *(outbuffer + offset + 0) = (this->service >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->service >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->service);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        int8_t real;
+        uint8_t base;
+      } u_status;
+      u_status.base = 0;
+      u_status.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->status = u_status.real;
+      offset += sizeof(this->status);
+      this->service |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->service |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->service);
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/NavSatStatus"; };
+    const char * getMD5(){ return "331cdbddfa4bc96ffc3b9ad98900a54c"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/PointCloud.h b/quad2/arduino/src/ros_lib/sensor_msgs/PointCloud.h
new file mode 100644
index 0000000..f04381c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/PointCloud.h
@@ -0,0 +1,78 @@
+#ifndef _ROS_sensor_msgs_PointCloud_h
+#define _ROS_sensor_msgs_PointCloud_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Point32.h"
+#include "sensor_msgs/ChannelFloat32.h"
+
+namespace sensor_msgs
+{
+
+  class PointCloud : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      uint8_t points_length;
+      geometry_msgs::Point32 st_points;
+      geometry_msgs::Point32 * points;
+      uint8_t channels_length;
+      sensor_msgs::ChannelFloat32 st_channels;
+      sensor_msgs::ChannelFloat32 * channels;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = points_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < points_length; i++){
+      offset += this->points[i].serialize(outbuffer + offset);
+      }
+      *(outbuffer + offset++) = channels_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < channels_length; i++){
+      offset += this->channels[i].serialize(outbuffer + offset);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      uint8_t points_lengthT = *(inbuffer + offset++);
+      if(points_lengthT > points_length)
+        this->points = (geometry_msgs::Point32*)realloc(this->points, points_lengthT * sizeof(geometry_msgs::Point32));
+      offset += 3;
+      points_length = points_lengthT;
+      for( uint8_t i = 0; i < points_length; i++){
+      offset += this->st_points.deserialize(inbuffer + offset);
+        memcpy( &(this->points[i]), &(this->st_points), sizeof(geometry_msgs::Point32));
+      }
+      uint8_t channels_lengthT = *(inbuffer + offset++);
+      if(channels_lengthT > channels_length)
+        this->channels = (sensor_msgs::ChannelFloat32*)realloc(this->channels, channels_lengthT * sizeof(sensor_msgs::ChannelFloat32));
+      offset += 3;
+      channels_length = channels_lengthT;
+      for( uint8_t i = 0; i < channels_length; i++){
+      offset += this->st_channels.deserialize(inbuffer + offset);
+        memcpy( &(this->channels[i]), &(this->st_channels), sizeof(sensor_msgs::ChannelFloat32));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/PointCloud"; };
+    const char * getMD5(){ return "d8e9c3f5afbdd8a130fd1d2763945fca"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/PointCloud2.h b/quad2/arduino/src/ros_lib/sensor_msgs/PointCloud2.h
new file mode 100644
index 0000000..ab8a544
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/PointCloud2.h
@@ -0,0 +1,155 @@
+#ifndef _ROS_sensor_msgs_PointCloud2_h
+#define _ROS_sensor_msgs_PointCloud2_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "sensor_msgs/PointField.h"
+
+namespace sensor_msgs
+{
+
+  class PointCloud2 : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      uint32_t height;
+      uint32_t width;
+      uint8_t fields_length;
+      sensor_msgs::PointField st_fields;
+      sensor_msgs::PointField * fields;
+      bool is_bigendian;
+      uint32_t point_step;
+      uint32_t row_step;
+      uint8_t data_length;
+      uint8_t st_data;
+      uint8_t * data;
+      bool is_dense;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->height);
+      *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->width);
+      *(outbuffer + offset++) = fields_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < fields_length; i++){
+      offset += this->fields[i].serialize(outbuffer + offset);
+      }
+      union {
+        bool real;
+        uint8_t base;
+      } u_is_bigendian;
+      u_is_bigendian.real = this->is_bigendian;
+      *(outbuffer + offset + 0) = (u_is_bigendian.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->is_bigendian);
+      *(outbuffer + offset + 0) = (this->point_step >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->point_step >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->point_step >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->point_step >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->point_step);
+      *(outbuffer + offset + 0) = (this->row_step >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->row_step >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->row_step >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->row_step >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->row_step);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      union {
+        bool real;
+        uint8_t base;
+      } u_is_dense;
+      u_is_dense.real = this->is_dense;
+      *(outbuffer + offset + 0) = (u_is_dense.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->is_dense);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->height);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->width);
+      uint8_t fields_lengthT = *(inbuffer + offset++);
+      if(fields_lengthT > fields_length)
+        this->fields = (sensor_msgs::PointField*)realloc(this->fields, fields_lengthT * sizeof(sensor_msgs::PointField));
+      offset += 3;
+      fields_length = fields_lengthT;
+      for( uint8_t i = 0; i < fields_length; i++){
+      offset += this->st_fields.deserialize(inbuffer + offset);
+        memcpy( &(this->fields[i]), &(this->st_fields), sizeof(sensor_msgs::PointField));
+      }
+      union {
+        bool real;
+        uint8_t base;
+      } u_is_bigendian;
+      u_is_bigendian.base = 0;
+      u_is_bigendian.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->is_bigendian = u_is_bigendian.real;
+      offset += sizeof(this->is_bigendian);
+      this->point_step |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->point_step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->point_step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->point_step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->point_step);
+      this->row_step |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->row_step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->row_step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->row_step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->row_step);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      this->st_data |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t));
+      }
+      union {
+        bool real;
+        uint8_t base;
+      } u_is_dense;
+      u_is_dense.base = 0;
+      u_is_dense.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->is_dense = u_is_dense.real;
+      offset += sizeof(this->is_dense);
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/PointCloud2"; };
+    const char * getMD5(){ return "1158d486dd51d683ce2f1be655c3c181"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/PointField.h b/quad2/arduino/src/ros_lib/sensor_msgs/PointField.h
new file mode 100644
index 0000000..4ac1cb5
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/PointField.h
@@ -0,0 +1,83 @@
+#ifndef _ROS_sensor_msgs_PointField_h
+#define _ROS_sensor_msgs_PointField_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace sensor_msgs
+{
+
+  class PointField : public ros::Msg
+  {
+    public:
+      char * name;
+      uint32_t offset;
+      uint8_t datatype;
+      uint32_t count;
+      enum { INT8 =  1 };
+      enum { UINT8 =  2 };
+      enum { INT16 =  3 };
+      enum { UINT16 =  4 };
+      enum { INT32 =  5 };
+      enum { UINT32 =  6 };
+      enum { FLOAT32 =  7 };
+      enum { FLOAT64 =  8 };
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_name = (uint32_t *)(outbuffer + offset);
+      *length_name = strlen( (const char*) this->name);
+      offset += 4;
+      memcpy(outbuffer + offset, this->name, *length_name);
+      offset += *length_name;
+      *(outbuffer + offset + 0) = (this->offset >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->offset >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->offset >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->offset >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->offset);
+      *(outbuffer + offset + 0) = (this->datatype >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->datatype);
+      *(outbuffer + offset + 0) = (this->count >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->count >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->count >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->count >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->count);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_name = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_name; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_name-1]=0;
+      this->name = (char *)(inbuffer + offset-1);
+      offset += length_name;
+      this->offset |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->offset |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->offset |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->offset |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->offset);
+      this->datatype |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->datatype);
+      this->count |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->count |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->count |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->count |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->count);
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/PointField"; };
+    const char * getMD5(){ return "268eacb2962780ceac86cbd17e328150"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/Range.h b/quad2/arduino/src/ros_lib/sensor_msgs/Range.h
new file mode 100644
index 0000000..cebbc50
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/Range.h
@@ -0,0 +1,133 @@
+#ifndef _ROS_sensor_msgs_Range_h
+#define _ROS_sensor_msgs_Range_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+
+namespace sensor_msgs
+{
+
+  class Range : public ros::Msg
+  {
+    public:
+      std_msgs::Header header;
+      uint8_t radiation_type;
+      float field_of_view;
+      float min_range;
+      float max_range;
+      float range;
+      enum { ULTRASOUND = 0 };
+      enum { INFRARED = 1 };
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      *(outbuffer + offset + 0) = (this->radiation_type >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->radiation_type);
+      union {
+        float real;
+        uint32_t base;
+      } u_field_of_view;
+      u_field_of_view.real = this->field_of_view;
+      *(outbuffer + offset + 0) = (u_field_of_view.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_field_of_view.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_field_of_view.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_field_of_view.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->field_of_view);
+      union {
+        float real;
+        uint32_t base;
+      } u_min_range;
+      u_min_range.real = this->min_range;
+      *(outbuffer + offset + 0) = (u_min_range.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_min_range.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_min_range.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_min_range.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->min_range);
+      union {
+        float real;
+        uint32_t base;
+      } u_max_range;
+      u_max_range.real = this->max_range;
+      *(outbuffer + offset + 0) = (u_max_range.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_max_range.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_max_range.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_max_range.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->max_range);
+      union {
+        float real;
+        uint32_t base;
+      } u_range;
+      u_range.real = this->range;
+      *(outbuffer + offset + 0) = (u_range.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_range.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_range.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_range.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->range);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      this->radiation_type |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->radiation_type);
+      union {
+        float real;
+        uint32_t base;
+      } u_field_of_view;
+      u_field_of_view.base = 0;
+      u_field_of_view.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_field_of_view.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_field_of_view.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_field_of_view.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->field_of_view = u_field_of_view.real;
+      offset += sizeof(this->field_of_view);
+      union {
+        float real;
+        uint32_t base;
+      } u_min_range;
+      u_min_range.base = 0;
+      u_min_range.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_min_range.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_min_range.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_min_range.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->min_range = u_min_range.real;
+      offset += sizeof(this->min_range);
+      union {
+        float real;
+        uint32_t base;
+      } u_max_range;
+      u_max_range.base = 0;
+      u_max_range.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_max_range.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_max_range.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_max_range.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->max_range = u_max_range.real;
+      offset += sizeof(this->max_range);
+      union {
+        float real;
+        uint32_t base;
+      } u_range;
+      u_range.base = 0;
+      u_range.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_range.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_range.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_range.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->range = u_range.real;
+      offset += sizeof(this->range);
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/Range"; };
+    const char * getMD5(){ return "c005c34273dc426c67a020a87bc24148"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/RegionOfInterest.h b/quad2/arduino/src/ros_lib/sensor_msgs/RegionOfInterest.h
new file mode 100644
index 0000000..db08d7b
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/RegionOfInterest.h
@@ -0,0 +1,94 @@
+#ifndef _ROS_sensor_msgs_RegionOfInterest_h
+#define _ROS_sensor_msgs_RegionOfInterest_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace sensor_msgs
+{
+
+  class RegionOfInterest : public ros::Msg
+  {
+    public:
+      uint32_t x_offset;
+      uint32_t y_offset;
+      uint32_t height;
+      uint32_t width;
+      bool do_rectify;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->x_offset >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->x_offset >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->x_offset >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->x_offset >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->x_offset);
+      *(outbuffer + offset + 0) = (this->y_offset >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->y_offset >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->y_offset >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->y_offset >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->y_offset);
+      *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->height);
+      *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->width);
+      union {
+        bool real;
+        uint8_t base;
+      } u_do_rectify;
+      u_do_rectify.real = this->do_rectify;
+      *(outbuffer + offset + 0) = (u_do_rectify.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->do_rectify);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->x_offset |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->x_offset |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->x_offset |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->x_offset |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->x_offset);
+      this->y_offset |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->y_offset |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->y_offset |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->y_offset |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->y_offset);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->height);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->width);
+      union {
+        bool real;
+        uint8_t base;
+      } u_do_rectify;
+      u_do_rectify.base = 0;
+      u_do_rectify.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->do_rectify = u_do_rectify.real;
+      offset += sizeof(this->do_rectify);
+     return offset;
+    }
+
+    const char * getType(){ return "sensor_msgs/RegionOfInterest"; };
+    const char * getMD5(){ return "bdb633039d588fcccb441a4d43ccfe09"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/sensor_msgs/SetCameraInfo.h b/quad2/arduino/src/ros_lib/sensor_msgs/SetCameraInfo.h
new file mode 100644
index 0000000..94b2170
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/sensor_msgs/SetCameraInfo.h
@@ -0,0 +1,96 @@
+#ifndef _ROS_SERVICE_SetCameraInfo_h
+#define _ROS_SERVICE_SetCameraInfo_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "sensor_msgs/CameraInfo.h"
+
+namespace sensor_msgs
+{
+
+static const char SETCAMERAINFO[] = "sensor_msgs/SetCameraInfo";
+
+  class SetCameraInfoRequest : public ros::Msg
+  {
+    public:
+      sensor_msgs::CameraInfo camera_info;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->camera_info.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->camera_info.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return SETCAMERAINFO; };
+    const char * getMD5(){ return "ee34be01fdeee563d0d99cd594d5581d"; };
+
+  };
+
+  class SetCameraInfoResponse : public ros::Msg
+  {
+    public:
+      bool success;
+      char * status_message;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_success;
+      u_success.real = this->success;
+      *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->success);
+      uint32_t * length_status_message = (uint32_t *)(outbuffer + offset);
+      *length_status_message = strlen( (const char*) this->status_message);
+      offset += 4;
+      memcpy(outbuffer + offset, this->status_message, *length_status_message);
+      offset += *length_status_message;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_success;
+      u_success.base = 0;
+      u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->success = u_success.real;
+      offset += sizeof(this->success);
+      uint32_t length_status_message = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_status_message; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_status_message-1]=0;
+      this->status_message = (char *)(inbuffer + offset-1);
+      offset += length_status_message;
+     return offset;
+    }
+
+    const char * getType(){ return SETCAMERAINFO; };
+    const char * getMD5(){ return "2ec6f3eff0161f4257b808b12bc830c2"; };
+
+  };
+
+  class SetCameraInfo {
+    public:
+    typedef SetCameraInfoRequest Request;
+    typedef SetCameraInfoResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Bool.h b/quad2/arduino/src/ros_lib/std_msgs/Bool.h
new file mode 100644
index 0000000..b58eb5f
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Bool.h
@@ -0,0 +1,50 @@
+#ifndef _ROS_std_msgs_Bool_h
+#define _ROS_std_msgs_Bool_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class Bool : public ros::Msg
+  {
+    public:
+      bool data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_data;
+      u_data.real = this->data;
+      *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        bool real;
+        uint8_t base;
+      } u_data;
+      u_data.base = 0;
+      u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data = u_data.real;
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Bool"; };
+    const char * getMD5(){ return "8b94c1b53db61fb6aed406028ad6332a"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Byte.h b/quad2/arduino/src/ros_lib/std_msgs/Byte.h
new file mode 100644
index 0000000..8f9f5d8
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Byte.h
@@ -0,0 +1,38 @@
+#ifndef _ROS_std_msgs_Byte_h
+#define _ROS_std_msgs_Byte_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/byte.h"
+
+namespace std_msgs
+{
+
+  class Byte : public ros::Msg
+  {
+    public:
+      std_msgs::byte data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->data.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->data.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Byte"; };
+    const char * getMD5(){ return "ad736a2e8818154c487bb80fe42ce43b"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/ByteMultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/ByteMultiArray.h
new file mode 100644
index 0000000..3a731e5
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/ByteMultiArray.h
@@ -0,0 +1,58 @@
+#ifndef _ROS_std_msgs_ByteMultiArray_h
+#define _ROS_std_msgs_ByteMultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+#include "std_msgs/byte.h"
+
+namespace std_msgs
+{
+
+  class ByteMultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      std_msgs::byte st_data;
+      std_msgs::byte * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      offset += this->data[i].serialize(outbuffer + offset);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (std_msgs::byte*)realloc(this->data, data_lengthT * sizeof(std_msgs::byte));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      offset += this->st_data.deserialize(inbuffer + offset);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(std_msgs::byte));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/ByteMultiArray"; };
+    const char * getMD5(){ return "70ea476cbcfd65ac2f68f3cda1e891fe"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Char.h b/quad2/arduino/src/ros_lib/std_msgs/Char.h
new file mode 100644
index 0000000..d844e3f
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Char.h
@@ -0,0 +1,38 @@
+#ifndef _ROS_std_msgs_Char_h
+#define _ROS_std_msgs_Char_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/char.h"
+
+namespace std_msgs
+{
+
+  class Char : public ros::Msg
+  {
+    public:
+      std_msgs::char data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->data.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->data.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Char"; };
+    const char * getMD5(){ return "1bf77f25acecdedba0e224b162199717"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/ColorRGBA.h b/quad2/arduino/src/ros_lib/std_msgs/ColorRGBA.h
new file mode 100644
index 0000000..49c72f4
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/ColorRGBA.h
@@ -0,0 +1,122 @@
+#ifndef _ROS_std_msgs_ColorRGBA_h
+#define _ROS_std_msgs_ColorRGBA_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class ColorRGBA : public ros::Msg
+  {
+    public:
+      float r;
+      float g;
+      float b;
+      float a;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        float real;
+        uint32_t base;
+      } u_r;
+      u_r.real = this->r;
+      *(outbuffer + offset + 0) = (u_r.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_r.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_r.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_r.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->r);
+      union {
+        float real;
+        uint32_t base;
+      } u_g;
+      u_g.real = this->g;
+      *(outbuffer + offset + 0) = (u_g.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_g.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_g.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_g.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->g);
+      union {
+        float real;
+        uint32_t base;
+      } u_b;
+      u_b.real = this->b;
+      *(outbuffer + offset + 0) = (u_b.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_b.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_b.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_b.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->b);
+      union {
+        float real;
+        uint32_t base;
+      } u_a;
+      u_a.real = this->a;
+      *(outbuffer + offset + 0) = (u_a.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_a.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_a.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_a.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->a);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        float real;
+        uint32_t base;
+      } u_r;
+      u_r.base = 0;
+      u_r.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_r.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_r.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_r.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->r = u_r.real;
+      offset += sizeof(this->r);
+      union {
+        float real;
+        uint32_t base;
+      } u_g;
+      u_g.base = 0;
+      u_g.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_g.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_g.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_g.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->g = u_g.real;
+      offset += sizeof(this->g);
+      union {
+        float real;
+        uint32_t base;
+      } u_b;
+      u_b.base = 0;
+      u_b.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_b.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_b.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_b.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->b = u_b.real;
+      offset += sizeof(this->b);
+      union {
+        float real;
+        uint32_t base;
+      } u_a;
+      u_a.base = 0;
+      u_a.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_a.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_a.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_a.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->a = u_a.real;
+      offset += sizeof(this->a);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/ColorRGBA"; };
+    const char * getMD5(){ return "a29a96539573343b1310c73607334b00"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Duration.h b/quad2/arduino/src/ros_lib/std_msgs/Duration.h
new file mode 100644
index 0000000..944e94c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Duration.h
@@ -0,0 +1,56 @@
+#ifndef _ROS_std_msgs_Duration_h
+#define _ROS_std_msgs_Duration_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "ros/duration.h"
+
+namespace std_msgs
+{
+
+  class Duration : public ros::Msg
+  {
+    public:
+      ros::Duration data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->data.sec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->data.sec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->data.sec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->data.sec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data.sec);
+      *(outbuffer + offset + 0) = (this->data.nsec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->data.nsec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->data.nsec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->data.nsec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data.nsec);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->data.sec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->data.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->data.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->data.sec);
+      this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->data.nsec);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Duration"; };
+    const char * getMD5(){ return "3e286caf4241d664e55f3ad380e2ae46"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Empty.h b/quad2/arduino/src/ros_lib/std_msgs/Empty.h
new file mode 100644
index 0000000..5b31e1c
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Empty.h
@@ -0,0 +1,34 @@
+#ifndef _ROS_std_msgs_Empty_h
+#define _ROS_std_msgs_Empty_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class Empty : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Empty"; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Float32.h b/quad2/arduino/src/ros_lib/std_msgs/Float32.h
new file mode 100644
index 0000000..a653732
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Float32.h
@@ -0,0 +1,56 @@
+#ifndef _ROS_std_msgs_Float32_h
+#define _ROS_std_msgs_Float32_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class Float32 : public ros::Msg
+  {
+    public:
+      float data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        float real;
+        uint32_t base;
+      } u_data;
+      u_data.real = this->data;
+      *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        float real;
+        uint32_t base;
+      } u_data;
+      u_data.base = 0;
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->data = u_data.real;
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Float32"; };
+    const char * getMD5(){ return "73fcbf46b49191e672908e50842a83d4"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Float32MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/Float32MultiArray.h
new file mode 100644
index 0000000..4d14679
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Float32MultiArray.h
@@ -0,0 +1,76 @@
+#ifndef _ROS_std_msgs_Float32MultiArray_h
+#define _ROS_std_msgs_Float32MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class Float32MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      float st_data;
+      float * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_datai;
+      u_datai.real = this->data[i];
+      *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (float*)realloc(this->data, data_lengthT * sizeof(float));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        float real;
+        uint32_t base;
+      } u_st_data;
+      u_st_data.base = 0;
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_data = u_st_data.real;
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(float));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Float32MultiArray"; };
+    const char * getMD5(){ return "6a40e0ffa6a17a503ac3f8616991b1f6"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Float64.h b/quad2/arduino/src/ros_lib/std_msgs/Float64.h
new file mode 100644
index 0000000..48b02a8
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Float64.h
@@ -0,0 +1,60 @@
+#ifndef _ROS_std_msgs_Float64_h
+#define _ROS_std_msgs_Float64_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class Float64 : public ros::Msg
+  {
+    public:
+      float data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      int32_t * val_data = (long *) &(this->data);
+      int32_t exp_data = (((*val_data)>>23)&255);
+      if(exp_data != 0)
+        exp_data += 1023-127;
+      int32_t sig_data = *val_data;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_data<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_data>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_data>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_data<<4) & 0xF0) | ((sig_data>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_data>>4) & 0x7F;
+      if(this->data < 0) *(outbuffer + offset -1) |= 0x80;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t * val_data = (uint32_t*) &(this->data);
+      offset += 3;
+      *val_data = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_data |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_data |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_data |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_data = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_data |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_data !=0)
+        *val_data |= ((exp_data)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->data = -this->data;
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Float64"; };
+    const char * getMD5(){ return "fdb28210bfa9d7c91146260178d9a584"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Float64MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/Float64MultiArray.h
new file mode 100644
index 0000000..293f6a2
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Float64MultiArray.h
@@ -0,0 +1,80 @@
+#ifndef _ROS_std_msgs_Float64MultiArray_h
+#define _ROS_std_msgs_Float64MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class Float64MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      float st_data;
+      float * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      int32_t * val_datai = (long *) &(this->data[i]);
+      int32_t exp_datai = (((*val_datai)>>23)&255);
+      if(exp_datai != 0)
+        exp_datai += 1023-127;
+      int32_t sig_datai = *val_datai;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = (sig_datai<<5) & 0xff;
+      *(outbuffer + offset++) = (sig_datai>>3) & 0xff;
+      *(outbuffer + offset++) = (sig_datai>>11) & 0xff;
+      *(outbuffer + offset++) = ((exp_datai<<4) & 0xF0) | ((sig_datai>>19)&0x0F);
+      *(outbuffer + offset++) = (exp_datai>>4) & 0x7F;
+      if(this->data[i] < 0) *(outbuffer + offset -1) |= 0x80;
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (float*)realloc(this->data, data_lengthT * sizeof(float));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      uint32_t * val_st_data = (uint32_t*) &(this->st_data);
+      offset += 3;
+      *val_st_data = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
+      *val_st_data |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
+      *val_st_data |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
+      *val_st_data |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
+      uint32_t exp_st_data = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
+      exp_st_data |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
+      if(exp_st_data !=0)
+        *val_st_data |= ((exp_st_data)-1023+127)<<23;
+      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_data = -this->st_data;
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(float));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Float64MultiArray"; };
+    const char * getMD5(){ return "4b7d974086d4060e7db4613a7e6c3ba4"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Header.h b/quad2/arduino/src/ros_lib/std_msgs/Header.h
new file mode 100644
index 0000000..043b662
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Header.h
@@ -0,0 +1,81 @@
+#ifndef _ROS_std_msgs_Header_h
+#define _ROS_std_msgs_Header_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "ros/time.h"
+
+namespace std_msgs
+{
+
+  class Header : public ros::Msg
+  {
+    public:
+      uint32_t seq;
+      ros::Time stamp;
+      char * frame_id;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->seq >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->seq >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->seq >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->seq >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->seq);
+      *(outbuffer + offset + 0) = (this->stamp.sec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->stamp.sec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->stamp.sec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->stamp.sec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->stamp.sec);
+      *(outbuffer + offset + 0) = (this->stamp.nsec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->stamp.nsec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->stamp.nsec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->stamp.nsec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->stamp.nsec);
+      uint32_t * length_frame_id = (uint32_t *)(outbuffer + offset);
+      *length_frame_id = strlen( (const char*) this->frame_id);
+      offset += 4;
+      memcpy(outbuffer + offset, this->frame_id, *length_frame_id);
+      offset += *length_frame_id;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->seq |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->seq |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->seq |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->seq |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->seq);
+      this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->stamp.sec);
+      this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->stamp.nsec);
+      uint32_t length_frame_id = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_frame_id; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_frame_id-1]=0;
+      this->frame_id = (char *)(inbuffer + offset-1);
+      offset += length_frame_id;
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Header"; };
+    const char * getMD5(){ return "2176decaecbce78abc3b96ef049fabed"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Int16.h b/quad2/arduino/src/ros_lib/std_msgs/Int16.h
new file mode 100644
index 0000000..43e2716
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Int16.h
@@ -0,0 +1,52 @@
+#ifndef _ROS_std_msgs_Int16_h
+#define _ROS_std_msgs_Int16_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class Int16 : public ros::Msg
+  {
+    public:
+      int16_t data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_data;
+      u_data.real = this->data;
+      *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_data;
+      u_data.base = 0;
+      u_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->data = u_data.real;
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Int16"; };
+    const char * getMD5(){ return "8524586e34fbd7cb1c08c5f5f1ca0e57"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Int16MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/Int16MultiArray.h
new file mode 100644
index 0000000..991a5f0
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Int16MultiArray.h
@@ -0,0 +1,72 @@
+#ifndef _ROS_std_msgs_Int16MultiArray_h
+#define _ROS_std_msgs_Int16MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class Int16MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      int16_t st_data;
+      int16_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_datai;
+      u_datai.real = this->data[i];
+      *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (int16_t*)realloc(this->data, data_lengthT * sizeof(int16_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_st_data;
+      u_st_data.base = 0;
+      u_st_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->st_data = u_st_data.real;
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(int16_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Int16MultiArray"; };
+    const char * getMD5(){ return "d9338d7f523fcb692fae9d0a0e9f067c"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Int32.h b/quad2/arduino/src/ros_lib/std_msgs/Int32.h
new file mode 100644
index 0000000..c0769c7
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Int32.h
@@ -0,0 +1,56 @@
+#ifndef _ROS_std_msgs_Int32_h
+#define _ROS_std_msgs_Int32_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class Int32 : public ros::Msg
+  {
+    public:
+      int32_t data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_data;
+      u_data.real = this->data;
+      *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_data;
+      u_data.base = 0;
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->data = u_data.real;
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Int32"; };
+    const char * getMD5(){ return "da5909fbe378aeaf85e547e830cc1bb7"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Int32MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/Int32MultiArray.h
new file mode 100644
index 0000000..e749933
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Int32MultiArray.h
@@ -0,0 +1,76 @@
+#ifndef _ROS_std_msgs_Int32MultiArray_h
+#define _ROS_std_msgs_Int32MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class Int32MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      int32_t st_data;
+      int32_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_datai;
+      u_datai.real = this->data[i];
+      *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (int32_t*)realloc(this->data, data_lengthT * sizeof(int32_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int32_t real;
+        uint32_t base;
+      } u_st_data;
+      u_st_data.base = 0;
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_data = u_st_data.real;
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(int32_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Int32MultiArray"; };
+    const char * getMD5(){ return "1d99f79f8b325b44fee908053e9c945b"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Int64.h b/quad2/arduino/src/ros_lib/std_msgs/Int64.h
new file mode 100644
index 0000000..59879eb
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Int64.h
@@ -0,0 +1,56 @@
+#ifndef _ROS_std_msgs_Int64_h
+#define _ROS_std_msgs_Int64_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class Int64 : public ros::Msg
+  {
+    public:
+      int64_t data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        int64_t real;
+        uint32_t base;
+      } u_data;
+      u_data.real = this->data;
+      *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        int64_t real;
+        uint32_t base;
+      } u_data;
+      u_data.base = 0;
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->data = u_data.real;
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Int64"; };
+    const char * getMD5(){ return "34add168574510e6e17f5d23ecc077ef"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Int64MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/Int64MultiArray.h
new file mode 100644
index 0000000..b9dbfbf
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Int64MultiArray.h
@@ -0,0 +1,76 @@
+#ifndef _ROS_std_msgs_Int64MultiArray_h
+#define _ROS_std_msgs_Int64MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class Int64MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      int64_t st_data;
+      int64_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int64_t real;
+        uint32_t base;
+      } u_datai;
+      u_datai.real = this->data[i];
+      *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (int64_t*)realloc(this->data, data_lengthT * sizeof(int64_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int64_t real;
+        uint32_t base;
+      } u_st_data;
+      u_st_data.base = 0;
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_data = u_st_data.real;
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(int64_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Int64MultiArray"; };
+    const char * getMD5(){ return "54865aa6c65be0448113a2afc6a49270"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Int8.h b/quad2/arduino/src/ros_lib/std_msgs/Int8.h
new file mode 100644
index 0000000..a1615ef
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Int8.h
@@ -0,0 +1,50 @@
+#ifndef _ROS_std_msgs_Int8_h
+#define _ROS_std_msgs_Int8_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class Int8 : public ros::Msg
+  {
+    public:
+      int8_t data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        int8_t real;
+        uint8_t base;
+      } u_data;
+      u_data.real = this->data;
+      *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        int8_t real;
+        uint8_t base;
+      } u_data;
+      u_data.base = 0;
+      u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data = u_data.real;
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Int8"; };
+    const char * getMD5(){ return "27ffa0c9c4b8fb8492252bcad9e5c57b"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Int8MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/Int8MultiArray.h
new file mode 100644
index 0000000..704a8d0
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Int8MultiArray.h
@@ -0,0 +1,70 @@
+#ifndef _ROS_std_msgs_Int8MultiArray_h
+#define _ROS_std_msgs_Int8MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class Int8MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      int8_t st_data;
+      int8_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int8_t real;
+        uint8_t base;
+      } u_datai;
+      u_datai.real = this->data[i];
+      *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (int8_t*)realloc(this->data, data_lengthT * sizeof(int8_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        int8_t real;
+        uint8_t base;
+      } u_st_data;
+      u_st_data.base = 0;
+      u_st_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->st_data = u_st_data.real;
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(int8_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Int8MultiArray"; };
+    const char * getMD5(){ return "d7c1af35a1b4781bbe79e03dd94b7c13"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/MultiArrayDimension.h b/quad2/arduino/src/ros_lib/std_msgs/MultiArrayDimension.h
new file mode 100644
index 0000000..1710b14
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/MultiArrayDimension.h
@@ -0,0 +1,70 @@
+#ifndef _ROS_std_msgs_MultiArrayDimension_h
+#define _ROS_std_msgs_MultiArrayDimension_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class MultiArrayDimension : public ros::Msg
+  {
+    public:
+      char * label;
+      uint32_t size;
+      uint32_t stride;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_label = (uint32_t *)(outbuffer + offset);
+      *length_label = strlen( (const char*) this->label);
+      offset += 4;
+      memcpy(outbuffer + offset, this->label, *length_label);
+      offset += *length_label;
+      *(outbuffer + offset + 0) = (this->size >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->size >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->size >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->size >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->size);
+      *(outbuffer + offset + 0) = (this->stride >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->stride >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->stride >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->stride >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->stride);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_label = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_label; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_label-1]=0;
+      this->label = (char *)(inbuffer + offset-1);
+      offset += length_label;
+      this->size |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->size |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->size |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->size |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->size);
+      this->stride |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->stride |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->stride |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->stride |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->stride);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/MultiArrayDimension"; };
+    const char * getMD5(){ return "4cd0c83a8683deae40ecdac60e53bfa8"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/MultiArrayLayout.h b/quad2/arduino/src/ros_lib/std_msgs/MultiArrayLayout.h
new file mode 100644
index 0000000..83d2c7b
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/MultiArrayLayout.h
@@ -0,0 +1,65 @@
+#ifndef _ROS_std_msgs_MultiArrayLayout_h
+#define _ROS_std_msgs_MultiArrayLayout_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayDimension.h"
+
+namespace std_msgs
+{
+
+  class MultiArrayLayout : public ros::Msg
+  {
+    public:
+      uint8_t dim_length;
+      std_msgs::MultiArrayDimension st_dim;
+      std_msgs::MultiArrayDimension * dim;
+      uint32_t data_offset;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset++) = dim_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < dim_length; i++){
+      offset += this->dim[i].serialize(outbuffer + offset);
+      }
+      *(outbuffer + offset + 0) = (this->data_offset >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->data_offset >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->data_offset >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->data_offset >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data_offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint8_t dim_lengthT = *(inbuffer + offset++);
+      if(dim_lengthT > dim_length)
+        this->dim = (std_msgs::MultiArrayDimension*)realloc(this->dim, dim_lengthT * sizeof(std_msgs::MultiArrayDimension));
+      offset += 3;
+      dim_length = dim_lengthT;
+      for( uint8_t i = 0; i < dim_length; i++){
+      offset += this->st_dim.deserialize(inbuffer + offset);
+        memcpy( &(this->dim[i]), &(this->st_dim), sizeof(std_msgs::MultiArrayDimension));
+      }
+      this->data_offset |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data_offset |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->data_offset |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->data_offset |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->data_offset);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/MultiArrayLayout"; };
+    const char * getMD5(){ return "0fed2a11c13e11c5571b4e2a995a91a3"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/String.h b/quad2/arduino/src/ros_lib/std_msgs/String.h
new file mode 100644
index 0000000..7e3d7ee
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/String.h
@@ -0,0 +1,48 @@
+#ifndef _ROS_std_msgs_String_h
+#define _ROS_std_msgs_String_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class String : public ros::Msg
+  {
+    public:
+      char * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_data = (uint32_t *)(outbuffer + offset);
+      *length_data = strlen( (const char*) this->data);
+      offset += 4;
+      memcpy(outbuffer + offset, this->data, *length_data);
+      offset += *length_data;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_data = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_data; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_data-1]=0;
+      this->data = (char *)(inbuffer + offset-1);
+      offset += length_data;
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/String"; };
+    const char * getMD5(){ return "992ce8a1687cec8c8bd883ec73ca41d1"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/Time.h b/quad2/arduino/src/ros_lib/std_msgs/Time.h
new file mode 100644
index 0000000..48997e4
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/Time.h
@@ -0,0 +1,56 @@
+#ifndef _ROS_std_msgs_Time_h
+#define _ROS_std_msgs_Time_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "ros/time.h"
+
+namespace std_msgs
+{
+
+  class Time : public ros::Msg
+  {
+    public:
+      ros::Time data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->data.sec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->data.sec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->data.sec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->data.sec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data.sec);
+      *(outbuffer + offset + 0) = (this->data.nsec >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->data.nsec >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->data.nsec >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->data.nsec >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data.nsec);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->data.sec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->data.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->data.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->data.sec);
+      this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->data.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->data.nsec);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/Time"; };
+    const char * getMD5(){ return "cd7166c74c552c311fbcc2fe5a7bc289"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/UInt16.h b/quad2/arduino/src/ros_lib/std_msgs/UInt16.h
new file mode 100644
index 0000000..41fd0a2
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/UInt16.h
@@ -0,0 +1,41 @@
+#ifndef _ROS_std_msgs_UInt16_h
+#define _ROS_std_msgs_UInt16_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class UInt16 : public ros::Msg
+  {
+    public:
+      uint16_t data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->data |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/UInt16"; };
+    const char * getMD5(){ return "1df79edf208b629fe6b81923a544552d"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/UInt16MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/UInt16MultiArray.h
new file mode 100644
index 0000000..9fc58ab
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/UInt16MultiArray.h
@@ -0,0 +1,61 @@
+#ifndef _ROS_std_msgs_UInt16MultiArray_h
+#define _ROS_std_msgs_UInt16MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class UInt16MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      uint16_t st_data;
+      uint16_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (uint16_t*)realloc(this->data, data_lengthT * sizeof(uint16_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      this->st_data |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->st_data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(uint16_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/UInt16MultiArray"; };
+    const char * getMD5(){ return "52f264f1c973c4b73790d384c6cb4484"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/UInt32.h b/quad2/arduino/src/ros_lib/std_msgs/UInt32.h
new file mode 100644
index 0000000..0ac94a4
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/UInt32.h
@@ -0,0 +1,45 @@
+#ifndef _ROS_std_msgs_UInt32_h
+#define _ROS_std_msgs_UInt32_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class UInt32 : public ros::Msg
+  {
+    public:
+      uint32_t data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->data |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/UInt32"; };
+    const char * getMD5(){ return "304a39449588c7f8ce2df6e8001c5fce"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/UInt32MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/UInt32MultiArray.h
new file mode 100644
index 0000000..b6f4205
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/UInt32MultiArray.h
@@ -0,0 +1,65 @@
+#ifndef _ROS_std_msgs_UInt32MultiArray_h
+#define _ROS_std_msgs_UInt32MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class UInt32MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      uint32_t st_data;
+      uint32_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (this->data[i] >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (this->data[i] >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (this->data[i] >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (uint32_t*)realloc(this->data, data_lengthT * sizeof(uint32_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      this->st_data |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      this->st_data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->st_data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      this->st_data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(uint32_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/UInt32MultiArray"; };
+    const char * getMD5(){ return "4d6a180abc9be191b96a7eda6c8a233d"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/UInt64.h b/quad2/arduino/src/ros_lib/std_msgs/UInt64.h
new file mode 100644
index 0000000..02468f0
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/UInt64.h
@@ -0,0 +1,56 @@
+#ifndef _ROS_std_msgs_UInt64_h
+#define _ROS_std_msgs_UInt64_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class UInt64 : public ros::Msg
+  {
+    public:
+      uint64_t data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        uint64_t real;
+        uint32_t base;
+      } u_data;
+      u_data.real = this->data;
+      *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        uint64_t real;
+        uint32_t base;
+      } u_data;
+      u_data.base = 0;
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->data = u_data.real;
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/UInt64"; };
+    const char * getMD5(){ return "1b2a79973e8bf53d7b53acb71299cb57"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/UInt64MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/UInt64MultiArray.h
new file mode 100644
index 0000000..c562c66
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/UInt64MultiArray.h
@@ -0,0 +1,76 @@
+#ifndef _ROS_std_msgs_UInt64MultiArray_h
+#define _ROS_std_msgs_UInt64MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class UInt64MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      uint64_t st_data;
+      uint64_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        uint64_t real;
+        uint32_t base;
+      } u_datai;
+      u_datai.real = this->data[i];
+      *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_datai.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_datai.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_datai.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (uint64_t*)realloc(this->data, data_lengthT * sizeof(uint64_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      union {
+        uint64_t real;
+        uint32_t base;
+      } u_st_data;
+      u_st_data.base = 0;
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_st_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->st_data = u_st_data.real;
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(uint64_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/UInt64MultiArray"; };
+    const char * getMD5(){ return "6088f127afb1d6c72927aa1247e945af"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/UInt8.h b/quad2/arduino/src/ros_lib/std_msgs/UInt8.h
new file mode 100644
index 0000000..5a70588
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/UInt8.h
@@ -0,0 +1,39 @@
+#ifndef _ROS_std_msgs_UInt8_h
+#define _ROS_std_msgs_UInt8_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace std_msgs
+{
+
+  class UInt8 : public ros::Msg
+  {
+    public:
+      uint8_t data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      this->data |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->data);
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/UInt8"; };
+    const char * getMD5(){ return "7c8164229e7d2c17eb95e9231617fdee"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/std_msgs/UInt8MultiArray.h b/quad2/arduino/src/ros_lib/std_msgs/UInt8MultiArray.h
new file mode 100644
index 0000000..bbc97e4
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/std_msgs/UInt8MultiArray.h
@@ -0,0 +1,59 @@
+#ifndef _ROS_std_msgs_UInt8MultiArray_h
+#define _ROS_std_msgs_UInt8MultiArray_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/MultiArrayLayout.h"
+
+namespace std_msgs
+{
+
+  class UInt8MultiArray : public ros::Msg
+  {
+    public:
+      std_msgs::MultiArrayLayout layout;
+      uint8_t data_length;
+      uint8_t st_data;
+      uint8_t * data;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->layout.serialize(outbuffer + offset);
+      *(outbuffer + offset++) = data_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < data_length; i++){
+      *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF;
+      offset += sizeof(this->data[i]);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->layout.deserialize(inbuffer + offset);
+      uint8_t data_lengthT = *(inbuffer + offset++);
+      if(data_lengthT > data_length)
+        this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t));
+      offset += 3;
+      data_length = data_lengthT;
+      for( uint8_t i = 0; i < data_length; i++){
+      this->st_data |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      offset += sizeof(this->st_data);
+        memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "std_msgs/UInt8MultiArray"; };
+    const char * getMD5(){ return "82373f1612381bb6ee473b5cd6f5d89c"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/tests/array_test/array_test.pde b/quad2/arduino/src/ros_lib/tests/array_test/array_test.pde
new file mode 100644
index 0000000..8aa72de
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/tests/array_test/array_test.pde
@@ -0,0 +1,49 @@
+/* 
+ * rosserial::geometry_msgs::PoseArray Test
+ * Sums an array, publishes sum 
+ */
+
+#include <ros.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseArray.h>
+
+
+ros::NodeHandle nh;
+
+
+bool set_; 
+
+
+geometry_msgs::Pose sum_msg;
+ros::Publisher p("sum", &sum_msg);
+
+void messageCb(const geometry_msgs::PoseArray& msg){
+  sum_msg.position.x = 0;
+  sum_msg.position.y = 0;
+  sum_msg.position.z = 0;
+  for(int i = 0; i < msg.poses_length; i++)
+  {
+    sum_msg.position.x += msg.poses[i].position.x;
+    sum_msg.position.y += msg.poses[i].position.y;
+    sum_msg.position.z += msg.poses[i].position.z;
+  }
+  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
+}
+
+ros::Subscriber<geometry_msgs::PoseArray> s("poses",messageCb);
+
+void setup()
+{ 
+  pinMode(13, OUTPUT);
+  nh.initNode();
+  nh.subscribe(s);
+  nh.advertise(p);
+}
+
+void loop()
+{  
+  p.publish(&sum_msg);
+  nh.spinOnce();
+  delay(10);
+}
+
diff --git a/quad2/arduino/src/ros_lib/tests/float64_test/float64_test.pde b/quad2/arduino/src/ros_lib/tests/float64_test/float64_test.pde
new file mode 100644
index 0000000..41a6f4a
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/tests/float64_test/float64_test.pde
@@ -0,0 +1,38 @@
+/* 
+ * rosserial::std_msgs::Float64 Test
+ * Receives a Float64 input, subtracts 1.0, and publishes it
+ */
+
+#include <ros.h>
+#include <std_msgs/Float64.h>
+
+
+ros::NodeHandle nh;
+
+float x; 
+
+void messageCb( const std_msgs::Float64& msg){
+  x = msg.data - 1.0;
+  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
+}
+
+std_msgs::Float64 test;
+ros::Subscriber<std_msgs::Float64> s("your_topic", &messageCb);
+ros::Publisher p("my_topic", &test);
+
+void setup()
+{
+  pinMode(13, OUTPUT);
+  nh.initNode();
+  nh.advertise(p);
+  nh.subscribe(s);
+}
+
+void loop()
+{
+  test.data = x;
+  p.publish( &test );
+  nh.spinOnce();
+  delay(10);
+}
+
diff --git a/quad2/arduino/src/ros_lib/tests/time_test/time_test.pde b/quad2/arduino/src/ros_lib/tests/time_test/time_test.pde
new file mode 100644
index 0000000..c5fa739
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/tests/time_test/time_test.pde
@@ -0,0 +1,30 @@
+/* 
+ * rosserial::std_msgs::Time Test
+ * Publishes current time
+ */
+
+#include <ros.h>
+#include <ros/time.h>
+#include <std_msgs/Time.h>
+
+
+ros::NodeHandle  nh;
+
+std_msgs::Time test;
+ros::Publisher p("my_topic", &test);
+
+void setup()
+{
+  pinMode(13, OUTPUT);
+  nh.initNode();
+  nh.advertise(p);
+}
+
+void loop()
+{  
+  test.data = nh.now();
+  p.publish( &test );
+  nh.spinOnce();
+  delay(10);
+}
+
diff --git a/quad2/arduino/src/ros_lib/tf/FrameGraph.h b/quad2/arduino/src/ros_lib/tf/FrameGraph.h
new file mode 100644
index 0000000..8cba3c9
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/tf/FrameGraph.h
@@ -0,0 +1,76 @@
+#ifndef _ROS_SERVICE_FrameGraph_h
+#define _ROS_SERVICE_FrameGraph_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace tf
+{
+
+static const char FRAMEGRAPH[] = "tf/FrameGraph";
+
+  class FrameGraphRequest : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return FRAMEGRAPH; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class FrameGraphResponse : public ros::Msg
+  {
+    public:
+      char * dot_graph;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_dot_graph = (uint32_t *)(outbuffer + offset);
+      *length_dot_graph = strlen( (const char*) this->dot_graph);
+      offset += 4;
+      memcpy(outbuffer + offset, this->dot_graph, *length_dot_graph);
+      offset += *length_dot_graph;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_dot_graph = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_dot_graph; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_dot_graph-1]=0;
+      this->dot_graph = (char *)(inbuffer + offset-1);
+      offset += length_dot_graph;
+     return offset;
+    }
+
+    const char * getType(){ return FRAMEGRAPH; };
+    const char * getMD5(){ return "c4af9ac907e58e906eb0b6e3c58478c0"; };
+
+  };
+
+  class FrameGraph {
+    public:
+    typedef FrameGraphRequest Request;
+    typedef FrameGraphResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/tf/tf.h b/quad2/arduino/src/ros_lib/tf/tf.h
new file mode 100644
index 0000000..c84c127
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/tf/tf.h
@@ -0,0 +1,56 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROS_TF_H_
+#define ROS_TF_H_
+
+#include "geometry_msgs/TransformStamped.h"
+
+namespace tf
+{
+  
+  geometry_msgs::Quaternion createQuaternionFromYaw(double yaw)
+  {
+    geometry_msgs::Quaternion q;
+    q.x = 0;
+    q.y = 0;
+    q.z = sin(yaw * 0.5);
+    q.w = cos(yaw * 0.5);
+    return q;
+  }
+
+}
+
+#endif
+
diff --git a/quad2/arduino/src/ros_lib/tf/tfMessage.h b/quad2/arduino/src/ros_lib/tf/tfMessage.h
new file mode 100644
index 0000000..683f792
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/tf/tfMessage.h
@@ -0,0 +1,54 @@
+#ifndef _ROS_tf_tfMessage_h
+#define _ROS_tf_tfMessage_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "geometry_msgs/TransformStamped.h"
+
+namespace tf
+{
+
+  class tfMessage : public ros::Msg
+  {
+    public:
+      uint8_t transforms_length;
+      geometry_msgs::TransformStamped st_transforms;
+      geometry_msgs::TransformStamped * transforms;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset++) = transforms_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < transforms_length; i++){
+      offset += this->transforms[i].serialize(outbuffer + offset);
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint8_t transforms_lengthT = *(inbuffer + offset++);
+      if(transforms_lengthT > transforms_length)
+        this->transforms = (geometry_msgs::TransformStamped*)realloc(this->transforms, transforms_lengthT * sizeof(geometry_msgs::TransformStamped));
+      offset += 3;
+      transforms_length = transforms_lengthT;
+      for( uint8_t i = 0; i < transforms_length; i++){
+      offset += this->st_transforms.deserialize(inbuffer + offset);
+        memcpy( &(this->transforms[i]), &(this->st_transforms), sizeof(geometry_msgs::TransformStamped));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return "tf/tfMessage"; };
+    const char * getMD5(){ return "94810edda583a504dfda3829e70d7eec"; };
+
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/tf/transform_broadcaster.h b/quad2/arduino/src/ros_lib/tf/transform_broadcaster.h
new file mode 100644
index 0000000..52165e0
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/tf/transform_broadcaster.h
@@ -0,0 +1,68 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROS_TRANSFORM_BROADCASTER_H_
+#define ROS_TRANSFORM_BROADCASTER_H_
+
+#include "tfMessage.h"
+
+namespace tf
+{
+
+  class TransformBroadcaster
+  {
+    public:
+      TransformBroadcaster() : publisher_("tf", &internal_msg) {}
+
+      void init(ros::NodeHandle &nh)
+      {
+        nh.advertise(publisher_);
+      }
+
+      void sendTransform(geometry_msgs::TransformStamped &transform)
+      {
+        internal_msg.transforms_length = 1;
+        internal_msg.transforms = &transform;
+        publisher_.publish(&internal_msg);
+      }
+
+    private:
+      tf::tfMessage internal_msg;
+      ros::Publisher publisher_;
+  };
+
+}
+
+#endif
+
diff --git a/quad2/arduino/src/ros_lib/time.cpp b/quad2/arduino/src/ros_lib/time.cpp
new file mode 100644
index 0000000..9b3f10d
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/time.cpp
@@ -0,0 +1,70 @@
+/* 
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "ros.h"
+#include "ros/time.h"
+
+namespace ros
+{
+  void normalizeSecNSec(unsigned long& sec, unsigned long& nsec){
+    unsigned long nsec_part= nsec % 1000000000UL;
+    unsigned long sec_part = nsec / 1000000000UL;
+    sec += sec_part;
+    nsec = nsec_part;
+  }
+
+  Time& Time::fromNSec(long t)
+  {
+    sec = t / 1000000000;
+    nsec = t % 1000000000;
+    normalizeSecNSec(sec, nsec);
+    return *this;
+  }
+
+  Time& Time::operator +=(const Duration &rhs)
+  {
+    sec += rhs.sec;
+    nsec += rhs.nsec;
+    normalizeSecNSec(sec, nsec);
+    return *this; 
+  }
+
+  Time& Time::operator -=(const Duration &rhs){
+    sec += -rhs.sec;
+    nsec += -rhs.nsec;
+    normalizeSecNSec(sec, nsec);
+    return *this;
+  }
+
+}
diff --git a/quad2/arduino/src/ros_lib/topic_tools/MuxAdd.h b/quad2/arduino/src/ros_lib/topic_tools/MuxAdd.h
new file mode 100644
index 0000000..d4bcecd
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/topic_tools/MuxAdd.h
@@ -0,0 +1,76 @@
+#ifndef _ROS_SERVICE_MuxAdd_h
+#define _ROS_SERVICE_MuxAdd_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace topic_tools
+{
+
+static const char MUXADD[] = "topic_tools/MuxAdd";
+
+  class MuxAddRequest : public ros::Msg
+  {
+    public:
+      char * topic;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_topic = (uint32_t *)(outbuffer + offset);
+      *length_topic = strlen( (const char*) this->topic);
+      offset += 4;
+      memcpy(outbuffer + offset, this->topic, *length_topic);
+      offset += *length_topic;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_topic = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_topic; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_topic-1]=0;
+      this->topic = (char *)(inbuffer + offset-1);
+      offset += length_topic;
+     return offset;
+    }
+
+    const char * getType(){ return MUXADD; };
+    const char * getMD5(){ return "d8f94bae31b356b24d0427f80426d0c3"; };
+
+  };
+
+  class MuxAddResponse : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return MUXADD; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class MuxAdd {
+    public:
+    typedef MuxAddRequest Request;
+    typedef MuxAddResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/topic_tools/MuxDelete.h b/quad2/arduino/src/ros_lib/topic_tools/MuxDelete.h
new file mode 100644
index 0000000..9ffed21
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/topic_tools/MuxDelete.h
@@ -0,0 +1,76 @@
+#ifndef _ROS_SERVICE_MuxDelete_h
+#define _ROS_SERVICE_MuxDelete_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace topic_tools
+{
+
+static const char MUXDELETE[] = "topic_tools/MuxDelete";
+
+  class MuxDeleteRequest : public ros::Msg
+  {
+    public:
+      char * topic;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_topic = (uint32_t *)(outbuffer + offset);
+      *length_topic = strlen( (const char*) this->topic);
+      offset += 4;
+      memcpy(outbuffer + offset, this->topic, *length_topic);
+      offset += *length_topic;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_topic = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_topic; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_topic-1]=0;
+      this->topic = (char *)(inbuffer + offset-1);
+      offset += length_topic;
+     return offset;
+    }
+
+    const char * getType(){ return MUXDELETE; };
+    const char * getMD5(){ return "d8f94bae31b356b24d0427f80426d0c3"; };
+
+  };
+
+  class MuxDeleteResponse : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return MUXDELETE; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class MuxDelete {
+    public:
+    typedef MuxDeleteRequest Request;
+    typedef MuxDeleteResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/topic_tools/MuxList.h b/quad2/arduino/src/ros_lib/topic_tools/MuxList.h
new file mode 100644
index 0000000..83647e2
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/topic_tools/MuxList.h
@@ -0,0 +1,92 @@
+#ifndef _ROS_SERVICE_MuxList_h
+#define _ROS_SERVICE_MuxList_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace topic_tools
+{
+
+static const char MUXLIST[] = "topic_tools/MuxList";
+
+  class MuxListRequest : public ros::Msg
+  {
+    public:
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+     return offset;
+    }
+
+    const char * getType(){ return MUXLIST; };
+    const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
+
+  };
+
+  class MuxListResponse : public ros::Msg
+  {
+    public:
+      uint8_t topics_length;
+      char* st_topics;
+      char* * topics;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      *(outbuffer + offset++) = topics_length;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      *(outbuffer + offset++) = 0;
+      for( uint8_t i = 0; i < topics_length; i++){
+      uint32_t * length_topicsi = (uint32_t *)(outbuffer + offset);
+      *length_topicsi = strlen( (const char*) this->topics[i]);
+      offset += 4;
+      memcpy(outbuffer + offset, this->topics[i], *length_topicsi);
+      offset += *length_topicsi;
+      }
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint8_t topics_lengthT = *(inbuffer + offset++);
+      if(topics_lengthT > topics_length)
+        this->topics = (char**)realloc(this->topics, topics_lengthT * sizeof(char*));
+      offset += 3;
+      topics_length = topics_lengthT;
+      for( uint8_t i = 0; i < topics_length; i++){
+      uint32_t length_st_topics = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_st_topics; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_st_topics-1]=0;
+      this->st_topics = (char *)(inbuffer + offset-1);
+      offset += length_st_topics;
+        memcpy( &(this->topics[i]), &(this->st_topics), sizeof(char*));
+      }
+     return offset;
+    }
+
+    const char * getType(){ return MUXLIST; };
+    const char * getMD5(){ return "b0eef9a05d4e829092fc2f2c3c2aad3d"; };
+
+  };
+
+  class MuxList {
+    public:
+    typedef MuxListRequest Request;
+    typedef MuxListResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
diff --git a/quad2/arduino/src/ros_lib/topic_tools/MuxSelect.h b/quad2/arduino/src/ros_lib/topic_tools/MuxSelect.h
new file mode 100644
index 0000000..dc0e975
--- /dev/null
+++ b/quad2/arduino/src/ros_lib/topic_tools/MuxSelect.h
@@ -0,0 +1,90 @@
+#ifndef _ROS_SERVICE_MuxSelect_h
+#define _ROS_SERVICE_MuxSelect_h
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace topic_tools
+{
+
+static const char MUXSELECT[] = "topic_tools/MuxSelect";
+
+  class MuxSelectRequest : public ros::Msg
+  {
+    public:
+      char * topic;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_topic = (uint32_t *)(outbuffer + offset);
+      *length_topic = strlen( (const char*) this->topic);
+      offset += 4;
+      memcpy(outbuffer + offset, this->topic, *length_topic);
+      offset += *length_topic;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_topic = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_topic; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_topic-1]=0;
+      this->topic = (char *)(inbuffer + offset-1);
+      offset += length_topic;
+     return offset;
+    }
+
+    const char * getType(){ return MUXSELECT; };
+    const char * getMD5(){ return "d8f94bae31b356b24d0427f80426d0c3"; };
+
+  };
+
+  class MuxSelectResponse : public ros::Msg
+  {
+    public:
+      char * prev_topic;
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      uint32_t * length_prev_topic = (uint32_t *)(outbuffer + offset);
+      *length_prev_topic = strlen( (const char*) this->prev_topic);
+      offset += 4;
+      memcpy(outbuffer + offset, this->prev_topic, *length_prev_topic);
+      offset += *length_prev_topic;
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      uint32_t length_prev_topic = *(uint32_t *)(inbuffer + offset);
+      offset += 4;
+      for(unsigned int k= offset; k< offset+length_prev_topic; ++k){
+          inbuffer[k-1]=inbuffer[k];
+      }
+      inbuffer[offset+length_prev_topic-1]=0;
+      this->prev_topic = (char *)(inbuffer + offset-1);
+      offset += length_prev_topic;
+     return offset;
+    }
+
+    const char * getType(){ return MUXSELECT; };
+    const char * getMD5(){ return "3db0a473debdbafea387c9e49358c320"; };
+
+  };
+
+  class MuxSelect {
+    public:
+    typedef MuxSelectRequest Request;
+    typedef MuxSelectResponse Response;
+  };
+
+}
+#endif
\ No newline at end of file
