EntityBase::requestSpeedChange sets target_speed only when it's reached

[gmajrobotec]

Description
The statement target_speed_ = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_); in line 483 sets the target speed only in cases where the target speed has already been reached, which seems counter intuitive. This statement’s position is inconsistent with lines 469

Solution
Line 483 should be moved down, outside of the “if” statement. This way the most up to date target speed will be set on each iteration until the target speed is reached. Then it will be consistent with line 469.

void EntityBase::requestSpeedChange(
  const speed_change::RelativeTargetSpeed & target_speed, bool continuous)
{
  if (!continuous && isTargetSpeedReached(target_speed)) {
    return;
  }
  if (continuous) {
    job_list_.append(
      /**
       * @brief If the target entity reaches the target speed, return true.
       */
      [this, target_speed](double) {
        if (other_status_.find(target_speed.reference_entity_name) == other_status_.end()) {
          return true;
        }
        target_speed_ = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_);
        return false;
      },
      [this]() {}, job::Type::LINEAR_VELOCITY, true, job::Event::POST_UPDATE);
  } else {
    job_list_.append(
      /**
       * @brief If the target entity reaches the target speed, return true.
       */
      [this, target_speed](double) {
        if (other_status_.find(target_speed.reference_entity_name) == other_status_.end()) {
          return true;
        }
        if (isTargetSpeedReached(target_speed)) {
-          target_speed_ = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_);
          return true;
        }
+          target_speed_ = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_);
        return false;
      },
      /**
       * @brief Cancel speed change request.
       */
      [this]() { target_speed_ = std::nullopt; }, job::Type::LINEAR_VELOCITY, true,
      job::Event::POST_UPDATE);
  }
}