HLExecControl.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: 11.07.2013.
00036  *********************************************************************/
00037 #include "HLExecControl.hpp"
00038 #include <labust_uvapp/EnableControl.h>
00039 
00040 using namespace labust::control;
00041 HLExecControl::HLExecControl(){this->onInit();};
00042 
00043 void HLExecControl::onInit()
00044 {
00045         for (int i=0; i<active_dofs.size(); ++i)         active_dofs[i]="";
00046         ros::NodeHandle nh;
00047         //Configure service
00048         registerServer = nh.advertiseService("RegisterController",
00049                         &HLExecControl::onRegReq, this);
00050 
00051         nuIn = nh.subscribe<auv_msgs::BodyVelocityReq>("nuRef", 1,
00052                         &HLExecControl::onNuIn,this);
00053         cntSel = nh.subscribe<std_msgs::String>("controller_select", 1,
00054                         &HLExecControl::onControllerSelect,this);
00055 
00056         nuRef = nh.advertise<auv_msgs::BodyVelocityReq>("nuRefMerged", 1);
00057 }
00058 
00059 bool HLExecControl::onRegReq(labust_uvapp::RegisterController::Request& req,
00060                                         labust_uvapp::RegisterController::Response& resp)
00061 {
00062         std::string name(req.__connection_header->at("callerid"));
00063         ROS_INFO("Registration request from node %s.",req.info.name.c_str());
00064         for(int i=0; i<req.info.dofs.size(); ++i)
00065         {
00066                 ROS_INFO("DOF %d.",req.info.dofs[i]);
00067         }
00068         controllers[req.info.name] = req.info;
00069 
00070         return true;
00071 }
00072 
00073 void HLExecControl::onControllerSelect(const std_msgs::String::ConstPtr& name)
00074 {
00075         if (controllers.find(name->data) == controllers.end())
00076         {
00077                 ROS_INFO("Controller %s was not registered.",name->data.c_str());
00078                 return;
00079         }
00080 
00081         std::vector<std::string> disableControllers;
00082         std::vector<std::string> enableControllers;
00083 
00084         labust_uvapp::ControllerInfo& info = controllers[name->data];
00085 
00086         for(int i=0; i<info.dofs.size(); ++i)
00087         {
00088                 int reqDOF = info.dofs[i];
00089                 ROS_INFO("Degree of freedom %d",reqDOF);
00090                 if (active_dofs[reqDOF] != "")
00091                 {
00092                         disableControllers.push_back(active_dofs[reqDOF]);
00093                         //schedule all sub-controllers of the disable controller for disable
00094                         disableControllers.insert(disableControllers.end(),
00095                                                                 controllers[active_dofs[reqDOF]].sub_controllers.begin(),
00096                                                                 controllers[active_dofs[reqDOF]].sub_controllers.end());
00097                  }
00098                 active_dofs[reqDOF] = name->data;
00099         }
00100 
00101         enableControllers.push_back(name->data);
00102         for (int i=0; i<info.sub_controllers.size(); ++i)
00103         {
00104                 //TEST IF REQUEIRED SUB-CONTROLLER IS REGISTERED
00105                 enableControllers.push_back(info.sub_controllers[i]);
00106         }
00107 
00108         ros::NodeHandle nh;
00109         for (int i=0; i<disableControllers.size(); ++i)
00110         {
00111                 ros::ServiceClient client = nh.serviceClient<labust_uvapp::EnableControl>(disableControllers[i] + "_enable");
00112                 labust_uvapp::EnableControl srv;
00113                 srv.request.enable = false;
00114                 if (!client.call(srv))
00115                 {
00116                         ROS_ERROR("Failed to call the %s configuration service.",disableControllers[i].c_str());
00117                 }
00118         };
00119 
00120         for (int i=0; i<enableControllers.size(); ++i)
00121         {
00122                 ros::ServiceClient client = nh.serviceClient<labust_uvapp::EnableControl>(enableControllers[i] + "_enable");
00123                 labust_uvapp::EnableControl srv;
00124                 srv.request.enable = true;
00125                 if (!client.call(srv))
00126                 {
00127                         ROS_ERROR("Failed to call the %s configuration service.",enableControllers[i].c_str());
00128                 }
00129         };
00130 }
00131 
00132 void HLExecControl::onNuIn(const auv_msgs::BodyVelocityReq::ConstPtr& nu)
00133 {
00134         std::string name(nu->__connection_header->at("callerid"));
00135         if (controllers.find(name) == controllers.end())
00136         {
00137                 ROS_INFO("Controller %s was not registered.",name.c_str());
00138         }
00139         //auv_msgs::BodyVelocityReq::Ptr merged(new auv_msgs::BodyVelocityReq());
00140 }
00141 
00142 int main(int argc, char* argv[])
00143 {
00144         ros::init(argc,argv,"execution_control");
00145         //Initialize
00146         HLExecControl exec;
00147         //Start execution.
00148         ros::spin();
00149         return 0;
00150 }
00151 
00152 
00153 


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