ExecControl.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 #ifndef EXECCONTROL_HPP_
00035 #define EXECCONTROL_HPP_
00036 #include <labust/control/ExecDepGraph.hpp>
00037 #include <labust/control/ExecPNGraph.hpp>
00038 #include <labust/control/PNController.hpp>
00039 #include <navcon_msgs/RegisterController.h>
00040 
00041 #include <std_msgs/String.h>
00042 #include <ros/ros.h>
00043 
00044 #include <boost/config.hpp>
00045 #include <boost/graph/adjacency_list.hpp>
00046 
00047 #include <Eigen/Dense>
00048 
00049 #include <string>
00050 #include <vector>
00051 #include <map>
00052 #include <list>
00053 
00054 namespace labust
00055 {
00056         namespace control
00057         {
00067                 class ExecControl
00068                 {
00069                         enum {u=0,v,w,p,q,r};
00070                         enum {X=0,Y,Z,K,M,N};
00071                         enum {alpha=0,beta,betaa};
00072                         enum {Kp=0,Ki,Kd,Kt};
00073 
00074                         typedef
00075                         struct ControllerInfo
00076                         {
00077                                 navcon_msgs::RegisterControllerRequest info;
00078                                 int graph_idx;
00079                                 int en_idx;
00080                                 int dis_idx;
00081 
00082                                 int place_num;
00083                                 int en_t_num;
00084                                 int dis_t_num;
00085                         } ControllerInfo;
00086 
00087                         typedef std::map<std::string, ControllerInfo> ControllerMap;
00088                   //typedef boost::property<boost::vertex_name_t, std::string> VertexProperty;
00089                         struct VertexProperty
00090                         {
00091                                 VertexProperty():
00092                                         name("uninitialized"),
00093                                         type(0),
00094                                         marked(false){};
00095 
00096                                 VertexProperty(const std::string& name, int type):
00097                                         name(name),
00098                                         type(type),
00099                                         marked(false){};
00100 
00101                                 enum {p=0, t=1};
00102                                 std::string name;
00103                                 int type;
00104                                 bool marked;
00105                         };
00106 
00107                         struct RVertexProperty
00108                         {
00109                                 Eigen::VectorXi marking;
00110                         };
00111 
00112                         struct REdgeProperty
00113                         {
00114                                 int t;
00115                                 int weight;
00116                         };
00117 
00118                   typedef boost::adjacency_list<boost::vecS, boost::vecS,
00119                                 boost::directedS, VertexProperty,
00120                                 boost::property < boost::edge_weight_t, int > > GraphType;
00121 
00122                   typedef boost::adjacency_list<boost::vecS, boost::vecS,
00123                                 boost::directedS, RVertexProperty,
00124                                 REdgeProperty > RGraphType;
00125 
00126                 public:
00130                         ExecControl();
00134                         void onInit();
00135 //                      /**
00136 //                       * Performs one iteration.
00137 //                       */
00138 //                      void step();
00139 //                      /**
00140 //                       * Start the controller loop.
00141 //                       */
00142 //                      void start();
00143 //
00144 //              private:
00145 //                      /**
00146 //                       * Handle incoming reference message.
00147 //                       */
00148 //                      void handleReference(const auv_msgs::BodyVelocityReq::ConstPtr& ref);
00149 //                      /**
00150 //                       * Handle incoming estimates message.
00151 //                       */
00152 //                      void handleEstimates(const auv_msgs::NavSts::ConstPtr& estimate);
00153 //                      /**
00154 //                       * Handle incoming estimates message.
00155 //                       */
00156 //                      void handleMeasurement(const auv_msgs::NavSts::ConstPtr& meas);
00157 //                      /**
00158 //                       * Handle incoming estimates message.
00159 //                       */
00160 //                      void handleWindup(const auv_msgs::BodyForceReq::ConstPtr& tau);
00161 //                      /**
00162 //                       * Handle incoming estimates message.
00163 //                       */
00164 //                      void handleManual(const sensor_msgs::Joy::ConstPtr& joy);
00168                         bool onRegisterController(navcon_msgs::RegisterController::Request& req,
00169                                         navcon_msgs::RegisterController::Response& resp);
00173                         void onActivateController(const std_msgs::String::ConstPtr& name);
00177                         void addToGraph(const std::string& name = "");
00182                         void add_pn_edge(int i, int j, GraphType& graph);
00186                         void find_path();
00190                         void reachability();
00194                         void get_firing(const std::string& name);
00195                         void get_firing2(const std::string& name);
00199                         void addToMatrix(const std::string& name);
00203                         bool firing_rec(int des_place, std::vector<int>& skip_transitions, std::vector<int>& visited_places);
00204                         bool firing_rec2(int des_place, std::vector<int>& skip_transitions, std::vector<int>& visited_places);
00205 //                      /**
00206 //                       * Handle the enable control request.
00207 //                       */
00208 //                      bool handleEnableControl(labust_uvapp::EnableControl::Request& req,
00209 //                                      labust_uvapp::EnableControl::Response& resp);
00210 //                      /**
00211 //                       * Dynamic reconfigure callback.
00212 //                       */
00213 //                      void dynrec_cb(labust_uvapp::VelConConfig& config, uint32_t level);
00214 //                      /**
00215 //                       * The safety test.
00216 //                       */
00217 //                      void safetyTest();
00218 //                      /**
00219 //                       * Update the dynamic reconfiguration settings.
00220 //                       */
00221 //                      void updateDynRecConfig();
00222 //                      /**
00223 //                       * Perform the identification step.
00224 //                       */
00225 //                      double doIdentification(int i);
00226 //                      /**
00227 //                       * The ROS node handles.
00228 //                       */
00229 //                      ros::NodeHandle nh,ph;
00230 //                      /**
00231 //                       * Last message times.
00232 //                       */
00233 //                      ros::Time lastRef, lastMan, lastEst, lastMeas;
00234 //                      /**
00235 //                       * Timeout
00236 //                       */
00237 //                      double timeout;
00238 //
00239 //                      /**
00240 //                       * Initialize the controller parameters etc.
00241 //                       */
00242 //                      void initialize_controller();
00243 //                      /**
00244 //                       * The velocity controllers.
00245 //                       */
00246 //                      PIDController controller[r+1];
00247 //                      /**
00248 //                       * The identification controllers.
00249 //                       */
00250 //                      boost::shared_ptr<SOIdentification> ident[r+1];
00251 //                      /**
00252 //                       * The mesurement needed for identification.
00253 //                       */
00254 //                      double measurement[r+1];
00255 //                      /**
00256 //                       * Joystick message.
00257 //                       */
00258 //                      float tauManual[N+1];
00259 //                      /**
00260 //                       * Joystick scaling.
00261 //                       */
00262 //                      double joy_scale, Ts;
00263 //                      /**
00264 //                       * Enable/disable controllers, external windup flag.
00265 //                       */
00266 //                      boost::array<int32_t,r+1> axis_control;
00267 //                      /**
00268 //                       * Timeout management.
00269 //                       */
00270 //                      bool suspend_axis[r+1];
00271 //                      /**
00272 //                       * Dynamic reconfigure server.
00273 //                       */
00274 //                      boost::recursive_mutex serverMux;
00275 //
00276 //                      /**
00277 //                       * The publisher of the TAU message.
00278 //                       */
00279                         ros::Publisher depGraphPub, pnGraphPub;
00280 //                      /**
00281 //                       * The subscribed topics.
00282 //                       */
00283                         ros::Subscriber activateController;
00287                         ros::ServiceServer registerController;
00288 //                      /**
00289 //                       * The dynamic reconfigure parameters.
00290 //                       */
00291 //                      labust_uvapp::VelConConfig config;
00292 //                      /**
00293 //                       * The dynamic reconfigure server.
00294 //                       */
00295 //                dynamic_reconfigure::Server<labust_uvapp::VelConConfig> server;
00296 //                /**
00297 //                 * Variable access helper mass.
00298 //                 */
00299 //                const static std::string dofName[r+1];
00300 
00304                         ControllerMap controllers;
00308                         std::vector<std::string> names;
00312                         std::string active_dof[6];
00316                         std::list<std::string> active_cnt;
00320                         GraphType graph;
00324                         RGraphType rgraph;
00328                         int tnum;
00332                         int pnum;
00336                         Eigen::MatrixXi Dm,Dp;
00337                         /*
00338                          * The current marking.
00339                          */
00340                         Eigen::VectorXi marking;
00344                         std::map<int, std::string> pname;
00348                         std::vector<int> firing_seq;
00352                         ExecDepGraph depGraph;
00356                         ExecPNGraph pnGraph;
00360                         PNController pnCon;
00361                 };
00362         }
00363 }
00364 
00365 /* EXECCONTROL_HPP_ */
00366 #endif


labust_execution
Author(s): Gyula Nagy
autogenerated on Fri Feb 7 2014 11:36:24