         press_act = (float)press_cal / 100.0;
 
         bool_msg.data = (press_act < 900);
-        float_msg.data = press_act;
+        pressure_msg.data = press_act;
 
         state_pub.publish(&bool_msg);
-        pressure_pub.publish(&float_msg);
-        servo_state_pub.publish(&servo_state_msg);
+        pressure_pub.publish(&pressure_msg);
+        servo_torque_pub.publish(&servo_torque_msg);
         servo_angle_pub.publish(&servo_angle_msg);
 
         publisher_timer = millis() + 100;


void init_servo()
{
    myservo.attach(5);
    myservo.write(servo_angle_msg.data);
}

void initBME()

ros::NodeHandle nh; //write with IDE
// ros::NodeHandle_<ArduinoHardware, 1, 2, 512, 512> nh;

std_msgs::Float64 pressure_msg;
std_msgs::Bool bool_msg;
std_msgs::Bool servo_torque_msg;
std_msgs::Float32 servo_angle_msg;

ros::Publisher pressure_pub("gripper_front/limb/right/pressure/state", &pressure_msg);
ros::Publisher state_pub("gripper_front/limb/right/pressure/grabbed/state", &bool_msg);
ros::Publisher servo_torque_pub("gripper_front/limb/right/servo/torque/state", &servo_torque_msg);
ros::Publisher servo_angle_pub("gripper_front/limb/right/servo/angle/state", &servo_angle_msg);

unsigned long  publisher_timer = 0;

Servo myservo;

void set_angle(void)
{
    myservo.write((int)(-servo_angle_msg.data + 90.0));
}

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

void servo_torqueCb(const std_msgs::Bool& servo_toggle_msg)
{
    if (servo_toggle_msg.data) {
        servo_torque_msg.data = true;
        if (!myservo.attached()) myservo.attach(5);
        set_angle();
    } else {
        servo_torque_msg.data = false;
        if (myservo.attached()) 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);

void setup() {
    servo_torque_msg.data = false;
    servo_angle_msg.data = 0;
    init_servo();
    pinMode(SS, OUTPUT);
    digitalWrite(SS, HIGH);
    nh.getHardware()->setBaud(115200);
    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);
    nh.advertise(servo_angle_pub);

    SPI.begin();
