You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
83 lines
2.2 KiB
83 lines
2.2 KiB
/*
|
|
* Copyright 2020 Daniel Duecker, TU Hamburg, Germany
|
|
* Copyright 2020 Philipp Hastedt, TU Hamburg, Germany
|
|
* based on prior work by Nils Rottmann and Austin Buchan
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
#include <boost/bind.hpp>
|
|
#include <Eigen/Eigen>
|
|
#include <gazebo/gazebo.hh>
|
|
#include <gazebo/physics/physics.hh>
|
|
#include <gazebo/common/common.hh>
|
|
#include <gazebo/common/Plugin.hh>
|
|
#include <rotors_model/motor_model.hpp>
|
|
#include "gazebo/transport/transport.hh"
|
|
#include "gazebo/msgs/msgs.hh"
|
|
#include "Float.pb.h"
|
|
#include "common.h"
|
|
|
|
namespace gazebo {
|
|
|
|
class GazeboUUVPlugin : public ModelPlugin {
|
|
public:
|
|
GazeboUUVPlugin() : ModelPlugin(){}
|
|
|
|
virtual ~GazeboUUVPlugin();
|
|
virtual void InitializeParams();
|
|
void ParseBuoyancy(sdf::ElementPtr _sdf);
|
|
void ApplyBuoyancy();
|
|
virtual void Publish();
|
|
protected:
|
|
virtual void UpdateForcesAndMoments();
|
|
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
|
|
virtual void OnUpdate(const common::UpdateInfo &);
|
|
|
|
private:
|
|
struct buoyancy_s {
|
|
std::string model_name;
|
|
physics::LinkPtr link;
|
|
ignition::math::Vector3d buoyancy_force;
|
|
ignition::math::Vector3d cob;
|
|
double height_scale_limit;
|
|
};
|
|
std::vector<buoyancy_s> buoyancy_links_;
|
|
std::string namespace_;
|
|
std::string link_base_;
|
|
|
|
/* Hydro coefficients - FOSSEN 2011 */
|
|
double X_u_;
|
|
double Y_v_;
|
|
double Z_w_;
|
|
double K_p_;
|
|
double M_q_;
|
|
double N_r_;
|
|
|
|
double X_udot_;
|
|
double Y_vdot_;
|
|
double Z_wdot_;
|
|
double K_pdot_;
|
|
double M_qdot_;
|
|
double N_rdot_;
|
|
|
|
transport::NodePtr node_handle_;
|
|
physics::ModelPtr model_;
|
|
physics::LinkPtr baseLink_;
|
|
event::ConnectionPtr updateConnection_;
|
|
|
|
};
|
|
}
|
|
|