Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef ROTORS_GAZEBO_PLUGINS_MOTOR_MODELS_H
00023 #define ROTORS_GAZEBO_PLUGINS_MOTOR_MODELS_H
00024
00025
00026 #include <stdio.h>
00027
00028
00029 #include <boost/bind.hpp>
00030 #include <Eigen/Eigen>
00031 #include <Eigen/Core>
00032 #include <gazebo/common/common.hh>
00033 #include <gazebo/common/Plugin.hh>
00034 #include <gazebo/gazebo.hh>
00035 #include <gazebo/physics/physics.hh>
00036 #include <mav_msgs/default_topics.h>
00037
00038
00039 #include "rotors_gazebo_plugins/common.h"
00040 #include "rotors_gazebo_plugins/motor_model.hpp"
00041 #include "Float32.pb.h"
00042 #include "CommandMotorSpeed.pb.h"
00043 #include "WindSpeed.pb.h"
00044
00045 namespace turning_direction {
00046 const static int CCW = 1;
00047 const static int CW = -1;
00048 }
00049
00050 enum class MotorType {
00051 kVelocity,
00052 kPosition,
00053 kForce
00054 };
00055
00056 namespace gazebo {
00057
00058
00059 typedef const boost::shared_ptr<const gz_mav_msgs::CommandMotorSpeed> GzCommandMotorInputMsgPtr;
00060 typedef const boost::shared_ptr<const gz_mav_msgs::WindSpeed> GzWindSpeedMsgPtr;
00061
00062
00063 static constexpr double kDefaultMaxForce = std::numeric_limits<double>::max();
00064 static constexpr double kDefaultMotorConstant = 8.54858e-06;
00065 static constexpr double kDefaultMomentConstant = 0.016;
00066 static constexpr double kDefaultTimeConstantUp = 1.0 / 80.0;
00067 static constexpr double kDefaultTimeConstantDown = 1.0 / 40.0;
00068 static constexpr double kDefaulMaxRotVelocity = 838.0;
00069 static constexpr double kDefaultRotorDragCoefficient = 1.0e-4;
00070 static constexpr double kDefaultRollingMomentCoefficient = 1.0e-6;
00071
00072 class GazeboMotorModel : public MotorModel, public ModelPlugin {
00073
00074 public:
00075 GazeboMotorModel()
00076 : ModelPlugin(),
00077 MotorModel(),
00078 command_sub_topic_(mav_msgs::default_topics::COMMAND_ACTUATORS),
00079 wind_speed_sub_topic_(mav_msgs::default_topics::WIND_SPEED),
00080 motor_speed_pub_topic_(mav_msgs::default_topics::MOTOR_MEASUREMENT),
00081 motor_position_pub_topic_(mav_msgs::default_topics::MOTOR_POSITION_MEASUREMENT),
00082 motor_force_pub_topic_(mav_msgs::default_topics::MOTOR_FORCE_MEASUREMENT),
00083 publish_speed_(true),
00084 publish_position_(false),
00085 publish_force_(false),
00086 motor_number_(0),
00087 turning_direction_(turning_direction::CW),
00088 motor_type_(MotorType::kVelocity),
00089 max_force_(kDefaultMaxForce),
00090 max_rot_velocity_(kDefaulMaxRotVelocity),
00091 moment_constant_(kDefaultMomentConstant),
00092 motor_constant_(kDefaultMotorConstant),
00093 ref_motor_input_(0.0),
00094 rolling_moment_coefficient_(kDefaultRollingMomentCoefficient),
00095 rotor_drag_coefficient_(kDefaultRotorDragCoefficient),
00096 rotor_velocity_slowdown_sim_(kDefaultRotorVelocitySlowdownSim),
00097 time_constant_down_(kDefaultTimeConstantDown),
00098 time_constant_up_(kDefaultTimeConstantUp),
00099 node_handle_(nullptr),
00100 wind_speed_W_(0, 0, 0),
00101 pubs_and_subs_created_(false) {}
00102
00103 virtual ~GazeboMotorModel();
00104
00105 virtual void InitializeParams();
00106 virtual void Publish();
00107
00108 protected:
00109 virtual void UpdateForcesAndMoments();
00110 virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00111 virtual void OnUpdate(const common::UpdateInfo & );
00112
00113 private:
00116 bool pubs_and_subs_created_;
00117
00122 void CreatePubsAndSubs();
00123
00125 double NormalizeAngle(double input);
00126
00127 std::string command_sub_topic_;
00128 std::string wind_speed_sub_topic_;
00129 std::string joint_name_;
00130 std::string link_name_;
00131 std::string motor_speed_pub_topic_;
00132 std::string motor_position_pub_topic_;
00133 std::string motor_force_pub_topic_;
00134 std::string namespace_;
00135
00136 bool publish_speed_;
00137 bool publish_position_;
00138 bool publish_force_;
00139
00140 int motor_number_;
00141 int turning_direction_;
00142 MotorType motor_type_;
00143
00144 double max_force_;
00145 double max_rot_velocity_;
00146 double moment_constant_;
00147 double motor_constant_;
00148 double ref_motor_input_;
00149 double rolling_moment_coefficient_;
00150 double rotor_drag_coefficient_;
00151 double rotor_velocity_slowdown_sim_;
00152 double time_constant_down_;
00153 double time_constant_up_;
00154
00155 common::PID pids_;
00156
00157 gazebo::transport::NodePtr node_handle_;
00158
00159 gazebo::transport::PublisherPtr motor_velocity_pub_;
00160
00161 gazebo::transport::PublisherPtr motor_position_pub_;
00162
00163 gazebo::transport::PublisherPtr motor_force_pub_;
00164
00165 gazebo::transport::SubscriberPtr command_sub_;
00166
00167 gazebo::transport::SubscriberPtr wind_speed_sub_;
00168
00169 physics::ModelPtr model_;
00170 physics::JointPtr joint_;
00171 physics::LinkPtr link_;
00172
00174 event::ConnectionPtr updateConnection_;
00175
00176 boost::thread callback_queue_thread_;
00177
00178 void QueueThread();
00179
00180 gz_std_msgs::Float32 turning_velocity_msg_;
00181 gz_std_msgs::Float32 position_msg_;
00182 gz_std_msgs::Float32 force_msg_;
00183
00184 void ControlCommandCallback(GzCommandMotorInputMsgPtr& command_motor_input_msg);
00185
00186 void WindSpeedCallback(GzWindSpeedMsgPtr& wind_speed_msg);
00187
00188 std::unique_ptr<FirstOrderFilter<double>> rotor_velocity_filter_;
00189 ignition::math::Vector3d wind_speed_W_;
00190 };
00191
00192 }
00193
00194 #endif // ROTORS_GAZEBO_PLUGINS_MOTOR_MODELS_H
rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43