00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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 }
00535
00536 #endif // HECTOR_QUADROTOR_CONTROLLER_HANDLES_H