LFControl.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, LABUST, UNIZG-FER
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the LABUST nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *  Author: Dula Nad
00035  *  Created: 03.05.2013.
00036  *********************************************************************/
00037 #include <labust/control/LFControl.hpp>
00038 #include <labust/tools/MatrixLoader.hpp>
00039 
00040 #include <auv_msgs/BodyVelocityReq.h>
00041 #include <boost/bind.hpp>
00042 
00043 #include <cmath>
00044 #include <vector>
00045 #include <string>
00046 
00047 using labust::control::LFControl;
00048 
00049 LFControl::LFControl():
00050                         nh(ros::NodeHandle()),
00051                         ph(ros::NodeHandle("~")),
00052                         lastEst(ros::Time::now()),
00053                         timeout(0.5),
00054                         LFKp(0.1),
00055                         Ts(0.1),
00056                         surge(0.5),
00057                         currSurge(0.5),
00058                         T0(labust::navigation::LFModel::zeros(3)),
00059                         Tt(labust::navigation::LFModel::zeros(3)),
00060                         enable(false),
00061                         useHeadingCnt(true)
00062 {this->onInit();}
00063 
00064 void LFControl::onInit()
00065 {;
00066         //Initialize publishers
00067         nuRef = nh.advertise<auv_msgs::BodyVelocityReq>("nuRef", 1);
00068 
00069         //Initialze subscribers
00070         stateHat = nh.subscribe<auv_msgs::NavSts>("stateHat", 1,
00071                         &LFControl::onEstimate,this);
00072         refPoint = nh.subscribe<geometry_msgs::PointStamped>("LFPoint", 1,
00073                         &LFControl::onNewPoint,this);
00074         enableControl = nh.advertiseService("LF_enable",
00075                         &LFControl::onEnableControl, this);
00076         openLoopSurge = nh.subscribe<std_msgs::Float32>("open_loop_surge", 1,
00077                                         &LFControl::onOpenLoopSurge,this);
00078         windup = nh.subscribe<auv_msgs::BodyForceReq>("tauAch", 1,
00079                         &LFControl::onWindup,this);
00080         nh.param("lf_controller/timeout",timeout,timeout);
00081         nh.param("lf_controller/use_heading",useHeadingCnt,useHeadingCnt);
00082 
00083         //Configure the dynamic reconfigure server
00084         //server.setCallback(boost::bind(&VelocityControl::dynrec_cb, this, _1, _2));
00085 
00086         initialize_controller();
00087         //config.__fromServer__(ph);
00088         //server.setConfigDefault(config);
00089         //this->updateDynRecConfig();
00090 }
00091 
00092 //void LFControl::updateDynRecConfig()
00093 //{
00094 //      ROS_INFO("Updating the dynamic reconfigure parameters.");
00095 //
00096 //      config.__fromServer__(ph);
00097 //      config.Surge_mode = axis_control[u];
00098 //      config.Sway_mode = axis_control[v];
00099 //      config.Heave_mode = axis_control[w];
00100 //      config.Roll_mode = axis_control[p];
00101 //      config.Pitch_mode = axis_control[q];
00102 //      config.Yaw_mode = axis_control[r];
00103 //
00104 //      config.High_level_controller="0 - None\n 1 - DP";
00105 //
00106 //      server.updateConfig(config);
00107 //}
00108 
00109 //void LFControl::dynrec_cb(labust_uvapp::VelConConfig& config, uint32_t level)
00110 //{
00111 //      this->config = config;
00112 //
00113 //      for(size_t i=u; i<=r; ++i)
00114 //      {
00115 //              int newMode(0);
00116 //              ph.getParam(dofName[i]+"_mode", newMode);
00117 //              //Stop the identification if it was aborted remotely.
00118 //              if ((axis_control[i] == identAxis) &&
00119 //                              (newMode != identAxis) &&
00120 //                              (ident[i] != 0)) ident[i].reset();
00121 //
00122 //              axis_control[i] = newMode;
00123 //      }
00124 //}
00125 
00126 void LFControl::onNewPoint(const geometry_msgs::PointStamped::ConstPtr& point)
00127 {
00128         using labust::navigation::LFModel;
00129         //LFModel::vector Tt(labust::navigation::LFModel::zeros(3));
00130         Tt(0) = point->point.x;
00131         Tt(1) = point->point.y;
00132         Tt(2) = point->point.z;
00133 
00134         line.setLine(T0,Tt);
00135 };
00136 
00137 bool LFControl::onEnableControl(labust_uvapp::EnableControl::Request& req,
00138                 labust_uvapp::EnableControl::Response& resp)
00139 {
00140         this->enable = req.enable;
00141         return true;
00142 }
00143 
00144 void LFControl::onOpenLoopSurge(const std_msgs::Float32::ConstPtr& surge)
00145 {
00146         this->surge = surge->data;
00147 }
00148 
00149 void LFControl::onWindup(const auv_msgs::BodyForceReq::ConstPtr& tauAch)
00150 {
00151         //Copy into controller
00152         headingController.windup = tauAch->disable_axis.yaw;
00153 };
00154 
00155 void LFControl::onEstimate(const auv_msgs::NavSts::ConstPtr& estimate)
00156 {
00157         //Copy into controller
00158         //Calculate DH.
00159         T0(0) = estimate->position.north;
00160         T0(1) = estimate->position.east;
00161         T0(2) = estimate->position.depth;
00162 
00163         currSurge = estimate->body_velocity.x;
00164         currYaw = estimate->orientation.yaw;
00165 
00166         lastEst = ros::Time::now();
00167         if (enable) this->step();
00168         //newEstimate = true;
00169         //if (newReference) step();
00170 };
00171 
00172 //void LFControl::safetyTest()
00173 //{
00174 //      bool refTimeout = (ros::Time::now() - lastRef).toSec() > timeout;
00175 //      bool estTimeout = (ros::Time::now() - lastEst).toSec() > timeout;
00176 //      bool manTimeout = (ros::Time::now() - lastMan).toSec() > timeout;
00177 //      bool measTimeout = (ros::Time::now() - lastMeas).toSec() > timeout;
00178 //      bool changed = false;
00179 //
00180 //      for (int i=u; i<=r;++i)
00181 //      {
00182 //              bool cntChannel = (refTimeout || estTimeout) && (axis_control[i] == controlAxis);
00183 //              if (cntChannel) ROS_WARN("Timeout on the control channel. Controlled axes will be disabled.");
00184 //              bool measChannel = measTimeout && (axis_control[i] == identAxis);
00185 //              if (measChannel) ROS_WARN("Timeout on the measurement channel. Stopping identifications in progress.");
00186 //              bool manChannel = manTimeout && (axis_control[i] == manualAxis);
00187 //              if (manChannel) ROS_WARN("Timeout on the manual channel. Manual axes will be disabled.");
00188 //
00189 //              suspend_axis[i] = (cntChannel || measChannel || manChannel);
00190 //      }
00191 //
00192 //      //Update only on change.
00193 //      //if (changed) this->updateDynRecConfig();
00194 //}
00195 
00196 void LFControl::step()
00197 {
00198         if (!enable) return;
00199 
00200         adjustDH();
00201         auv_msgs::BodyVelocityReq nu;
00202         //this->safetyTest();
00203         nu.goal.requester = "lf_control";
00204         nu.twist.linear.x = surge;
00205 //      if (fabs(currYaw - line.gamma()) < M_PI/2)
00206 //      {
00207 //              double dd = currSurge*sin(currYaw - line.gamma());
00208 //              nu.twist.angular.z = dh_controller.step(0,-line.calculatedH(T0(0),T0(1),T0(2)));
00209 //              std::cout<<nu.twist.angular.z<<", dH:"<<line.calculatedH(T0(0),T0(1),T0(2))<<std::endl;
00210 //      }
00211 //      else
00212 //      {
00213 //              nu.twist.angular.z = fabs(line.gamma() - currYaw);
00214 //              std::cout<<"Direct turn control"<<std::endl;
00215 //      }
00216 
00217         if (!useHeadingCnt)
00218         {
00219                 double dd = -currSurge*sin(currYaw - line.gamma());
00220                 nu.twist.angular.z = dh_controller.step(0,-line.calculatedH(T0(0),T0(1),T0(2)));
00221                 nu.twist.angular.z = labust::math::coerce(nu.twist.angular.z,-0.5,0.5);
00222                 std::cout<<nu.twist.angular.z<<", dH:"<<line.calculatedH(T0(0),T0(1),T0(2))<<std::endl;
00223         }
00224         else
00225         {
00226            double dh = line.calculatedH(T0(0),T0(1),T0(2));
00227            double d=sin(line.gamma())*(Tt(0)-T0(0))-cos(line.gamma())*(Tt(1)-T0(1));
00228            double z=labust::math::coerce(LFKp*dh,-0.7,0.7);
00229            std::cout<<"Distance to line:"<<d<<","<<dh<<", correction:"<<z<<","<<asin(z)<<std::endl;
00230            headingController.desired = labust::math::wrapRad(line.gamma() + asin(z));
00231            headingController.state = labust::math::wrapRad(currYaw);
00232                  float errorWrap = labust::math::wrapRad(headingController.desired - headingController.state);
00233                  PIFFExtController_stepWrap(&headingController,Ts, errorWrap);
00234                  nu.twist.angular.z = headingController.output;
00235         }
00236 
00237         nuRef.publish(nu);
00238 }
00239 
00240 void LFControl::start()
00241 {
00242         ros::spin();
00243 }
00244 
00245 void LFControl::adjustDH()
00246 {
00247         //Check for zero surge
00248         if (fabs(currSurge) < 0.1) currSurge=0.1;
00249         //currSurge=0.3;
00250 
00251         double Kph = wh*wh/currSurge;
00252         double Kdh = 2*wh/currSurge;
00253         double aAngle = M_PI/8;
00254         this->dh_controller.setGains(Kph,0, Kdh,0);
00255         double dsat(Kdh/Kph*currSurge*sin(aAngle));
00256 
00257         labust::math::Limit<double> limit(-Kph*dsat,Kph*dsat);
00258         this->dh_controller.setPLimits(limit);
00259 }
00260 
00261 void LFControl::initialize_controller()
00262 {
00263         ROS_INFO("Initializing line following controller...");
00264 
00265         nh.param("lf_controller/closed_loop_freq",wh,1.0);
00266         nh.param("lf_controller/default_surge",surge,surge);
00267         nh.param("lf_controller/sampling",Ts,Ts);
00268         double w(0.3);
00269         nh.param("lf_controller/heading_closed_loop_freq", w,w);
00270         nh.param("lf_controller/heading_LFKp", LFKp,LFKp);
00271         dh_controller.setTs(Ts);
00272         adjustDH();
00273 
00274         enum {Kp=0, Ki, Kd, Kt};
00275         PIDController_init(&headingController);
00276         headingController.gains[Kp] = 2*w;
00277         headingController.gains[Ki] = w*w;
00278         headingController.autoTracking = 0;
00279 
00280         ROS_INFO("Line following controller initialized.");
00281 }


labust_uvapp
Author(s): Dula Nad
autogenerated on Sun Mar 1 2015 12:12:22