 
 void servoCb(const std_msgs::Int16& angle_msg)
 {
-    int angle;
-    angle = angle_msg.data;
-    if (angle < 0) angle = 0;
-    if (angle > 180) angle = 180;
-
-    myservo.write(angle);
-}
-
-ros::Subscriber<std_msgs::Int16> servo_sub("vacuum_gripper/limb/right/servo", &servoCb);
+    if (servo_state_msg.data == false) return;
+    if (angle_msg.data < 0) servo_angle_msg.data = 0;
+    else if (angle_msg.data > 180) servo_angle_msg.data = 180;
+    else servo_angle_msg.data = angle_msg.data;
+    myservo.write(servo_angle_msg.data);
+}
+
+void servo_toggleCb(const std_msgs::Bool& servo_toggle_msg)
+{
+    if (servo_toggle_msg.data) {
+        servo_state_msg.data = true;
+        if (!myservo.attached()) myservo.attach(5);
+        myservo.write(servo_angle_msg.data);
+    } else {
+        servo_state_msg.data = false;
+        if (myservo.attached()) myservo.detach();
+    }
+}
+
+ros::Subscriber<std_msgs::Int16> servo_angle_sub("vacuum_gripper/limb/right/servo_angle", &servoCb);
+ros::Subscriber<std_msgs::Bool> servo_state_sub("vacuum_gripper/limb/right/servo_state", &servo_toggleCb);
 
 void setup() {
+    servo_state_msg.data = false;
+    servo_angle_msg.data = 90;
     init_servo();
     pinMode(SS, OUTPUT);
     digitalWrite(SS, HIGH);
-    nh.getHardware()->setBaud(57600);
+    nh.getHardware()->setBaud(115200);
     nh.initNode();
-    nh.subscribe(servo_sub);
+    nh.subscribe(servo_angle_sub);
+    nh.subscribe(servo_state_sub);
     nh.advertise(pressure_pub);
     nh.advertise(state_pub);
+    nh.advertise(servo_state_pub);
+    nh.advertise(servo_angle_pub);
 
     SPI.begin();
     SPI.setClockDivider(SPI_CLOCK_DIV8);

void init_servo()
{
    myservo.attach(5);
}

void initBME()

    dig_P7 = (data[19] << 8) | data[18];
    dig_P8 = (data[21] << 8) | data[20];
    dig_P9 = (data[23] << 8) | data[22];
}

void readData()
