perception_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.
 *
*/


/* Note - this code was originally derived from the ARIAC 
 * PopulationPlugin https://bitbucket.org/osrf/ariac/src/master/osrf_gear/include/osrf_gear/PopulationPlugin.hh
*/

#ifndef VRX_GAZEBO_PERCEPTION_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_PERCEPTION_SCORING_PLUGIN_HH_

#include <geographic_msgs/GeoPoseStamped.h>
#include <ros/ros.h>
#include <vector>
#include <memory>
#include <string>
#include <gazebo/physics/PhysicsTypes.hh>
#include <gazebo/physics/World.hh>
#include <gazebo/transport/transport.hh>
#include <sdf/sdf.hh>
#include "vrx_gazebo/scoring_plugin.hh"


class PerceptionObject
{
  public: double time;

  public: double duration;

  public: std::string type;

  public: std::string name;

  public: ignition::math::Pose3d trialPose;

  private: ignition::math::Pose3d origPose;

  public: gazebo::physics::EntityPtr modelPtr;

  public: bool active = false;

  public: double error = -1.0;

  public: PerceptionObject(const double& _time,
                 const double& _duration,
                 const std::string& _type,
                 const std::string& _name,
                 const ignition::math::Pose3d& _trialPose,
                 const gazebo::physics::WorldPtr _world);

  public: void SetError(const double& _error);

  public: void StartTrial(const gazebo::physics::EntityPtr& _frame);

  public: void EndTrial();

  public: std::string Str();
};


class PerceptionScoringPlugin : public ScoringPlugin
{
  public: PerceptionScoringPlugin();

  public: virtual ~PerceptionScoringPlugin();

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

  protected: void OnUpdate();

  private: void OnAttempt(
    const geographic_msgs::GeoPoseStamped::ConstPtr &_msg);

  private: void Restart();

  // Documentation inherited.
  private: void OnRunning() override;

  // Documentation inherited.
  private: void ReleaseVehicle() override;

  private: int attemptBal = 0;

  private: std::string ns;

  private: std::string objectTopic;

  private: ros::NodeHandle nh;

  private: ros::Subscriber objectSub;

  public: gazebo::physics::WorldPtr world;

  public: sdf::ElementPtr sdf;

  public: std::vector<PerceptionObject> objects;

  public: gazebo::event::ConnectionPtr connection;

  public: gazebo::common::Time startTime;

  public: bool loopForever = false;

  public: std::string frameName = std::string();

  public: gazebo::physics::EntityPtr frame;

  public: gazebo::common::Time lastUpdateTime;

  private: int objectsDespawned =0;
};

#endif