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


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

void setup() {
    init_servo();
    pinMode(SS, OUTPUT);
    digitalWrite(SS, HIGH);
    nh.getHardware()->setBaud(57600);
    nh.initNode();
    nh.subscribe(servo_sub);
    nh.advertise(pressure_pub);
    nh.advertise(state_pub);

    SPI.begin();
    SPI.setClockDivider(SPI_CLOCK_DIV8);

int16_t dig_P7;
int16_t dig_P8;
int16_t dig_P9;

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

std_msgs::Float64 float_msg;
std_msgs::Bool bool_msg;
std_msgs::Bool servo_state_msg;
std_msgs::Int16 servo_angle_msg;

ros::Publisher pressure_pub("vacuum_gripper/limb/right/pressure", &float_msg);
ros::Publisher state_pub("gripper_grabbed/limb/right/state", &bool_msg);
ros::Publisher servo_state_pub("vacuum_gripper/limb/right/servo_state_info", &servo_state_msg);
ros::Publisher servo_angle_pub("vacuum_gripper/limb/right/servo_angle_info", &servo_angle_msg);

unsigned long  publisher_timer = 0;

