navigation_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_NAVIGATION_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_NAVIGATION_SCORING_PLUGIN_HH_

#include <string>
#include <vector>
#include <gazebo/common/Events.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/World.hh>
#include <ignition/math/Pose3.hh>
#include <sdf/sdf.hh>
#include "vrx_gazebo/scoring_plugin.hh"

class NavigationScoringPlugin : public ScoringPlugin
{
  private: enum class GateState
  {
    VEHICLE_OUTSIDE,

    VEHICLE_BEFORE,

    VEHICLE_AFTER,

    CROSSED,

    INVALID,
  };

  private: class Gate
  {
    public: Gate(const gazebo::physics::LinkPtr _leftMarkerModel,
                 const gazebo::physics::LinkPtr _rightMarkerModel);

    public: GateState IsPoseInGate(
      const ignition::math::Pose3d &_robotWorldPose) const;

    public: void Update();

    public: gazebo::physics::LinkPtr leftMarkerModel;

    public: gazebo::physics::LinkPtr rightMarkerModel;

    public: ignition::math::Pose3d pose;

    public: double width;

    public: GateState state = GateState::VEHICLE_OUTSIDE;
  };

  // Constructor.
  public: NavigationScoringPlugin();

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

  private: bool ParseGates(sdf::ElementPtr _sdf);

  private: bool AddGate(const std::string &_leftMarkerName,
                        const std::string &_rightMarkerName);

  private: void Update();

  private: void Fail();

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

  // Name of Course
  private: gazebo::physics::ModelPtr course;

  private: std::vector<Gate> gates;

  private: int numGates;

  private: gazebo::event::ConnectionPtr updateConnection;

  private: unsigned int numCollisions = 0;

  private: double obstaclePenalty = 10.0;
};

#endif