gazebo_fw_dynamics_plugin.h

Go to the documentation of this file.

/*
 * Copyright 2017 Pavel Vechersky, ASL, ETH Zurich, Switzerland
 *
 * 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_FW_DYNAMICS_PLUGIN_H
#define ROTORS_GAZEBO_PLUGINS_FW_DYNAMICS_PLUGIN_H

#include <gazebo/common/Plugin.hh>
#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <mav_msgs/default_topics.h>

#include "Actuators.pb.h"
#include "RollPitchYawrateThrust.pb.h"
#include "WindSpeed.pb.h"

#include "rotors_gazebo_plugins/common.h"
#include "rotors_gazebo_plugins/fw_parameters.h"

namespace gazebo {

typedef const boost::shared_ptr<const gz_sensor_msgs::Actuators>
    GzActuatorsMsgPtr;
typedef const boost::shared_ptr<const gz_mav_msgs::RollPitchYawrateThrust>
    GzRollPitchYawrateThrustMsgPtr;
typedef const boost::shared_ptr<const gz_mav_msgs::WindSpeed>
    GzWindSpeedMsgPtr;

// Default values.
static constexpr bool kDefaultIsInputJoystick = false;

// Constants.
static constexpr double kAirDensity = 1.18;
static constexpr double kGravity = 9.81;
static constexpr double kMinAirSpeedThresh = 0.1;

class GazeboFwDynamicsPlugin : public ModelPlugin {
 public:
  GazeboFwDynamicsPlugin();

  virtual ~GazeboFwDynamicsPlugin();

 protected:
  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

  void OnUpdate(const common::UpdateInfo&);

  void UpdateForcesAndMoments();

  double NormalizedInputToAngle(const ControlSurface& surface, double input);

 private:
  bool is_input_joystick_;

  bool pubs_and_subs_created_;

  void CreatePubsAndSubs();

  std::string namespace_;
  std::string actuators_sub_topic_;
  std::string roll_pitch_yawrate_thrust_sub_topic_;
  std::string wind_speed_sub_topic_;

  transport::NodePtr node_handle_;

  gazebo::transport::SubscriberPtr actuators_sub_;
  gazebo::transport::SubscriberPtr roll_pitch_yawrate_thrust_sub_;
  gazebo::transport::SubscriberPtr wind_speed_sub_;

  physics::WorldPtr world_;
  physics::ModelPtr model_;
  physics::LinkPtr link_;
  event::ConnectionPtr updateConnection_;

  ignition::math::Vector3d W_wind_speed_W_B_;

  FWAerodynamicParameters aero_params_;

  FWVehicleParameters vehicle_params_;

  double delta_aileron_left_;
  double delta_aileron_right_;
  double delta_elevator_;
  double delta_flap_;
  double delta_rudder_;
  double throttle_;

  void ActuatorsCallback(GzActuatorsMsgPtr& actuators_msg);

  void RollPitchYawrateThrustCallback(GzRollPitchYawrateThrustMsgPtr&
                                          roll_pitch_yawrate_thrust_msg);

  void WindSpeedCallback(GzWindSpeedMsgPtr& wind_speed_msg);
};

}  // namespace gazebo

#endif // ROTORS_GAZEBO_PLUGINS_FW_DYNAMICS_PLUGIN_H