void ServoPlugin::Update()

in simulation_ws/src/rover/hector_gazebo_plugins/src/servo_plugin.cpp [200:268]

• 64 lines of code
• 14 McCabe index (conditional complexity)

void ServoPlugin::Update()
{
  // handle callbacks
  queue_.callAvailable();

  common::Time stepTime;
#if (GAZEBO_MAJOR_VERSION >= 8)
  stepTime = world->SimTime() - prevUpdateTime;
#else
  stepTime = world->GetSimTime() - prevUpdateTime;
#endif

  if (controlPeriod == 0.0 || stepTime > controlPeriod) {
    CalculateVelocities();
    publish_joint_states();
#if (GAZEBO_MAJOR_VERSION >= 8)
    prevUpdateTime = world->SimTime();
#else
    prevUpdateTime = world->GetSimTime();
#endif
  }

  if (enableMotors)
  {
    servo[FIRST].joint->SetVelocity(0, servo[FIRST].velocity);
    if (countOfServos > 1) {
      servo[SECOND].joint->SetVelocity(0, servo[SECOND].velocity);
      if (countOfServos > 2) {
        servo[THIRD].joint->SetVelocity(0, servo[THIRD].velocity);
      }
    }

#if (GAZEBO_MAJOR_VERSION > 4)
    servo[FIRST].joint->SetEffortLimit(0, maximumTorque);
    if (countOfServos > 1) {
      servo[SECOND].joint->SetEffortLimit(0, maximumTorque);
      if (countOfServos > 2) {
        servo[THIRD].joint->SetEffortLimit(0, maximumTorque);
      }
    }
#else
    servo[FIRST].joint->SetMaxForce(0, maximumTorque);
    if (countOfServos > 1) {
      servo[SECOND].joint->SetMaxForce(0, maximumTorque);
      if (countOfServos > 2) {
        servo[THIRD].joint->SetMaxForce(0, maximumTorque);
      }
    }
#endif
  } else {
#if (GAZEBO_MAJOR_VERSION > 4)
    servo[FIRST].joint->SetEffortLimit(0, 0.0);
    if (countOfServos > 1) {
      servo[SECOND].joint->SetEffortLimit(0, 0.0);
      if (countOfServos > 2) {
        servo[THIRD].joint->SetEffortLimit(0, 0.0);
      }
    }
#else
    servo[FIRST].joint->SetMaxForce(0, 0.0);
    if (countOfServos > 1) {
      servo[SECOND].joint->SetMaxForce(0, 0.0);
      if (countOfServos > 2) {
        servo[THIRD].joint->SetMaxForce(0, 0.0);
      }
    }
#endif
  }
}