-/*
- * 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
- */
-
+//TODO: Create "library" for each component
 #include <Wire.h>
 #include <ros.h>
 #include <std_msgs/String.h>
 #include <std_msgs/UInt8.h>
 #include <std_msgs/Empty.h>
+#include <std_msgs/Bool.h>
 #include <OneWire.h>
 #include <DallasTemperature.h>
 

  }
}

void setup()
{  
  nh.initNode();
  nh.advertise(pub_temp);
  nh.advertise(pub_count);
  nh.subscribe(sub_temp);
  nh.subscribe(sub_count);
  
  uint32_t sensor_count = 0;
  // Create analog temperature sensor

  send_message(sensor);
}

static void count_callback(const std_msgs::Empty& empty) {
  count_msg.data = (uint8_t)sizeof(sensors) / sizeof(sensors[0]);
  pub_temp.publish(&temp_msg);
}

static void heater_callback(const std_msgs::Bool& state) {
  if(state.data) {
    digitalWrite(HEATER_PIN, HIGH);
  } else {
    digitalWrite(HEATER_PIN, LOW);
  }
}

static ros::Subscriber<std_msgs::UInt8> sub_temp("req_temperature", &temperature_callback);
static ros::Subscriber<std_msgs::Empty> sub_count("req_count", &count_callback);
static ros::Subscriber<std_msgs::Bool> sub_heater("heater", &heater_callback);

static void increment_sample_rate(TemperatureSensor* sensor) {
  if(sensor->sample_rate == MAX_SAMPLE_RATE) {
