twist_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 <hector_quadrotor_controller/pid.h>
00031 
00032 #include <controller_interface/controller.h>
00033 
00034 #include <geometry_msgs/TwistStamped.h>
00035 #include <geometry_msgs/WrenchStamped.h>
00036 #include <std_srvs/Empty.h>
00037 
00038 #include <ros/subscriber.h>
00039 #include <ros/callback_queue.h>
00040 
00041 #include <boost/thread.hpp>
00042 
00043 #include <limits>
00044 
00045 namespace hector_quadrotor_controller {
00046 
00047 using namespace controller_interface;
00048 
00049 class TwistController : public controller_interface::Controller<QuadrotorInterface>
00050 {
00051 public:
00052   TwistController()
00053   {}
00054 
00055   ~TwistController()
00056   {}
00057 
00058   bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
00059   {
00060     // get interface handles
00061     pose_          = interface->getPose();
00062     twist_         = interface->getTwist();
00063     acceleration_  = interface->getAcceleration();
00064     twist_input_   = interface->addInput<TwistCommandHandle>("twist");
00065     wrench_output_ = interface->addOutput<WrenchCommandHandle>("wrench");
00066     node_handle_ = root_nh;
00067 
00068     // subscribe to commanded twist (geometry_msgs/TwistStamped) and cmd_vel (geometry_msgs/Twist)
00069     twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1));
00070     cmd_vel_subscriber_ = node_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1));
00071 
00072     // engage/shutdown service servers
00073     engage_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("engage", boost::bind(&TwistController::engageCallback, this, _1, _2));
00074     shutdown_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2));
00075 
00076     // initialize PID controllers
00077     pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy"));
00078     pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy"));
00079     pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z"));
00080     pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy"));
00081     pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy"));
00082     pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z"));
00083 
00084     // load other parameters
00085     controller_nh.getParam("auto_engage", auto_engage_ = true);
00086     controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5);
00087     controller_nh.getParam("limits/force/z", limits_.force.z);
00088     controller_nh.getParam("limits/torque/xy", limits_.torque.x);
00089     controller_nh.getParam("limits/torque/xy", limits_.torque.y);
00090     controller_nh.getParam("limits/torque/z", limits_.torque.z);
00091     root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
00092 
00093     // get mass and inertia from QuadrotorInterface
00094     interface->getMassAndInertia(mass_, inertia_);
00095 
00096     command_given_in_stabilized_frame_ = false;
00097 
00098     return true;
00099   }
00100 
00101   void reset()
00102   {
00103     pid_.linear.x.reset();
00104     pid_.linear.y.reset();
00105     pid_.linear.z.reset();
00106     pid_.angular.x.reset();
00107     pid_.angular.y.reset();
00108     pid_.angular.z.reset();
00109 
00110     wrench_.wrench.force.x  = 0.0;
00111     wrench_.wrench.force.y  = 0.0;
00112     wrench_.wrench.force.z  = 0.0;
00113     wrench_.wrench.torque.x = 0.0;
00114     wrench_.wrench.torque.y = 0.0;
00115     wrench_.wrench.torque.z = 0.0;
00116 
00117     linear_z_control_error_ = 0.0;
00118     motors_running_ = false;
00119   }
00120 
00121   void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command)
00122   {
00123     boost::mutex::scoped_lock lock(command_mutex_);
00124 
00125     command_ = *command;
00126     if (command_.header.stamp.isZero()) command_.header.stamp = ros::Time::now();
00127     command_given_in_stabilized_frame_ = false;
00128 
00129     // start controller if it not running
00130     if (!isRunning()) this->startRequest(command_.header.stamp);
00131   }
00132 
00133   void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr& command)
00134   {
00135     boost::mutex::scoped_lock lock(command_mutex_);
00136 
00137     command_.twist = *command;
00138     command_.header.stamp = ros::Time::now();
00139     command_given_in_stabilized_frame_ = true;
00140 
00141     // start controller if it not running
00142     if (!isRunning()) this->startRequest(command_.header.stamp);
00143   }
00144 
00145   bool engageCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00146   {
00147     boost::mutex::scoped_lock lock(command_mutex_);
00148 
00149     ROS_INFO_NAMED("twist_controller", "Engaging motors!");
00150     motors_running_ = true;
00151     return true;
00152   }
00153 
00154   bool shutdownCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00155   {
00156     boost::mutex::scoped_lock lock(command_mutex_);
00157 
00158     ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
00159     motors_running_ = false;
00160     return true;
00161   }
00162 
00163   void starting(const ros::Time &time)
00164   {
00165     reset();
00166     wrench_output_->start();
00167   }
00168 
00169   void stopping(const ros::Time &time)
00170   {
00171     wrench_output_->stop();
00172   }
00173 
00174   void update(const ros::Time& time, const ros::Duration& period)
00175   {
00176     boost::mutex::scoped_lock lock(command_mutex_);
00177 
00178     // Get twist command input
00179     if (twist_input_->connected() && twist_input_->enabled()) {
00180       command_.twist = twist_input_->getCommand();
00181       command_given_in_stabilized_frame_ = false;
00182     }
00183 
00184     // Get current state and command
00185     Twist command = command_.twist;
00186     Twist twist = twist_->twist();
00187     Twist twist_body;
00188     twist_body.linear =  pose_->toBody(twist.linear);
00189     twist_body.angular = pose_->toBody(twist.angular);
00190 
00191     // Transform to world coordinates if necessary (yaw only)
00192     if (command_given_in_stabilized_frame_) {
00193       double yaw = pose_->getYaw();
00194       Twist transformed = command;
00195       transformed.linear.x  = cos(yaw) * command.linear.x  - sin(yaw) * command.linear.y;
00196       transformed.linear.y  = sin(yaw) * command.linear.x  + cos(yaw) * command.linear.y;
00197       transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y;
00198       transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y;
00199       command = transformed;
00200     }
00201 
00202     // Get gravity and load factor
00203     const double gravity = 9.8065;
00204     double load_factor = 1. / (  pose_->pose().orientation.w * pose_->pose().orientation.w
00205                                  - pose_->pose().orientation.x * pose_->pose().orientation.x
00206                                  - pose_->pose().orientation.y * pose_->pose().orientation.y
00207                                  + pose_->pose().orientation.z * pose_->pose().orientation.z );
00208     // Note: load_factor could be NaN or Inf...?
00209     if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit)) load_factor = load_factor_limit;
00210 
00211     // Auto engage/shutdown
00212     if (auto_engage_) {
00213       if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0) {
00214         motors_running_ = true;
00215         ROS_INFO_NAMED("twist_controller", "Engaging motors!");
00216       } else if (motors_running_ && command.linear.z < -0.1 /* && (twist.linear.z > -0.1 && twist.linear.z < 0.1) */) {
00217         double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5);
00218         if (linear_z_control_error_ > 0.0) linear_z_control_error_ = 0.0; // positive control errors should not affect shutdown
00219         if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit) {
00220           motors_running_ = false;
00221           ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
00222         } else {
00223           ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit);
00224         }
00225       } else {
00226         linear_z_control_error_ = 0.0;
00227       }
00228 
00229       // flip over?
00230       if (motors_running_ && load_factor < 0.0) {
00231         motors_running_ = false;
00232         ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!");
00233       }
00234     }
00235 
00236     // Update output
00237     if (motors_running_) {
00238       Vector3 acceleration_command;
00239       acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period);
00240       acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period);
00241       acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity;
00242       Vector3 acceleration_command_body = pose_->toBody(acceleration_command);
00243 
00244       ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear:               [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]");
00245       ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular:         [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]");
00246       ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear:       [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]");
00247       ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular:      [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]");
00248       ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration:               [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]");
00249       ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]");
00250       ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body:  [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]");
00251 
00252       wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period);
00253       wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update( acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period);
00254       wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update( command.angular.z, twist.angular.z, 0.0, period);
00255       wrench_.wrench.force.x  = 0.0;
00256       wrench_.wrench.force.y  = 0.0;
00257       wrench_.wrench.force.z  = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity);
00258 
00259       if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z) wrench_.wrench.force.z = limits_.force.z;
00260       if (wrench_.wrench.force.z <= std::numeric_limits<double>::min()) wrench_.wrench.force.z = std::numeric_limits<double>::min();
00261       if (limits_.torque.x > 0.0) {
00262         if (wrench_.wrench.torque.x >  limits_.torque.x) wrench_.wrench.torque.x =  limits_.torque.x;
00263         if (wrench_.wrench.torque.x < -limits_.torque.x) wrench_.wrench.torque.x = -limits_.torque.x;
00264       }
00265       if (limits_.torque.y > 0.0) {
00266         if (wrench_.wrench.torque.y >  limits_.torque.y) wrench_.wrench.torque.y =  limits_.torque.y;
00267         if (wrench_.wrench.torque.y < -limits_.torque.y) wrench_.wrench.torque.y = -limits_.torque.y;
00268       }
00269       if (limits_.torque.z > 0.0) {
00270         if (wrench_.wrench.torque.z >  limits_.torque.z) wrench_.wrench.torque.z =  limits_.torque.z;
00271         if (wrench_.wrench.torque.z < -limits_.torque.z) wrench_.wrench.torque.z = -limits_.torque.z;
00272       }
00273 
00274       ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force:       [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]");
00275       ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque:      [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]");
00276 
00277     } else {
00278       reset();
00279     }
00280 
00281     // set wrench output
00282     wrench_.header.stamp = time;
00283     wrench_.header.frame_id = base_link_frame_;
00284     wrench_output_->setCommand(wrench_.wrench);
00285   }
00286 
00287 private:
00288   PoseHandlePtr pose_;
00289   TwistHandlePtr twist_;
00290   AccelerationHandlePtr acceleration_;
00291   TwistCommandHandlePtr twist_input_;
00292   WrenchCommandHandlePtr wrench_output_;
00293 
00294   ros::NodeHandle node_handle_;
00295   ros::Subscriber twist_subscriber_;
00296   ros::Subscriber cmd_vel_subscriber_;
00297   ros::ServiceServer engage_service_server_;
00298   ros::ServiceServer shutdown_service_server_;
00299 
00300   geometry_msgs::TwistStamped command_;
00301   geometry_msgs::WrenchStamped wrench_;
00302   bool command_given_in_stabilized_frame_;
00303   std::string base_link_frame_;
00304 
00305   struct {
00306     struct {
00307       PID x;
00308       PID y;
00309       PID z;
00310     } linear, angular;
00311   } pid_;
00312 
00313   geometry_msgs::Wrench limits_;
00314   bool auto_engage_;
00315   double load_factor_limit;
00316   double mass_;
00317   double inertia_[3];
00318 
00319   bool motors_running_;
00320   double linear_z_control_error_;
00321   boost::mutex command_mutex_;
00322 
00323 };
00324 
00325 } // namespace hector_quadrotor_controller
00326 
00327 #include <pluginlib/class_list_macros.h>
00328 PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase)


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