handles.h
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 #ifndef HECTOR_QUADROTOR_CONTROLLER_HANDLES_H
00030 #define HECTOR_QUADROTOR_CONTROLLER_HANDLES_H
00031 
00032 #include <geometry_msgs/Pose.h>
00033 #include <geometry_msgs/Twist.h>
00034 #include <geometry_msgs/Wrench.h>
00035 #include <sensor_msgs/Imu.h>
00036 #include <hector_uav_msgs/MotorStatus.h>
00037 #include <hector_uav_msgs/MotorCommand.h>
00038 #include <hector_uav_msgs/AttitudeCommand.h>
00039 #include <hector_uav_msgs/YawrateCommand.h>
00040 #include <hector_uav_msgs/ThrustCommand.h>
00041 
00042 namespace hector_quadrotor_controller {
00043 
00044 class QuadrotorInterface;
00045 
00046 using geometry_msgs::Pose;
00047 using geometry_msgs::Point;
00048 using geometry_msgs::Quaternion;
00049 using geometry_msgs::Twist;
00050 using geometry_msgs::Vector3;
00051 using geometry_msgs::Wrench;
00052 using sensor_msgs::Imu;
00053 using hector_uav_msgs::MotorStatus;
00054 using hector_uav_msgs::MotorCommand;
00055 using hector_uav_msgs::AttitudeCommand;
00056 using hector_uav_msgs::YawrateCommand;
00057 using hector_uav_msgs::ThrustCommand;
00058 
00059 template <class Derived, typename T>
00060 class Handle_
00061 {
00062 public:
00063   typedef T ValueType;
00064   typedef Handle_<Derived, T> Base;
00065 
00066   Handle_(const std::string& name, const std::string& field = std::string()) : interface_(0), name_(name), field_(field), value_(0) {}
00067   Handle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field), value_(0) {}
00068   Handle_(QuadrotorInterface *interface, const ValueType *source, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field) { *this = source; }
00069   virtual ~Handle_() {}
00070 
00071   virtual const std::string& getName() const { return name_; }
00072   virtual const std::string& getField() const { return field_; }
00073 
00074   virtual bool connected() const { return get(); }
00075   virtual void reset() { value_ = 0; }
00076 
00077   Derived& operator=(const ValueType *source) { value_ = source; return static_cast<Derived &>(*this); }
00078   const ValueType *get() const { return value_; }
00079   const ValueType &operator*() const { return *value_; }
00080 
00081 protected:
00082   QuadrotorInterface *interface_;
00083   const std::string name_;
00084   const std::string field_;
00085   const ValueType *value_;
00086 };
00087 
00088 class PoseHandle : public Handle_<PoseHandle, Pose>
00089 {
00090 public:
00091   using Base::operator=;
00092 
00093   PoseHandle() : Base("pose") {}
00094   PoseHandle(QuadrotorInterface *interface) : Base(interface, "pose") {}
00095   PoseHandle(QuadrotorInterface *interface, const Pose *pose) : Base(interface, pose, "pose") {}
00096   virtual ~PoseHandle() {}
00097 
00098   const ValueType& pose() const { return *get(); }
00099 
00100   void getEulerRPY(double &roll, double &pitch, double &yaw) const;
00101   double getYaw() const;
00102   Vector3 toBody(const Vector3& nav) const;
00103   Vector3 fromBody(const Vector3& nav) const;
00104 };
00105 typedef boost::shared_ptr<PoseHandle> PoseHandlePtr;
00106 
00107 class TwistHandle : public Handle_<TwistHandle, Twist>
00108 {
00109 public:
00110   using Base::operator=;
00111 
00112   TwistHandle() : Base("twist") {}
00113   TwistHandle(QuadrotorInterface *interface) : Base(interface, "twist") {}
00114   TwistHandle(QuadrotorInterface *interface, const Twist *twist) : Base(interface, twist, "twist") {}
00115   virtual ~TwistHandle() {}
00116 
00117   const ValueType& twist() const { return *get(); }
00118 };
00119 typedef boost::shared_ptr<TwistHandle> TwistHandlePtr;
00120 
00121 class AccelerationHandle : public Handle_<AccelerationHandle, Vector3>
00122 {
00123 public:
00124   using Base::operator=;
00125 
00126   AccelerationHandle(QuadrotorInterface *interface, const Vector3 *acceleration) : Base(interface, acceleration, "acceleration") {}
00127   virtual ~AccelerationHandle() {}
00128 
00129   const ValueType& acceleration() const { return *get(); }
00130 };
00131 typedef boost::shared_ptr<AccelerationHandle> AccelerationHandlePtr;
00132 
00133 class StateHandle : public PoseHandle, public TwistHandle
00134 {
00135 public:
00136   StateHandle() {}
00137   StateHandle(QuadrotorInterface *interface, const Pose *pose, const Twist *twist) : PoseHandle(interface, pose), TwistHandle(interface, twist) {}
00138   virtual ~StateHandle() {}
00139 
00140   virtual bool connected() const { return PoseHandle::connected() && TwistHandle::connected(); }
00141 };
00142 typedef boost::shared_ptr<PoseHandle> PoseHandlePtr;
00143 
00144 class ImuHandle : public Handle_<ImuHandle, Imu>
00145 {
00146 public:
00147   using Base::operator=;
00148 
00149   ImuHandle() : Base("imu") {}
00150   ImuHandle(QuadrotorInterface *interface, const Imu *imu) : Base(interface, imu, "imu") {}
00151   virtual ~ImuHandle() {}
00152 
00153   const ValueType& imu() const { return *get(); }
00154 };
00155 typedef boost::shared_ptr<ImuHandle> ImuHandlePtr;
00156 
00157 class MotorStatusHandle : public Handle_<MotorStatusHandle, MotorStatus>
00158 {
00159 public:
00160   using Base::operator=;
00161 
00162   MotorStatusHandle() : Base("motor_status") {}
00163   MotorStatusHandle(QuadrotorInterface *interface, const MotorStatus *motor_status) : Base(interface, motor_status, "motor_status") {}
00164   virtual ~MotorStatusHandle() {}
00165 
00166   const ValueType& motorStatus() const { return *get(); }
00167 };
00168 typedef boost::shared_ptr<MotorStatusHandle> MotorStatusHandlePtr;
00169 
00170 class CommandHandle
00171 {
00172 public:
00173   CommandHandle() : interface_(0), new_value_(false) {}
00174   CommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field) : interface_(interface), name_(name), field_(field), new_value_(false) {}
00175   virtual ~CommandHandle() {}
00176 
00177   virtual const std::string& getName() const { return name_; }
00178   virtual const std::string& getField() const { return field_; }
00179   virtual bool connected() const = 0;
00180   virtual void reset() {}
00181 
00182   void *get() const { return 0; }
00183 
00184   bool enabled();
00185   bool start();
00186   void stop();
00187   void disconnect();
00188 
00189   template <typename T> T* ownData(T* data) { my_.reset(data); return data; }
00190 
00191   template <typename Derived> bool connectFrom(const Derived& output) {
00192     Derived *me = dynamic_cast<Derived *>(this);
00193     if (!me) return false;
00194     ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", output.getName().c_str(), &output, me->getName().c_str(), me);
00195     return (*me = output.get()).connected();
00196   }
00197 
00198   template <typename Derived> bool connectTo(Derived& input) const {
00199     const Derived *me = dynamic_cast<const Derived *>(this);
00200     if (!me) return false;
00201     ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", me->getName().c_str(), me, input.getName().c_str(), &input);
00202     return (input = me->get()).connected();
00203   }
00204 
00205 private:
00206   QuadrotorInterface *interface_;
00207   const std::string name_;
00208   const std::string field_;
00209   boost::shared_ptr<void> my_;
00210 
00211 protected:
00212   mutable bool new_value_;
00213   bool wasNew() const { bool old = new_value_; new_value_ = false; return old; }
00214 };
00215 typedef boost::shared_ptr<CommandHandle> CommandHandlePtr;
00216 
00217 namespace internal {
00218   template <class Derived> struct FieldAccessor {
00219     static typename Derived::ValueType *get(void *) { return 0; }
00220   };
00221 }
00222 
00223 template <class Derived, typename T, class Parent = CommandHandle>
00224 class CommandHandle_ : public Parent
00225 {
00226 public:
00227   typedef T ValueType;
00228   typedef CommandHandle_<Derived, T, Parent> Base;
00229 
00230   CommandHandle_() : command_(0) {}
00231   CommandHandle_(const Parent &other) : Parent(other), command_(0) {}
00232   CommandHandle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Parent(interface, name, field), command_(0) {}
00233   virtual ~CommandHandle_() {}
00234 
00235   virtual bool connected() const { return get(); }
00236   virtual void reset() { command_ = 0; Parent::reset(); }
00237 
00238   using Parent::operator=;
00239   Derived& operator=(ValueType *source) { command_ = source; return static_cast<Derived &>(*this); }
00240   ValueType *get() const { return command_ ? command_ : internal::FieldAccessor<Derived>::get(Parent::get()); }
00241   ValueType &operator*() const { return *command_; }
00242 
00243   ValueType& command() { return *get(); }
00244   const ValueType& getCommand() const { this->new_value_ = false; return *get(); }
00245   void setCommand(const ValueType& command) { this->new_value_ = true; *get() = command; }
00246   bool getCommand(ValueType& command) const { command = *get(); return this->wasNew(); }
00247 
00248   bool update(ValueType& command) const {
00249     if (!connected()) return false;
00250     command = getCommand();
00251     return true;
00252   }
00253 
00254 protected:
00255   ValueType *command_;
00256 };
00257 
00258 class PoseCommandHandle : public CommandHandle_<PoseCommandHandle, Pose>
00259 {
00260 public:
00261   using Base::operator=;
00262 
00263   PoseCommandHandle() {}
00264   PoseCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {}
00265   PoseCommandHandle(Pose* command) { *this = command; }
00266   virtual ~PoseCommandHandle() {}
00267 };
00268 typedef boost::shared_ptr<PoseCommandHandle> PoseCommandHandlePtr;
00269 
00270 class HorizontalPositionCommandHandle : public CommandHandle_<HorizontalPositionCommandHandle, Point, PoseCommandHandle>
00271 {
00272 public:
00273   using Base::operator=;
00274 
00275   HorizontalPositionCommandHandle() {}
00276   HorizontalPositionCommandHandle(const PoseCommandHandle& other) : Base(other) {}
00277   HorizontalPositionCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.xy") {}
00278   HorizontalPositionCommandHandle(Point* command) { *this = command; }
00279   virtual ~HorizontalPositionCommandHandle() {}
00280 
00281   using Base::getCommand;
00282   virtual bool getCommand(double &x, double &y) const {
00283     x = get()->x;
00284     y = get()->y;
00285     return wasNew();
00286   }
00287 
00288   using Base::setCommand;
00289   virtual void setCommand(double x, double y)
00290   {
00291     this->new_value_ = true;
00292     get()->x = x;
00293     get()->y = y;
00294   }
00295 
00296   using Base::update;
00297   bool update(Pose& command) const {
00298     if (!connected()) return false;
00299     getCommand(command.position.x, command.position.y);
00300     return true;
00301   }
00302 
00303   void getError(const PoseHandle &pose, double &x, double &y) const;
00304 };
00305 typedef boost::shared_ptr<HorizontalPositionCommandHandle> HorizontalPositionCommandHandlePtr;
00306 
00307 namespace internal {
00308   template <> struct FieldAccessor<HorizontalPositionCommandHandle> {
00309     static Point *get(Pose *pose) { return &(pose->position); }
00310   };
00311 }
00312 
00313 class HeightCommandHandle : public CommandHandle_<HeightCommandHandle, double, PoseCommandHandle>
00314 {
00315 public:
00316   using Base::operator=;
00317 
00318   HeightCommandHandle() {}
00319   HeightCommandHandle(const PoseCommandHandle& other) : Base(other) {}
00320   HeightCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.z") {}
00321   HeightCommandHandle(double *command) { *this = command; }
00322   virtual ~HeightCommandHandle() {}
00323 
00324   using Base::update;
00325   bool update(Pose& command) const {
00326     if (!connected()) return false;
00327     command.position.z = getCommand();
00328     return true;
00329   }
00330 
00331   double getError(const PoseHandle &pose) const;
00332 };
00333 typedef boost::shared_ptr<HeightCommandHandle> HeightCommandHandlePtr;
00334 
00335 namespace internal {
00336   template <> struct FieldAccessor<HeightCommandHandle> {
00337     static double *get(Pose *pose) { return &(pose->position.z); }
00338   };
00339 }
00340 
00341 class HeadingCommandHandle : public CommandHandle_<HeadingCommandHandle, Quaternion, PoseCommandHandle>
00342 {
00343 public:
00344   using Base::operator=;
00345 
00346   HeadingCommandHandle() {}
00347   HeadingCommandHandle(const PoseCommandHandle& other) : Base(other), scalar_(0) {}
00348   HeadingCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "orientation.yaw"), scalar_(0) {}
00349   HeadingCommandHandle(Quaternion *command) { *this = command; }
00350   virtual ~HeadingCommandHandle() {}
00351 
00352   using Base::getCommand;
00353   double getCommand() const;
00354 
00355   using Base::setCommand;
00356   void setCommand(double command);
00357 
00358   using Base::update;
00359   bool update(Pose& command) const;
00360 
00361   double getError(const PoseHandle &pose) const;
00362 
00363 protected:
00364   double *scalar_;
00365 };
00366 typedef boost::shared_ptr<HeadingCommandHandle> HeadingCommandHandlePtr;
00367 
00368 namespace internal {
00369   template <> struct FieldAccessor<HeadingCommandHandle> {
00370     static Quaternion *get(Pose *pose) { return &(pose->orientation); }
00371   };
00372 }
00373 
00374 class TwistCommandHandle : public CommandHandle_<TwistCommandHandle, Twist>
00375 {
00376 public:
00377 
00378   using Base::operator=;
00379 
00380   TwistCommandHandle() {}
00381   TwistCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {}
00382   TwistCommandHandle(Twist* command) { *this = command; }
00383   virtual ~TwistCommandHandle() {}
00384 };
00385 typedef boost::shared_ptr<TwistCommandHandle> TwistCommandHandlePtr;
00386 
00387 class HorizontalVelocityCommandHandle : public CommandHandle_<HorizontalVelocityCommandHandle, Vector3, TwistCommandHandle>
00388 {
00389 public:
00390   using Base::operator=;
00391 
00392   HorizontalVelocityCommandHandle() {}
00393   HorizontalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
00394   HorizontalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.xy") {}
00395   HorizontalVelocityCommandHandle(Vector3* command) { *this = command; }
00396   virtual ~HorizontalVelocityCommandHandle() {}
00397 
00398   using Base::getCommand;
00399   bool getCommand(double &x, double &y) const {
00400     x = get()->x;
00401     y = get()->y;
00402     return true;
00403   }
00404 
00405   using Base::setCommand;
00406   void setCommand(double x, double y)
00407   {
00408     get()->x = x;
00409     get()->y = y;
00410   }
00411 
00412   using Base::update;
00413   bool update(Twist& command) const {
00414     if (!connected()) return false;
00415     getCommand(command.linear.x, command.linear.y);
00416     return true;
00417   }
00418 };
00419 typedef boost::shared_ptr<HorizontalVelocityCommandHandle> HorizontalVelocityCommandHandlePtr;
00420 
00421 namespace internal {
00422   template <> struct FieldAccessor<HorizontalVelocityCommandHandle> {
00423     static Vector3 *get(Twist *twist) { return &(twist->linear); }
00424   };
00425 }
00426 
00427 class VerticalVelocityCommandHandle : public CommandHandle_<VerticalVelocityCommandHandle, double, TwistCommandHandle>
00428 {
00429 public:
00430   using Base::operator=;
00431 
00432   VerticalVelocityCommandHandle() {}
00433   VerticalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
00434   VerticalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.z") {}
00435   VerticalVelocityCommandHandle(double* command) { *this = command; }
00436   virtual ~VerticalVelocityCommandHandle() {}
00437 
00438   using Base::update;
00439   bool update(Twist& command) const {
00440     if (!connected()) return false;
00441     command.linear.z = *get();
00442     return true;
00443   }
00444 };
00445 typedef boost::shared_ptr<VerticalVelocityCommandHandle> VerticalVelocityCommandHandlePtr;
00446 
00447 namespace internal {
00448   template <> struct FieldAccessor<VerticalVelocityCommandHandle> {
00449     static double *get(Twist *twist) { return &(twist->linear.z); }
00450   };
00451 }
00452 
00453 class AngularVelocityCommandHandle : public CommandHandle_<AngularVelocityCommandHandle, double, TwistCommandHandle>
00454 {
00455 public:
00456   using Base::operator=;
00457 
00458   AngularVelocityCommandHandle() {}
00459   AngularVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
00460   AngularVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "angular.z") {}
00461   AngularVelocityCommandHandle(double* command) { *this = command; }
00462   virtual ~AngularVelocityCommandHandle() {}
00463 
00464   using Base::update;
00465   bool update(Twist& command) const {
00466     if (!connected()) return false;
00467     command.linear.z = *get();
00468     return true;
00469   }
00470 };
00471 typedef boost::shared_ptr<AngularVelocityCommandHandle> AngularVelocityCommandHandlePtr;
00472 
00473 namespace internal {
00474   template <> struct FieldAccessor<AngularVelocityCommandHandle> {
00475     static double *get(Twist *twist) { return &(twist->angular.z); }
00476   };
00477 }
00478 
00479 class WrenchCommandHandle : public CommandHandle_<WrenchCommandHandle, Wrench>
00480 {
00481 public:
00482   using Base::operator=;
00483 
00484   WrenchCommandHandle() {}
00485   WrenchCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
00486   virtual ~WrenchCommandHandle() {}
00487 };
00488 typedef boost::shared_ptr<WrenchCommandHandle> WrenchCommandHandlePtr;
00489 
00490 class MotorCommandHandle : public CommandHandle_<MotorCommandHandle, MotorCommand>
00491 {
00492 public:
00493   using Base::operator=;
00494 
00495   MotorCommandHandle() {}
00496   MotorCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
00497   virtual ~MotorCommandHandle() {}
00498 };
00499 typedef boost::shared_ptr<MotorCommandHandle> MotorCommandHandlePtr;
00500 
00501 class AttitudeCommandHandle : public CommandHandle_<AttitudeCommandHandle, AttitudeCommand>
00502 {
00503 public:
00504   using Base::operator=;
00505 
00506   AttitudeCommandHandle() {}
00507   AttitudeCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
00508   virtual ~AttitudeCommandHandle() {}
00509 };
00510 typedef boost::shared_ptr<AttitudeCommandHandle> AttitudeCommandHandlePtr;
00511 
00512 class YawrateCommandHandle : public CommandHandle_<YawrateCommandHandle, YawrateCommand>
00513 {
00514 public:
00515   using Base::operator=;
00516 
00517   YawrateCommandHandle() {}
00518   YawrateCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
00519   virtual ~YawrateCommandHandle() {}
00520 };
00521 typedef boost::shared_ptr<YawrateCommandHandle> YawrateCommandHandlePtr;
00522 
00523 class ThrustCommandHandle : public CommandHandle_<ThrustCommandHandle, ThrustCommand>
00524 {
00525 public:
00526   using Base::operator=;
00527 
00528   ThrustCommandHandle() {}
00529   ThrustCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
00530   virtual ~ThrustCommandHandle() {}
00531 };
00532 typedef boost::shared_ptr<ThrustCommandHandle> ThrustCommandHandlePtr;
00533 
00534 } // namespace hector_quadrotor_controller
00535 
00536 #endif // HECTOR_QUADROTOR_CONTROLLER_HANDLES_H


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