Go to the documentation of this file.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
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef HLCONTROL_HPP_
00038 #define HLCONTROL_HPP_
00039 #include <auv_msgs/NavSts.h>
00040 #include <auv_msgs/BodyVelocityReq.h>
00041 #include <navcon_msgs/ConfigureAxes.h>
00042 #include <ros/ros.h>
00043
00044 #include <boost/thread/mutex.hpp>
00045
00046 namespace labust
00047 {
00048 namespace control
00049 {
00050 struct NoEnable{static const bool enable=true;};
00051 struct NoWindup{
00052 template <class Base>
00053 inline void get_windup(Base b){};
00054 };
00055
00056 struct DisableAxis
00057 {
00058 DisableAxis()
00059 {
00060 for (int i=0; i<6; ++i) disable_axis[i]=1;
00061 }
00062 bool disable_axis[6];
00063 };
00064
00066 class ConfigureAxesPolicy
00067 {
00068 public:
00069 ConfigureAxesPolicy()
00070 {
00071 ros::NodeHandle nh;
00072 configureAxes = nh.advertiseService("ConfigureAxes",
00073 &ConfigureAxesPolicy::onConfiguration, this);
00074 }
00075
00076 bool onConfiguration(navcon_msgs::ConfigureAxes::Request& req,
00077 navcon_msgs::ConfigureAxes::Response& resp)
00078 {
00079 this->disable_axis = req.disable_axis;
00080 return true;
00081 }
00082
00083 protected:
00087 auv_msgs::Bool6Axis disable_axis;
00091 ros::ServiceServer configureAxes;
00092 };
00093
00099 template <
00100 class Controller,
00101 class Enable = NoEnable,
00102 class Windup = NoWindup,
00103 class OutputType = auv_msgs::BodyVelocityReq,
00104 class InputType = auv_msgs::NavSts,
00105 class ReferenceType = auv_msgs::NavSts
00106 >
00107 class HLControl : public Controller, Enable, Windup
00108 {
00109 public:
00113 HLControl()
00114 {
00115 onInit();
00116 }
00120 void onInit()
00121 {
00122 ros::NodeHandle nh;
00123
00124 outPub = nh.advertise<OutputType>("out", 1);
00125
00126
00127 stateSub = nh.subscribe<InputType>("state", 1,
00128 &HLControl::onEstimate,this);
00129 trackState = nh.subscribe<ReferenceType>("ref", 1,
00130 &HLControl::onRef,this);
00131
00132 Controller::init();
00133 }
00134
00135 private:
00136
00137 void onRef(const typename ReferenceType::ConstPtr& ref)
00138 {
00139 boost::mutex::scoped_lock l(cnt_mux);
00140 this->ref = *ref;
00141 }
00142
00143 void onEstimate(const typename InputType::ConstPtr& estimate)
00144 {
00145 if (!Enable::enable)
00146 {
00147 lastEn = false;
00148 return;
00149 }
00150
00151
00152 Windup::get_windup(this);
00153 boost::mutex::scoped_lock l(cnt_mux);
00154 if (Enable::enable && !lastEn)
00155 {
00156 this->reset(ref, *estimate);
00157 lastEn = true;
00158 }
00159 outPub.publish(Controller::step(ref, *estimate));
00160 }
00161
00165 ros::Publisher outPub;
00169 ros::Subscriber stateSub, trackState;
00173 ReferenceType ref;
00177 bool disabled_axis[6], lastEn;
00181 boost::mutex cnt_mux;
00182 };
00183 }
00184 }
00185
00186
00187 #endif