pose_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/PoseStamped.h>
00035 #include <geometry_msgs/TwistStamped.h>
00036 
00037 #include <ros/subscriber.h>
00038 #include <ros/callback_queue.h>
00039 
00040 namespace hector_quadrotor_controller {
00041 
00042 using namespace controller_interface;
00043 
00044 class PoseController : public controller_interface::Controller<QuadrotorInterface>
00045 {
00046 public:
00047   PoseController() {}
00048 
00049   ~PoseController() {}
00050 
00051   bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
00052   {
00053     // get interface handles
00054     pose_         = interface->getPose();
00055     twist_        = interface->getTwist();
00056 
00057     pose_input_   = interface->addInput<PoseCommandHandle>("pose");
00058     twist_input_  = interface->addInput<TwistCommandHandle>("pose/twist");
00059     twist_limit_  = interface->addInput<TwistCommandHandle>("pose/twist_limit");
00060     twist_output_ = interface->addOutput<TwistCommandHandle>("twist");
00061 
00062     node_handle_ = root_nh;
00063 
00064     // subscribe to commanded pose and velocity
00065     pose_subscriber_ = node_handle_.subscribe<geometry_msgs::PoseStamped>("command/pose", 1, boost::bind(&PoseController::poseCommandCallback, this, _1));
00066     twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&PoseController::twistCommandCallback, this, _1));
00067 
00068     // initialize PID controllers
00069     pid_.x.init(ros::NodeHandle(controller_nh, "xy"));
00070     pid_.y.init(ros::NodeHandle(controller_nh, "xy"));
00071     pid_.z.init(ros::NodeHandle(controller_nh, "z"));
00072     pid_.yaw.init(ros::NodeHandle(controller_nh, "yaw"));
00073 
00074     return true;
00075   }
00076 
00077   void reset()
00078   {
00079     pid_.x.reset();
00080     pid_.y.reset();
00081     pid_.z.reset();
00082     pid_.yaw.reset();
00083   }
00084 
00085   void poseCommandCallback(const geometry_msgs::PoseStampedConstPtr& command)
00086   {
00087     pose_command_ = *command;
00088     if (!(pose_input_->connected())) *pose_input_ = &(pose_command_.pose);
00089     pose_input_->start();
00090 
00091     ros::Time start_time = command->header.stamp;
00092     if (start_time.isZero()) start_time = ros::Time::now();
00093     if (!isRunning()) this->startRequest(start_time);
00094   }
00095 
00096   void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command)
00097   {
00098     twist_command_ = *command;
00099     if (!(twist_input_->connected())) *twist_input_ = &(twist_command_.twist);
00100     twist_input_->start();
00101 
00102     ros::Time start_time = command->header.stamp;
00103     if (start_time.isZero()) start_time = ros::Time::now();
00104     if (!isRunning()) this->startRequest(start_time);
00105   }
00106 
00107   void starting(const ros::Time &time)
00108   {
00109     reset();
00110     twist_output_->start();
00111   }
00112 
00113   void stopping(const ros::Time &time)
00114   {
00115     twist_output_->stop();
00116   }
00117 
00118   void update(const ros::Time& time, const ros::Duration& period)
00119   {
00120     Twist output;
00121 
00122     // check command timeout
00123     // TODO
00124 
00125     // return if no pose command is available
00126     if (pose_input_->enabled()) {
00127       // control horizontal position
00128       double error_n, error_w;
00129       HorizontalPositionCommandHandle(*pose_input_).getError(*pose_, error_n, error_w);
00130       output.linear.x = pid_.x.update(error_n, twist_->twist().linear.x, period);
00131       output.linear.y = pid_.y.update(error_w, twist_->twist().linear.y, period);
00132 
00133       // control height
00134       output.linear.z = pid_.z.update(HeightCommandHandle(*pose_input_).getError(*pose_), twist_->twist().linear.z, period);
00135 
00136       // control yaw angle
00137       output.angular.z = pid_.yaw.update(HeadingCommandHandle(*pose_input_).getError(*pose_), twist_->twist().angular.z, period);
00138     }
00139 
00140     // add twist command if available
00141     if (twist_input_->enabled())
00142     {
00143       output.linear.x  += twist_input_->getCommand().linear.x;
00144       output.linear.y  += twist_input_->getCommand().linear.y;
00145       output.linear.z  += twist_input_->getCommand().linear.z;
00146       output.angular.x += twist_input_->getCommand().angular.x;
00147       output.angular.y += twist_input_->getCommand().angular.y;
00148       output.angular.z += twist_input_->getCommand().angular.z;
00149     }
00150 
00151     // limit twist
00152     if (twist_limit_->enabled())
00153     {
00154       double linear_xy = sqrt(output.linear.x*output.linear.x + output.linear.y*output.linear.y);
00155       double limit_linear_xy  = std::max(twist_limit_->get()->linear.x, twist_limit_->get()->linear.y);
00156       if (limit_linear_xy > 0.0 && linear_xy > limit_linear_xy) {
00157         output.linear.x *= limit_linear_xy / linear_xy;
00158         output.linear.y *= limit_linear_xy / linear_xy;
00159       }
00160       if (twist_limit_->get()->linear.z > 0.0 && fabs(output.linear.z) > twist_limit_->get()->linear.z) {
00161         output.linear.z *= twist_limit_->get()->linear.z / fabs(output.linear.z);
00162       }
00163       double angular_xy = sqrt(output.angular.x*output.angular.x + output.angular.y*output.angular.y);
00164       double limit_angular_xy  = std::max(twist_limit_->get()->angular.x, twist_limit_->get()->angular.y);
00165       if (limit_angular_xy > 0.0 && angular_xy > limit_angular_xy) {
00166         output.angular.x *= limit_angular_xy / angular_xy;
00167         output.angular.y *= limit_angular_xy / angular_xy;
00168       }
00169       if (twist_limit_->get()->angular.z > 0.0 && fabs(output.angular.z) > twist_limit_->get()->angular.z) {
00170         output.angular.z *= twist_limit_->get()->angular.z / fabs(output.angular.z);
00171       }
00172     }
00173 
00174     // set twist output
00175     twist_output_->setCommand(output);
00176   }
00177 
00178 private:
00179   PoseHandlePtr pose_;
00180   PoseCommandHandlePtr pose_input_;
00181   TwistHandlePtr twist_;
00182   TwistCommandHandlePtr twist_input_;
00183   TwistCommandHandlePtr twist_limit_;
00184   TwistCommandHandlePtr twist_output_;
00185 
00186   geometry_msgs::PoseStamped pose_command_;
00187   geometry_msgs::TwistStamped twist_command_;
00188 
00189   ros::NodeHandle node_handle_;
00190   ros::Subscriber pose_subscriber_;
00191   ros::Subscriber twist_subscriber_;
00192 
00193   struct {
00194     PID x;
00195     PID y;
00196     PID z;
00197     PID yaw;
00198   } pid_;
00199 };
00200 
00201 } // namespace hector_quadrotor_controller
00202 
00203 #include <pluginlib/class_list_macros.h>
00204 PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::PoseController, controller_interface::ControllerBase)


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