gazebo_motor_model.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  *
00008  * Licensed under the Apache License, Version 2.0 (the "License");
00009  * you may not use this file except in compliance with the License.
00010  * You may obtain a copy of the License at
00011  *
00012  *     http://www.apache.org/licenses/LICENSE-2.0
00013 
00014  * Unless required by applicable law or agreed to in writing, software
00015  * distributed under the License is distributed on an "AS IS" BASIS,
00016  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00017  * See the License for the specific language governing permissions and
00018  * limitations under the License.
00019  */
00020 
00021 
00022 #ifndef ROTORS_GAZEBO_PLUGINS_MOTOR_MODELS_H
00023 #define ROTORS_GAZEBO_PLUGINS_MOTOR_MODELS_H
00024 
00025 // SYSTEM
00026 #include <stdio.h>
00027 
00028 // 3RD PARTY
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>  // This comes from the mav_comm repo
00037 
00038 // USER
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 } // namespace turning_direction
00049 
00050 enum class MotorType {
00051   kVelocity,
00052   kPosition,
00053   kForce
00054 };
00055 
00056 namespace gazebo {
00057 
00058 // Changed name from speed to input for more generality. TODO(kajabo): integrate general actuator command.
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 // Set the max_force_ to the max double value. The limitations get handled by the FirstOrderFilter.
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 & /*_info*/);
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 } // namespace gazebo {
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