ARIAC.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2016 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef _ARIAC_HH_
#define _ARIAC_HH_

#include <ostream>
#include <map>
#include <string>
#include <vector>

#include <gazebo/gazebo.hh>

namespace ariac
{
  using namespace gazebo;

  typedef std::string KitType_t;
  typedef std::string TrayID_t;
  typedef std::string GoalID_t;

  class TrayScore
  {
    public: friend std::ostream &operator<<(std::ostream &_out,
                                            const TrayScore &_obj)
    {
      _out << "<tray_score " << _obj.trayID << ">" << std::endl;
      _out << "Total score: [" << _obj.total() << "]" << std::endl;
      _out << "Complete: [" << (_obj.isComplete ? "true" : "false") << "]" << std::endl;
      _out << "Part presence score: [" << _obj.partPresence << "]" << std::endl;
      _out << "All parts bonus: [" << _obj.allPartsBonus << "]" << std::endl;
      _out << "Part pose score: [" << _obj.partPose << "]" << std::endl;
      _out << "</tray_score>" << std::endl;
      return _out;
    }
    public: TrayID_t trayID;
            double partPresence = 0.0;
            double allPartsBonus = 0.0;
            double partPose = 0.0;
            bool isComplete = false;

            double total() const
            {
              return partPresence + allPartsBonus + partPose;
            }
  };

  class GoalScore
  {
    public: friend std::ostream &operator<<(std::ostream &_out,
                                            const GoalScore &_obj)
    {
      _out << "<goal_score " << _obj.goalID << ">" << std::endl;
      _out << "Total score: [" << _obj.total() << "]" << std::endl;
      _out << "Time taken: [" << _obj.timeTaken << "]" << std::endl;
      _out << "Complete: [" << (_obj.isComplete() ? "true" : "false") << "]" << std::endl;
      for (const auto & item : _obj.trayScores)
      {
        _out << item.second << std::endl;
      }
      _out << "</goal_score>" << std::endl;
      return _out;
    }

    public: std::map<TrayID_t, TrayScore> trayScores;

            GoalID_t goalID;

            double timeTaken = 0.0;

            bool isComplete() const
            {
              bool isGoalComplete = !this->trayScores.empty();
              for (const auto & item : this->trayScores)
              {
                isGoalComplete &= item.second.isComplete;
                if (!isGoalComplete)
                {
                  break;
                }
              }
              return isGoalComplete;
            };

            double total() const
            {
              double total = 0.0;
              for (const auto & item : this->trayScores)
              {
                total += item.second.total();
              }
              return total;
            };
  };

  class GameScore
  {
    public: friend std::ostream &operator<<(std::ostream &_out,
                                            const GameScore &_obj)
    {
      _out << "<game_score>" << std::endl;
      _out << "Total score: [" << _obj.total() << "]" << std::endl;
      _out << "Total process time: [" << _obj.totalProcessTime << "]" << std::endl;
      _out << "Part travel time: [" << _obj.partTravelTime << "]" << std::endl;
      for (const auto & item : _obj.goalScores)
      {
        _out << item.second << std::endl;
      }
      _out << "</game_score>" << std::endl;
      return _out;
    }

    public: double totalProcessTime = 0.0;
            double partTravelTime = 0.0;
            double planningTime = 0.0;
            double partTravelDistance = 0.0;
            double manipulatorTravelDistance = 0.0;

            // The score of each of the goals during the game.
            std::map<GoalID_t, GoalScore> goalScores;

            double total() const
            {
              double total = 0;
              total += totalProcessTime;
              total += partTravelTime;
              total += planningTime;
              total += partTravelDistance;
              total += manipulatorTravelDistance;

              for (const auto & item : this->goalScores)
              {
                total += item.second.total();
              }
              return total;
            };
  };

  // TODO: this should have a different data type
  class ScoringParameters
  {
    public: friend bool operator==(const ScoringParameters &sp1, const ScoringParameters &sp2)
    {
      return (
        sp1.objectPresence == sp2.objectPresence &&
        sp1.objectPosition == sp2.objectPosition &&
        sp1.objectOrientation == sp2.objectOrientation &&
        sp1.allObjectsBonusFactor == sp2.allObjectsBonusFactor &&
        sp1.distanceThresh == sp2.distanceThresh);
    }

    public: friend bool operator!=(const ScoringParameters &sp1, const ScoringParameters &sp2)
    {
      return !(sp1 == sp2);
    }

    public: double objectPresence = 1.0;
    public: double objectPosition = 0.0;
    public: double objectOrientation = 1.0;

    // Bonus when all objects in the tray: factor * (number of objects)
    public: double allObjectsBonusFactor = 1.0;

    // Acceptable distance in meters to object's target position.
    // The measured distance is between the center of the model and its target,
    // projected onto the tray.
    public: double distanceThresh = 0.03;

    // Acceptable difference in radians to object's target orientation.
    // The measured difference is from a top-down view of the tray, but only if
    // the quaternions are aligned.
    public: double orientationThresh = 0.1;
  };

  TrayID_t DetermineModelType(const std::string &modelName)
  {
    TrayID_t modelType(modelName);

    // Trim namespaces
    size_t index = modelType.find_last_of('|');
    modelType = modelType.substr(index + 1);

    // Trim trailing underscore and number caused by inserting multiple of the same model
    index = modelType.find_last_not_of("0123456789");
    if (modelType[index] == '_' && index > 1)
    {
      modelType = modelType.substr(0, index);
    }

    return modelType;
  }

  class KitObject
  {
    public: friend std::ostream &operator<<(std::ostream &_out,
                                            const KitObject &_obj)
    {
      _out << "<object>" << std::endl;
      _out << "Type: [" << _obj.type << "]" << std::endl;
      _out << "Pose: [" << _obj.pose << "]" << std::endl;
      _out << "</object>" << std::endl;
      return _out;
    }

    public: std::string type;

    public: math::Pose pose;

  };

  class Kit
  {
    public: friend std::ostream &operator<<(std::ostream &_out,
                                            const Kit &_kit)
    {
      _out << "<kit type='" << _kit.kitType << "'>";
      for (const auto & obj : _kit.objects)
        _out << std::endl << obj;
      _out << std::endl << "</kit>" << std::endl;

      return _out;
    }

    public: KitType_t kitType;

    public: std::vector<KitObject> objects;
  };

  class Goal
  {
    public: bool operator<(const Goal &_goal) const
    {
      return this->startTime < _goal.startTime;
    }

    public: friend std::ostream &operator<<(std::ostream &_out,
                                            const Goal &_goal)
    {
      _out << "<Goal>" << std::endl;
      _out << "Start time: [" << _goal.startTime << "]" << std::endl;
      _out << "Allowed time: [" << _goal.allowedTime << "]" << std::endl;
      _out << "Kits:" << std::endl;
      for (const auto & item : _goal.kits)
      {
        _out << item.second << std::endl;
      }
      _out << "</goal>" << std::endl;

      return _out;
    }

    public: GoalID_t goalID;

    public: double startTime;

    public: double allowedTime;

    public: std::map<KitType_t, Kit> kits;

    public: double timeTaken;
  };
}
#endif