 ros::Publisher servo_angle_pub("gripper_front/limb/right/servo/angle/state", &servo_angle_msg);
 
 unsigned long  publisher_timer = 0;
+bool state = false;
 
 Servo myservo;
 

    nh.initNode();
    nh.subscribe(servo_angle_sub);
    nh.subscribe(servo_torque_sub);
    nh.advertise(pressure_pub);
    nh.advertise(state_pub);
    nh.advertise(servo_torque_pub);

    }
}

void servo_enableCb(const std_msgs::Bool& servo_enable_msg)
{
    state = servo_enable_msg.data;
    if (!state) {
        servo_torque_msg.data = false;
        servo_angle_msg.data = 0;
        myservo.detach();
    }
}

ros::Subscriber<std_msgs::Float32> servo_angle_sub("gripper_front/limb/right/servo/angle", &servo_angleCb);
ros::Subscriber<std_msgs::Bool> servo_torque_sub("gripper_front/limb/right/servo/torque", &servo_torqueCb);
ros::Subscriber<std_msgs::Bool> servo_enable_sub("gripper_front/enable", &servo_enableCb);

void setup() {
    servo_torque_msg.data = false;
