HLControl.hpp
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  *  Created on: 26.06.2013.
00035  *  Author: Dula Nad
00036  *********************************************************************/
00037 #ifndef HLCONTROL_HPP_
00038 #define HLCONTROL_HPP_
00039 #include <labust_uvapp/EnableControl.h>
00040 
00041 #include <auv_msgs/NavSts.h>
00042 #include <auv_msgs/BodyForceReq.h>
00043 #include <auv_msgs/BodyVelocityReq.h>
00044 #include <geometry_msgs/PointStamped.h>
00045 #include <std_msgs/Float32.h>
00046 #include <std_msgs/Bool.h>
00047 #include <ros/ros.h>
00048 
00049 namespace labust
00050 {
00051         namespace control
00052         {
00056                 class WindupPolicy
00057                 {
00058                 public:
00059                         WindupPolicy()
00060                         {
00061                                 ros::NodeHandle nh;
00062                                 windupSub = nh.subscribe<auv_msgs::BodyForceReq>("tauAch", 1,
00063                                                 &WindupPolicy::onWindup,this);
00064                         }
00065 
00066                         void onWindup(const auv_msgs::BodyForceReq::ConstPtr& tauAch)
00067                         {
00068                                 ROS_INFO("Handle windup policy.");
00069                                 this->tauAch = *tauAch;
00070                         }
00071 
00072                         template <class Base>
00073                         inline void get_windup(Base* b){b->windup(tauAch);};
00074 
00075                 protected:
00076                         ros::Subscriber windupSub;
00077                         auv_msgs::BodyForceReq tauAch;
00078                 };
00079 
00080                 class EnablePolicy
00081                 {
00082                 public:
00083                         EnablePolicy():
00084                                 enable(false)
00085                         {
00086                                 ros::NodeHandle nh;
00087                                 enableControl = nh.advertiseService("Enable",
00088                                                 &EnablePolicy::onEnableControl, this);
00089                         }
00090 
00091                         bool onEnableControl(labust_uvapp::EnableControl::Request& req,
00092                                         labust_uvapp::EnableControl::Response& resp)
00093                         {
00094                                 this->enable = req.enable;
00095                                 return true;
00096                         }
00097 
00098                 protected:
00102                         bool enable;
00106                         ros::ServiceServer enableControl;
00107                 };
00108 
00109                 struct NoEnable{static const bool enable=true;};
00110                 struct NoWindup{
00111                         template <class Base>
00112                         inline void get_windup(Base b){};};
00113 
00119                 template <
00120                 class Controller,
00121                 class Enable = NoEnable,
00122                 class Windup = NoWindup,
00123                 class OutputType = auv_msgs::BodyVelocityReq,
00124                 class InputType = auv_msgs::NavSts
00125                 >
00126                 class HLControl : public Controller, Enable, Windup
00127                 {
00128                 public:
00132                         HLControl()
00133                         {
00134                                 onInit();
00135                         }
00139                         void onInit()
00140                         {
00141                                 ros::NodeHandle nh;
00142                                 //Initialize publishers
00143                                 refPub = nh.advertise<OutputType>("ref", 1);
00144 
00145                                 //Initialze subscribers
00146                                 stateSub = nh.subscribe<InputType>("state", 1,
00147                                                 &HLControl::onEstimate,this);
00148 
00149                                 Controller::init();
00150                         }
00151 
00152                 private:
00153 
00154                         void onEstimate(const typename InputType::ConstPtr& estimate)
00155                         {
00156                                 if (!Enable::enable) return;
00157                                 //Copy into controller
00158                                 typename OutputType::Ptr ref(new OutputType());
00159                                 Windup::get_windup(this);
00160                                 Controller::step(estimate, ref);
00161                                 refPub.publish(ref);
00162                         }
00163 
00167                         ros::Publisher refPub;
00171                         ros::Subscriber stateSub;
00172                 };
00173         }
00174 }
00175 
00176 /* HLCONTROL_HPP_ */
00177 #endif


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