<commit_before>
// Copyright (c) 2015, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/core/path-optimization/config-optimization.hh>

#include <boost/algorithm/string.hpp>

#include <hpp/util/debug.hh>
#include <hpp/util/pointer.hh>

#include <hpp/model/device.hh>
#include <hpp/model/joint.hh>
#include <hpp/constraints/configuration-constraint.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/distance.hh>
#include <hpp/core/steering-method.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/numerical-constraint.hh>

namespace hpp {
  namespace core {
    namespace pathOptimization {
      namespace {
        // Compute the length of a vector of paths assuming that each element
        // is optimal for the given distance.
        static value_type pathLength (const PathVectorPtr_t& path,
            const DistancePtr_t& distance)
        {
          value_type result = 0;
          for (std::size_t i=0; i<path->numberPaths (); ++i) {
            const PathPtr_t& element (path->pathAtRank (i));
            result += (*distance) (element->initial (), element->end ());
          }
          return result;
        }
      }

      ConfigOptimization::Parameters::Parameters () :
        addConfigConstraintToPath (ConfigOptimizationTraits::addConfigConstraintToPath ()),
        numberOfPass (ConfigOptimizationTraits::numberOfPass ()),
        numberOfIterations (ConfigOptimizationTraits::numberOfIterations ()),
        getGoal (ConfigOptimizationTraits::getGoal)
      {}

      ConfigOptimizationPtr_t ConfigOptimization::create (const Problem& problem)
      {
        return createWithTraits <ConfigOptimizationTraits> (problem);
      }

      ConfigOptimization::ConfigOptimization (const Problem& problem) :
        PathOptimizer (problem)
      {
      }

      PathVectorPtr_t ConfigOptimization::optimize (const PathVectorPtr_t& path)
      {
        PathVectorPtr_t unpacked = PathVector::create (path->outputSize(),
            path->outputDerivativeSize ());
        path->flatten (unpacked);
        PathVectorPtr_t opted = unpacked;
        const std::size_t N = problem().robot()->configSize ();
        const std::size_t P = unpacked->numberPaths();

        Optimizers_t optimizers;
        const std::size_t rIndex = buildOptimizers (*unpacked, optimizers);
        vector_t configs (N*(P+1)), newConfigs;
        buildConfigVector (*unpacked, configs);
        newConfigs = configs;

        SteeringMethod& sm (*problem ().steeringMethod ());
        value_type length = pathLength (unpacked, problem().distance());
        hppDout (info, "ConfigOptimization: length " << length);
        value_type alpha = parameters.alphaInit;
        for (std::size_t ipass = 0; ipass < parameters.numberOfPass; ++ipass) {
          PathVectorPtr_t optedF = PathVector::create (path->outputSize(),
              path->outputDerivativeSize ());
          PathVectorPtr_t optedB = PathVector::create (path->outputSize(),
              path->outputDerivativeSize ());
          bool didChangeF, didChangeB;
          bool successF = pass <true> (configs, newConfigs, optimizers,
              rIndex  , alpha, optedF, didChangeF);
          if (successF) {
            bool successB = pass <false> (configs, newConfigs, optimizers,
                rIndex + 1, alpha, optedB, didChangeB);
            if (successB && (didChangeF || didChangeB)) {
              PathPtr_t link = sm (
                  newConfigs.segment ((rIndex    )*N, N),
                  newConfigs.segment ((rIndex + 1)*N, N));
              if (!link) {
                link = sm (
                    newConfigs.segment ((rIndex + 1)*N, N),
                    newConfigs.segment ((rIndex    )*N, N));
                if (link) link = link->reverse ();
              }
              if (isValid (link)) {
                opted = optedF;
                opted->appendPath (link);
                opted->concatenate (*optedB);
                /// OptedF is a valid optimization.
                value_type optedLength = pathLength (opted, problem().distance());
                if (optedLength < length) {
                  unpacked = opted;
                  length = optedLength;
                  hppDout (info, "ConfigOptimization: accepted length " << length);
                  /// Update configs
                  configs = newConfigs;
                  continue;
                }
              }
            }
          }
          /// Failure
          hppDout (info, "ConfigOptimization: pass " << ipass << " failed");
          alpha /= 2.;
        }

        return opted;
      }

      bool ConfigOptimization::isValid (const PathPtr_t& p) const
      {
        PathPtr_t vp;
        PathValidationReportPtr_t r;
        if (!p) return false;
        else {
          return problem ().pathValidation ()->validate (p, false, vp, r);
        }
      }

      NumericalConstraintPtr_t ConfigOptimization::createNumConstraint
        (const PathVector& path) const
      {
        std::vector <bool> mask (problem().robot()->numberDof(), true);
        // TODO: there are some DOF that should not be optimized using this
        // method. Here, we consider we do not want to optimized the root joint.
        // This is not a very good solution. For a freeflyer joint for instance,
        // you may want not to optimize the two first joints...
        const JointVector_t& jv = problem().robot()->getJointVector ();
        for (JointVector_t::const_iterator it = jv.begin ();
            it != jv.end (); ++it) {
          for (size_type i = 0; i < (*it)->numberDof (); i++)
            if (parameters.shouldFilter (*it, i))
              mask[(*it)->rankInVelocity () + i] = false;
        }
        Configuration_t goal = parameters.getGoal (path);
        NumericalConstraintPtr_t cc = NumericalConstraint::create
          (constraints::ConfigurationConstraint::create
           ("Optimization constraint", problem().robot(), goal, mask)
           );
        cc->function().context ("optimization");
        return cc;
      }

      bool ConfigOptimization::Optimizer::optimize (ConfigurationOut_t q,
          const std::size_t numIter,
          const value_type alpha) const
      {
        return proj->optimize (q, numIter, alpha);
      }

      ConfigProjectorPtr_t ConfigOptimizationTraits::getConfigProjector
        (const PathPtr_t& before, const PathPtr_t& /*after*/,
         bool& isReverse)
      {
        isReverse = false;
        ConfigProjectorPtr_t proj;
        ConstraintPtr_t c = before->constraints();
        if (c) {
          proj = HPP_DYNAMIC_PTR_CAST (ConfigProjector, c);
          if (!proj) {
            ConstraintSetPtr_t set =
              HPP_DYNAMIC_PTR_CAST (ConstraintSet, c);
            if (!set)
              throw std::runtime_error ("There are constraints of unknown types.");
            if (set->configProjector ()) {
              proj = set->configProjector ();
            }
          }
        }
        return proj;
      }

      std::size_t ConfigOptimization::buildOptimizers (const PathVector& pv,
          Optimizers_t& projectors)
      {
        NumericalConstraintPtr_t cc = createNumConstraint (pv);

        const std::size_t P = pv.numberPaths ();
        projectors.resize (P - 1);
        PathPtr_t cur = pv.pathAtRank (0);
        bool reverse = false, reverseStarted = false;
        std::size_t indexReverse = P-1;
        for (std::size_t i = 0; i < P - 1; ++i) {
          PathPtr_t next = pv.pathAtRank (i + 1);

          projectors[i].proj = parameters.getConfigProjector (cur, next, reverse);
          if (!projectors[i].proj) {
            projectors[i].proj = ConfigProjector::create ( problem().robot(),
                "Optimizer", 1e6, parameters.numberOfIterations);
          } else {
            projectors[i].proj = HPP_STATIC_PTR_CAST (ConfigProjector,
                projectors[i].proj->copy ());
          }
          // TODO: this is not robust since we do not know if level 1 is the
          // last
          projectors[i].proj->add (cc, SizeIntervals_t (0), 1);
          projectors[i].proj->lastIsOptional (true);

          if (reverse) {
            if (!reverseStarted) {
              indexReverse = i;
              reverseStarted = true;
            }
          }
          cur = next;
        }
        return indexReverse;
      }

      void ConfigOptimization::buildConfigVector (const PathVector& path,
          vectorOut_t configs) const
      {
        const std::size_t N = problem().robot()->configSize ();
        const std::size_t P = path.numberPaths();
        configs.resize (N * (P+1));

        std::size_t iP = 0;
        configs.segment (iP, N) = path.initial ();
        for (std::size_t i = 0; i < path.numberPaths(); ++i) {
          PathPtr_t cur = path.pathAtRank (i);
          iP += N;
          configs.segment (iP, N) = cur->end ();
        }
      }

      bool ConfigOptimizationTraits::shouldFilter (const JointPtr_t joint,
          const size_type /* iDof */)
      {
        // If joint if the root joint or if parent joint is the root joint and
        // root joint is an anchor joint,
        // filter it
        if (joint->parentJoint () == NULL ||
            (joint->parentJoint ()->parentJoint ()==NULL
             && joint->parentJoint()->numberDof () == 0)
           )
          return true;
        /// Filter joint name containing "base_joint..."
        if (boost::algorithm::contains (joint->name (), "base_joint"))
          return true;
        return false;
      }

      template <bool forward>
      bool ConfigOptimization::pass (
          vectorIn_t configs, vectorOut_t newConfigs,
          const Optimizers_t& optimizers, const std::size_t& index,
          const value_type& alpha, PathVectorPtr_t opted, bool& didChange
          ) const
      {
        SteeringMethod& sm (*problem ().steeringMethod ());
        std::list <PathPtr_t> paths;
        const std::size_t N = opted->outputSize ();
        const std::size_t P = optimizers.size () + 1;
        didChange = false;
        for (std::size_t i = (forward)? 1          : P-1;
                             (forward)? i <= index : i >= index;
                             (forward)? ++i        : --i) {
          newConfigs.segment (i * N, N) = configs.segment (i * N, N);
          std::size_t iN = (forward)? i - 1 : i + 1;
          /// Optimize
          bool isOpted = optimizers[i-1].optimize (
              newConfigs.segment (i * N, N),
              parameters.numberOfIterations, alpha);
          PathPtr_t current;
          if (isOpted)
            current = sm (newConfigs.segment (iN*N, N),
                          newConfigs.segment (i *N, N));
          if (!isOpted || !isValid (current)) {
            /// Try from last newConfigs to old ones.
            current = sm (newConfigs.segment (iN*N, N),
                             configs.segment (i *N, N));
            if (isValid (current)) {
              /// We do not change that configuration.
              newConfigs.segment (i * N, N) = configs.segment (i * N, N);
            } else {
              /// We should backtrack to previous iteration
              hppDout (info, "ConfigOptimization: failed at path rank " << i);
              return false;
            }
          } else
            didChange = true;
          if (forward) paths.push_back  (current);
          else         paths.push_front (current->reverse ());
        }

        /// Build the path
        for (std::list<PathPtr_t>::iterator it = paths.begin();
            it != paths.end (); ++it)
            opted->appendPath (*it);
        return true;
      }
    } // namespace pathOptimization
  } // namespace core
} // namespace hpp
<commit_msg>Cosmetic change: Comment code and add debug output.<commit_after>
// Copyright (c) 2015, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/core/path-optimization/config-optimization.hh>

#include <boost/algorithm/string.hpp>

#include <hpp/util/debug.hh>
#include <hpp/util/pointer.hh>

#include <hpp/model/device.hh>
#include <hpp/model/joint.hh>
#include <hpp/constraints/configuration-constraint.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/distance.hh>
#include <hpp/core/steering-method.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/numerical-constraint.hh>

namespace hpp {
  namespace core {
    namespace pathOptimization {
      namespace {
        // Compute the length of a vector of paths assuming that each element
        // is optimal for the given distance.
        static value_type pathLength (const PathVectorPtr_t& path,
            const DistancePtr_t& distance)
        {
          value_type result = 0;
          for (std::size_t i=0; i<path->numberPaths (); ++i) {
            const PathPtr_t& element (path->pathAtRank (i));
            result += (*distance) (element->initial (), element->end ());
          }
          return result;
        }
      }

      ConfigOptimization::Parameters::Parameters () :
        addConfigConstraintToPath (ConfigOptimizationTraits::addConfigConstraintToPath ()),
        numberOfPass (ConfigOptimizationTraits::numberOfPass ()),
        numberOfIterations (ConfigOptimizationTraits::numberOfIterations ()),
        getGoal (ConfigOptimizationTraits::getGoal)
      {}

      ConfigOptimizationPtr_t ConfigOptimization::create (const Problem& problem)
      {
        return createWithTraits <ConfigOptimizationTraits> (problem);
      }

      ConfigOptimization::ConfigOptimization (const Problem& problem) :
        PathOptimizer (problem)
      {
      }

      PathVectorPtr_t ConfigOptimization::optimize (const PathVectorPtr_t& path)
      {
        PathVectorPtr_t unpacked = PathVector::create (path->outputSize(),
            path->outputDerivativeSize ());
        path->flatten (unpacked);
        PathVectorPtr_t opted = unpacked;
        const std::size_t N = problem().robot()->configSize ();
        const std::size_t P = unpacked->numberPaths();

        // Build the optimizers and config vectors
        Optimizers_t optimizers;
        const std::size_t rIndex = buildOptimizers (*unpacked, optimizers);
        vector_t configs (N*(P+1)), newConfigs;
        buildConfigVector (*unpacked, configs);
        newConfigs = configs;

        SteeringMethod& sm (*problem ().steeringMethod ());
        value_type length = pathLength (unpacked, problem().distance());
        hppDout (info, "ConfigOptimization: length " << length);
        value_type alpha = parameters.alphaInit;
        // Loop over pass index.
        for (std::size_t ipass = 0; ipass < parameters.numberOfPass; ++ipass) {
          PathVectorPtr_t optedF = PathVector::create (path->outputSize(),
              path->outputDerivativeSize ());
          PathVectorPtr_t optedB = PathVector::create (path->outputSize(),
              path->outputDerivativeSize ());
          bool didChangeF, didChangeB;
          // Forward pass: from path 0 to rIndex
          bool successF = pass <true> (configs, newConfigs, optimizers,
              rIndex  , alpha, optedF, didChangeF);
          if (successF) {
            // Backward pass from P-1 ro rIndex + 1
            bool successB = pass <false> (configs, newConfigs, optimizers,
                rIndex + 1, alpha, optedB, didChangeB);
            if (successB && (didChangeF || didChangeB)) {
              // Try to link the two passes (the steering method may not be
              // symmetric so try in both directions)
              PathPtr_t link = sm (
                  newConfigs.segment ((rIndex    )*N, N),
                  newConfigs.segment ((rIndex + 1)*N, N));
              if (!link) {
                link = sm (
                    newConfigs.segment ((rIndex + 1)*N, N),
                    newConfigs.segment ((rIndex    )*N, N));
                if (link) link = link->reverse ();
              }
              if (isValid (link)) {
                opted = optedF;
                opted->appendPath (link);
                opted->concatenate (*optedB);
                /// OptedF is a valid optimization.
                value_type optedLength = pathLength (opted, problem().distance());
                if (optedLength < length) {
                  unpacked = opted;
                  length = optedLength;
                  hppDout (info, "ConfigOptimization: accepted length "
                      << length << ", alpha = " << alpha);
                  /// Update configs
                  configs = newConfigs;
                  continue;
                }
              } else {
                hppDout (info, "ConfigOptimization: "
                    "Could not link forward and backward pass");
              }
            }
          }
          /// Failure
          hppDout (info, "ConfigOptimization: pass " << ipass << " failed"
                      << ", alpha = " << alpha);
          alpha /= 2.;
        }

        return opted;
      }

      bool ConfigOptimization::isValid (const PathPtr_t& p) const
      {
        PathPtr_t vp;
        PathValidationReportPtr_t r;
        if (!p) return false;
        else {
          return problem ().pathValidation ()->validate (p, false, vp, r);
        }
      }

      NumericalConstraintPtr_t ConfigOptimization::createNumConstraint
        (const PathVector& path) const
      {
        std::vector <bool> mask (problem().robot()->numberDof(), true);
        // TODO: there are some DOF that should not be optimized using this
        // method. Here, we consider we do not want to optimized the root joint.
        // This is not a very good solution. For a freeflyer joint for instance,
        // you may want not to optimize the two first joints...
        const JointVector_t& jv = problem().robot()->getJointVector ();
        for (JointVector_t::const_iterator it = jv.begin ();
            it != jv.end (); ++it) {
          for (size_type i = 0; i < (*it)->numberDof (); i++)
            if (parameters.shouldFilter (*it, i))
              mask[(*it)->rankInVelocity () + i] = false;
        }
        Configuration_t goal = parameters.getGoal (path);
        NumericalConstraintPtr_t cc = NumericalConstraint::create
          (constraints::ConfigurationConstraint::create
           ("Optimization constraint", problem().robot(), goal, mask)
           );
        cc->function().context ("optimization");
        return cc;
      }

      bool ConfigOptimization::Optimizer::optimize (ConfigurationOut_t q,
          const std::size_t numIter,
          const value_type alpha) const
      {
        return proj->optimize (q, numIter, alpha);
      }

      ConfigProjectorPtr_t ConfigOptimizationTraits::getConfigProjector
        (const PathPtr_t& before, const PathPtr_t& /*after*/,
         bool& isReverse)
      {
        isReverse = false;
        ConfigProjectorPtr_t proj;
        ConstraintPtr_t c = before->constraints();
        if (c) {
          proj = HPP_DYNAMIC_PTR_CAST (ConfigProjector, c);
          if (!proj) {
            ConstraintSetPtr_t set =
              HPP_DYNAMIC_PTR_CAST (ConstraintSet, c);
            if (!set)
              throw std::runtime_error ("There are constraints of unknown types.");
            if (set->configProjector ()) {
              proj = set->configProjector ();
            }
          }
        }
        return proj;
      }

      std::size_t ConfigOptimization::buildOptimizers (const PathVector& pv,
          Optimizers_t& projectors)
      {
        NumericalConstraintPtr_t cc = createNumConstraint (pv);

        const std::size_t P = pv.numberPaths ();
        projectors.resize (P - 1);
        PathPtr_t cur = pv.pathAtRank (0);
        bool reverse = false, reverseStarted = false;
        std::size_t indexReverse = P-1;
        for (std::size_t i = 0; i < P - 1; ++i) {
          PathPtr_t next = pv.pathAtRank (i + 1);

          projectors[i].proj = parameters.getConfigProjector (cur, next, reverse);
          if (!projectors[i].proj) {
            projectors[i].proj = ConfigProjector::create ( problem().robot(),
                "Optimizer", 1e6, parameters.numberOfIterations);
          } else {
            projectors[i].proj = HPP_STATIC_PTR_CAST (ConfigProjector,
                projectors[i].proj->copy ());
          }
          // TODO: this is not robust since we do not know if level 1 is the
          // last
          projectors[i].proj->add (cc, SizeIntervals_t (0), 1);
          projectors[i].proj->lastIsOptional (true);

          if (reverse) {
            if (!reverseStarted) {
              indexReverse = i;
              reverseStarted = true;
            }
          }
          cur = next;
        }
        return indexReverse;
      }

      void ConfigOptimization::buildConfigVector (const PathVector& path,
          vectorOut_t configs) const
      {
        const std::size_t N = problem().robot()->configSize ();
        const std::size_t P = path.numberPaths();
        configs.resize (N * (P+1));

        std::size_t iP = 0;
        configs.segment (iP, N) = path.initial ();
        for (std::size_t i = 0; i < path.numberPaths(); ++i) {
          PathPtr_t cur = path.pathAtRank (i);
          iP += N;
          configs.segment (iP, N) = cur->end ();
        }
      }

      bool ConfigOptimizationTraits::shouldFilter (const JointPtr_t joint,
          const size_type /* iDof */)
      {
        // If joint if the root joint or if parent joint is the root joint and
        // root joint is an anchor joint,
        // filter it
        if (joint->parentJoint () == NULL ||
            (joint->parentJoint ()->parentJoint ()==NULL
             && joint->parentJoint()->numberDof () == 0)
           )
          return true;
        /// Filter joint name containing "base_joint..."
        if (boost::algorithm::contains (joint->name (), "base_joint"))
          return true;
        return false;
      }

      template <bool forward>
      bool ConfigOptimization::pass (
          vectorIn_t configs, vectorOut_t newConfigs,
          const Optimizers_t& optimizers, const std::size_t& index,
          const value_type& alpha, PathVectorPtr_t opted, bool& didChange
          ) const
      {
        SteeringMethod& sm (*problem ().steeringMethod ());
        std::list <PathPtr_t> paths;
        const std::size_t N = opted->outputSize ();
        const std::size_t P = optimizers.size () + 1;
        didChange = false;
        for (std::size_t i = (forward)? 1          : P-1;
                             (forward)? i <= index : i >= index;
                             (forward)? ++i        : --i) {
          newConfigs.segment (i * N, N) = configs.segment (i * N, N);
          std::size_t iN = (forward)? i - 1 : i + 1;
          /// Optimize
          bool isOpted = optimizers[i-1].optimize (
              newConfigs.segment (i * N, N),
              parameters.numberOfIterations, alpha);
          PathPtr_t current;
          if (isOpted)
            current = sm (newConfigs.segment (iN*N, N),
                          newConfigs.segment (i *N, N));
          if (!isOpted || !isValid (current)) {
            /// Try from last newConfigs to old ones.
            current = sm (newConfigs.segment (iN*N, N),
                             configs.segment (i *N, N));
            if (isValid (current)) {
              /// We do not change that configuration.
              newConfigs.segment (i * N, N) = configs.segment (i * N, N);
            } else {
              /// We should backtrack to previous iteration
              hppDout (info, "ConfigOptimization: failed at path rank " << i);
              return false;
            }
          } else
            didChange = true;
          if (forward) paths.push_back  (current);
          else         paths.push_front (current->reverse ());
        }

        /// Build the path
        for (std::list<PathPtr_t>::iterator it = paths.begin();
            it != paths.end (); ++it)
            opted->appendPath (*it);
        if (forward) {
          hppDout (info, "ConfigOptimization: forward pass succeeded.");
        } else {
          hppDout (info, "ConfigOptimization: backward pass succeeded.");
        }
        return true;
      }
    } // namespace pathOptimization
  } // namespace core
} // namespace hpp
<|endoftext|>


<commit_before>#include "catch.hpp"
#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time, to seed rand */

#include "ActuatorPwm.h"
#include "Ticks.h"
#include <cstring>
// delay ms miliseconds and return current time afterwards

ticks_millis_t delay(int ms) {
    ticks_millis_t newTime;
    for (int i = 0; i < ms; i++) {
        newTime = ticks.millis();
    }
    return newTime;
}

// delay random nr of milliseconds with maximum max

ticks_millis_t random_delay(int max) {
    int ms = rand() % (max - 1);
    return delay(ms);
}

uint8_t randomIntervalTest(ActuatorPwm* act, uint8_t duty, int delayMax) {
    act->setPwm(duty);
    ticks_millis_t lowToHighTime = ticks.millis();
    ticks_millis_t highToLowTime = ticks.millis();
    ticks_millis_t totalHighTime = 0;
    ticks_millis_t totalLowTime = 0;

    printf("\n\n*** Results running 100 periods and random 0-%d ms update intervals,"
            "with duty cycle %u and period %u ***\n",
            delayMax, duty, act->getPeriod());

    // run for 100 periods
    for (int i = 0; i < 100; i++) {
        do {
            highToLowTime = random_delay(delayMax);
            act->updatePwm();
        } while (act->isActive());
        ticks_millis_t highTime = highToLowTime - lowToHighTime;
        if (i > 0) { // skip first cycle in totals, it can be incomplete
            totalHighTime += highTime;
        }
        printf("_/ %10u \t \\_ %10u \t", lowToHighTime, highToLowTime);
        printf("high: %10u ms \t", highTime);
        do {
            lowToHighTime = random_delay(delayMax);
            act->updatePwm();
        } while (!act->isActive());
        ticks_millis_t lowTime = lowToHighTime - highToLowTime;
        if (i > 0) { // skip first cycle in totals, it can have old duty cycle
            totalLowTime += lowTime;
            printf("low: %10u ms \n", lowTime);
        }
        else{
            printf("low: %10u ms (ignored)\n", lowTime);
        }

    }
    double avgDuty = double(totalHighTime) / (totalHighTime + totalLowTime);
    int avgDutyInt = int(round(avgDuty * 255));
    printf("total high time: %10u\ntotal low time:  %10u \navg duty: %7.3f = %u/255\n",
            totalHighTime, totalLowTime, avgDuty, avgDutyInt);
    return avgDutyInt;
}

TEST_CASE("Test ActuatorPWM class with ValueActuator as driver", "[actuatorpwm]") {
    srand(time(NULL));
    Actuator * v = new ValueActuator();
    ActuatorPwm * act = new ActuatorPwm(v, 0);
    // intToTemp is a macro to initialize temperatures in Celsius

    SECTION("PWM value is initialized to 0") {
        REQUIRE(act->getPwm() == 0);
    }

    SECTION("Test PWM can be set and read") {
        act->setPwm(100);
        REQUIRE(act->getPwm() == 100);
        act->setPwm(255);
        REQUIRE(act->getPwm() == 255);
        act->setPwm(0);
        REQUIRE(act->getPwm() == 0);
    }

    SECTION("Test ticks.millis() to increment by 1 every call") {
        printf("Testing time:");
        ticks_millis_t start = ticks.millis();
        ticks_millis_t time;
        for (ticks_millis_t i = start + 1; i <= start + 10; i++) {
            time = ticks.millis();
            printf("% d", time);
            REQUIRE(i == time);
        }
        printf(".\n");
    }

    SECTION("Test that on and off time matches duty cycle when updating every ms") {
        const uint8_t duty = 100;
        act->setPwm(duty);
        ticks_millis_t lowToHighTime1 = ticks.millis();
        ticks_millis_t highToLowTime1 = ticks.millis();
        ticks_millis_t lowToHighTime2 = ticks.millis();
        do {
            lowToHighTime1 = delay(1);
            act->updatePwm();
        } while (!act->isActive());
        do {
            highToLowTime1 = delay(1);
            act->updatePwm();
        } while (act->isActive());
        do {
            lowToHighTime2 = delay(1);
            act->updatePwm();
        } while (!act->isActive());

        ticks_millis_t timeHigh = highToLowTime1 - lowToHighTime1;
        ticks_millis_t timeLow = lowToHighTime2 - highToLowTime1;
        ticks_millis_t actualDuty = timeHigh * 255 / (timeHigh + timeLow);
        printf("*** Timestamps testing one period with duty cycle %d and period %d***\n", duty, act->getPeriod());
        printf("lowToHigh1: %d \t highToLowTime1: %d \t lowToHigh2 %d \n", lowToHighTime1, highToLowTime1, lowToHighTime2);
        printf("time high: %d \t time low: %d \t actual duty cycle: %d\n", timeHigh, timeLow, actualDuty);
        REQUIRE(actualDuty == 100);
    }

    SECTION("Test that average duty cycle is correct, even with random update intervals") {
        REQUIRE((int) randomIntervalTest(act, 100, 500) == 100);
        REQUIRE((int) randomIntervalTest(act, 5, 500) == 5);
        REQUIRE((int) randomIntervalTest(act, 1, 500) == 1);
        REQUIRE((int) randomIntervalTest(act, 254, 500) == 254);
    }

    SECTION("Test output stays low with PWM value 0") {
        act->setPwm(0);
        // wait for new period
        while (act->isActive()) {
            delay(1);
            act->updatePwm();
        }
        while (!act->isActive()) {
            delay(1);
            act->updatePwm();
        }
        for (uint32_t i = 0; i < 10 * act->getPeriod(); i++) {
            delay(1);
            act->updatePwm();
            if (act->isActive()) {
                REQUIRE_FALSE(act->isActive());
            } // prevents many assertions
            // REQUIRE_FALSE(act->isActive());
        }
    }

    SECTION("Test output stays high with PWM value 255") {
        act->setPwm(255);
        // wait for new period
        while (act->isActive()) {
            delay(1);
            act->updatePwm();
        }
        while (!act->isActive()) {
            delay(1);
            act->updatePwm();
        }
        for (uint32_t i = 0; i < 10 * act->getPeriod(); i++) {
            delay(1);
            act->updatePwm();
            if (!act->isActive()) {
                REQUIRE(act->isActive());
            } // prevents many assertions
            // REQUIRE_TRUE(act->isActive());
        }
    }
}
<commit_msg>Prettier printing in unit test<commit_after>#include "catch.hpp"
#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time, to seed rand */

#include "ActuatorPwm.h"
#include "Ticks.h"
#include <cstring>
// delay ms miliseconds and return current time afterwards

ticks_millis_t delay(int ms) {
    ticks_millis_t newTime;
    for (int i = 0; i < ms; i++) {
        newTime = ticks.millis();
    }
    return newTime;
}

// delay random nr of milliseconds with maximum max

ticks_millis_t random_delay(int max) {
    int ms = rand() % (max - 1);
    return delay(ms);
}

uint8_t randomIntervalTest(ActuatorPwm* act, uint8_t duty, int delayMax) {
    act->setPwm(duty);
    ticks_millis_t lowToHighTime = ticks.millis();
    ticks_millis_t highToLowTime = ticks.millis();
    ticks_millis_t totalHighTime = 0;
    ticks_millis_t totalLowTime = 0;

    printf("\n\n*** Results running 100 periods and random 0-%d ms update intervals,"
            "with duty cycle %u and period %u ***\n",
            delayMax, duty, act->getPeriod());

    printf("\n\nl->h timestamp  h->l timestamp       high time       low time\n");
    // run for 100 periods
    for (int i = 0; i < 100; i++) {
        do {
            highToLowTime = random_delay(delayMax);
            act->updatePwm();
        } while (act->isActive());
        ticks_millis_t highTime = highToLowTime - lowToHighTime;
        if (i > 0) { // skip first cycle in totals, it can be incomplete
            totalHighTime += highTime;
        }
        printf("_/  %10u \t \\_ %10u \t", lowToHighTime, highToLowTime);
        printf(" %10u ms \t", highTime);
        do {
            lowToHighTime = random_delay(delayMax);
            act->updatePwm();
        } while (!act->isActive());
        ticks_millis_t lowTime = lowToHighTime - highToLowTime;
        if (i > 0) { // skip first cycle in totals, it can have old duty cycle
            totalLowTime += lowTime;
            printf("%10u ms \n", lowTime);
        }
        else{
            printf("%10u ms (ignored)\n", lowTime);
        }

    }
    double avgDuty = double(totalHighTime) / (totalHighTime + totalLowTime);
    int avgDutyInt = int(round(avgDuty * 255));
    printf("total high time: %10u\ntotal low time:  %10u \navg duty: %7.3f = %u/255\n",
            totalHighTime, totalLowTime, avgDuty, avgDutyInt);
    return avgDutyInt;
}

TEST_CASE("Test ActuatorPWM class with ValueActuator as driver", "[actuatorpwm]") {
    srand(time(NULL));
    Actuator * v = new ValueActuator();
    ActuatorPwm * act = new ActuatorPwm(v, 0);
    // intToTemp is a macro to initialize temperatures in Celsius

    SECTION("PWM value is initialized to 0") {
        REQUIRE(act->getPwm() == 0);
    }

    SECTION("Test PWM can be set and read") {
        act->setPwm(100);
        REQUIRE(act->getPwm() == 100);
        act->setPwm(255);
        REQUIRE(act->getPwm() == 255);
        act->setPwm(0);
        REQUIRE(act->getPwm() == 0);
    }

    SECTION("Test ticks.millis() to increment by 1 every call") {
        printf("Testing time:");
        ticks_millis_t start = ticks.millis();
        ticks_millis_t time;
        for (ticks_millis_t i = start + 1; i <= start + 10; i++) {
            time = ticks.millis();
            printf("% d", time);
            REQUIRE(i == time);
        }
        printf(".\n");
    }

    SECTION("Test that on and off time matches duty cycle when updating every ms") {
        const uint8_t duty = 100;
        act->setPwm(duty);
        ticks_millis_t lowToHighTime1 = ticks.millis();
        ticks_millis_t highToLowTime1 = ticks.millis();
        ticks_millis_t lowToHighTime2 = ticks.millis();
        do {
            lowToHighTime1 = delay(1);
            act->updatePwm();
        } while (!act->isActive());
        do {
            highToLowTime1 = delay(1);
            act->updatePwm();
        } while (act->isActive());
        do {
            lowToHighTime2 = delay(1);
            act->updatePwm();
        } while (!act->isActive());

        ticks_millis_t timeHigh = highToLowTime1 - lowToHighTime1;
        ticks_millis_t timeLow = lowToHighTime2 - highToLowTime1;
        ticks_millis_t actualDuty = timeHigh * 255 / (timeHigh + timeLow);
        printf("*** Timestamps testing one period with duty cycle %d and period %d***\n", duty, act->getPeriod());
        printf("lowToHigh1: %d \t highToLowTime1: %d \t lowToHigh2 %d \n", lowToHighTime1, highToLowTime1, lowToHighTime2);
        printf("time high: %d \t time low: %d \t actual duty cycle: %d\n", timeHigh, timeLow, actualDuty);
        REQUIRE(actualDuty == 100);
    }

    SECTION("Test that average duty cycle is correct, even with random update intervals") {
        REQUIRE((int) randomIntervalTest(act, 100, 500) == 100);
        REQUIRE((int) randomIntervalTest(act, 5, 500) == 5);
        REQUIRE((int) randomIntervalTest(act, 1, 500) == 1);
        REQUIRE((int) randomIntervalTest(act, 254, 500) == 254);
    }

    SECTION("Test output stays low with PWM value 0") {
        act->setPwm(0);
        // wait for new period
        while (act->isActive()) {
            delay(1);
            act->updatePwm();
        }
        while (!act->isActive()) {
            delay(1);
            act->updatePwm();
        }
        for (uint32_t i = 0; i < 10 * act->getPeriod(); i++) {
            delay(1);
            act->updatePwm();
            if (act->isActive()) {
                REQUIRE_FALSE(act->isActive());
            } // prevents many assertions
            // REQUIRE_FALSE(act->isActive());
        }
    }

    SECTION("Test output stays high with PWM value 255") {
        act->setPwm(255);
        // wait for new period
        while (act->isActive()) {
            delay(1);
            act->updatePwm();
        }
        while (!act->isActive()) {
            delay(1);
            act->updatePwm();
        }
        for (uint32_t i = 0; i < 10 * act->getPeriod(); i++) {
            delay(1);
            act->updatePwm();
            if (!act->isActive()) {
                REQUIRE(act->isActive());
            } // prevents many assertions
            // REQUIRE_TRUE(act->isActive());
        }
    }
}
<|endoftext|>


<commit_before>
/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: saldata.hxx,v $
 *
 *  $Revision: 1.19 $
 *
 *  last change: $Author: kz $ $Date: 2008-03-05 16:54:25 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _SV_SALDATA_HXX
#define _SV_SALDATA_HXX

#include "premac.h"
#include <Cocoa/Cocoa.h>
#include "postmac.h"

#include "vcl/sv.h"
#include "vcl/svdata.hxx"
#include "vcl/salwtype.hxx"
#include "vcl/ptrstyle.hxx"

#include <list>
#include <vector>
#include <hash_set>

#include <cstdio>
#include <cstdarg>

class AquaSalInstance;
class SalObject;
class SalFrame;
class SalVirtualDevice;
class SalPrinter;
class SystemFontList;

// ------------------
// - Some constants -
// ------------------

#define SAL_CLIPRECT_COUNT 16

// -----------
// - SalData -
// -----------

class AquaSalFrame;
struct FrameHash : public std::hash<sal_IntPtr>
{
    size_t operator()(const AquaSalFrame* frame) const
    { return std::hash<sal_IntPtr>::operator()( reinterpret_cast<const sal_IntPtr>(frame) ); }
};

#define INVALID_CURSOR_PTR (NSCursor*)0xdeadbeef

struct SalData
{

    SALTIMERPROC                                  mpTimerProc;      // timer callback proc
    AquaSalInstance                              *mpFirstInstance;  // pointer of first instance
    std::list<AquaSalFrame*>                      maFrames;         // pointer of first frame
    std::hash_set<const AquaSalFrame*,FrameHash>  maFrameCheck;     // for fast check of frame existance
    SalObject                                    *mpFirstObject;    // pointer of first object window
    SalVirtualDevice                             *mpFirstVD;        // first VirDev
    SalPrinter                                   *mpFirstPrinter;   // first printing printer
    SystemFontList                               *mpFontList;

    CGColorSpaceRef                               mxRGBSpace;
    CGColorSpaceRef                               mxGraySpace;

    std::vector< NSCursor* >                      maCursors;
    std::vector< NSMenuItem* >                    maFallbackMenu;

    static oslThreadKey                           s_aAutoReleaseKey;

    bool              mbIsScrollbarDoubleMax;   // TODO: support DoubleMin and DoubleBoth too

    SalData();
    ~SalData();

    NSCursor* getCursor( PointerStyle i_eStyle );

    static void ensureThreadAutoreleasePool();
    static void drainThreadAutoreleasePool();
};

inline void SetSalData( SalData* pData ) { ImplGetSVData()->mpSalData = (void*)pData; }
inline SalData *GetSalData() { return (SalData*)ImplGetSVData()->mpSalData; }
inline SalData *GetAppSalData() { return (SalData*)ImplGetAppSVData()->mpSalData; }

// --- Prototypes ---

BOOL ImplSalYieldMutexTryToAcquire();
void ImplSalYieldMutexAcquire();
void ImplSalYieldMutexRelease();

#endif  // _SV_SALDATA_HXX
<commit_msg>INTEGRATION: CWS changefileheader (1.19.22); FILE MERGED 2008/03/28 15:44:02 rt 1.19.22.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: saldata.hxx,v $
 * $Revision: 1.20 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

#ifndef _SV_SALDATA_HXX
#define _SV_SALDATA_HXX

#include "premac.h"
#include <Cocoa/Cocoa.h>
#include "postmac.h"

#include "vcl/sv.h"
#include "vcl/svdata.hxx"
#include "vcl/salwtype.hxx"
#include "vcl/ptrstyle.hxx"

#include <list>
#include <vector>
#include <hash_set>

#include <cstdio>
#include <cstdarg>

class AquaSalInstance;
class SalObject;
class SalFrame;
class SalVirtualDevice;
class SalPrinter;
class SystemFontList;

// ------------------
// - Some constants -
// ------------------

#define SAL_CLIPRECT_COUNT 16

// -----------
// - SalData -
// -----------

class AquaSalFrame;
struct FrameHash : public std::hash<sal_IntPtr>
{
    size_t operator()(const AquaSalFrame* frame) const
    { return std::hash<sal_IntPtr>::operator()( reinterpret_cast<const sal_IntPtr>(frame) ); }
};

#define INVALID_CURSOR_PTR (NSCursor*)0xdeadbeef

struct SalData
{

    SALTIMERPROC                                  mpTimerProc;      // timer callback proc
    AquaSalInstance                              *mpFirstInstance;  // pointer of first instance
    std::list<AquaSalFrame*>                      maFrames;         // pointer of first frame
    std::hash_set<const AquaSalFrame*,FrameHash>  maFrameCheck;     // for fast check of frame existance
    SalObject                                    *mpFirstObject;    // pointer of first object window
    SalVirtualDevice                             *mpFirstVD;        // first VirDev
    SalPrinter                                   *mpFirstPrinter;   // first printing printer
    SystemFontList                               *mpFontList;

    CGColorSpaceRef                               mxRGBSpace;
    CGColorSpaceRef                               mxGraySpace;

    std::vector< NSCursor* >                      maCursors;
    std::vector< NSMenuItem* >                    maFallbackMenu;

    static oslThreadKey                           s_aAutoReleaseKey;

    bool              mbIsScrollbarDoubleMax;   // TODO: support DoubleMin and DoubleBoth too

    SalData();
    ~SalData();

    NSCursor* getCursor( PointerStyle i_eStyle );

    static void ensureThreadAutoreleasePool();
    static void drainThreadAutoreleasePool();
};

inline void SetSalData( SalData* pData ) { ImplGetSVData()->mpSalData = (void*)pData; }
inline SalData *GetSalData() { return (SalData*)ImplGetSVData()->mpSalData; }
inline SalData *GetAppSalData() { return (SalData*)ImplGetAppSVData()->mpSalData; }

// --- Prototypes ---

BOOL ImplSalYieldMutexTryToAcquire();
void ImplSalYieldMutexAcquire();
void ImplSalYieldMutexRelease();

#endif  // _SV_SALDATA_HXX
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: decoview.hxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: kz $ $Date: 2008-04-04 11:00:23 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _SV_DECOVIEW_HXX
#define _SV_DECOVIEW_HXX

#ifndef _SV_SV_H
#include <vcl/sv.h>
#endif

#ifndef _VCL_DLLAPI_H
#include <vcl/dllapi.h>
#endif

#ifndef _SV_SYMBOL_HXX
#include <vcl/symbol.hxx>
#endif

class Rectangle;
class Color;
class OutputDevice;

// ------------------------
// - DecorationView-Types -
// ------------------------

// Flags for DrawSymbol()
#define SYMBOL_DRAW_MONO                    ((USHORT)0x0001)
#define SYMBOL_DRAW_DISABLE                 ((USHORT)0x0002)

// Flags for DrawFrame()
#define FRAME_DRAW_IN                       ((USHORT)0x0001)
#define FRAME_DRAW_OUT                      ((USHORT)0x0002)
#define FRAME_DRAW_GROUP                    ((USHORT)0x0003)
#define FRAME_DRAW_DOUBLEIN                 ((USHORT)0x0004)
#define FRAME_DRAW_DOUBLEOUT                ((USHORT)0x0005)
#define FRAME_DRAW_MENU                     ((USHORT)0x0010)
#define FRAME_DRAW_WINDOWBORDER             ((USHORT)0x0020)
#define FRAME_DRAW_BORDERWINDOWBORDER       ((USHORT)0x0040)
#define FRAME_DRAW_MONO                     ((USHORT)0x1000)
#define FRAME_DRAW_NODRAW                   ((USHORT)0x8000)
#define FRAME_DRAW_STYLE                    ((USHORT)0x000F)

// Flags for DrawHighlightFrame()
#define FRAME_HIGHLIGHT_IN                  ((USHORT)0x0001)
#define FRAME_HIGHLIGHT_OUT                 ((USHORT)0x0002)
#define FRAME_HIGHLIGHT_TESTBACKGROUND      ((USHORT)0x4000)
#define FRAME_HIGHLIGHT_STYLE               ((USHORT)0x000F)

// Flags for DrawButton()
#define BUTTON_DRAW_DEFAULT                 ((USHORT)0x0001)
#define BUTTON_DRAW_NOLIGHTBORDER           ((USHORT)0x0002)
#define BUTTON_DRAW_PRESSED                 ((USHORT)0x0004)
#define BUTTON_DRAW_CHECKED                 ((USHORT)0x0008)
#define BUTTON_DRAW_DONTKNOW                ((USHORT)0x0010)
#define BUTTON_DRAW_MONO                    ((USHORT)0x0020)
#define BUTTON_DRAW_NOFILL                  ((USHORT)0x0040)
#define BUTTON_DRAW_DISABLED                ((USHORT)0x0080)
#define BUTTON_DRAW_HIGHLIGHT               ((USHORT)0x0100)
#define BUTTON_DRAW_FLAT                    ((USHORT)0x0200)
#define BUTTON_DRAW_NOTOPLIGHTBORDER        ((USHORT)0x0400)
#define BUTTON_DRAW_NOBOTTOMSHADOWBORDER    ((USHORT)0x0800)
#define BUTTON_DRAW_NOLEFTLIGHTBORDER       ((USHORT)0x1000)
#define BUTTON_DRAW_NOTEXT                  ((USHORT)0x2000)
#define BUTTON_DRAW_NOIMAGE             ((USHORT)0x4000)
#define BUTTON_DRAW_NODRAW                  ((USHORT)0x8000)

// ------------------
// - DecorationView -
// ------------------

class VCL_DLLPUBLIC DecorationView
{
private:
    OutputDevice*       mpOutDev;

public:
                        DecorationView( OutputDevice* pOutDev )
                            { mpOutDev = pOutDev; }

    void                DrawSymbol( const Rectangle& rRect, SymbolType eType,
                                    const Color& rColor, USHORT nStyle = 0 );
    void                DrawFrame( const Rectangle& rRect,
                                   const Color& rLeftTopColor,
                                   const Color& rRightBottomColor );
    void                DrawHighlightFrame( const Rectangle& rRect,
                                            USHORT nStyle = FRAME_HIGHLIGHT_OUT );
    Rectangle           DrawFrame( const Rectangle& rRect, USHORT nStyle = FRAME_DRAW_OUT );
    Rectangle           DrawButton( const Rectangle& rRect, USHORT nStyle );
};

#endif // _SV_DECOVIEW_HXX
<commit_msg>INTEGRATION: CWS changefileheader (1.2.320); FILE MERGED 2008/04/01 13:01:07 thb 1.2.320.2: #i85898# Stripping all external header guards 2008/03/28 15:44:11 rt 1.2.320.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: decoview.hxx,v $
 * $Revision: 1.4 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

#ifndef _SV_DECOVIEW_HXX
#define _SV_DECOVIEW_HXX

#include <vcl/sv.h>
#include <vcl/dllapi.h>
#include <vcl/symbol.hxx>

class Rectangle;
class Color;
class OutputDevice;

// ------------------------
// - DecorationView-Types -
// ------------------------

// Flags for DrawSymbol()
#define SYMBOL_DRAW_MONO                    ((USHORT)0x0001)
#define SYMBOL_DRAW_DISABLE                 ((USHORT)0x0002)

// Flags for DrawFrame()
#define FRAME_DRAW_IN                       ((USHORT)0x0001)
#define FRAME_DRAW_OUT                      ((USHORT)0x0002)
#define FRAME_DRAW_GROUP                    ((USHORT)0x0003)
#define FRAME_DRAW_DOUBLEIN                 ((USHORT)0x0004)
#define FRAME_DRAW_DOUBLEOUT                ((USHORT)0x0005)
#define FRAME_DRAW_MENU                     ((USHORT)0x0010)
#define FRAME_DRAW_WINDOWBORDER             ((USHORT)0x0020)
#define FRAME_DRAW_BORDERWINDOWBORDER       ((USHORT)0x0040)
#define FRAME_DRAW_MONO                     ((USHORT)0x1000)
#define FRAME_DRAW_NODRAW                   ((USHORT)0x8000)
#define FRAME_DRAW_STYLE                    ((USHORT)0x000F)

// Flags for DrawHighlightFrame()
#define FRAME_HIGHLIGHT_IN                  ((USHORT)0x0001)
#define FRAME_HIGHLIGHT_OUT                 ((USHORT)0x0002)
#define FRAME_HIGHLIGHT_TESTBACKGROUND      ((USHORT)0x4000)
#define FRAME_HIGHLIGHT_STYLE               ((USHORT)0x000F)

// Flags for DrawButton()
#define BUTTON_DRAW_DEFAULT                 ((USHORT)0x0001)
#define BUTTON_DRAW_NOLIGHTBORDER           ((USHORT)0x0002)
#define BUTTON_DRAW_PRESSED                 ((USHORT)0x0004)
#define BUTTON_DRAW_CHECKED                 ((USHORT)0x0008)
#define BUTTON_DRAW_DONTKNOW                ((USHORT)0x0010)
#define BUTTON_DRAW_MONO                    ((USHORT)0x0020)
#define BUTTON_DRAW_NOFILL                  ((USHORT)0x0040)
#define BUTTON_DRAW_DISABLED                ((USHORT)0x0080)
#define BUTTON_DRAW_HIGHLIGHT               ((USHORT)0x0100)
#define BUTTON_DRAW_FLAT                    ((USHORT)0x0200)
#define BUTTON_DRAW_NOTOPLIGHTBORDER        ((USHORT)0x0400)
#define BUTTON_DRAW_NOBOTTOMSHADOWBORDER    ((USHORT)0x0800)
#define BUTTON_DRAW_NOLEFTLIGHTBORDER       ((USHORT)0x1000)
#define BUTTON_DRAW_NOTEXT                  ((USHORT)0x2000)
#define BUTTON_DRAW_NOIMAGE             ((USHORT)0x4000)
#define BUTTON_DRAW_NODRAW                  ((USHORT)0x8000)

// ------------------
// - DecorationView -
// ------------------

class VCL_DLLPUBLIC DecorationView
{
private:
    OutputDevice*       mpOutDev;

public:
                        DecorationView( OutputDevice* pOutDev )
                            { mpOutDev = pOutDev; }

    void                DrawSymbol( const Rectangle& rRect, SymbolType eType,
                                    const Color& rColor, USHORT nStyle = 0 );
    void                DrawFrame( const Rectangle& rRect,
                                   const Color& rLeftTopColor,
                                   const Color& rRightBottomColor );
    void                DrawHighlightFrame( const Rectangle& rRect,
                                            USHORT nStyle = FRAME_HIGHLIGHT_OUT );
    Rectangle           DrawFrame( const Rectangle& rRect, USHORT nStyle = FRAME_DRAW_OUT );
    Rectangle           DrawButton( const Rectangle& rRect, USHORT nStyle );
};

#endif // _SV_DECOVIEW_HXX
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: alpha.cxx,v $
 *
 *  $Revision: 1.5 $
 *
 *  last change: $Author: rt $ $Date: 2004-11-03 16:04:49 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef _DEBUG_HXX
#include <tools/debug.hxx>
#endif
#ifndef _SV_BMPACC_HXX
#include <bmpacc.hxx>
#endif
#ifndef _TOOLS_COLOR_HXX
#include <tools/color.hxx>
#endif
#ifndef _SV_ALPHA_HXX
#include <alpha.hxx>
#endif

// -------------
// - AlphaMask -
// -------------

AlphaMask::AlphaMask()
{
}

// -----------------------------------------------------------------------------

AlphaMask::AlphaMask( const Bitmap& rBitmap ) :
    Bitmap( rBitmap )
{
    if( !!rBitmap )
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );
}

// -----------------------------------------------------------------------------

AlphaMask::AlphaMask( const AlphaMask& rAlphaMask ) :
    Bitmap( rAlphaMask )
{
}

// -----------------------------------------------------------------------------

AlphaMask::AlphaMask( const Size& rSizePixel, BYTE* pEraseTransparency ) :
    Bitmap( rSizePixel, 8, &Bitmap::GetGreyPalette( 256 ) )
{
    if( pEraseTransparency )
        Bitmap::Erase( Color( *pEraseTransparency, *pEraseTransparency, *pEraseTransparency ) );
}

// -----------------------------------------------------------------------------

AlphaMask::~AlphaMask()
{
}

// -----------------------------------------------------------------------------

AlphaMask& AlphaMask::operator=( const Bitmap& rBitmap )
{
    *(Bitmap*) this = rBitmap;

    if( !!rBitmap )
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );

    return *this;
}

// -----------------------------------------------------------------------------

const Bitmap& AlphaMask::ImplGetBitmap() const
{
    return( (const Bitmap&) *this );
}

// -----------------------------------------------------------------------------

void AlphaMask::ImplSetBitmap( const Bitmap& rBitmap )
{
    DBG_ASSERT( ( 8 == rBitmap.GetBitCount() ) && rBitmap.HasGreyPalette(), "AlphaMask::ImplSetBitmap: invalid bitmap" );
    *(Bitmap*) this = rBitmap;
}

// -----------------------------------------------------------------------------

Bitmap AlphaMask::GetBitmap() const
{
    return ImplGetBitmap();
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Crop( const Rectangle& rRectPixel )
{
    return Bitmap::Crop( rRectPixel );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Expand( ULONG nDX, ULONG nDY, BYTE* pInitTransparency )
{
    Color aColor;

    if( pInitTransparency )
        aColor = Color( *pInitTransparency, *pInitTransparency, *pInitTransparency );

    return Bitmap::Expand( nDX, nDY, pInitTransparency ? &aColor : NULL );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::CopyPixel( const Rectangle& rRectDst, const Rectangle& rRectSrc,
                           const AlphaMask* pAlphaSrc )
{
    return Bitmap::CopyPixel( rRectDst, rRectSrc, (Bitmap*) pAlphaSrc );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Erase( BYTE cTransparency )
{
    return Bitmap::Erase( Color( cTransparency, cTransparency, cTransparency ) );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Invert()
{
    BitmapWriteAccess*  pAcc = AcquireWriteAccess();
    BOOL                bRet = FALSE;

    if( pAcc && pAcc->GetBitCount() == 8 )
    {
        BitmapColor aCol( 0 );
        const long  nWidth = pAcc->Width(), nHeight = pAcc->Height();
        BYTE*       pMap = new BYTE[ 256 ];

        for( long i = 0; i < 256; i++ )
            pMap[ i ] = ~(BYTE) i;

        for( long nY = 0L; nY < nHeight; nY++ )
        {
            for( long nX = 0L; nX < nWidth; nX++ )
            {
                aCol.SetIndex( pMap[ pAcc->GetPixel( nY, nX ).GetIndex() ] );
                pAcc->SetPixel( nY, nX, aCol );
            }
        }

        delete[] pMap;
        bRet = TRUE;
    }

    if( pAcc )
        ReleaseAccess( pAcc );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Mirror( ULONG nMirrorFlags )
{
    return Bitmap::Mirror( nMirrorFlags );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Scale( const Size& rNewSize, ULONG nScaleFlag )
{
    BOOL bRet = Bitmap::Scale( rNewSize, nScaleFlag );

    if( bRet && ( nScaleFlag == BMP_SCALE_INTERPOLATE ) )
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Scale( const double& rScaleX, const double& rScaleY, ULONG nScaleFlag )
{
    BOOL bRet = Bitmap::Scale( rScaleX, rScaleY, nScaleFlag );

    if( bRet && ( nScaleFlag == BMP_SCALE_INTERPOLATE ) )
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Rotate( long nAngle10, BYTE cFillTransparency )
{
    return Bitmap::Rotate( nAngle10, Color( cFillTransparency, cFillTransparency, cFillTransparency ) );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Replace( const Bitmap& rMask, BYTE cReplaceTransparency )
{
    BitmapReadAccess*   pMaskAcc = ( (Bitmap&) rMask ).AcquireReadAccess();
    BitmapWriteAccess*  pAcc = AcquireWriteAccess();
    BOOL                bRet = FALSE;

    if( pMaskAcc && pAcc )
    {
        const BitmapColor   aReplace( cReplaceTransparency );
        const long          nWidth = Min( pMaskAcc->Width(), pAcc->Width() );
        const long          nHeight = Min( pMaskAcc->Height(), pAcc->Height() );
        const BitmapColor   aMaskWhite( pMaskAcc->GetBestMatchingColor( Color( COL_WHITE ) ) );

        for( long nY = 0L; nY < nHeight; nY++ )
            for( long nX = 0L; nX < nWidth; nX++ )
                if( pMaskAcc->GetPixel( nY, nX ) == aMaskWhite )
                    pAcc->SetPixel( nY, nX, aReplace );
    }

    ( (Bitmap&) rMask ).ReleaseAccess( pMaskAcc );
    ReleaseAccess( pAcc );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Replace( BYTE cSearchTransparency, BYTE cReplaceTransparency, ULONG nTol )
{
    BitmapWriteAccess*  pAcc = AcquireWriteAccess();
    BOOL                bRet = FALSE;

    DBG_ASSERT( !nTol, "AlphaMask::Replace: nTol not used yet" );

    if( pAcc && pAcc->GetBitCount() == 8 )
    {
        const long nWidth = pAcc->Width(), nHeight = pAcc->Height();

        if( pAcc->GetScanlineFormat() == BMP_FORMAT_8BIT_PAL )
        {
            for( long nY = 0L; nY < nHeight; nY++ )
            {
                Scanline pScan = pAcc->GetScanline( nY );

                for( long nX = 0L; nX < nWidth; nX++, pScan++ )
                {
                    if( *pScan == cSearchTransparency )
                        *pScan = cReplaceTransparency;
                }
            }
        }
        else
        {
            BitmapColor aReplace( cReplaceTransparency );

            for( long nY = 0L; nY < nHeight; nY++ )
            {
                for( long nX = 0L; nX < nWidth; nX++ )
                {
                    if( pAcc->GetPixel( nY, nX ).GetIndex() == cSearchTransparency )
                        pAcc->SetPixel( nY, nX, aReplace );
                }
            }
        }

        bRet = TRUE;
    }

    if( pAcc )
        ReleaseAccess( pAcc );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Replace( BYTE* pSearchTransparencies, BYTE* pReplaceTransparencies,
                         ULONG nColorCount, ULONG* pTols )
{
    Color*  pSearchColors = new Color[ nColorCount ];
    Color*  pReplaceColors = new Color[ nColorCount ];
    BOOL    bRet;

    for( ULONG i = 0; i < nColorCount; i++ )
    {
        const BYTE cSearchTransparency = pSearchTransparencies[ i ];
        const BYTE cReplaceTransparency = pReplaceTransparencies[ i ];

        pSearchColors[ i ] = Color( cSearchTransparency, cSearchTransparency, cSearchTransparency );
        pReplaceColors[ i ] = Color( cReplaceTransparency, cReplaceTransparency, cReplaceTransparency );
    }

    bRet = Bitmap::Replace( pSearchColors, pReplaceColors, nColorCount, pTols ) &&
           Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );

    delete[] pSearchColors;
    delete[] pReplaceColors;

    return bRet;
}

// -----------------------------------------------------------------------------

void AlphaMask::ReleaseAccess( BitmapReadAccess* pAccess )
{
    if( pAccess )
    {
        Bitmap::ReleaseAccess( pAccess );
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );
    }
}
<commit_msg>INTEGRATION: CWS perform02 (1.5.88); FILE MERGED 2005/02/14 16:22:00 ssa 1.5.88.1: #i42762# improve and use AlphaMask::CopyPixel() to avoid palette lookups<commit_after>/*************************************************************************
 *
 *  $RCSfile: alpha.cxx,v $
 *
 *  $Revision: 1.6 $
 *
 *  last change: $Author: rt $ $Date: 2005-03-29 14:37:00 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/
#ifndef _DEBUG_HXX
#include <tools/debug.hxx>
#endif
#ifndef _SV_BMPACC_HXX
#include <bmpacc.hxx>
#endif
#ifndef _TOOLS_COLOR_HXX
#include <tools/color.hxx>
#endif
#ifndef _SV_ALPHA_HXX
#include <alpha.hxx>
#endif

// -------------
// - AlphaMask -
// -------------

AlphaMask::AlphaMask()
{
}

// -----------------------------------------------------------------------------

AlphaMask::AlphaMask( const Bitmap& rBitmap ) :
    Bitmap( rBitmap )
{
    if( !!rBitmap )
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );
}

// -----------------------------------------------------------------------------

AlphaMask::AlphaMask( const AlphaMask& rAlphaMask ) :
    Bitmap( rAlphaMask )
{
}

// -----------------------------------------------------------------------------

AlphaMask::AlphaMask( const Size& rSizePixel, BYTE* pEraseTransparency ) :
    Bitmap( rSizePixel, 8, &Bitmap::GetGreyPalette( 256 ) )
{
    if( pEraseTransparency )
        Bitmap::Erase( Color( *pEraseTransparency, *pEraseTransparency, *pEraseTransparency ) );
}

// -----------------------------------------------------------------------------

AlphaMask::~AlphaMask()
{
}

// -----------------------------------------------------------------------------

AlphaMask& AlphaMask::operator=( const Bitmap& rBitmap )
{
    *(Bitmap*) this = rBitmap;

    if( !!rBitmap )
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );

    return *this;
}

// -----------------------------------------------------------------------------

const Bitmap& AlphaMask::ImplGetBitmap() const
{
    return( (const Bitmap&) *this );
}

// -----------------------------------------------------------------------------

void AlphaMask::ImplSetBitmap( const Bitmap& rBitmap )
{
    DBG_ASSERT( ( 8 == rBitmap.GetBitCount() ) && rBitmap.HasGreyPalette(), "AlphaMask::ImplSetBitmap: invalid bitmap" );
    *(Bitmap*) this = rBitmap;
}

// -----------------------------------------------------------------------------

Bitmap AlphaMask::GetBitmap() const
{
    return ImplGetBitmap();
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Crop( const Rectangle& rRectPixel )
{
    return Bitmap::Crop( rRectPixel );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Expand( ULONG nDX, ULONG nDY, BYTE* pInitTransparency )
{
    Color aColor;

    if( pInitTransparency )
        aColor = Color( *pInitTransparency, *pInitTransparency, *pInitTransparency );

    return Bitmap::Expand( nDX, nDY, pInitTransparency ? &aColor : NULL );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::CopyPixel( const Rectangle& rRectDst, const Rectangle& rRectSrc,
                           const AlphaMask* pAlphaSrc )
{
    // Note: this code is copied from Bitmap::CopyPixel but avoids any palette lookups
    // this optimization is possible because the palettes of AlphaMasks are always identical (8bit GreyPalette, see ctor)

    const Size  aSizePix( GetSizePixel() );
    Rectangle   aRectDst( rRectDst );
    BOOL        bRet = FALSE;

    aRectDst.Intersection( Rectangle( Point(), aSizePix ) );

    if( !aRectDst.IsEmpty() )
    {
        if( pAlphaSrc && ( *pAlphaSrc != *this ) )
        {
            Bitmap*         pSrc = (Bitmap*) pAlphaSrc;
            const Size      aCopySizePix( pSrc->GetSizePixel() );
            Rectangle       aRectSrc( rRectSrc );

            aRectSrc.Intersection( Rectangle( Point(), aCopySizePix ) );

            if( !aRectSrc.IsEmpty() )
            {
                BitmapReadAccess* pReadAcc = pSrc->AcquireReadAccess();

                if( pReadAcc )
                {
                    BitmapWriteAccess* pWriteAcc = AcquireWriteAccess();

                    if( pWriteAcc )
                    {
                        const long  nWidth = Min( aRectSrc.GetWidth(), aRectDst.GetWidth() );
                        const long  nHeight = Min( aRectSrc.GetHeight(), aRectDst.GetHeight() );
                        const long  nSrcEndX = aRectSrc.Left() + nWidth;
                        const long  nSrcEndY = aRectSrc.Top() + nHeight;
                        long        nDstY = aRectDst.Top();

                        for( long nSrcY = aRectSrc.Top(); nSrcY < nSrcEndY; nSrcY++, nDstY++ )
                            for( long nSrcX = aRectSrc.Left(), nDstX = aRectDst.Left(); nSrcX < nSrcEndX; nSrcX++, nDstX++ )
                                pWriteAcc->SetPixel( nDstY, nDstX, pReadAcc->GetPixel( nSrcY, nSrcX ) );

                        ReleaseAccess( pWriteAcc );
                        bRet = ( nWidth > 0L ) && ( nHeight > 0L );
                    }

                    pSrc->ReleaseAccess( pReadAcc );
                }
            }
        }
        else
        {
            Rectangle aRectSrc( rRectSrc );

            aRectSrc.Intersection( Rectangle( Point(), aSizePix ) );

            if( !aRectSrc.IsEmpty() && ( aRectSrc != aRectDst ) )
            {
                BitmapWriteAccess*  pWriteAcc = AcquireWriteAccess();

                if( pWriteAcc )
                {
                    const long  nWidth = Min( aRectSrc.GetWidth(), aRectDst.GetWidth() );
                    const long  nHeight = Min( aRectSrc.GetHeight(), aRectDst.GetHeight() );
                    const long  nSrcX = aRectSrc.Left();
                    const long  nSrcY = aRectSrc.Top();
                    const long  nSrcEndX1 = nSrcX + nWidth - 1L;
                    const long  nSrcEndY1 = nSrcY + nHeight - 1L;
                    const long  nDstX = aRectDst.Left();
                    const long  nDstY = aRectDst.Top();
                    const long  nDstEndX1 = nDstX + nWidth - 1L;
                    const long  nDstEndY1 = nDstY + nHeight - 1L;

                    if( ( nDstX <= nSrcX ) && ( nDstY <= nSrcY ) )
                    {
                        for( long nY = nSrcY, nYN = nDstY; nY <= nSrcEndY1; nY++, nYN++ )
                            for( long nX = nSrcX, nXN = nDstX; nX <= nSrcEndX1; nX++, nXN++ )
                                pWriteAcc->SetPixel( nYN, nXN, pWriteAcc->GetPixel( nY, nX ) );
                    }
                    else if( ( nDstX <= nSrcX ) && ( nDstY >= nSrcY ) )
                    {
                        for( long nY = nSrcEndY1, nYN = nDstEndY1; nY >= nSrcY; nY--, nYN-- )
                            for( long nX = nSrcX, nXN = nDstX; nX <= nSrcEndX1; nX++, nXN++ )
                                pWriteAcc->SetPixel( nYN, nXN, pWriteAcc->GetPixel( nY, nX ) );
                    }
                    else if( ( nDstX >= nSrcX ) && ( nDstY <= nSrcY ) )
                    {
                        for( long nY = nSrcY, nYN = nDstY; nY <= nSrcEndY1; nY++, nYN++ )
                            for( long nX = nSrcEndX1, nXN = nDstEndX1; nX >= nSrcX; nX--, nXN-- )
                                pWriteAcc->SetPixel( nYN, nXN, pWriteAcc->GetPixel( nY, nX ) );
                    }
                    else
                    {
                        for( long nY = nSrcEndY1, nYN = nDstEndY1; nY >= nSrcY; nY--, nYN-- )
                            for( long nX = nSrcEndX1, nXN = nDstEndX1; nX >= nSrcX; nX--, nXN-- )
                                pWriteAcc->SetPixel( nYN, nXN, pWriteAcc->GetPixel( nY, nX ) );
                    }

                    ReleaseAccess( pWriteAcc );
                    bRet = TRUE;
                }
            }
        }
    }

    return bRet;

}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Erase( BYTE cTransparency )
{
    return Bitmap::Erase( Color( cTransparency, cTransparency, cTransparency ) );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Invert()
{
    BitmapWriteAccess*  pAcc = AcquireWriteAccess();
    BOOL                bRet = FALSE;

    if( pAcc && pAcc->GetBitCount() == 8 )
    {
        BitmapColor aCol( 0 );
        const long  nWidth = pAcc->Width(), nHeight = pAcc->Height();
        BYTE*       pMap = new BYTE[ 256 ];

        for( long i = 0; i < 256; i++ )
            pMap[ i ] = ~(BYTE) i;

        for( long nY = 0L; nY < nHeight; nY++ )
        {
            for( long nX = 0L; nX < nWidth; nX++ )
            {
                aCol.SetIndex( pMap[ pAcc->GetPixel( nY, nX ).GetIndex() ] );
                pAcc->SetPixel( nY, nX, aCol );
            }
        }

        delete[] pMap;
        bRet = TRUE;
    }

    if( pAcc )
        ReleaseAccess( pAcc );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Mirror( ULONG nMirrorFlags )
{
    return Bitmap::Mirror( nMirrorFlags );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Scale( const Size& rNewSize, ULONG nScaleFlag )
{
    BOOL bRet = Bitmap::Scale( rNewSize, nScaleFlag );

    if( bRet && ( nScaleFlag == BMP_SCALE_INTERPOLATE ) )
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Scale( const double& rScaleX, const double& rScaleY, ULONG nScaleFlag )
{
    BOOL bRet = Bitmap::Scale( rScaleX, rScaleY, nScaleFlag );

    if( bRet && ( nScaleFlag == BMP_SCALE_INTERPOLATE ) )
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Rotate( long nAngle10, BYTE cFillTransparency )
{
    return Bitmap::Rotate( nAngle10, Color( cFillTransparency, cFillTransparency, cFillTransparency ) );
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Replace( const Bitmap& rMask, BYTE cReplaceTransparency )
{
    BitmapReadAccess*   pMaskAcc = ( (Bitmap&) rMask ).AcquireReadAccess();
    BitmapWriteAccess*  pAcc = AcquireWriteAccess();
    BOOL                bRet = FALSE;

    if( pMaskAcc && pAcc )
    {
        const BitmapColor   aReplace( cReplaceTransparency );
        const long          nWidth = Min( pMaskAcc->Width(), pAcc->Width() );
        const long          nHeight = Min( pMaskAcc->Height(), pAcc->Height() );
        const BitmapColor   aMaskWhite( pMaskAcc->GetBestMatchingColor( Color( COL_WHITE ) ) );

        for( long nY = 0L; nY < nHeight; nY++ )
            for( long nX = 0L; nX < nWidth; nX++ )
                if( pMaskAcc->GetPixel( nY, nX ) == aMaskWhite )
                    pAcc->SetPixel( nY, nX, aReplace );
    }

    ( (Bitmap&) rMask ).ReleaseAccess( pMaskAcc );
    ReleaseAccess( pAcc );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Replace( BYTE cSearchTransparency, BYTE cReplaceTransparency, ULONG nTol )
{
    BitmapWriteAccess*  pAcc = AcquireWriteAccess();
    BOOL                bRet = FALSE;

    DBG_ASSERT( !nTol, "AlphaMask::Replace: nTol not used yet" );

    if( pAcc && pAcc->GetBitCount() == 8 )
    {
        const long nWidth = pAcc->Width(), nHeight = pAcc->Height();

        if( pAcc->GetScanlineFormat() == BMP_FORMAT_8BIT_PAL )
        {
            for( long nY = 0L; nY < nHeight; nY++ )
            {
                Scanline pScan = pAcc->GetScanline( nY );

                for( long nX = 0L; nX < nWidth; nX++, pScan++ )
                {
                    if( *pScan == cSearchTransparency )
                        *pScan = cReplaceTransparency;
                }
            }
        }
        else
        {
            BitmapColor aReplace( cReplaceTransparency );

            for( long nY = 0L; nY < nHeight; nY++ )
            {
                for( long nX = 0L; nX < nWidth; nX++ )
                {
                    if( pAcc->GetPixel( nY, nX ).GetIndex() == cSearchTransparency )
                        pAcc->SetPixel( nY, nX, aReplace );
                }
            }
        }

        bRet = TRUE;
    }

    if( pAcc )
        ReleaseAccess( pAcc );

    return bRet;
}

// -----------------------------------------------------------------------------

BOOL AlphaMask::Replace( BYTE* pSearchTransparencies, BYTE* pReplaceTransparencies,
                         ULONG nColorCount, ULONG* pTols )
{
    Color*  pSearchColors = new Color[ nColorCount ];
    Color*  pReplaceColors = new Color[ nColorCount ];
    BOOL    bRet;

    for( ULONG i = 0; i < nColorCount; i++ )
    {
        const BYTE cSearchTransparency = pSearchTransparencies[ i ];
        const BYTE cReplaceTransparency = pReplaceTransparencies[ i ];

        pSearchColors[ i ] = Color( cSearchTransparency, cSearchTransparency, cSearchTransparency );
        pReplaceColors[ i ] = Color( cReplaceTransparency, cReplaceTransparency, cReplaceTransparency );
    }

    bRet = Bitmap::Replace( pSearchColors, pReplaceColors, nColorCount, pTols ) &&
           Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );

    delete[] pSearchColors;
    delete[] pReplaceColors;

    return bRet;
}

// -----------------------------------------------------------------------------

void AlphaMask::ReleaseAccess( BitmapReadAccess* pAccess )
{
    if( pAccess )
    {
        Bitmap::ReleaseAccess( pAccess );
        Bitmap::Convert( BMP_CONVERSION_8BIT_GREYS );
    }
}
<|endoftext|>


<commit_before>#include "ros/ros.h"
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <eigen_conversions/eigen_msg.h>
#include "geometry_msgs/Wrench.h"
#include "geometry_msgs/Pose.h"
#include "uranus_dp/State.h"
#include "uranus_dp/SetControlMode.h"
#include "../src/control_mode_enum.h"

class OpenLoopControllerTest : public ::testing::Test
{
public:
    OpenLoopControllerTest()
    {
        pub    = nh.advertise<geometry_msgs::Wrench>("wrench_setpoints", 1);
        sub    = nh.subscribe("rov_forces", 5, &OpenLoopControllerTest::Callback, this);
        client = nh.serviceClient<uranus_dp::SetControlMode>("set_control_mode");

        message_received = false;
    }

    void SetUp()
    {
        while (!IsNodeReady())
        {
            ros::spinOnce();
        }
    }

    void PublishSetpoint(double X, double Y, double Z, double K, double M, double N)
    {
        geometry_msgs::Wrench msg;
        msg.force.x  = X;
        msg.force.y  = Y;
        msg.force.z  = Z;
        msg.torque.x = K;
        msg.torque.y = M;
        msg.torque.z = N;
        pub.publish(msg);
    }

    void WaitForMessage()
    {
        while (!message_received)
        {
            ros::spinOnce();
        }
    }

    ros::ServiceClient client; // Should be private?
    Eigen::Matrix<double,6,1> tau;
    static const double EPSILON = 1e-6; // Max absolute error

 private:
    ros::NodeHandle nh;
    ros::Publisher  pub;
    ros::Subscriber sub;

    bool message_received;

    void Callback(const geometry_msgs::Wrench& msg)
    {
        tf::wrenchMsgToEigen(msg, tau);
        message_received = true;
    }

    bool IsNodeReady()
    {
        return (pub.getNumSubscribers() > 0) && (sub.getNumPublishers() > 0);
    }
};

class QuaternionPdControllerTest : public ::testing::Test
{
public:
    QuaternionPdControllerTest()
    {
        setpointPub = nh.advertise<geometry_msgs::Pose>("pose_setpoints", 10);
        statePub    = nh.advertise<uranus_dp::State>("state_estimate", 10);
        sub         = nh.subscribe("rov_forces", 10, &QuaternionPdControllerTest::Callback, this);
        client      = nh.serviceClient<uranus_dp::SetControlMode>("set_control_mode");

        message_received = false;
    }

    void SetUp()
    {
        while (!IsNodeReady())
        {
            ros::spinOnce();
        }
    }

    void PublishSetpoint(Eigen::Vector3d p, Eigen::Quaterniond q)
    {
        geometry_msgs::Pose msg;
        tf::pointEigenToMsg(p, msg.position);
        tf::quaternionEigenToMsg(q, msg.orientation);
        setpointPub.publish(msg);
    }

    void PublishState(Eigen::Vector3d p, Eigen::Quaterniond q, Eigen::Vector3d v, Eigen::Vector3d omega)
    {
        uranus_dp::State msg;
        tf::pointEigenToMsg(p, msg.pose.position);
        tf::quaternionEigenToMsg(q, msg.pose.orientation);
        tf::vectorEigenToMsg(v, msg.twist.linear);
        tf::vectorEigenToMsg(omega, msg.twist.angular);
        statePub.publish(msg);
    }

    void WaitForMessage()
    {
        while (!message_received)
        {
            ros::spinOnce();
        }
    }

    ros::ServiceClient client; // Should be private?
    Eigen::Matrix<double,6,1> tau;
    static const double EPSILON = 1e-6; // Max absolute error

 private:
    ros::NodeHandle nh;
    ros::Publisher  setpointPub;
    ros::Publisher statePub;
    ros::Subscriber sub;

    bool message_received;

    void Callback(const geometry_msgs::Wrench& msg)
    {
        tf::wrenchMsgToEigen(msg, tau);
        message_received = true;
    }

    bool IsNodeReady()
    {
        return (setpointPub.getNumSubscribers() > 0) && (sub.getNumPublishers() > 0);
    }
};

 /*
  * Make sure we receive any messages at all.
  */
TEST_F(OpenLoopControllerTest, CheckResponsiveness)
{
    ros::Duration(0.5).sleep();

    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::OPEN_LOOP;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode OPEN_LOOP not set.");

    PublishSetpoint(0,0,0,0,0,0);
    WaitForMessage();
    EXPECT_TRUE(true);
}

 /*
  * Make sure open loop outputs the same as the input.
  */
TEST_F(OpenLoopControllerTest, DirectFeedthrough)
{
    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::OPEN_LOOP;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode OPEN_LOOP not set.");

    PublishSetpoint(-9.966, -7.907, 7.626, -6.023, 1.506, 2.805);
    WaitForMessage();
    EXPECT_NEAR(tau(0), -9.966, EPSILON);
    EXPECT_NEAR(tau(1), -7.907, EPSILON);
    EXPECT_NEAR(tau(2),  7.626, EPSILON);
    EXPECT_NEAR(tau(3), -6.023, EPSILON);
    EXPECT_NEAR(tau(4),  1.506, EPSILON);
    EXPECT_NEAR(tau(5),  2.805, EPSILON);
}

 /*
  * Input zero speed and nonzero pose. Input same pose state and pose setpoint. Expect zero output.
  */
TEST_F(QuaternionPdControllerTest, ZeroErrorZeroOutput)
{
    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::STATIONKEEPING;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode STATIONKEEPING not set.");

    Eigen::Vector3d    p(1,2,3);
    Eigen::Quaterniond q(4,5,6,7);
    q.normalize();

    Eigen::Vector3d v(0,0,0);
    Eigen::Vector3d omega(0,0,0);

    PublishState(p, q, v, omega);
    PublishSetpoint(p, q);

    WaitForMessage();

    EXPECT_NEAR(tau(0), 0, EPSILON);
    EXPECT_NEAR(tau(1), 0, EPSILON);
    EXPECT_NEAR(tau(2), 0, EPSILON);
    EXPECT_NEAR(tau(3), 0, EPSILON);
    EXPECT_NEAR(tau(4), 0, EPSILON);
    EXPECT_NEAR(tau(5), 0, EPSILON);
}

 /*
  * Give error only in surge, assure rov force command only in surge.
  */
TEST_F(QuaternionPdControllerTest, OnlySurge)
{
    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::STATIONKEEPING;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode STATIONKEEPING not set.");

    Eigen::Vector3d    p(0,0,0);
    Eigen::Quaterniond q(1,0,0,0);
    q.normalize();
    Eigen::Vector3d    v(0,0,0);
    Eigen::Vector3d    omega(0,0,0);

    Eigen::Vector3d p_sp(1,0,0);

    PublishState(p, q, v, omega);
    PublishSetpoint(p_sp, q);

    ros::Duration(0.1).sleep(); // Controller runs at 10 Hz, must give it time to compute new value.
    WaitForMessage();

    EXPECT_NEAR(tau(0), 30, EPSILON);
    EXPECT_NEAR(tau(1),  0, EPSILON);
    EXPECT_NEAR(tau(2),  0, EPSILON);
    EXPECT_NEAR(tau(3),  0, EPSILON);
    EXPECT_NEAR(tau(4),  0, EPSILON);
    EXPECT_NEAR(tau(5),  0, EPSILON);
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    ros::init(argc, argv, "controller_test");
    ROS_INFO("Launching node controller_test.");

    int ret = RUN_ALL_TESTS();
    ros::shutdown();
    return ret;
}
<commit_msg>Add yaw test<commit_after>#include "ros/ros.h"
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <eigen_conversions/eigen_msg.h>
#include "geometry_msgs/Wrench.h"
#include "geometry_msgs/Pose.h"
#include "uranus_dp/State.h"
#include "uranus_dp/SetControlMode.h"
#include "../src/control_mode_enum.h"

class OpenLoopControllerTest : public ::testing::Test
{
public:
    OpenLoopControllerTest()
    {
        pub    = nh.advertise<geometry_msgs::Wrench>("wrench_setpoints", 1);
        sub    = nh.subscribe("rov_forces", 5, &OpenLoopControllerTest::Callback, this);
        client = nh.serviceClient<uranus_dp::SetControlMode>("set_control_mode");

        message_received = false;
    }

    void SetUp()
    {
        while (!IsNodeReady())
        {
            ros::spinOnce();
        }
    }

    void PublishSetpoint(double X, double Y, double Z, double K, double M, double N)
    {
        geometry_msgs::Wrench msg;
        msg.force.x  = X;
        msg.force.y  = Y;
        msg.force.z  = Z;
        msg.torque.x = K;
        msg.torque.y = M;
        msg.torque.z = N;
        pub.publish(msg);
    }

    void WaitForMessage()
    {
        while (!message_received)
        {
            ros::spinOnce();
        }
    }

    ros::ServiceClient client; // Should be private?
    Eigen::Matrix<double,6,1> tau;
    static const double EPSILON = 1e-6; // Max absolute error

 private:
    ros::NodeHandle nh;
    ros::Publisher  pub;
    ros::Subscriber sub;

    bool message_received;

    void Callback(const geometry_msgs::Wrench& msg)
    {
        tf::wrenchMsgToEigen(msg, tau);
        message_received = true;
    }

    bool IsNodeReady()
    {
        return (pub.getNumSubscribers() > 0) && (sub.getNumPublishers() > 0);
    }
};

class QuaternionPdControllerTest : public ::testing::Test
{
public:
    QuaternionPdControllerTest()
    {
        setpointPub = nh.advertise<geometry_msgs::Pose>("pose_setpoints", 10);
        statePub    = nh.advertise<uranus_dp::State>("state_estimate", 10);
        sub         = nh.subscribe("rov_forces", 10, &QuaternionPdControllerTest::Callback, this);
        client      = nh.serviceClient<uranus_dp::SetControlMode>("set_control_mode");

        message_received = false;
    }

    void SetUp()
    {
        while (!IsNodeReady())
        {
            ros::spinOnce();
        }
    }

    void PublishSetpoint(Eigen::Vector3d p, Eigen::Quaterniond q)
    {
        geometry_msgs::Pose msg;
        tf::pointEigenToMsg(p, msg.position);
        tf::quaternionEigenToMsg(q, msg.orientation);
        setpointPub.publish(msg);
    }

    void PublishState(Eigen::Vector3d p, Eigen::Quaterniond q, Eigen::Vector3d v, Eigen::Vector3d omega)
    {
        uranus_dp::State msg;
        tf::pointEigenToMsg(p, msg.pose.position);
        tf::quaternionEigenToMsg(q, msg.pose.orientation);
        tf::vectorEigenToMsg(v, msg.twist.linear);
        tf::vectorEigenToMsg(omega, msg.twist.angular);
        statePub.publish(msg);
    }

    void WaitForMessage()
    {
        while (!message_received)
        {
            ros::spinOnce();
        }
    }

    ros::ServiceClient client; // Should be private?
    Eigen::Matrix<double,6,1> tau;
    static const double EPSILON = 1e-6; // Max absolute error

 private:
    ros::NodeHandle nh;
    ros::Publisher  setpointPub;
    ros::Publisher statePub;
    ros::Subscriber sub;

    bool message_received;

    void Callback(const geometry_msgs::Wrench& msg)
    {
        tf::wrenchMsgToEigen(msg, tau);
        message_received = true;
    }

    bool IsNodeReady()
    {
        return (setpointPub.getNumSubscribers() > 0) && (sub.getNumPublishers() > 0);
    }
};

 /*
  * Make sure we receive any messages at all.
  */
TEST_F(OpenLoopControllerTest, CheckResponsiveness)
{
    ros::Duration(0.5).sleep();

    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::OPEN_LOOP;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode OPEN_LOOP not set.");

    PublishSetpoint(0,0,0,0,0,0);
    WaitForMessage();
    EXPECT_TRUE(true);
}

 /*
  * Make sure open loop outputs the same as the input.
  */
TEST_F(OpenLoopControllerTest, DirectFeedthrough)
{
    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::OPEN_LOOP;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode OPEN_LOOP not set.");

    PublishSetpoint(-9.966, -7.907, 7.626, -6.023, 1.506, 2.805);
    WaitForMessage();
    EXPECT_NEAR(tau(0), -9.966, EPSILON);
    EXPECT_NEAR(tau(1), -7.907, EPSILON);
    EXPECT_NEAR(tau(2),  7.626, EPSILON);
    EXPECT_NEAR(tau(3), -6.023, EPSILON);
    EXPECT_NEAR(tau(4),  1.506, EPSILON);
    EXPECT_NEAR(tau(5),  2.805, EPSILON);
}

 /*
  * Input zero speed and nonzero pose. Input same pose state and pose setpoint. Expect zero output.
  */
TEST_F(QuaternionPdControllerTest, ZeroErrorZeroOutput)
{
    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::STATIONKEEPING;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode STATIONKEEPING not set.");

    Eigen::Vector3d    p(1,2,3);
    Eigen::Quaterniond q(4,5,6,7);
    q.normalize();

    Eigen::Vector3d v(0,0,0);
    Eigen::Vector3d omega(0,0,0);

    PublishState(p, q, v, omega);
    PublishSetpoint(p, q);

    WaitForMessage();

    EXPECT_NEAR(tau(0), 0, EPSILON);
    EXPECT_NEAR(tau(1), 0, EPSILON);
    EXPECT_NEAR(tau(2), 0, EPSILON);
    EXPECT_NEAR(tau(3), 0, EPSILON);
    EXPECT_NEAR(tau(4), 0, EPSILON);
    EXPECT_NEAR(tau(5), 0, EPSILON);
}

 /*
  * Give error only in surge, assure rov force command only in surge.
  */
TEST_F(QuaternionPdControllerTest, OnlySurge)
{
    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::STATIONKEEPING;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode STATIONKEEPING not set.");

    Eigen::Vector3d    p(0,0,0);
    Eigen::Quaterniond q(1,0,0,0);
    q.normalize();
    Eigen::Vector3d    v(0,0,0);
    Eigen::Vector3d    omega(0,0,0);

    Eigen::Vector3d p_sp(1,0,0);

    PublishState(p, q, v, omega);
    PublishSetpoint(p_sp, q);

    ros::Duration(0.5).sleep(); // Controller runs at 10 Hz, must give it time to compute new value.
    WaitForMessage();

    EXPECT_NEAR(tau(0), 30, EPSILON);
    EXPECT_NEAR(tau(1),  0, EPSILON);
    EXPECT_NEAR(tau(2),  0, EPSILON);
    EXPECT_NEAR(tau(3),  0, EPSILON);
    EXPECT_NEAR(tau(4),  0, EPSILON);
    EXPECT_NEAR(tau(5),  0, EPSILON);
}

 /*
  * Give error only in sway, assure rov force command only in sway.
  */
TEST_F(QuaternionPdControllerTest, OnlySway)
{
    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::STATIONKEEPING;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode STATIONKEEPING not set.");

    Eigen::Vector3d    p(0,0,0);
    Eigen::Quaterniond q(1,0,0,0);
    Eigen::Vector3d    v(0,0,0);
    Eigen::Vector3d    omega(0,0,0);

    Eigen::Vector3d p_sp(0,1,0);

    PublishState(p, q, v, omega);
    PublishSetpoint(p_sp, q);

    ros::Duration(0.5).sleep(); // Controller runs at 10 Hz, must give it time to compute new value.
    WaitForMessage();

    EXPECT_NEAR(tau(0),  0, EPSILON);
    EXPECT_NEAR(tau(1), 30, EPSILON);
    EXPECT_NEAR(tau(2),  0, EPSILON);
    EXPECT_NEAR(tau(3),  0, EPSILON);
    EXPECT_NEAR(tau(4),  0, EPSILON);
    EXPECT_NEAR(tau(5),  0, EPSILON);
}

 /*
  * Give error only in yaw, assure rov force command only in yaw.
  */
TEST_F(QuaternionPdControllerTest, OnlyYaw)
{
    uranus_dp::SetControlMode srv;
    srv.request.mode = ControlModes::STATIONKEEPING;
    if (!client.call(srv))
        ROS_ERROR("Failed to call service set_control_mode. New mode STATIONKEEPING not set.");

    Eigen::Vector3d    p(0,0,0);
    Eigen::Quaterniond q(1,0,0,0);
    Eigen::Vector3d    v(0,0,0);
    Eigen::Vector3d    omega(0,0,0);

    Eigen::Quaterniond q_sp(0.923879532511287,0,0,0.382683432365090); // Equal to 45 degrees yaw
    q_sp.normalize();

    PublishState(p, q, v, omega);
    PublishSetpoint(p, q_sp);

    ros::Duration(0.5).sleep(); // Controller runs at 10 Hz, must give it time to compute new value.
    WaitForMessage();

    EXPECT_NEAR(tau(0),  0, EPSILON);
    EXPECT_NEAR(tau(1),  0, EPSILON);
    EXPECT_NEAR(tau(2),  0, EPSILON);
    EXPECT_NEAR(tau(3),  0, EPSILON);
    EXPECT_NEAR(tau(4),  0, EPSILON);
    EXPECT_NEAR(tau(5), 76.536686473017951, EPSILON);
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    ros::init(argc, argv, "controller_test");

    int ret = RUN_ALL_TESTS();
    ros::shutdown();
    return ret;
}
<|endoftext|>