gazebo_wind_plugin.h
5.11 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
/*
* Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
* Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
* Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
* Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
* Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
* Copyright 2016 Anton Matosov
*
* 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 ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_PLUGIN_H
#define ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_PLUGIN_H
#include <string>
#include <random>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include "Wind.pb.h"
namespace gazebo {
// Default values
static const std::string kDefaultNamespace = "";
static const std::string kDefaultFrameId = "world";
static constexpr double kDefaultWindVelocityMean = 0.0;
static constexpr double kDefaultWindVelocityMax = 100.0;
static constexpr double kDefaultWindVelocityVariance = 0.0;
static constexpr double kDefaultWindGustVelocityMean = 0.0;
static constexpr double kDefaultWindGustVelocityMax = 10.0;
static constexpr double kDefaultWindGustVelocityVariance = 0.0;
static constexpr double kDefaultWindGustStart = 10.0;
static constexpr double kDefaultWindGustDuration = 0.0;
static const ignition::math::Vector3d kDefaultWindDirectionMean = ignition::math::Vector3d(1, 0, 0);
static const ignition::math::Vector3d kDefaultWindGustDirectionMean = ignition::math::Vector3d(0, 1, 0);
static constexpr double kDefaultWindDirectionVariance = 0.0;
static constexpr double kDefaultWindGustDirectionVariance = 0.0;
/// \brief This gazebo plugin simulates wind acting on a model.
class GazeboWindPlugin : public WorldPlugin {
public:
GazeboWindPlugin()
: WorldPlugin(),
namespace_(kDefaultNamespace),
wind_pub_topic_("world_wind"),
wind_velocity_mean_(kDefaultWindVelocityMean),
wind_velocity_max_(kDefaultWindVelocityMax),
wind_velocity_variance_(kDefaultWindVelocityVariance),
wind_gust_velocity_mean_(kDefaultWindGustVelocityMean),
wind_gust_velocity_max_(kDefaultWindGustVelocityMax),
wind_gust_velocity_variance_(kDefaultWindGustVelocityVariance),
wind_direction_mean_(kDefaultWindDirectionMean),
wind_direction_variance_(kDefaultWindDirectionVariance),
wind_gust_direction_mean_(kDefaultWindGustDirectionMean),
wind_gust_direction_variance_(kDefaultWindGustDirectionVariance),
frame_id_(kDefaultFrameId),
pub_interval_(0.5),
node_handle_(NULL) {}
virtual ~GazeboWindPlugin();
protected:
/// \brief Load the plugin.
/// \param[in] _model Pointer to the model that loaded this plugin.
/// \param[in] _sdf SDF element that describes the plugin.
void Load(physics::WorldPtr world, sdf::ElementPtr sdf);
/// \brief Called when the world is updated.
/// \param[in] _info Update timing information.
void OnUpdate(const common::UpdateInfo& /*_info*/);
private:
/// \brief Pointer to the update event connection.
event::ConnectionPtr update_connection_;
physics::WorldPtr world_;
std::string namespace_;
std::string frame_id_;
std::string wind_pub_topic_;
double wind_velocity_mean_;
double wind_velocity_max_;
double wind_velocity_variance_;
double wind_gust_velocity_mean_;
double wind_gust_velocity_max_;
double wind_gust_velocity_variance_;
double pub_interval_;
std::default_random_engine wind_velocity_generator_;
std::normal_distribution<double> wind_velocity_distribution_;
std::default_random_engine wind_gust_velocity_generator_;
std::normal_distribution<double> wind_gust_velocity_distribution_;
ignition::math::Vector3d xyz_offset_;
ignition::math::Vector3d wind_direction_mean_;
ignition::math::Vector3d wind_gust_direction_mean_;
double wind_direction_variance_;
double wind_gust_direction_variance_;
std::default_random_engine wind_direction_generator_;
std::normal_distribution<double> wind_direction_distribution_X_;
std::normal_distribution<double> wind_direction_distribution_Y_;
std::normal_distribution<double> wind_direction_distribution_Z_;
std::default_random_engine wind_gust_direction_generator_;
std::normal_distribution<double> wind_gust_direction_distribution_X_;
std::normal_distribution<double> wind_gust_direction_distribution_Y_;
std::normal_distribution<double> wind_gust_direction_distribution_Z_;
common::Time wind_gust_end_;
common::Time wind_gust_start_;
common::Time last_time_;
transport::NodePtr node_handle_;
transport::PublisherPtr wind_pub_;
physics_msgs::msgs::Wind wind_msg;
};
}
#endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_PLUGIN_H