void LoCoBotController::hardwareStatusPublish()

in robots/LoCoBot/locobot_control/src/locobot_controller.cpp [1240:1349]
92 lines of code
17 McCabe index (conditional complexity)

void LoCoBotController::hardwareStatusPublish(const ros::TimerEvent &) {
  std_msgs::Bool status;
  status.data = true;
  bool result = false;
  const char *log = NULL;

  uint8_t id_array[num_motors_];
  uint8_t id_cnt = 0;
  int32_t shutdownInfo[num_motors_];
  std::map<int, std::string> array_id2motor_name;

  for (auto const& group:use_group_)
  {
    if (group.second) {
      for (auto const &motor:group_motors_[group.first]) {
        array_id2motor_name[id_cnt] = motor;
        id_array[id_cnt++] = (uint8_t) dynamixel_name_2ids_[motor];
      }
    }
  }

  if (dxl_wb_->getProtocolVersion() == 2.0f) {
    std::unique_lock<std::mutex> lock(dynamixel_mutex_);
    result = dxl_wb_->syncRead(SYNC_READ_HANDLER_FOR_SHUTDOWN,
                               id_array,
                               num_motors_,
                               &log);
    if (!result) {
      ROS_ERROR("%s", log);
    }

    result = dxl_wb_->getSyncReadData(SYNC_READ_HANDLER_FOR_SHUTDOWN,
                                      id_array,
                                      id_cnt,
                                      control_items_["Hardware_Error_Status"]->address,
                                      control_items_["Hardware_Error_Status"]->data_length,
                                      shutdownInfo,
                                      &log);

    if (!result) {
      ROS_ERROR("%s", log);
    }
  } else if (dxl_wb_->getProtocolVersion() == 1.0f) {
    std::unique_lock<std::mutex> lock(dynamixel_mutex_);
    int idx = 0;
    for (int i = 0; i < id_cnt; i++) {
      uint32_t torque_enable_info[control_items_["Shutdown"]->data_length];
      result = dxl_wb_->readRegister((uint8_t) id_array[i],
                                     control_items_["Hardware_Error_Status"]->address,
                                     control_items_["Hardware_Error_Status"]->data_length,
                                     torque_enable_info,
                                     &log);
      if (!result) {
        ROS_ERROR("%s", log);
      }
      shutdownInfo[idx] = torque_enable_info[0];
    }
  }

  int32_t byte_info = 0;
  bool has_overload_error = false;
  bool has_electrical_error = false;
  bool has_encoder_error = false;
  bool has_overheating_error = false;
  bool has_input_voltage_error = false;

  for (uint8_t index = 0; index < id_cnt; index++) {
    byte_info = shutdownInfo[index];

    if (byte_info != 0) {

      status.data = false;
      status_pub_.publish(status);

      ROS_ERROR("Motor %s ID=%d has Hardware_Error_Status byte %d",
                array_id2motor_name[index].c_str(), (uint8_t) id_array[index], byte_info);

      has_overload_error = (bool) getBit(byte_info, HARDWARE_ERROR_STATUS_OVERLOAD);
      if (has_overload_error) {
        ROS_ERROR("Motor %s ID=%d has Overload error", array_id2motor_name[index].c_str(), (uint8_t) id_array[index]);
      }

      has_electrical_error = (bool) getBit(byte_info, HARDWARE_ERROR_STATUS_ELECTRICAL_SHOCK);
      if (has_electrical_error) {
        ROS_ERROR("Motor %s ID=%d has Electrical Shock error", array_id2motor_name[index].c_str(), (uint8_t) id_array[index]);
      }

      has_encoder_error = (bool) getBit(byte_info, HARDWARE_ERROR_STATUS_ENCODER);
      if (has_encoder_error) {
        ROS_ERROR("Motor %s ID=%d has Motor Encoder error", array_id2motor_name[index].c_str(), (uint8_t) id_array[index]);
      }

      has_overheating_error = (bool) getBit(byte_info, HARDWARE_ERROR_STATUS_OVERHEATING);
      if (has_overheating_error) {
        ROS_ERROR("Motor %s ID=%d has Overheating error", array_id2motor_name[index].c_str(), (uint8_t) id_array[index]);
      }

      has_input_voltage_error = (bool) getBit(byte_info, HARDWARE_ERROR_STATUS_INPUT_VOLTAGE);
      if (has_input_voltage_error) {
        ROS_ERROR("Motor %s ID=%d has Input Volatage error", array_id2motor_name[index].c_str(), (uint8_t) id_array[index]);
      }

      std::unique_lock<std::mutex> lock(hardware_mutex_);
      is_hardware_ok_ = false;

    }
  }

  status_pub_.publish(status);
}