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_MULTI_COPTER_H 00023 #define ROTORS_GAZEBO_PLUGINS_MULTI_COPTER_H 00024 00025 #include <Eigen/Eigen> 00026 00027 #include "rotors_gazebo_plugins/motor_controller.hpp" 00028 #include "rotors_gazebo_plugins/motor_model.hpp" 00029 00030 class MultiCopter 00031 { 00032 public: 00033 MultiCopter(int amount_rotors) : 00034 position_(Eigen::Vector3d::Zero()), 00035 velocity_(Eigen::Vector3d::Zero()), 00036 attitude_(Eigen::Quaterniond::Identity()), 00037 rotor_rot_vels_(Eigen::VectorXd::Zero(amount_rotors)) 00038 {} 00039 MultiCopter(int amount_rotors, 00040 MotorController& motor_controller) : 00041 MultiCopter(amount_rotors) 00042 { 00043 setMotorController(motor_controller); 00044 } 00045 ~MultiCopter(); 00046 00047 void setMotorController( 00048 MotorController& motor_controller) { 00049 motor_controller_ = motor_controller; 00050 } 00051 00052 Eigen::VectorXd getRefMotorVelocities(double dt) { 00053 return motor_controller_.getMotorVelocities(dt); 00054 } 00055 00056 Eigen::VectorXd getMotorVelocities() { 00057 return rotor_rot_vels_; 00058 } 00059 00060 virtual Eigen::Vector4d simulateMAV(double dt, 00061 Eigen::VectorXd ref_rotor_rot_vels) = 0; 00062 virtual void initializeParams() = 0; 00063 virtual void publish() = 0; 00064 00065 const MotorController motorController() { 00066 return motor_controller_; 00067 } 00068 const Eigen::Vector3d position() {return position_;} 00069 const Eigen::Vector3d velocity() {return velocity_;} 00070 const Eigen::Quaterniond attitude() {return attitude_;} 00071 00072 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00073 protected: 00074 MotorController motor_controller_; 00075 Eigen::Vector3d position_; 00076 Eigen::Vector3d velocity_; 00077 Eigen::Quaterniond attitude_; 00078 Eigen::Vector3d angular_rate_; 00079 Eigen::VectorXd rotor_rot_vels_; 00080 }; 00081 00082 #endif // ROTORS_GAZEBO_PLUGINS_MULTI_COPTER_H