DPControl.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/DPControl.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::DPControl;
00048 
00049 DPControl::DPControl():
00050                         nh(ros::NodeHandle()),
00051                         ph(ros::NodeHandle("~")),
00052                         lastEst(ros::Time::now()),
00053                         timeout(0.5),
00054                         Ts(0.1),
00055                         safetyRadius(0.5),
00056                         surge(1),
00057                         enable(false),
00058                         inRegion(false)
00059 {this->onInit();}
00060 
00061 void DPControl::onInit()
00062 {;
00063         //Initialize publishers
00064         nuRef = nh.advertise<auv_msgs::BodyVelocityReq>("nuRef", 1);
00065 
00066         //Initialze subscribers
00067         stateHat = nh.subscribe<auv_msgs::NavSts>("stateHat", 1,
00068                         &DPControl::onEstimate,this);
00069         refPoint = nh.subscribe<geometry_msgs::PointStamped>("LFPoint", 1,
00070                         &DPControl::onNewPoint,this);
00071         refTrack = nh.subscribe<auv_msgs::NavSts>("TrackPoint", 1,
00072                         &DPControl::onTrackPoint,this);
00073         windup = nh.subscribe<auv_msgs::BodyForceReq>("tauAch", 1,
00074                         &DPControl::onWindup,this);
00075         enableControl = nh.advertiseService("DP_enable",
00076                         &DPControl::onEnableControl, this);
00077         openLoopSurge = nh.subscribe<std_msgs::Float32>("open_loop_surge", 1,
00078                         &DPControl::onOpenLoopSurge,this);
00079 
00080         nh.param("dp_controller/timeout",timeout,timeout);
00081 
00082         //Configure the dynamic reconfigure server
00083         //server.setCallback(boost::bind(&VelocityControl::dynrec_cb, this, _1, _2));
00084 
00085         initialize_controller();
00086         //config.__fromServer__(ph);
00087         //server.setConfigDefault(config);
00088         //this->updateDynRecConfig();
00089 }
00090 
00091 //void DPControl::updateDynRecConfig()
00092 //{
00093 //      ROS_INFO("Updating the dynamic reconfigure parameters.");
00094 //
00095 //      config.__fromServer__(ph);
00096 //      config.Surge_mode = axis_control[u];
00097 //      config.Sway_mode = axis_control[v];
00098 //      config.Heave_mode = axis_control[w];
00099 //      config.Roll_mode = axis_control[p];
00100 //      config.Pitch_mode = axis_control[q];
00101 //      config.Yaw_mode = axis_control[r];
00102 //
00103 //      config.High_level_controller="0 - None\n 1 - DP";
00104 //
00105 //      server.updateConfig(config);
00106 //}
00107 
00108 //void DPControl::dynrec_cb(labust_uvapp::VelConConfig& config, uint32_t level)
00109 //{
00110 //      this->config = config;
00111 //
00112 //      for(size_t i=u; i<=r; ++i)
00113 //      {
00114 //              int newMode(0);
00115 //              ph.getParam(dofName[i]+"_mode", newMode);
00116 //              //Stop the identification if it was aborted remotely.
00117 //              if ((axis_control[i] == identAxis) &&
00118 //                              (newMode != identAxis) &&
00119 //                              (ident[i] != 0)) ident[i].reset();
00120 //
00121 //              axis_control[i] = newMode;
00122 //      }
00123 //}
00124 
00125 void DPControl::onNewPoint(const geometry_msgs::PointStamped::ConstPtr& point)
00126 {
00127         trackPoint.position.north = point->point.x;
00128         trackPoint.position.east = point->point.y;
00129         trackPoint.position.depth = point->point.z;
00130 };
00131 
00132 bool DPControl::onEnableControl(labust_uvapp::EnableControl::Request& req,
00133                 labust_uvapp::EnableControl::Response& resp)
00134 {
00135         this->enable = req.enable;
00136         return true;
00137 }
00138 
00139 void DPControl::onOpenLoopSurge(const std_msgs::Float32::ConstPtr& surge)
00140 {
00141         this->surge = surge->data;
00142 }
00143 
00144 void DPControl::onEstimate(const auv_msgs::NavSts::ConstPtr& estimate)
00145 {
00146         //Copy into controller
00147         state = *estimate;
00148         lastEst = ros::Time::now();
00149         if (enable) this->step();
00150 };
00151 
00152 void DPControl::onTrackPoint(const auv_msgs::NavSts::ConstPtr& ref)
00153 {
00154         //Copy into controller
00155         trackPoint = *ref;
00156 };
00157 
00158 void DPControl::onWindup(const auv_msgs::BodyForceReq::ConstPtr& tauAch)
00159 {
00160         //Copy into controller
00161         headingController.windup = tauAch->disable_axis.yaw;
00162         distanceController.windup = tauAch->disable_axis.x;
00163 };
00164 
00165 //void DPControl::safetyTest()
00166 //{
00167 //      bool refTimeout = (ros::Time::now() - lastRef).toSec() > timeout;
00168 //      bool estTimeout = (ros::Time::now() - lastEst).toSec() > timeout;
00169 //      bool manTimeout = (ros::Time::now() - lastMan).toSec() > timeout;
00170 //      bool measTimeout = (ros::Time::now() - lastMeas).toSec() > timeout;
00171 //      bool changed = false;
00172 //
00173 //      for (int i=u; i<=r;++i)
00174 //      {
00175 //              bool cntChannel = (refT*imeout || estTimeout) && (axis_control[i] == controlAxis);
00176 //              if (cntChannel) ROS_WARN("Timeout on the control channel. Controlled axes will be disabled.");
00177 //              bool measChannel = measTimeout && (axis_control[i] == identAxis);
00178 //              if (measChannel) ROS_WARN("Timeout on the measurement channel. Stopping identifications in progress.");
00179 //              bool manChannel = manTimeout && (axis_control[i] == manualAxis);
00180 //              if (manChannel) ROS_WARN("Timeout on the manual channel. Manual axes will be disabled.");
00181 //
00182 //              suspend_axis[i] = (cntChannel || measChannel || manChannel);
00183 //      }
00184 //
00185 //      //Update only on change.
00186 //      //if (changed) this->updateDynRecConfig();
00187 //}
00188 
00189 void DPControl::step()
00190 {
00191         if (!enable) return;
00192         //this->safetyTest();
00193 
00194         auv_msgs::BodyVelocityReq nu;
00195         //For 2D non-holonomic case
00196         double dy(trackPoint.position.east - state.position.east);
00197         double dx(trackPoint.position.north - state.position.north);
00198         //double angle = state.orientation.yaw;
00199         //dy += safetyRadius*sin(angle);
00200         //dx += safetyRadius*cos(angle);
00201         double dist(sqrt(dy*dy+dx*dx));
00202         double angleDiff(labust::math::wrapRad(fabs(atan2(dy,dx) - labust::math::wrapRad(state.orientation.yaw))));
00203 
00204         distanceController.desired = -0.5*safetyRadius;
00205         distanceController.state = -dist;
00206         headingController.state = state.orientation.yaw;
00207 
00208         headingController.desired = atan2(dy,dx);
00209         headingController.feedforward = trackPoint.orientation_rate.yaw;
00210         distanceController.feedforward = sqrt(pow(trackPoint.body_velocity.x,2) + pow(trackPoint.body_velocity.y,2));
00211 
00212         float errorWrap = labust::math::wrapRad(headingController.desired - headingController.state);
00213         PIFFExtController_stepWrap(&headingController,Ts, errorWrap);
00214 
00215         nu.goal.requester = "dp_control";
00216         nu.twist.angular.z = headingController.output;
00217         //Limit the outgoing surge to a sensible value
00218         nu.twist.linear.x = 0;
00219         if ((dist < 5*safetyRadius) && (angleDiff < M_PI/2))
00220         {
00221                 //distanceController.state = -dx*cos(state.orientation.yaw) - dy*sin(state.orientation.yaw);
00222                 PIFFExtController_step(&distanceController,Ts);
00223                 nu.twist.linear.x = distanceController.output;
00224         }
00225         else if (angleDiff < M_PI/2)
00226         {
00227                 nu.twist.linear.x = surge;
00228         }
00229 
00230         inRegion = (dist < safetyRadius) || (inRegion && (dist < 2*safetyRadius));
00231 
00232         if (inRegion)
00233         {
00234                 nu.twist.linear.x = 0;
00235                 nu.twist.angular.z = 0;
00236         }
00237 
00238         nuRef.publish(nu);
00239 }
00240 
00241 void DPControl::start()
00242 {
00243         ros::spin();
00244 }
00245 
00246 void DPControl::initialize_controller()
00247 {
00248         ROS_INFO("Initializing dynamic positioning controller...");
00249 
00250         Eigen::Vector2d closedLoopFreq(Eigen::Vector2d::Ones());
00251         labust::tools::getMatrixParam(nh,"dp_controller/closed_loop_freq", closedLoopFreq);
00252         nh.param("dp_controller/sampling",Ts,Ts);
00253         nh.param("dp_controller/safetyRadius",safetyRadius, safetyRadius);
00254         nh.param("dp_controller/openLoopSurge",surge, surge);
00255 
00256         enum {Kp=0, Ki, Kd, Kt};
00257         PIDController_init(&headingController);
00258         headingController.gains[Kp] = 2*closedLoopFreq(1);
00259         headingController.gains[Ki] = closedLoopFreq(1)*closedLoopFreq(1);
00260         headingController.autoTracking = 0;
00261 
00262         PIDController_init(&distanceController);
00263         distanceController.gains[Kp] = 2*closedLoopFreq(0);
00264         distanceController.gains[Ki] = closedLoopFreq(0)*closedLoopFreq(0);
00265         distanceController.autoTracking = 0;
00266 
00267         ROS_INFO("Line following controller initialized.");
00268 }


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