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/HLControl.hpp>
00038 #include <labust/control/EnablePolicy.hpp>
00039 #include <labust/control/PSatDController.h>
00040 #include <std_msgs/Float32.h>
00041 #include <labust/math/Line.hpp>
00042
00043 #include <Eigen/Dense>
00044 #include <auv_msgs/BodyVelocityReq.h>
00045 #include <ros/ros.h>
00046
00047 namespace labust
00048 {
00049 namespace control{
00051 struct LFControl
00052 {
00053 LFControl():Ts(0.1),surge(0),aAngle(M_PI/8),wh(0.5){};
00054
00055 void init()
00056 {
00057 ros::NodeHandle nh;
00058 openLoopSurge = nh.subscribe<std_msgs::Float32>("open_loop_surge", 1,
00059 &LFControl::onOpenLoopSurge,this);
00060
00061 initialize_controller();
00062 }
00063
00064 void onOpenLoopSurge(const std_msgs::Float32::ConstPtr& surge)
00065 {
00066 this->surge = surge->data;
00067 }
00068
00069 void reset(const auv_msgs::NavSts& ref, const auv_msgs::NavSts& state){};
00070
00071 auv_msgs::BodyVelocityReqPtr step(const auv_msgs::NavSts& ref,
00072 const auv_msgs::NavSts& state)
00073 {
00074
00075 Eigen::Vector3d T0;
00076 T0<<ref.position.north,
00077 ref.position.east,
00078 ref.position.depth;
00079
00080 if (T0 != line.getT2())
00081 {
00082 Eigen::Vector3d T1;
00083 T1<<state.position.north,
00084 state.position.east,
00085 state.position.depth;
00086 line.setLine(T1,T0);
00087 }
00088
00089
00090 PSatD_tune(&con,wh,aAngle,surge);
00091 double dd = -surge*sin(state.orientation.yaw- line.gamma());
00092 con.desired=0;
00093 con.state = -line.calculatedH(T0(0),T0(1),T0(2));
00094 PSatD_dStep(&con,Ts,dd);
00095 auv_msgs::BodyVelocityReqPtr nu(new auv_msgs::BodyVelocityReq());
00096 nu->twist.angular.z = con.output;
00097
00098 return nu;
00099 }
00100
00101 void initialize_controller()
00102 {
00103 ROS_INFO("Initializing LF controller...");
00104 ros::NodeHandle nh;
00105 nh.param("lf_controller/closed_loop_freq",wh,wh);
00106 nh.param("lf_controller/default_surge",surge,surge);
00107 nh.param("lf_controller/approach_angle",aAngle,aAngle);
00108 nh.param("lf_controller/sampling",Ts,Ts);
00109
00110 PIDBase_init(&con);
00111 PSatD_tune(&con,wh,aAngle,surge);
00112
00113 ROS_INFO("LF controller initialized.");
00114 }
00115
00116 private:
00117 PIDBase con;
00118 ros::Subscriber openLoopSurge;
00119 double Ts;
00120 double surge, aAngle, wh;
00121 labust::math::Line line;
00122 };
00123 }}
00124
00125 int main(int argc, char* argv[])
00126 {
00127 ros::init(argc,argv,"lf_control");
00128
00129 labust::control::HLControl<labust::control::LFControl,
00130 labust::control::EnableServicePolicy> controller;
00131 ros::spin();
00132
00133 return 0;
00134 }
00135
00136
00137