scoring_plugin.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2019 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 VMRC_GAZEBO_SCORING_PLUGIN_HH_
#define VMRC_GAZEBO_SCORING_PLUGIN_HH_

#include <ros/ros.h>
#include <memory>
#include <string>
#include <vector>
#include <gazebo/common/Events.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/physics/World.hh>
#include <sdf/sdf.hh>
#include "vmrc_gazebo/Task.h"

class ScoringPlugin : public gazebo::WorldPlugin
{
  public: ScoringPlugin() = default;

  // Documentation inherited.
  protected: void Load(gazebo::physics::WorldPtr _world,
                       sdf::ElementPtr _sdf);

  protected: double Score() const;

  protected: void SetScore(double _newScore);

  protected: std::string TaskName() const;

  protected: std::string TaskState() const;

  protected: gazebo::common::Time ElapsedTime() const;

  protected: gazebo::common::Time RemainingTime() const;

  protected: void Finish();

  private: void Update();

  private: void UpdateTime();

  private: void UpdateTaskState();

  private: void UpdateTaskMessage();

  private: void PublishStats();

  private: void ReleaseVehicle();

  private: virtual void OnReady();

  private: virtual void OnRunning();

  private: virtual void OnFinished();

  private: bool ParseSDFParameters();

  private: bool ParseJoints();

  protected: gazebo::physics::WorldPtr world;

  protected: std::string taskName = "undefined";

  protected: gazebo::event::ConnectionPtr updateConnection;

  protected: std::string topic = "/vmrc/task/info";

  protected: double score = 0.0;

  protected: std::string vehicleName;

  protected: gazebo::physics::ModelPtr vehicleModel;

  private: sdf::ElementPtr sdf;

  private: double initialStateDuration = 30.0;

  private: double readyStateDuration = 60.0;

  private: double runningStateDuration = 300.0;

  private: gazebo::common::Time readyTime;

  private: gazebo::common::Time runningTime;

  private: gazebo::common::Time finishTime;

  private: gazebo::common::Time currentTime;

  // \brief Elapsed time since the start of the task (running state).
  private: gazebo::common::Time elapsedTime;

  private: gazebo::common::Time remainingTime;

  private: bool timedOut = false;

  private: gazebo::common::Time lastStatsSent = gazebo::common::Time::Zero;

  private: std::string taskState = "initial";

  private: vmrc_gazebo::Task taskMsg;

  private: std::vector<std::string> lockJointNames;

  private: std::unique_ptr<ros::NodeHandle> rosNode;

  private: ros::Publisher taskPub;
};

#endif