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

ros::Publisher servo_angle_pub("gripper_front/limb/right/servo/angle/state", &servo_angle_msg);

unsigned long  publisher_timer = 0;

Servo myservo;



void servo_angleCb(const std_msgs::Float32& angle_msg)
{
    if (!state) return;
    if (angle_msg.data < -pi/2.0) servo_angle_msg.data = -pi/2.0;
    else if (angle_msg.data > pi/2.0) servo_angle_msg.data = pi/2.0;
    else servo_angle_msg.data = angle_msg.data;
    if (servo_torque_msg.data == false) return;
    set_angle();
}

void servo_torqueCb(const std_msgs::Bool& servo_toggle_msg)
{
    if (!state) return;
    if (servo_toggle_msg.data) {
        servo_torque_msg.data = true;
        if (!myservo.attached()) myservo.attach(5);
