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 VRX_GAZEBO_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_SCORING_PLUGIN_HH_

#include <ros/ros.h>
#include <gazebo/msgs/gz_string.pb.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 <gazebo/transport/transport.hh>
#include "vrx_gazebo/Task.h"
#include "vrx_gazebo/Contact.h"


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

  public: void Exit();

  // 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();

  protected: virtual void ReleaseVehicle();

  protected: void SetTimeoutScore(double _timeoutScore);

  protected: double GetTimeoutScore();

  protected: double GetRunningStateDuration();

  private: void Update();

  private: void UpdateTime();

  private: void UpdateTaskState();

  private: void UpdateTaskMessage();

  private: void PublishStats();

  private: virtual void OnReady();

  private: virtual void OnRunning();

  private: virtual void OnFinished();

  private: virtual void OnCollision();

  private: void OnCollisionMsg(ConstContactsPtr &_contacts);

  private: bool ParseSDFParameters();

  private: bool ParseJoints();

  protected: gazebo::physics::WorldPtr world;

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

  protected: std::string vehicleName;

  protected: gazebo::physics::ModelPtr vehicleModel;

  protected: gazebo::common::Time lastCollisionTime;

  private: gazebo::transport::NodePtr gzNode;

  private: gazebo::transport::SubscriberPtr collisionSub;

  private: gazebo::transport::PublisherPtr serverControlPub;

  private: gazebo::event::ConnectionPtr updateConnection;

  private: std::string taskInfoTopic = "/vrx/task/info";

  private: bool debug = true;

  private: std::string contactDebugTopic = "/vrx/debug/contact";

  private: double score = 0.0;

  private: sdf::ElementPtr sdf;

  private: double initialStateDuration = 30.0;

  private: double readyStateDuration = 60.0;

  protected: 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: float CollisionBuffer = 3.0;

  private: int collisionCounter = 0;

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

  private: std::vector<gazebo::common::Time> collisionTimestamps;

  private: bool timedOut = false;

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

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

  protected: vrx_gazebo::Task taskMsg;

  private: vrx_gazebo::Contact contactMsg;

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

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

  protected: ros::Publisher taskPub;

  private: ros::Publisher contactPub;

  private: double timeoutScore = -1.0;
};

#endif