dp_control.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: 01.02.2013.
00036 *********************************************************************/
00037 #include <labust/control/DPControl.hpp>
00038 #include <labust/control/HLControl.hpp>
00039 #include <labust/tools/MatrixLoader.hpp>
00040 #include <labust/math/NumberManipulation.hpp>
00041 #include <Eigen/Dense>
00042 #include <boost/thread/mutex.hpp>
00043 
00044 #include <geometry_msgs/PointStamped.h>
00045 
00046 namespace labust{namespace control{
00048 struct FADPControl
00049 {
00050         enum {x=0,y,psi};
00051 
00052         FADPControl():Ts(0.1){};
00053 
00054         void init()
00055         {
00056                 ros::NodeHandle nh;
00057                 refTrack = nh.subscribe<auv_msgs::NavSts>("TrackPoint", 1,
00058                                 &FADPControl::onTrackPoint,this);
00059                 refPoint = nh.subscribe<geometry_msgs::PointStamped>("LFPoint", 1,
00060                                 &FADPControl::onNewPoint,this);
00061                 headingRef = nh.subscribe<std_msgs::Float32>("heading_ref", 1,
00062                                         &FADPControl::onHeadingRef,this);
00063 
00064                 initialize_controller();
00065         }
00066 
00067         void onTrackPoint(const auv_msgs::NavSts::ConstPtr& ref)
00068         {
00069                 boost::mutex::scoped_lock l(cnt_mux);
00070                 trackPoint = *ref;
00071 
00072                 con[x].desired =  trackPoint.position.north;
00073                 con[y].desired =  trackPoint.position.east;
00074                 con[x].feedforward=  trackPoint.body_velocity.x*cos(trackPoint.orientation.yaw);
00075                 con[y].feedforward =  trackPoint.body_velocity.x*sin(trackPoint.orientation.yaw);
00076         };
00077 
00078         void onHeadingRef(const std_msgs::Float32::ConstPtr& hdg)
00079         {
00080                 boost::mutex::scoped_lock l(cnt_mux);
00081                 con[psi].desired = hdg->data;
00082         };
00083 
00084         void onNewPoint(const geometry_msgs::PointStamped::ConstPtr& point)
00085         {
00086                 boost::mutex::scoped_lock l(cnt_mux);
00087                 con[x].desired = point->point.x;
00088                 con[y].desired = point->point.y;
00089                 con[x].feedforward=  0;
00090                 con[y].feedforward =  0;
00091         };
00092 
00093         void windup(const auv_msgs::BodyForceReq& tauAch)
00094         {
00095                 //Copy into controller
00096                 boost::mutex::scoped_lock l(cnt_mux);
00097                 con[x].windup = tauAch.disable_axis.x;
00098                 con[y].windup = tauAch.disable_axis.y;
00099                 con[psi].windup = tauAch.disable_axis.yaw;
00100         };
00101 
00102         void step(const auv_msgs::NavSts::ConstPtr& state, auv_msgs::BodyVelocityReqPtr nu)
00103         {
00104                 boost::mutex::scoped_lock l(cnt_mux);
00105                 con[x].state = state->position.north;
00106                 con[y].state = state->position.east;
00107                 con[psi].state = state->orientation.yaw;
00108 
00109                 PIFFExtController_step(&con[x],Ts);
00110                 PIFFExtController_step(&con[y],Ts);
00111                 float errorWrap = labust::math::wrapRad(
00112                         con[psi].desired - con[psi].state);
00113                 PIFFExtController_stepWrap(&con[psi],Ts, errorWrap);
00114 
00115                 nu->header.stamp = ros::Time::now();
00116                 nu->goal.requester = "fadp_controller";
00117 
00118                 Eigen::Vector2f out, in;
00119                 Eigen::Matrix2f R;
00120                 in<<con[x].output,con[y].output;
00121                 double yaw(state->orientation.yaw);
00122                 R<<cos(yaw),-sin(yaw),sin(yaw),cos(yaw);
00123 
00124                 out = R.transpose()*in;
00125 
00126                 nu->twist.linear.x = out[0];
00127                 nu->twist.linear.y = out[1];
00128                 nu->twist.angular.z = con[psi].output;
00129         }
00130 
00131         void initialize_controller()
00132         {
00133                 ROS_INFO("Initializing dynamic positioning controller...");
00134 
00135                 ros::NodeHandle nh;
00136                 Eigen::Vector3d closedLoopFreq(Eigen::Vector3d::Ones());
00137                 labust::tools::getMatrixParam(nh,"dp_controller/closed_loop_freq", closedLoopFreq);
00138                 nh.param("dp_controller/sampling",Ts,Ts);
00139 
00140                 enum {Kp=0, Ki, Kd, Kt};
00141                 for (size_t i=0; i<3;++i)
00142                 {
00143                         PIDController_init(&con[i]);
00144                         con[i].gains[Kp] = 2*closedLoopFreq(i);
00145                         con[i].gains[Ki] = closedLoopFreq(i)*closedLoopFreq(i);
00146                         con[i].autoTracking = 0;
00147                 }
00148 
00149                 ROS_INFO("Dynamic positioning controller initialized.");
00150         }
00151 
00152 private:
00153         PIDController con[3];
00154         ros::Subscriber refTrack, refPoint, headingRef;
00155         auv_msgs::NavSts trackPoint;
00156         boost::mutex cnt_mux;
00157         double Ts;
00158 };
00159 }}
00160 
00161 int main(int argc, char* argv[])
00162 {
00163         ros::init(argc,argv,"dp_control");
00164 
00165         bool underactuated(true);
00166 
00167         ros::NodeHandle nh;
00168         nh.param("dp_controller/underactuated",underactuated,underactuated);
00169 
00170         if (underactuated)
00171         {
00172                 //Initialize
00173                 labust::control::DPControl controller;
00174                 //Start execution.
00175                 controller.start();
00176         }
00177         else
00178         {
00179                 labust::control::HLControl<labust::control::FADPControl,
00180                         labust::control::EnablePolicy,
00181                         labust::control::WindupPolicy> controller;
00182                 ros::spin();
00183         }
00184 
00185         return 0;
00186 }
00187 
00188 
00189 


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