arduino代码

libraries/Rosserial_Arduino_Library/examples/ADC/ADC.pde

@@ -0,0 +1,52 @@
+/* 
+ * 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.
+ */
+
+#if (ARDUINO >= 100)
+ #include <Arduino.h>
+#else
+ #include <WProgram.h>
+#endif
+#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();
+}
+

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

libraries/Rosserial_Arduino_Library/examples/BlinkM/BlinkM.pde

@@ -0,0 +1,162 @@
+/*
+*  RosSerial BlinkM Example
+*  This program shows how to control a blinkm
+*  from an arduino using RosSerial
+*/
+
+#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);
+}
+

libraries/Rosserial_Arduino_Library/examples/BlinkM/BlinkM_funcs.h

@@ -0,0 +1,440 @@
+/*
+ * 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 <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, 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, 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.write(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.read();
+    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.write('A');
+  Wire.write(newaddress);
+  Wire.write(0xD0);
+  Wire.write(0x0D);  // dood!
+  Wire.write(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.write('a');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)1);  // general call
+  if( Wire.available() ) {
+    byte b = Wire.read();
+    return b;
+  }
+  return -1;
+}
+
+// Gets the BlinkM firmware version
+static int BlinkM_getVersion(byte addr)
+{
+  Wire.beginTransmission(addr);
+  Wire.write('Z');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)2);
+  if( Wire.available() ) {
+    byte major_ver = Wire.read();
+    byte minor_ver = Wire.read();
+    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.write('f');
+  Wire.write(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.write('t');
+  Wire.write(timeadj);
+  Wire.endTransmission();  
+}
+
+// Fades to an RGB color
+static void BlinkM_fadeToRGB(byte addr, byte red, byte grn, byte blu)
+{
+  Wire.beginTransmission(addr);
+  Wire.write('c');
+  Wire.write(red);
+  Wire.write(grn);
+  Wire.write(blu);
+  Wire.endTransmission();
+}
+
+// Fades to an HSB color
+static void BlinkM_fadeToHSB(byte addr, byte hue, byte saturation, byte brightness)
+{
+  Wire.beginTransmission(addr);
+  Wire.write('h');
+  Wire.write(hue);
+  Wire.write(saturation);
+  Wire.write(brightness);
+  Wire.endTransmission();
+}
+
+// Sets an RGB color immediately
+static void BlinkM_setRGB(byte addr, byte red, byte grn, byte blu)
+{
+  Wire.beginTransmission(addr);
+  Wire.write('n');
+  Wire.write(red);
+  Wire.write(grn);
+  Wire.write(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.write('C');
+  Wire.write(rrnd);
+  Wire.write(grnd);
+  Wire.write(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.write('H');
+  Wire.write(hrnd);
+  Wire.write(srnd);
+  Wire.write(brnd);
+  Wire.endTransmission();
+}
+
+//
+static void BlinkM_getRGBColor(byte addr, byte* r, byte* g, byte* b)
+{
+  Wire.beginTransmission(addr);
+  Wire.write('g');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)3);
+  if( Wire.available() ) {
+    *r = Wire.read();
+    *g = Wire.read();
+    *b = Wire.read();
+  }
+}
+
+//
+static void BlinkM_playScript(byte addr, byte script_id, byte reps, byte pos)
+{
+  Wire.beginTransmission(addr);
+  Wire.write('p');
+  Wire.write(script_id);
+  Wire.write(reps);
+  Wire.write(pos);
+  Wire.endTransmission();
+}
+
+//
+static void BlinkM_stopScript(byte addr)
+{
+  Wire.beginTransmission(addr);
+  Wire.write('o');
+  Wire.endTransmission();
+}
+
+//
+static void BlinkM_setScriptLengthReps(byte addr, byte script_id, 
+                                       byte len, byte reps)
+{
+  Wire.beginTransmission(addr);
+  Wire.write('L');
+  Wire.write(script_id);
+  Wire.write(len);
+  Wire.write(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.write('R');
+  Wire.write(script_id);
+  Wire.write(pos);
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)5);
+  while( Wire.available() < 5 ) ; // FIXME: wait until we get 7 bytes
+  script_line->dur    = Wire.read();
+  script_line->cmd[0] = Wire.read();
+  script_line->cmd[1] = Wire.read();
+  script_line->cmd[2] = Wire.read();
+  script_line->cmd[3] = Wire.read();
+}
+
+//
+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.write('W');
+  Wire.write(script_id);
+  Wire.write(pos);
+  Wire.write(dur);
+  Wire.write(cmd);
+  Wire.write(arg1);
+  Wire.write(arg2);
+  Wire.write(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.write('B');
+  Wire.write(mode);             // default 0x01 == Play script
+  Wire.write(script_id);        // default 0x00 == script #0
+  Wire.write(reps);             // default 0x00 == repeat infinitely
+  Wire.write(fadespeed);        // default 0x08 == usually overridden by sketch 
+  Wire.write(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.write('i');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)1);
+  if( Wire.available() ) {
+    byte b = Wire.read();
+    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.write('i');
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (byte)4);
+  while( Wire.available() < 4 ) ; // FIXME: wait until we get 4 bytes
+    
+  inputs[0] = Wire.read();
+  inputs[1] = Wire.read();
+  inputs[2] = Wire.read();
+  inputs[3] = Wire.read();
+
+  return 0;
+}

libraries/Rosserial_Arduino_Library/examples/Clapper/Clapper.pde

@@ -0,0 +1,94 @@
+/* 
+ * 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.
+ */
+
+#if (ARDUINO >= 100)
+ #include <Arduino.h>
+#else
+ #include <WProgram.h>
+#endif
+#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();
+}

libraries/Rosserial_Arduino_Library/examples/Esp8266/HelloWorld.ino

@@ -0,0 +1,75 @@
+/*
+ * rosserial Publisher Example
+ * Prints "hello world!"
+ * This intend to connect to a Wifi Access Point 
+ * and a rosserial socket server.
+ * You can launch the rosserial socket server with 
+ * roslaunch rosserial_server socket.launch
+ * The default port is 11411
+ *
+ */
+#include <ESP8266WiFi.h>
+#include <ros.h>
+#include <std_msgs/String.h>
+
+const char* ssid     = "your-ssid";
+const char* password = "your-password";
+// Set the rosserial socket server IP address
+IPAddress server(192,168,1,1);
+// Set the rosserial socket server port
+const uint16_t serverPort = 11411;
+
+ros::NodeHandle nh;
+// Make a chatter publisher
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+// Be polite and say hello
+char hello[13] = "hello world!";
+
+void setup()
+{
+  // Use ESP8266 serial to monitor the process
+  Serial.begin(115200);
+  Serial.println();
+  Serial.print("Connecting to ");
+  Serial.println(ssid);
+
+  // Connect the ESP8266 the the wifi AP
+  WiFi.begin(ssid, password);
+  while (WiFi.status() != WL_CONNECTED) {
+    delay(500);
+    Serial.print(".");
+  }
+  Serial.println("");
+  Serial.println("WiFi connected");
+  Serial.println("IP address: ");
+  Serial.println(WiFi.localIP());
+
+  // Set the connection to rosserial socket server
+  nh.getHardware()->setConnection(server, serverPort);
+  nh.initNode();
+
+  // Another way to get IP
+  Serial.print("IP = ");
+  Serial.println(nh.getHardware()->getLocalIP());
+
+  // Start to be polite
+  nh.advertise(chatter);
+}
+
+void loop()
+{
+
+  if (nh.connected()) {
+    Serial.println("Connected");
+    // Say hello
+    str_msg.data = hello;
+    chatter.publish( &str_msg );
+  } else {
+    Serial.println("Not Connected");
+  }
+  nh.spinOnce();
+  // Loop exproximativly at 1Hz
+  delay(1000);
+}

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

libraries/Rosserial_Arduino_Library/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();
+}
+

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

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

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

libraries/Rosserial_Arduino_Library/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()

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

libraries/Rosserial_Arduino_Library/examples/ServoControl/ServoControl.pde

@@ -0,0 +1,49 @@
+/*
+ * 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
+ */
+
+#if (ARDUINO >= 100)
+ #include <Arduino.h>
+#else
+ #include <WProgram.h>
+#endif
+
+#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);
+}

libraries/Rosserial_Arduino_Library/examples/Temperature/Temperature.pde

@@ -0,0 +1,72 @@
+/*
+ * 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 <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.read();  // receive high byte (full degrees)
+      lsb = Wire.read();  // 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();
+}

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

libraries/Rosserial_Arduino_Library/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();
+}

libraries/Rosserial_Arduino_Library/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
+  digitalWrite(button_pin, HIGH);
+  
+  //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();
+}

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