motor_controller.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_quadrotor_controller/quadrotor_interface.h>
00030 #include <controller_interface/controller.h>
00031 
00032 #include <geometry_msgs/WrenchStamped.h>
00033 #include <std_srvs/Empty.h>
00034 
00035 #include <ros/subscriber.h>
00036 #include <ros/callback_queue.h>
00037 
00038 namespace hector_quadrotor_controller {
00039 
00040 using namespace controller_interface;
00041 
00042 class MotorController : public controller_interface::Controller<QuadrotorInterface>
00043 {
00044 public:
00045   MotorController()
00046     : node_handle_(0)
00047   {}
00048 
00049   ~MotorController()
00050   {
00051     if (node_handle_) {
00052       node_handle_->shutdown();
00053       delete node_handle_;
00054       node_handle_ = 0;
00055     }
00056   }
00057 
00058   bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
00059   {
00060     // get interface handles
00061     wrench_input_  = interface->addInput<WrenchCommandHandle>("wrench");
00062     motor_output_  = interface->addOutput<MotorCommandHandle>("motor");
00063 
00064     // initialize NodeHandle
00065     delete node_handle_;
00066     node_handle_ = new ros::NodeHandle(root_nh);
00067 
00068     // load parameters
00069     controller_nh.getParam("force_per_voltage", parameters_.force_per_voltage = 0.559966216);
00070     controller_nh.getParam("torque_per_voltage", parameters_.torque_per_voltage = 7.98598e-3);
00071     controller_nh.getParam("lever", parameters_.lever = 0.275);
00072     root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
00073 
00074     // TODO: calculate these parameters from the quadrotor_propulsion parameters
00075 //    quadrotor_propulsion:
00076 //            k_t: 0.015336864714397
00077 //            k_m: -7.011631909766668e-5
00078 
00079 //            CT2s: -1.3077e-2
00080 //            CT1s: -2.5224e-4
00081 //            CT0s:  1.538190483976698e-5
00082 
00083     return true;
00084   }
00085 
00086   void reset()
00087   {
00088     wrench_.wrench.force.x  = 0.0;
00089     wrench_.wrench.force.y  = 0.0;
00090     wrench_.wrench.force.z  = 0.0;
00091     wrench_.wrench.torque.x = 0.0;
00092     wrench_.wrench.torque.y = 0.0;
00093     wrench_.wrench.torque.z = 0.0;
00094 
00095     motor_.force.assign(4, 0.0);
00096     motor_.torque.assign(4, 0.0);
00097     motor_.frequency.clear();
00098     motor_.voltage.assign(4, 0.0);
00099   }
00100 
00101   void wrenchCommandCallback(const geometry_msgs::WrenchStampedConstPtr& command)
00102   {
00103     wrench_ = *command;
00104     if (wrench_.header.stamp.isZero()) wrench_.header.stamp = ros::Time::now();
00105 
00106     // start controller if it not running
00107     if (!isRunning()) this->startRequest(wrench_.header.stamp);
00108   }
00109 
00110   void starting(const ros::Time &time)
00111   {
00112     reset();
00113     motor_output_->start();
00114   }
00115 
00116   void stopping(const ros::Time &time)
00117   {
00118     motor_output_->stop();
00119   }
00120 
00121   void update(const ros::Time& time, const ros::Duration& period)
00122   {
00123     // Get wrench command input
00124     if (wrench_input_->connected() && wrench_input_->enabled()) {
00125       wrench_.wrench = wrench_input_->getCommand();
00126     }
00127 
00128     // Update output
00129     if (wrench_.wrench.force.z > 0.0) {
00130 
00131       double nominal_thrust_per_motor = wrench_.wrench.force.z / 4.0;
00132       motor_.force[0] =  nominal_thrust_per_motor - wrench_.wrench.torque.y / 2.0 / parameters_.lever;
00133       motor_.force[1] =  nominal_thrust_per_motor - wrench_.wrench.torque.x / 2.0 / parameters_.lever;
00134       motor_.force[2] =  nominal_thrust_per_motor + wrench_.wrench.torque.y / 2.0 / parameters_.lever;
00135       motor_.force[3] =  nominal_thrust_per_motor + wrench_.wrench.torque.x / 2.0 / parameters_.lever;
00136 
00137       double nominal_torque_per_motor = wrench_.wrench.torque.z / 4.0;
00138       motor_.voltage[0] = motor_.force[0] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage;
00139       motor_.voltage[1] = motor_.force[1] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage;
00140       motor_.voltage[2] = motor_.force[2] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage;
00141       motor_.voltage[3] = motor_.force[3] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage;
00142 
00143       motor_.torque[0] = motor_.voltage[0] * parameters_.torque_per_voltage;
00144       motor_.torque[1] = motor_.voltage[1] * parameters_.torque_per_voltage;
00145       motor_.torque[2] = motor_.voltage[2] * parameters_.torque_per_voltage;
00146       motor_.torque[3] = motor_.voltage[3] * parameters_.torque_per_voltage;
00147 
00148       if (motor_.voltage[0] < 0.0) motor_.voltage[0] = 0.0;
00149       if (motor_.voltage[1] < 0.0) motor_.voltage[1] = 0.0;
00150       if (motor_.voltage[2] < 0.0) motor_.voltage[2] = 0.0;
00151       if (motor_.voltage[3] < 0.0) motor_.voltage[3] = 0.0;
00152 
00153     } else {
00154       reset();
00155     }
00156 
00157     // set wrench output
00158     motor_.header.stamp = time;
00159     motor_.header.frame_id = "base_link";
00160     motor_output_->setCommand(motor_);
00161   }
00162 
00163 private:
00164   WrenchCommandHandlePtr wrench_input_;
00165   MotorCommandHandlePtr motor_output_;
00166 
00167   ros::NodeHandle *node_handle_;
00168   ros::Subscriber wrench_subscriber_;
00169   ros::ServiceServer engage_service_server_;
00170   ros::ServiceServer shutdown_service_server_;
00171 
00172   geometry_msgs::WrenchStamped wrench_;
00173   hector_uav_msgs::MotorCommand motor_;
00174   std::string base_link_frame_;
00175 
00176   struct {
00177     double force_per_voltage;     // coefficient for linearized volts to force conversion for a single motor [N / V]
00178     double torque_per_voltage;    // coefficient for linearized volts to force conversion for a single motor [Nm / V]
00179     double lever;                 // the lever arm from origin to the motor axes (symmetry assumption) [m]
00180   } parameters_;
00181 };
00182 
00183 } // namespace hector_quadrotor_controller
00184 
00185 #include <pluginlib/class_list_macros.h>
00186 PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::MotorController, controller_interface::ControllerBase)


hector_quadrotor_controller
Author(s): Johannes Meyer
autogenerated on Sun Jul 10 2016 04:49:53