 
 void servo_angleCb(const std_msgs::Float32& angle_msg)
 {
-    if (servo_torque_msg.data == false) return;
+    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);

    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);

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;

