usv_gazebo_wind_plugin.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2017  Brian Bingham
 *
 * 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 USV_GAZEBO_PLUGINS_WIND_HH_
#define USV_GAZEBO_PLUGINS_WIND_HH_

#include <ros/ros.h>
#include <memory>
#include <random>
#include <string>
#include <vector>
#include <gazebo/common/CommonTypes.hh>
#include <gazebo/common/Plugin.hh>
#include <ignition/math/Vector3.hh>
#include <gazebo/physics/physics.hh>

#include <sdf/sdf.hh>

namespace gazebo
{
  class UsvWindPlugin : public WorldPlugin
  {
    struct WindObj
    {
      // cppcheck-suppress unusedStructMember
      bool init = false;
      std::string modelName;
      physics::ModelPtr model;
      std::string linkName;
      physics::LinkPtr link;
      ignition::math::Vector3d windCoeff;
    };
    public: UsvWindPlugin();

    public: virtual ~UsvWindPlugin() = default;

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

    protected: virtual void Update();

    private: std::vector<UsvWindPlugin::WindObj> windObjs;

    private: bool windObjsInit = false;

    private: unsigned int windObjsInitCount = 0;

    private: physics::WorldPtr world;

    private: ignition::math::Vector3d windDirection;

    private: double windMeanVelocity;

    private: double gainConstant;

    private: double filterGain;

    private: double timeConstant;

    private: double previousTime;

    private: double varVel;

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

    private: ros::Publisher windSpeedPub;

    private: ros::Publisher windDirectionPub;

    private: std::string topicWindSpeed = "/vrx/debug/wind/speed";

    private: std::string topicWindDirection = "/vrx/debug/wind/direction";

    private: double lastPublishTime = 0;

    private: double updateRate;

    private: event::ConnectionPtr updateConnection;

    private: bool debug = true;

    private: std::unique_ptr<std::mt19937> randGenerator;
  };
}

#endif