controllers_manager.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Robot Control and Pattern Recognition Group, Warsaw University of Technology.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Robot Control and Pattern Recognition Group,
00014  *       Warsaw University of Technology nor the names of its contributors may
00015  *       be used to endorse or promote products derived from this software
00016  *       without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 /*
00032  * ControllersManager.cpp
00033  *
00034  *  Created on: 04-02-2012
00035  *      Author: Konrad Banachowicz
00036  */
00037 
00038 #include <ocl/Component.hpp>
00039 
00040 #include "controllers_manager.h"
00041 
00042 ControllersManager::ControllersManager(const std::string& name) :
00043   RTT::TaskContext(name, PreOperational), number_of_controllers_prop_("NumberOfControllers"), default_controller_prop_("DefaultController"), as(this, "ControllersManager",
00044                                                          boost::bind(&ControllersManager::goalCB, this, _1),
00045                                                          boost::bind(&ControllersManager::cancelCB, this, _1), true)
00046 {
00047   this->addProperty(number_of_controllers_prop_);
00048   this->addProperty(default_controller_prop_);
00049 }
00050 
00051 ControllersManager::~ControllersManager()
00052 {
00053 
00054 }
00055 
00056 bool ControllersManager::configureHook()
00057 {
00058   
00059   
00060   if ((number_of_controllers_ = number_of_controllers_prop_.get()) == 0)
00061   {
00062     return false;
00063   }
00064 
00065    RTT::Logger::log(RTT::Logger::Error) << "number of controllers " << number_of_controllers_
00066         << RTT::endlog();
00067 
00068   for (unsigned int i = 0; i < number_of_controllers_; i++)
00069   {
00070     controllers_names_.push_back(((RTT::Property<std::string>*)this->getProperty(std::string("controller_") + (char)(i + 48) + "_name"))->get());
00071     controllers_enable_port_.push_back(boost::shared_ptr<RTT::OutputPort<bool> > (new RTT::OutputPort<bool>));
00072     controllers_busy_port_.push_back(boost::shared_ptr<RTT::InputPort<bool> > (new RTT::InputPort<bool>));
00073     
00074     this->addPort(controllers_names_[i] + "Enable" , *controllers_enable_port_[i]);
00075     this->addPort(controllers_names_[i] + "Busy" , *controllers_busy_port_[i]);
00076   }
00077 
00078   return true;
00079 }
00080 
00081 bool ControllersManager::startHook()
00082 {
00083   std::string default_controller = default_controller_prop_.get();
00084   
00085   active_controller_ = -1;
00086   
00087   for(size_t i = 0; i < controllers_names_.size(); i++)
00088   {
00089     if(controllers_names_[i] == default_controller)
00090     {
00091       active_controller_ = i;
00092     }
00093   }
00094   
00095   if(active_controller_ == -1)
00096     return false;
00097     
00098   for(size_t i = 0; i < number_of_controllers_; i++)
00099   {
00100     if(i == active_controller_)
00101     {
00102       controllers_enable_port_[i]->write(true);
00103     } else 
00104     {
00105       controllers_enable_port_[i]->write(false);
00106     }
00107   }
00108   
00109   return true;
00110 }
00111 
00112 void ControllersManager::updateHook()
00113 {
00114   as.spinOnce();
00115   
00116   if(active_controller_ == next_controller_)
00117   {
00118     for(size_t i = 0; i < number_of_controllers_; i++)
00119     {
00120       if(i == active_controller_)
00121       {
00122         controllers_enable_port_[i]->write(true);
00123       } else 
00124       {
00125         controllers_enable_port_[i]->write(false);
00126       }
00127     }
00128   } else
00129   {
00130     for(size_t i = 0; i < number_of_controllers_; i++)
00131     { 
00132       controllers_enable_port_[i]->write(false);
00133     }
00134     
00135     bool busy = true;
00136     controllers_busy_port_[active_controller_]->read(busy);
00137     
00138     if(!busy)
00139     {
00140       active_controller_ = next_controller_;
00141       activeGoal.setSucceeded();
00142     }
00143   }
00144   
00145 }
00146 
00147 void ControllersManager::goalCB(GoalHandle gh)
00148 {
00149     int controller = -1;
00150     
00151     Goal g = gh.getGoal();
00152 
00153     activeGoal = gh;
00154  
00155     for(size_t i = 0; i < controllers_names_.size(); i++)
00156     {
00157       if(controllers_names_[i] == g->controller_id)
00158       {
00159         controller = i;
00160       }
00161     }
00162  
00163     if(controller == -1)
00164     {
00165       gh.setRejected();
00166     } else 
00167     {
00168       gh.setAccepted();
00169       next_controller_ = controller;
00170     }
00171 }
00172 
00173 void ControllersManager::cancelCB(GoalHandle gh)
00174 {
00175   goal_active = false;
00176 }
00177 
00178 ORO_CREATE_COMPONENT( ControllersManager )


oro_controllers_manager
Author(s): Konrad Banachowicz
autogenerated on Mon Oct 6 2014 03:10:24