   send_message(sensor);
 }
 
-static void send_count(void) {
+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 count_callback(const std_msgs::Empty& empty) {
-  send_count();
+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) {

  create_sensor(sensor_count, ANALOG_PIN, TYPE_ANALOG, &analog_sensor);
  sensors[sensor_count++] = analog_sensor;

  TemperatureSensor one_wire_sensor;
  create_sensor(sensor_count, ONE_WIRE_PIN, TYPE_ONE_WIRE, &one_wire_sensor);
  sensors[sensor_count++] = one_wire_sensor;

  TemperatureSensor i2c_sensor0;
  create_sensor(sensor_count, TMP102_ADDR_0, TYPE_I2C, &i2c_sensor0);
  sensors[sensor_count++] = i2c_sensor0;
}

void loop()
{
  TemperatureSensor* sensor;
  for(uint32_t c = 0; c < SENSOR_COUNT; c++) {

#define MAX_SAMPLE_RATE 10
#define MAX_SAMPLE_COUNT 10
#define MAX_DELTA 0.5

#define HEATER_PIN 22

typedef struct one_wire_data {
  OneWire one_wire;
