stationkeeping_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_STATIONKEEPING_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_STATIONKEEPING_SCORING_PLUGIN_HH_

#include <geographic_msgs/GeoPoseStamped.h>
#include <ros/ros.h>
#include <memory>
#include <string>
#include <gazebo/common/Events.hh>
#include <gazebo/common/Timer.hh>
#include <gazebo/physics/World.hh>
#include <sdf/sdf.hh>
#include "vrx_gazebo/scoring_plugin.hh"
#include "vrx_gazebo/waypoint_markers.hh"

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

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

  private: void Update();

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

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

  private: void PublishGoal();

  private: gazebo::event::ConnectionPtr updateConnection;

  private: std::string goalTopic = "/vrx/station_keeping/goal";

  private: std::string poseErrorTopic = "/vrx/station_keeping/pose_error";

  private: std::string meanErrorTopic = "/vrx/station_keeping/rms_error";

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

  private: ros::Publisher goalPub;

  private: ros::Publisher poseErrorPub;

  private: ros::Publisher meanErrorPub;

  private: double goalX;

  private: double goalY;

  private: double goalYaw;

  private: double goalLat;

  private: double goalLon;

  private: double poseError;

  private: unsigned int sampleCount = 0;

  private: double totalPoseError = 0;

  private: double meanError;

  private: gazebo::common::Timer timer;

  private: WaypointMarkers waypointMarkers;
};

#endif