bool LoCoBotController::initDynamixels()

in robots/LoCoBot/locobot_control/src/locobot_controller.cpp [193:229]

34 lines of code
13 McCabe index (conditional complexity)


bool LoCoBotController::initDynamixels(void) {
  const char *log;

  for (auto const &dxl:dynamixel_name_2ids_) {
    dxl_wb_->torqueOff((uint8_t) dxl.second);

    if(torque_control_ && (dxl.first == "joint_1" || dxl.first == "joint_2" || dxl.first == "joint_3" || dxl.first == "joint_4" || dxl.first == "joint2Dual")){
      dxl_wb_->itemWrite(dxl.second, "Operating_Mode", CURRENT_CONTROL_MODE, &log);
      usleep(10000);
    }
    else{
      dxl_wb_->itemWrite(dxl.second, "Operating_Mode", POSITION_CONTROL_MODE, &log);
      usleep(10000);
      for (auto const &info:dynamixel_info_) {
        if (dxl.first == info.first) {
          if (info.second.item_name != "ID" && info.second.item_name != "Baud_Rate") {
            bool
                result = dxl_wb_->itemWrite((uint8_t) dxl.second, info.second.item_name.c_str(), info.second.value, &log);
            if (!result) {
              ROS_ERROR("%s", log);
              ROS_ERROR("Failed to write value[%d] on items[%s] to Dynamixel[Name : %s, ID : %d]",
                        info.second.value,
                        info.second.item_name.c_str(),
                        dxl.first.c_str(),
                        dxl.second);
              return false;
            }
          }
        }
      }
  }
    dxl_wb_->torqueOn((uint8_t) dxl.second);
  }
  is_initialized_ = true;

  return true;
}