labust_mission: missionExecution.hpp Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002  * missionExecution.hpp
00003  *
00004  *  Created on: Apr 22, 2014
00005  *      Author: Filip Mandic
00006  *
00007  ********************************************************************/
00008 
00009 /*********************************************************************
00010 * Software License Agreement (BSD License)
00011 *
00012 *  Copyright (c) 2014, LABUST, UNIZG-FER
00013 *  All rights reserved.
00014 *
00015 *  Redistribution and use in source and binary forms, with or without
00016 *  modification, are permitted provided that the following conditions
00017 *  are met:
00018 *
00019 *   * Redistributions of source code must retain the above copyright
00020 *     notice, this list of conditions and the following disclaimer.
00021 *   * Redistributions in binary form must reproduce the above
00022 *     copyright notice, this list of conditions and the following
00023 *     disclaimer in the documentation and/or other materials provided
00024 *     with the distribution.
00025 *   * Neither the name of the LABUST nor the names of its
00026 *     contributors may be used to endorse or promote products derived
00027 *     from this software without specific prior written permission.
00028 *
00029 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00030 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00031 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00032 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00033 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00034 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00035 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00036 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00037 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00038 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00039 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00040 *  POSSIBILITY OF SUCH DAMAGE.
00041 *********************************************************************/
00042 
00043 #ifndef MISSIONEXECUTION_HPP_
00044 #define MISSIONEXECUTION_HPP_
00045 
00046 
00047 #include <labust_mission/labustMission.hpp>
00048 #include <labust_mission/primitiveManager.hpp>
00049 #include <exprtk/exprtk.hpp>
00050 
00051 #include <decision_making/SynchCout.h>
00052 #include <decision_making/BT.h>
00053 #include <decision_making/FSM.h>
00054 #include <decision_making/ROSTask.h>
00055 #include <decision_making/DecisionMaking.h>
00056 
00057 #include <boost/function.hpp>
00058 #include <boost/algorithm/string.hpp>
00059 
00060 extern decision_making::EventQueue* mainEventQueue;
00061 
00062 namespace ser = ros::serialization;
00063 
00064 /*********************************************************************
00065  ***  MissionExecution class definition
00066  ********************************************************************/
00067 
00068 namespace labust {
00069         namespace mission {
00070 
00071                 class MissionExecution{
00072 
00073                 public:
00074 
00075                         /*****************************************************************
00076                          ***  Class functions
00077                          ****************************************************************/
00078 
00079                         MissionExecution(ros::NodeHandle& nh);
00080 
00081                     void evaluatePrimitive(string primitiveString);
00082 
00083                         /*****************************************************************
00084                          ***  State machine primitive states
00085                          ****************************************************************/
00086 
00087                     void execute_primitive();
00088 
00089                     void dynamic_postitioning_state();
00090 
00091                     void go2point_FA_hdg_state();
00092 
00093                     void go2point_FA_state();
00094 
00095                     void go2point_UA_state();
00096 
00097                     void course_keeping_FA_state();
00098 
00099                     void course_keeping_UA_state();
00100 
00101                     void iso_state();
00102 
00103                     void path_following_state();
00104 
00105                     void pointer_state();
00106 
00107                         /*****************************************************************
00108                          ***  ROS Subscriptions Callback
00109                          ****************************************************************/
00110 
00111                         void onStateHat(const auv_msgs::NavSts::ConstPtr& data);
00112 
00113                         void onDataEventsContainer(const misc_msgs::DataEventsContainer::ConstPtr& data);
00114 
00115                         void onEventString(const std_msgs::String::ConstPtr& msg);
00116 
00117                         void onReceivePrimitive(const misc_msgs::SendPrimitive::ConstPtr& data);
00118 
00119                         /*********************************************************************
00120                          *** Helper functions
00121                          ********************************************************************/
00122 
00123                         void requestPrimitive();
00124 
00125                         void setTimeout(double timeout);
00126 
00127                         //void setRefreshRate(double timeout, boost::function<void(void)> onRefreshCallback );
00128 
00129                         void onTimeout(const ros::TimerEvent& timer);
00130 
00131                         void onPrimitiveEndReset();
00132 
00133                         /*********************************************************************
00134                          ***  Class variables
00135                          ********************************************************************/
00136 
00138                         labust::controller::PrimitiveManager CM;
00139 
00141                         ros::NodeHandle nh_;
00142 
00144                         ros::Timer timer;
00145 
00147                         ros::Publisher pubRequestPrimitive, pubEventString;
00148 
00150                         ros::Subscriber subDataEventsContainer, subEventString, subReceivePrimitive, subStateHat;
00151 
00153                         ros::ServiceClient srvExprEval;
00154 
00156                         auv_msgs::NavSts state;
00157 
00159                         auv_msgs::NED oldPosition;
00160 
00162                         misc_msgs::SendPrimitive receivedPrimitive;
00163 
00165                         vector<string> eventsActive, primitiveStrContainer;
00166 
00168                         map<string, double> primitiveMap;
00169 
00171                         map<string, string> primitiveStringMap;
00172 
00174                         bool checkEventFlag, timeoutActive;
00175 
00177                         int nextPrimitive;
00178 
00180                         bool missionActive;
00181                 };
00182 
00183                 /*****************************************************************
00184                  ***  Class functions
00185                  ****************************************************************/
00186 
00187                 MissionExecution::MissionExecution(ros::NodeHandle& nh):checkEventFlag(false),
00188                                                                                                                                         nextPrimitive(1),
00189                                                                                                                                         timeoutActive(false),
00190                                                                                                                                         missionActive(false){
00191 
00193                         subEventString = nh.subscribe<std_msgs::String>("eventString",1, &MissionExecution::onEventString, this);
00194                         subReceivePrimitive = nh.subscribe<misc_msgs::SendPrimitive>("sendPrimitive",1, &MissionExecution::onReceivePrimitive, this);
00195                         subDataEventsContainer = nh.subscribe<misc_msgs::DataEventsContainer>("dataEventsContainer",1, &MissionExecution::onDataEventsContainer, this);
00196                         subStateHat = nh.subscribe<auv_msgs::NavSts>("stateHat",1, &MissionExecution::onStateHat, this);
00197 
00199                         pubRequestPrimitive = nh.advertise<std_msgs::UInt16>("requestPrimitive",1);
00200                         pubEventString = nh.advertise<std_msgs::String>("eventString",1);
00201 
00202 
00204                         srvExprEval = nh.serviceClient<misc_msgs::EvaluateExpression>("evaluate_expression");
00205 
00207 
00208                         primitiveMap.insert(std::pair<string, double>("north", 0.0));
00209                         primitiveMap.insert(std::pair<string, double>("east", 0.0));
00210                         primitiveMap.insert(std::pair<string, double>("depth", 0.0));
00211                         primitiveMap.insert(std::pair<string, double>("heading", 0.0));
00212                         primitiveMap.insert(std::pair<string, double>("course", 0.0));
00213                         primitiveMap.insert(std::pair<string, double>("speed", 0.0));
00214                         primitiveMap.insert(std::pair<string, double>("victory_radius", 0.0));
00215                         primitiveMap.insert(std::pair<string, double>("dof", 0.0));
00216                         primitiveMap.insert(std::pair<string, double>("command", 0.0));
00217                         primitiveMap.insert(std::pair<string, double>("hysteresis", 0.0));
00218                         primitiveMap.insert(std::pair<string, double>("reference", 0.0));
00219                         primitiveMap.insert(std::pair<string, double>("sampling_rate", 0.0));
00220                         primitiveMap.insert(std::pair<string, double>("victory_radius", 0.0));
00221                         primitiveMap.insert(std::pair<string, double>("timeout", 0.0));
00222 
00223                         primitiveStringMap.insert(std::pair<string, string>("radius_topic", ""));
00224                         primitiveStringMap.insert(std::pair<string, string>("center_topic", ""));
00225                         primitiveStringMap.insert(std::pair<string, string>("target_topic", ""));
00226                         primitiveStringMap.insert(std::pair<string, string>("point", ""));
00227 
00228                 }
00229 
00230             void MissionExecution::evaluatePrimitive(string primitiveString){
00231 
00232                 /*** Reset data ***/
00233                 primitiveMap["timeout"] = 0;
00234 
00235                         misc_msgs::EvaluateExpression evalExpr;
00236                         primitiveStrContainer = labust::utilities::split(primitiveString, ':');
00237 
00238                         for(vector<string>::iterator it = primitiveStrContainer.begin(); it != primitiveStrContainer.end(); it = it + 2){
00239 
00240                                 size_t found = (*(it+1)).find_first_of('#');
00241                                 if(found != string::npos){
00242                                         primitiveStringMap[*it] =  (*(it+1)).erase(found,1);
00244                                         ROS_ERROR("Evaluation %s: %s", (*it).c_str(),primitiveStringMap[*it].c_str());
00245                                         continue;
00246                                 }
00247 
00248                                 evalExpr.request.expression = (*(it+1)).c_str();
00249                                 primitiveMap[*it] =  (labust::utilities::callService(srvExprEval, evalExpr)).response.result;
00250 
00251                                 ROS_ERROR("Evaluation %s: %f", (*it).c_str(),primitiveMap[*it]);
00252 
00253                         }
00254                 }
00255 
00256                 /*****************************************************************
00257                  ***  State machine primitive states
00258                  ****************************************************************/
00259 
00260             void MissionExecution::execute_primitive()
00261             {
00262 
00263             }
00264 
00265             void MissionExecution::dynamic_postitioning_state(){
00266 
00268                         evaluatePrimitive(receivedPrimitive.primitiveString.data);
00270                 if(!timeoutActive && primitiveMap["timeout"] > 0)
00271                         setTimeout(primitiveMap["timeout"]);
00273                         CM.dynamic_positioning(true, primitiveMap["north"], primitiveMap["east"], primitiveMap["heading"]);
00274                         oldPosition.north = primitiveMap["north"];
00275                         oldPosition.east = primitiveMap["east"];
00276                         oldPosition.depth = primitiveMap["depth"];
00277 
00278             }
00279 
00280             void MissionExecution::go2point_FA_hdg_state(){
00281 
00282 
00284                         evaluatePrimitive(receivedPrimitive.primitiveString.data);
00286                         if(!timeoutActive && primitiveMap["timeout"] > 0)
00287                                 setTimeout(primitiveMap["timeout"]);
00289                         CM.go2point_FA_hdg(true, oldPosition.north, oldPosition.east, primitiveMap["north"], primitiveMap["east"], primitiveMap["speed"], primitiveMap["heading"], primitiveMap["victory_radius"]);
00290                         ROS_ERROR("go2pointFA: T1: %f, %f, T2: %f, %f, Speed: %f, Heading: %f, VictoryRadius: %f  ", oldPosition.north, oldPosition.east, primitiveMap["north"], primitiveMap["east"], primitiveMap["speed"], primitiveMap["heading"], primitiveMap["victory_radius"]);
00291 
00292                         oldPosition.north = primitiveMap["north"];
00293                         oldPosition.east = primitiveMap["east"];
00294                         oldPosition.depth = primitiveMap["depth"];
00295             }
00296 
00297             void MissionExecution::go2point_FA_state(){
00298 
00299 
00301                         evaluatePrimitive(receivedPrimitive.primitiveString.data);
00303                         if(!timeoutActive && primitiveMap["timeout"] > 0)
00304                                 setTimeout(primitiveMap["timeout"]);
00306                         CM.go2point_FA(true, oldPosition.north, oldPosition.east, primitiveMap["north"], primitiveMap["east"], primitiveMap["speed"], primitiveMap["victory_radius"]);
00307                         ROS_ERROR("go2pointFA: T1: %f, %f, T2: %f, %f, Speed: %f, Heading: %f, VictoryRadius: %f  ", oldPosition.north, oldPosition.east, primitiveMap["north"], primitiveMap["east"], primitiveMap["speed"], primitiveMap["heading"], primitiveMap["victory_radius"]);
00308 
00309                         oldPosition.north = primitiveMap["north"];
00310                         oldPosition.east = primitiveMap["east"];
00311                         oldPosition.depth = primitiveMap["depth"];
00312             }
00313 
00314             void MissionExecution::go2point_UA_state(){
00315 
00316                         evaluatePrimitive(receivedPrimitive.primitiveString.data);
00318                         if(!timeoutActive && primitiveMap["timeout"] > 0)
00319                                 setTimeout(primitiveMap["timeout"]);
00320                         CM.go2point_UA(true, oldPosition.north, oldPosition.east, primitiveMap["north"], primitiveMap["east"], primitiveMap["speed"], primitiveMap["victory_radius"]);
00321                         oldPosition.north = primitiveMap["north"];
00322                         oldPosition.east = primitiveMap["east"];
00323             }
00324 
00325             void MissionExecution::course_keeping_FA_state(){
00326 
00327                         evaluatePrimitive(receivedPrimitive.primitiveString.data);
00329                         if(!timeoutActive && primitiveMap["timeout"] > 0)
00330                                 setTimeout(primitiveMap["timeout"]);
00331                         CM.course_keeping_FA(true, primitiveMap["course"], primitiveMap["speed"], primitiveMap["heading"]);
00332             }
00333 
00334             void MissionExecution::course_keeping_UA_state(){
00335 
00336                         evaluatePrimitive(receivedPrimitive.primitiveString.data);
00338                         if(!timeoutActive && primitiveMap["timeout"] > 0)
00339                                 setTimeout(primitiveMap["timeout"]);
00340                         CM.course_keeping_UA(true, primitiveMap["course"], primitiveMap["speed"]);
00341             }
00342 
00343 /*          void MissionExecution::iso_state(){
00344 
00345                 * Evaluate primitive data with current values
00346                         evaluatePrimitive(receivedPrimitive.primitiveString.data);
00347                 * Activate primitive timeout
00348                         if(!timeoutActive && primitiveMap["timeout"] > 0)
00349                                 setTimeout(primitiveMap["timeout"]);
00350                         * Activate primitive
00351                         CM.ISOprimitive(true, primitiveMap["dof"], primitiveMap["command"], primitiveMap["hysteresis"], primitiveMap["reference"], primitiveMap["sampling_rate"]);
00352             }
00353 
00354             void MissionExecution::path_following_state(){
00355 //
00356                         evaluatePrimitive(receivedPrimitive.primitiveString.data);
00357                 * Activate primitive timeout
00358                         if(!timeoutActive && primitiveMap["timeout"] > 0)
00359                                 setTimeout(primitiveMap["timeout"]);
00360 //                      CM.go2point_FA(true, oldPosition.north, oldPosition.east, primitiveMap["north"], primitiveMap["east"], primitiveMap["speed"], primitiveMap["heading"], primitiveMap["victory_radius"]);
00361 //                      oldPosition.north = primitiveMap["north"];
00362 //                      oldPosition.east = primitiveMap["east"];
00363             }
00364 
00365             void MissionExecution::pointer_state(){
00366 
00367                         evaluatePrimitive(receivedPrimitive.primitiveString.data);
00368                 * Activate primitive timeout
00369                         if(!timeoutActive && primitiveMap["timeout"] > 0)
00370                                 setTimeout(primitiveMap["timeout"]);
00371 //                      CM.go2point_FA(true, oldPosition.north, oldPosition.east, primitiveMap["north"], primitiveMap["east"], primitiveMap["speed"], primitiveMap["heading"], primitiveMap["victory_radius"]);
00372 //                      oldPosition.north = primitiveMap["north"];
00373 //                      oldPosition.east = primitiveMap["east"];
00374             }*/
00375 
00376                 /*****************************************************************
00377                  ***  ROS Subscriptions Callback
00378                  ****************************************************************/
00379 
00381                 void MissionExecution::onDataEventsContainer(const misc_msgs::DataEventsContainer::ConstPtr& data){
00382 
00384                         if(checkEventFlag){
00385 
00387                                 int flag = 0, i = 0;
00388 
00389                                 for(std::vector<uint8_t>::iterator it = receivedPrimitive.event.onEventNext.begin() ;
00390                                                                                                                 it != receivedPrimitive.event.onEventNext.end(); ++it){
00391 
00393                                         if(data->eventsVar[receivedPrimitive.event.onEventNextActive[i++]-1] == 1){
00394 
00395                                                 flag = 1;
00396                                                 nextPrimitive = *it;
00397                                                 ROS_ERROR("Event active:: %d", receivedPrimitive.event.onEventNextActive[i-1]);
00398 
00399                                                 onPrimitiveEndReset();
00400                                                 mainEventQueue->riseEvent("/PRIMITIVE_FINISHED");
00401                                         }
00402 
00404                                         if (flag) break;
00405                                 }
00406                         }
00407                 }
00408 
00410                 void MissionExecution::onReceivePrimitive(const misc_msgs::SendPrimitive::ConstPtr& data){
00411 
00412                         receivedPrimitive = *data;
00413 
00415                         if(receivedPrimitive.event.onEventNextActive.empty() == 0){
00416                                 checkEventFlag = true;
00417                         }
00418 
00420                         if(data->primitiveID != none){
00421 
00422                                 string id_string(PRIMITIVES[data->primitiveID]);
00423                                 id_string = "/" + boost::to_upper_copy(id_string);
00424                                 mainEventQueue->riseEvent(id_string.c_str());
00425                         }else{
00426                                 ROS_ERROR("Mission ended.");
00427                                 std_msgs::String msg;
00428                                 msg.data = "/STOP";
00429                                 pubEventString.publish(msg);
00430                                 //mainEventQueue->riseEvent("/STOP");
00431                         }
00432                 }
00433 
00434                 /*
00435                  * Collect state measurements
00436                  */
00437                 void MissionExecution::onStateHat(const auv_msgs::NavSts::ConstPtr& data)
00438                 {
00439                         state = *data;
00440                 }
00441 
00442                 /*********************************************************************
00443                  *** Helper functions
00444                  ********************************************************************/
00445 
00447                 void MissionExecution::onEventString(const std_msgs::String::ConstPtr& msg){
00448 
00449                         if(strcmp(msg->data.c_str(),"/START_DISPATCHER") == 0 && missionActive)
00450                         {
00451                                 mainEventQueue->riseEvent("/STOP");
00452                                 onPrimitiveEndReset();
00453                                 nextPrimitive = 1;
00454                         }
00455 
00456                         mainEventQueue->riseEvent(msg->data.c_str());
00457                         ROS_ERROR("EventString: %s",msg->data.c_str());
00458                         if(strcmp(msg->data.c_str(),"/STOP") == 0){
00459                                 onPrimitiveEndReset();
00460                                 nextPrimitive = 1;
00461                         }
00462                 }
00463 
00465                 void MissionExecution::requestPrimitive(){
00466 
00467                         std_msgs::UInt16 req;
00468                         req.data = nextPrimitive++;
00469                         pubRequestPrimitive.publish(req);
00470                 }
00471 
00473                 void MissionExecution::setTimeout(double timeout){
00474 
00475                         if(timeout != 0){
00476                                 ROS_ERROR("Setting timeout: %f", timeout);
00477                                 timer = nh_.createTimer(ros::Duration(timeout), &MissionExecution::onTimeout, this, true);
00478                                 timeoutActive = true;
00479                         }
00480                 }
00481 
00483                 void MissionExecution::onTimeout(const ros::TimerEvent& timer){
00484 
00485                         ROS_ERROR("Timeout");
00486                         onPrimitiveEndReset();
00487                         mainEventQueue->riseEvent("/TIMEOUT");
00488                 }
00489 
00491                 void MissionExecution::onPrimitiveEndReset(){
00492 
00494                         timer.stop();
00496                         timeoutActive = checkEventFlag = false;
00497                         missionActive = false;
00498                 }
00499         }
00500 }
00501 
00502 #endif /* MISSIONEXECUTION_HPP_ */