void LoCoBotController::gripperController()

in robots/LoCoBot/locobot_control/src/locobot_controller.cpp [553:649]
76 lines of code
17 McCabe index (conditional complexity)

void LoCoBotController::gripperController(const ros::TimerEvent &) {
  bool result = false;
  const char *log = NULL;

  if (use_group_["gripper"]) {
    int gripper_motor_id = dynamixel_name_2ids_[group_motors_["gripper"][0]];
    int gripper_state_id = motor_id_2state_list_id_[gripper_motor_id];
    // close gripper
    if (gripper_state_ == 1) {
      if (prev_gripper_state_ != gripper_state_) {
        // shifting to PWM mode
        std::unique_lock<std::mutex> lock(dynamixel_mutex_);
        dxl_wb_->itemWrite(gripper_motor_id, "Torque_Enable", 0, &log);
        dxl_wb_->itemWrite(gripper_motor_id, "Operating_Mode", 16, &log);
        usleep(10000);
        dxl_wb_->itemWrite(gripper_motor_id, "Torque_Enable", 1, &log);
        prev_gripper_state_ = gripper_state_;
      }

      int32_t load = 0.0;
      load = (int32_t) dynamixel_state_list_.dynamixel_state[gripper_state_id].present_current;

      if (std::abs(load) > GRIPPER_MAX_LOAD) {
        // sometime gripper gets stuck.. this part is to overcome that
        if (prev_gripper_load_ == 0) {
          std::unique_lock<std::mutex> lock(dynamixel_mutex_);
          dxl_wb_->itemWrite(gripper_motor_id, "Goal_PWM", -700, &log);
          usleep(100000);
        }

        load = (int32_t) dynamixel_state_list_.dynamixel_state[gripper_state_id].present_current;

        if (std::abs(load) > GRIPPER_MAX_LOAD) {
          prev_gripper_load_ = 1;
        }
      }

      int32_t position = (int32_t) dynamixel_state_list_.dynamixel_state[gripper_state_id].present_position;
      if ((position < GRIPPER_CLOSE_VALUE)
          || prev_gripper_load_ == 1) {
        std::unique_lock<std::mutex> lock(dynamixel_mutex_);
        dxl_wb_->itemWrite(gripper_motor_id, "Goal_PWM", 0, &log);
        gripper_state_ = -1;
      } else {
        std::unique_lock<std::mutex> lock(dynamixel_mutex_);
        dxl_wb_->itemWrite(gripper_motor_id, "Goal_PWM", -GRIPPER_PWM, &log);
      }

    }
      // open gripper
    else if (gripper_state_ == 0) {

      std::unique_lock<std::mutex> lock(dynamixel_mutex_);
      if (prev_gripper_state_ != gripper_state_) {
        // shifting to position mode
        dxl_wb_->itemWrite(gripper_motor_id, "Torque_Enable", 0, &log);
        dxl_wb_->itemWrite(gripper_motor_id, "Operating_Mode", 3, &log);
        usleep(10000);
        dxl_wb_->itemWrite(gripper_motor_id, "Torque_Enable", 1, &log);
        prev_gripper_state_ = gripper_state_;
      }
      dxl_wb_->goalPosition(gripper_motor_id, GRIPPER_OPEN_VALUE);
      prev_gripper_load_ = 0;

    gripper_state_ = -1;
    }
  int32_t position = (int32_t) dynamixel_state_list_.dynamixel_state[gripper_state_id].present_position;
  int32_t delta = 50;
  // publish state
  if(gripper_cmd_ == -1 or gripper_cmd_ == 0){
    if(position > GRIPPER_OPEN_VALUE - delta && position < GRIPPER_OPEN_VALUE + delta){
      // fully open 0
      gripper_state_msg_.data = 0;
      }
    else{
      // unknown -1
      gripper_state_msg_.data = -1;
      }
  }
  else if(gripper_cmd_ == 1){
   if(gripper_state_ != -1){
      // closing
      gripper_state_msg_.data = 1;
      }
   else if(position < GRIPPER_CLOSE_VALUE){
      // fully closed 2
      gripper_state_msg_.data = 3;
      }
   else{
      // have object in hand 1
      gripper_state_msg_.data = 2;
      }
    }
    gripper_state_pub_.publish(gripper_state_msg_);
  }

}