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 #include <labust/control/EnablePolicy.hpp>
00038 #include <labust/control/ManControl.hpp>
00039 #include <labust/math/NumberManipulation.hpp>
00040 #include <labust/tools/MatrixLoader.hpp>
00041
00042 #include <auv_msgs/NavSts.h>
00043 #include <auv_msgs/BodyVelocityReq.h>
00044 #include <std_msgs/Bool.h>
00045
00046 #include <boost/thread/mutex.hpp>
00047
00048 namespace labust
00049 {
00050 namespace control{
00052 struct RefManual
00053 {
00054 enum {u=0,v,w,p,q,r,a};
00055 RefManual():
00056 nu_max(Eigen::Vector6d::Zero()),
00057 manRefFlag(Eigen::Vector6i::Zero()),
00058 manRef(Eigen::MatrixXd::Zero(7,1)),
00059 Ts(0.1),
00060 stateReady(false),
00061 stateAcquired(false),
00062 useFF(false),
00063 lastFF(Eigen::Vector6d::Zero()),
00064 ffstate(Eigen::Vector6d::Zero()),
00065 yawRef(0.0),
00066 joyTreshold(0.001),
00067 fineThresholdYaw(0.5),
00068 fineThresholdPos(0.25),
00069 fineThresholdAlt(0.5),
00070 yawFlag(false),
00071 enable(false){this->init();};
00072
00073 void init()
00074 {
00075 ros::NodeHandle nh;
00076
00077
00078 stateRef = nh.advertise<auv_msgs::NavSts>("stateRef", 1);
00079
00080 manRefHeadingPub = nh.advertise<std_msgs::Bool>("manRefHeading", 1);
00081 manRefPositionNorthPub = nh.advertise<std_msgs::Bool>("manRefNorthPosition", 1);
00082 manRefPositionEastPub = nh.advertise<std_msgs::Bool>("manRefEastPosition", 1);
00083 manRefPositionDepthPub = nh.advertise<std_msgs::Bool>("manRefDepthPosition", 1);
00084 manRefAltitudePub = nh.advertise<std_msgs::Bool>("manRefAltitude", 1);
00085
00086
00087
00088 stateHat = nh.subscribe<auv_msgs::NavSts>("stateHat", 1,
00089 &RefManual::onEstimate,this);
00090
00091 nuRefSub = nh.subscribe<auv_msgs::BodyVelocityReq>("nuRef", 1,
00092 &RefManual::onNuRef,this);
00093
00094 joyIn = nh.subscribe<sensor_msgs::Joy>("joy", 1,
00095 &RefManual::onJoy,this);
00096
00097 enableControl = nh.advertiseService("Enable",
00098 &RefManual::onEnableControl, this);
00099
00100 initialize_manual();
00101 }
00102
00103 bool onEnableControl(navcon_msgs::EnableControl::Request& req,
00104 navcon_msgs::EnableControl::Response& resp)
00105 {
00106 this->enable = req.enable;
00107 if (req.enable)
00108 {
00109 manRefFlag = Eigen::Vector6i::Zero();
00110 if (stateAcquired)
00111 {
00112 baseRef = lastState;
00113 stateReady = true;
00114 }
00115 else
00116 {
00117 stateReady = false;
00118 return false;
00119 }
00120 }
00121 else
00122 {
00123 stateReady = false;
00124 }
00125
00126 return true;
00127 }
00128
00129
00130 void onEstimate(const auv_msgs::NavSts::ConstPtr& state)
00131 {
00132 boost::mutex::scoped_lock l(cnt_mux);
00133 lastState = *state;
00134 stateAcquired = true;
00135 if (!stateReady) baseRef = lastState;
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145 }
00146
00147 void onNuRef(const auv_msgs::BodyVelocityReq::ConstPtr& state)
00148 {
00149
00150 }
00151
00152 void onJoy(const sensor_msgs::Joy::ConstPtr& joy)
00153 {
00154 if (!stateReady) return;
00155 if (!stateAcquired) return;
00156
00157 Eigen::Vector6d mapped;
00158 mapper.remap(*joy, mapped);
00159 mapped = nu_max.cwiseProduct(mapped);
00160
00161 Eigen::Vector2f out, in;
00162 Eigen::Matrix2f R;
00163 in<<mapped[u],mapped[v];
00164 double yaw(baseRef.orientation.yaw);
00165 R<<cos(yaw),-sin(yaw),sin(yaw),cos(yaw);
00166 out = R*in;
00167
00168
00169 std_msgs::Bool data;
00170 if(std::abs(mapped[r])<fineThresholdYaw)
00171 {
00172 data.data = true;
00173 manRefHeadingPub.publish(data);
00174 if(manRefFlag[r] == false)
00175 {
00176 manRef[r] = lastState.orientation.yaw;
00177 manRefFlag[r] = true;
00178 }
00179
00180 if (std::abs(mapped[r]) > joyTreshold)
00181 {
00182 manRef[r] += mapped[r]*Ts;
00183 }
00184 }
00185 else
00186 {
00187 data.data = false;
00188 manRefHeadingPub.publish(data);
00189 manRefFlag[r]= false;
00190 }
00191
00192 if(std::abs(out[u])<fineThresholdPos)
00193 {
00194 data.data = true;
00195 manRefPositionNorthPub.publish(data);
00196 if(manRefFlag[u] == false)
00197 {
00198 manRef[u] = lastState.position.north;
00199 manRefFlag[u] = true;
00200 }
00201
00202 if (std::abs(out(u)) > joyTreshold)
00203 {
00204 manRef[u] += out(u)*Ts;
00205 }
00206 }
00207 else
00208 {
00209 data.data = false;
00210 manRefPositionNorthPub.publish(data);
00211 manRefFlag[u]= false;
00212 }
00213
00214
00215 if(std::abs(out[v])<fineThresholdPos)
00216 {
00217 data.data = true;
00218 manRefPositionEastPub.publish(data);
00219 if(manRefFlag[v] == false)
00220 {
00221 manRef[v] = lastState.position.east;
00222 manRefFlag[v] = true;
00223 }
00224
00225 if (std::abs(out(v)) > joyTreshold)
00226 {
00227 manRef[v] += out(v)*Ts;
00228 }
00229 }
00230 else
00231 {
00232 data.data = false;
00233 manRefPositionEastPub.publish(data);
00234 manRefFlag[v]= false;
00235 }
00236
00237
00238 if (std::abs(mapped[w]) < fineThresholdAlt)
00239 {
00240 data.data = true;
00241 manRefPositionDepthPub.publish(data);
00242 manRefAltitudePub.publish(data);
00243
00244 if(manRefFlag[w] == false)
00245 {
00246 manRef[w] = lastState.position.depth;
00247 manRef[a] = lastState.altitude;
00248 manRefFlag[w] = true;
00249 }
00250
00251 if (std::abs(mapped[w]) > joyTreshold)
00252 {
00253 manRef[w] += mapped[w]*Ts;
00254 manRef[a] -= mapped[w]*Ts;
00255 }
00256 }
00257 else
00258 {
00259 data.data = false;
00260 manRefPositionDepthPub.publish(data);
00261 manRefAltitudePub.publish(data);
00262 manRefFlag[w]= false;
00263 }
00264
00265 baseRef.header.stamp = ros::Time::now();
00266 baseRef.header.frame_id = "local";
00267
00268 baseRef.position.north = (std::abs(out[u])<fineThresholdPos)?manRef[u]:lastState.position.north;
00269 baseRef.position.east = (std::abs(out[v])<fineThresholdPos)?manRef[v]:lastState.position.east;
00270 baseRef.position.depth = (std::abs(mapped[w])<fineThresholdAlt)?manRef[w]:lastState.position.depth;
00271 baseRef.orientation.roll += mapped[p]*Ts;
00272 baseRef.orientation.pitch += mapped[q]*Ts;
00273 baseRef.orientation.yaw = (std::abs(mapped[r])<fineThresholdYaw)?manRef[r]:lastState.orientation.yaw;
00274
00275 baseRef.altitude = (std::abs(mapped[w])<fineThresholdAlt)?manRef[a]:lastState.altitude;
00276
00277 if (useFF)
00278 {
00279
00280
00281 baseRef.body_velocity.x = mapped[u];
00282 baseRef.body_velocity.y = mapped[v];
00283 baseRef.body_velocity.z = mapped[w];
00284 baseRef.orientation_rate.roll = mapped[p];
00285 baseRef.orientation_rate.pitch = mapped[q];
00286 baseRef.orientation_rate.yaw = mapped[r];
00287
00288 }
00289
00290 auv_msgs::NavSts::Ptr refOut(new auv_msgs::NavSts());
00291 *refOut = baseRef;
00292 refOut->orientation.roll = labust::math::wrapRad(refOut->orientation.roll);
00293 refOut->orientation.pitch = labust::math::wrapRad(refOut->orientation.pitch);
00294 refOut->orientation.yaw = labust::math::wrapRad(refOut->orientation.yaw);
00295 ROS_ERROR("stateRef pub");
00296 stateRef.publish(refOut);
00297 }
00298
00299 double guide_test(double baseRef, double state, double speed, double max)
00300 {
00301
00302
00303
00304 if (fabs(speed) < max/100)
00305 {
00306 return baseRef + speed;
00307 }
00308 else
00309 {
00310 return state + speed;
00311 }
00312 }
00313
00314 void initialize_manual()
00315 {
00316 ROS_INFO("Initializing manual ref controller...");
00317
00318 ros::NodeHandle nh;
00319 labust::tools::getMatrixParam(nh,"ref_manual/maximum_speeds", nu_max);
00320 nh.param("ref_manual/sampling_time",Ts,Ts);
00321 nh.param("ref_manual/feedforward_speeds",useFF,useFF);
00322 nh.param("ref_manual/joy_deadzone",joyTreshold,joyTreshold);
00323 nh.param("ref_manual/finezone_yaw",fineThresholdYaw,fineThresholdYaw);
00324 nh.param("ref_manual/finezone_pos",fineThresholdPos,fineThresholdPos);
00325 nh.param("ref_manual/finezone_alt",fineThresholdAlt,fineThresholdAlt);
00326
00327 ROS_INFO("Manual ref controller initialized.");
00328 }
00329
00330 private:
00331 ros::Subscriber stateHat, joyIn, nuRefSub;
00332 ros::Publisher stateRef, manRefHeadingPub, manRefPositionNorthPub,
00333 manRefPositionEastPub, manRefPositionDepthPub, manRefAltitudePub;
00334 Eigen::Vector6d nu_max;
00335 Eigen::VectorXd manRef;
00336 Eigen::Vector6i manRefFlag;
00337 double Ts;
00338 JoystickMapping mapper;
00339 auv_msgs::NavSts baseRef;
00340 boost::mutex cnt_mux;
00341 auv_msgs::NavSts lastState;
00342 bool stateReady;
00343 bool stateAcquired;
00344 bool useFF;
00345 bool enable;
00346 bool yawFlag;
00347 Eigen::Vector6d lastFF;
00348 Eigen::Vector6d ffstate;
00349 ros::ServiceServer enableControl;
00350 double yawRef;
00351 double joyTreshold;
00352 double fineThresholdYaw;
00353 double fineThresholdPos;
00354 double fineThresholdAlt;
00355 };
00356 }}
00357
00358 int main(int argc, char* argv[])
00359 {
00360 ros::init(argc,argv,"ref_manual");
00361
00362
00363 labust::control::RefManual controller;
00364 ros::Duration(10).sleep();
00365 ros::spin();
00366
00367 return 0;
00368 }
00369
00370
00371