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 #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
00044 #include <boost/thread/mutex.hpp>
00045
00046 namespace labust
00047 {
00048 namespace control{
00050 template <class Enable>
00051 struct RefManual : public Enable
00052 {
00053 enum {u=0,v,w,p,q,r};
00054 RefManual():
00055 nu_max(Eigen::Vector6d::Zero()),
00056 Ts(0.1),
00057 stateReady(false){this->init();};
00058
00059 void init()
00060 {
00061 ros::NodeHandle nh;
00062
00063 stateRef = nh.advertise<auv_msgs::NavSts>("stateRef", 1);
00064
00065
00066 stateHat = nh.subscribe<auv_msgs::NavSts>("stateHat", 1,
00067 &RefManual::onEstimate,this);
00068 joyIn = nh.subscribe<sensor_msgs::Joy>("joy", 1,
00069 &RefManual::onJoy,this);
00070
00071 initialize_manual();
00072 }
00073
00074 void onEstimate(const auv_msgs::NavSts::ConstPtr& state)
00075 {
00076 boost::mutex::scoped_lock l(cnt_mux);
00077
00078 if (Enable::enable && !stateReady)
00079 {
00080 this->baseRef.position = state->position;
00081 this->baseRef.orientation = state->orientation;
00082 this->baseRef.position.depth = -state->altitude;
00083 }
00084 stateReady = Enable::enable;
00085 }
00086
00087 void onJoy(const sensor_msgs::Joy::ConstPtr& joy)
00088 {
00089 if (!stateReady) return;
00090
00091 Eigen::Vector6d mapped;
00092 mapper.remap(*joy, mapped);
00093 mapped = nu_max.cwiseProduct(mapped);
00094
00095 Eigen::Vector2f out, in;
00096 Eigen::Matrix2f R;
00097 in<<mapped[u]*nu_max[u]*Ts,mapped[v]*nu_max[v]*Ts;
00098 double yaw(baseRef.orientation.yaw);
00099 R<<cos(yaw),-sin(yaw),sin(yaw),cos(yaw);
00100 out = R*in;
00101
00102 baseRef.header.stamp = ros::Time::now();
00103 baseRef.header.frame_id = "local";
00104
00105 baseRef.position.north += out(u);
00106 baseRef.position.east += out(v);
00107
00108 baseRef.position.depth += mapped[w]*Ts*nu_max(w);
00109 baseRef.orientation.roll += mapped[p]*Ts*nu_max(p);
00110 baseRef.orientation.pitch += mapped[q]*Ts*nu_max(q);
00111 baseRef.orientation.yaw += mapped[r]*Ts*nu_max(r);
00112 baseRef.body_velocity.x = mapped[u]*nu_max[u];
00113 baseRef.body_velocity.y = mapped[v]*nu_max[v];
00114 baseRef.body_velocity.z = mapped[w]*nu_max[w];
00115
00116 stateRef.publish(baseRef);
00117 }
00118
00119 void initialize_manual()
00120 {
00121 ROS_INFO("Initializing manual ref controller...");
00122
00123 ros::NodeHandle nh;
00124 labust::tools::getMatrixParam(nh,"ref_manual/maximum_speeds", nu_max);
00125 nh.param("ref_manual/sampling_time",Ts,Ts);
00126
00127 ROS_INFO("Manual ref controller initialized.");
00128 }
00129
00130 private:
00131 ros::Subscriber stateHat, joyIn;
00132 ros::Publisher stateRef;
00133 Eigen::Vector6d nu_max;
00134 double Ts;
00135 JoystickMapping mapper;
00136 auv_msgs::NavSts baseRef;
00137 boost::mutex cnt_mux;
00138 bool stateReady;
00139 };
00140 }}
00141
00142 int main(int argc, char* argv[])
00143 {
00144 ros::init(argc,argv,"ref_manual");
00145
00146 labust::control::RefManual<labust::control::EnableServicePolicy> controller;
00147 ros::spin();
00148
00149 return 0;
00150 }
00151
00152
00153