quadrotor_interface.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 
00031 #include <cmath>
00032 
00033 namespace hector_quadrotor_controller {
00034 
00035 QuadrotorInterface::QuadrotorInterface()
00036 {}
00037 
00038 QuadrotorInterface::~QuadrotorInterface()
00039 {}
00040 
00041 bool QuadrotorInterface::enabled(const CommandHandle *handle) const
00042 {
00043   if (!handle || !handle->connected()) return false;
00044   std::string resource = handle->getName();
00045   return enabled_.count(resource) > 0;
00046 }
00047 
00048 bool QuadrotorInterface::start(const CommandHandle *handle)
00049 {
00050   if (!handle || !handle->connected()) return false;
00051   if (enabled(handle)) return true;
00052   std::string resource = handle->getName();
00053   enabled_[resource] = handle;
00054   ROS_DEBUG_NAMED("quadrotor_interface", "Enabled %s control", resource.c_str());
00055   return true;
00056 }
00057 
00058 void QuadrotorInterface::stop(const CommandHandle *handle)
00059 {
00060   if (!handle) return;
00061   if (!enabled(handle)) return;
00062   std::string resource = handle->getName();
00063   std::map<std::string, const CommandHandle *>::iterator it = enabled_.lower_bound(resource);
00064   if (it != enabled_.end() && it->second == handle) enabled_.erase(it);
00065   ROS_DEBUG_NAMED("quadrotor_interface", "Disabled %s control", resource.c_str());
00066 }
00067 
00068 void QuadrotorInterface::disconnect(const CommandHandle *handle)
00069 {
00070   if (!handle) return;
00071   std::string resource = handle->getName();
00072   if (inputs_.count(resource)) {
00073     const CommandHandlePtr& input = inputs_.at(resource);
00074     if (input.get() != handle) input->reset();
00075   }
00076   if (outputs_.count(resource)) {
00077     const CommandHandlePtr& output = outputs_.at(resource);
00078     if (output.get() != handle) output->reset();
00079   }
00080 }
00081 
00082 const Pose *QuadrotorInterface::getPoseCommand()          const { return getCommand<PoseCommandHandle>("pose"); }
00083 const Twist *QuadrotorInterface::getTwistCommand()        const { return getCommand<TwistCommandHandle>("twist"); }
00084 const Wrench *QuadrotorInterface::getWrenchCommand()      const { return getCommand<WrenchCommandHandle>("wrench"); }
00085 const MotorCommand *QuadrotorInterface::getMotorCommand() const { return getCommand<MotorCommandHandle>("motor"); }
00086 
00087 void PoseHandle::getEulerRPY(double &roll, double &pitch, double &yaw) const
00088 {
00089   const Quaternion::_w_type& w = pose().orientation.w;
00090   const Quaternion::_x_type& x = pose().orientation.x;
00091   const Quaternion::_y_type& y = pose().orientation.y;
00092   const Quaternion::_z_type& z = pose().orientation.z;
00093   roll  =  atan2(2.*y*z + 2.*w*x, z*z - y*y - x*x + w*w);
00094   pitch = -asin(2.*x*z - 2.*w*y);
00095   yaw   =  atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
00096 }
00097 
00098 double PoseHandle::getYaw() const
00099 {
00100   const Quaternion::_w_type& w = pose().orientation.w;
00101   const Quaternion::_x_type& x = pose().orientation.x;
00102   const Quaternion::_y_type& y = pose().orientation.y;
00103   const Quaternion::_z_type& z = pose().orientation.z;
00104   return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
00105 }
00106 
00107 Vector3 PoseHandle::toBody(const Vector3& nav) const
00108 {
00109   const Quaternion::_w_type& w = pose().orientation.w;
00110   const Quaternion::_x_type& x = pose().orientation.x;
00111   const Quaternion::_y_type& y = pose().orientation.y;
00112   const Quaternion::_z_type& z = pose().orientation.z;
00113   Vector3 body;
00114   body.x = (w*w+x*x-y*y-z*z) * nav.x + (2.*x*y + 2.*w*z) * nav.y + (2.*x*z - 2.*w*y) * nav.z;
00115   body.y = (2.*x*y - 2.*w*z) * nav.x + (w*w-x*x+y*y-z*z) * nav.y + (2.*y*z + 2.*w*x) * nav.z;
00116   body.z = (2.*x*z + 2.*w*y) * nav.x + (2.*y*z - 2.*w*x) * nav.y + (w*w-x*x-y*y+z*z) * nav.z;
00117   return body;
00118 }
00119 
00120 Vector3 PoseHandle::fromBody(const Vector3& body) const
00121 {
00122   const Quaternion::_w_type& w = pose().orientation.w;
00123   const Quaternion::_x_type& x = pose().orientation.x;
00124   const Quaternion::_y_type& y = pose().orientation.y;
00125   const Quaternion::_z_type& z = pose().orientation.z;
00126   Vector3 nav;
00127   nav.x = (w*w+x*x-y*y-z*z) * body.x + (2.*x*y - 2.*w*z) * body.y + (2.*x*z + 2.*w*y) * body.z;
00128   nav.y = (2.*x*y + 2.*w*z) * body.x + (w*w-x*x+y*y-z*z) * body.y + (2.*y*z - 2.*w*x) * body.z;
00129   nav.z = (2.*x*z - 2.*w*y) * body.x + (2.*y*z + 2.*w*x) * body.y + (w*w-x*x-y*y+z*z) * body.z;
00130   return nav;
00131 }
00132 
00133 void HorizontalPositionCommandHandle::getError(const PoseHandle &pose, double &x, double &y) const
00134 {
00135   getCommand(x, y);
00136   x -= pose.get()->position.x;
00137   y -= pose.get()->position.y;
00138 }
00139 
00140 double HeightCommandHandle::getError(const PoseHandle &pose) const
00141 {
00142   return getCommand() - pose.get()->position.z;
00143 }
00144 
00145 void HeadingCommandHandle::setCommand(double command)
00146 {
00147   if (get()) {
00148     get()->x = 0.0;
00149     get()->y = 0.0;
00150     get()->z = sin(command / 2.);
00151     get()->w = cos(command / 2.);
00152   }
00153 
00154   if (scalar_) {
00155     *scalar_ = command;
00156   }
00157 }
00158 
00159 double HeadingCommandHandle::getCommand() const {
00160   if (scalar_) return *scalar_;
00161   const Quaternion::_w_type& w = get()->w;
00162   const Quaternion::_x_type& x = get()->x;
00163   const Quaternion::_y_type& y = get()->y;
00164   const Quaternion::_z_type& z = get()->z;
00165   return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
00166 }
00167 
00168 bool HeadingCommandHandle::update(Pose& command) const {
00169   if (get()) {
00170     command.orientation = *get();
00171     return true;
00172   }
00173   if (scalar_) {
00174     command.orientation.x = 0.0;
00175     command.orientation.y = 0.0;
00176     command.orientation.z = sin(*scalar_ / 2.);
00177     command.orientation.x = cos(*scalar_ / 2.);
00178     return true;
00179   }
00180   return false;
00181 }
00182 
00183 double HeadingCommandHandle::getError(const PoseHandle &pose) const {
00184   static const double M_2PI = 2.0 * M_PI;
00185   double error = getCommand() - pose.getYaw() + M_PI;
00186   error -= floor(error / M_2PI) * M_2PI;
00187   return error - M_PI;
00188 }
00189 
00190 bool CommandHandle::enabled()    { return interface_->enabled(this); }
00191 bool CommandHandle::start()      { return interface_->start(this); }
00192 void CommandHandle::stop()       { interface_->stop(this); }
00193 void CommandHandle::disconnect() { interface_->disconnect(this); }
00194 
00195 } // namespace hector_quadrotor_controller


hector_quadrotor_controller
Author(s): Johannes Meyer
autogenerated on Thu Aug 27 2015 13:17:47