WMPNodeManager.h
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------
00002  *---------------------           ros_rt_wmp              --------------------
00003  *------------------------------------------------------------------------
00004  *                                                         V0.1B  15/09/11
00005  *
00006  *
00007  *  File: ./src/WMPNodeManager.h
00008  *  Authors: Danilo Tardioli
00009  *  ----------------------------------------------------------------------
00010  *  Copyright (C) 2000-2011, Universidad de Zaragoza, SPAIN
00011  *
00012  *  Contact Addresses: Danilo Tardioli                   dantard@unizar.es
00013  *
00014  *  RT-WMP is free software; you can  redistribute it and/or  modify it
00015  *  under the terms of the GNU General Public License  as published by the
00016  *  Free Software Foundation;  either  version 2, or (at  your option) any
00017  *  later version.
00018  *
00019  *  RT-WMP  is distributed  in the  hope  that  it will be   useful, but
00020  *  WITHOUT  ANY  WARRANTY;     without  even the   implied   warranty  of
00021  *  MERCHANTABILITY  or  FITNESS FOR A  PARTICULAR PURPOSE.    See the GNU
00022  *  General Public License for more details.
00023  *
00024  *  You should have received  a  copy of  the  GNU General Public  License
00025  *  distributed with RT-WMP;  see file COPYING.   If not,  write to the
00026  *  Free Software  Foundation,  59 Temple Place  -  Suite 330,  Boston, MA
00027  *  02111-1307, USA.
00028  *
00029  *  As a  special exception, if you  link this  unit  with other  files to
00030  *  produce an   executable,   this unit  does  not  by  itself cause  the
00031  *  resulting executable to be covered by the  GNU General Public License.
00032  *  This exception does  not however invalidate  any other reasons why the
00033  *  executable file might be covered by the GNU Public License.
00034  *
00035  *----------------------------------------------------------------------*/
00036 
00037 #ifndef WMPNODEMANAGER_H_
00038 #define WMPNODEMANAGER_H_
00039 extern "C"{
00040         char lqm_get_val(int i, int j);
00041 }
00042 
00043 
00044 class WMPNodeManager: public WMPServiceManager<ros_rt_wmp_msgs::WMPControl> {
00045         ros::NodeHandle *nh;
00046         std::map<std::string, Manager *> stuffs;
00047         ros::ServiceServer service;
00048 public:
00049 
00050         WMPNodeManager(ros::NodeHandle * n, int port, std::string name) :
00051 
00052                         WMPServiceManager<ros_rt_wmp_msgs::WMPControl>(n, port, name) {
00053                 nh = this->n;
00054                 service = n->advertiseService("wmp_control", &WMPNodeManager::manage,
00055                                 this);
00056                 boost::thread(boost::bind(&WMPNodeManager::publishInfo, this));
00057                 createThreads();
00058         }
00059 
00060         virtual ~WMPNodeManager() {
00061 
00062         }
00063 
00064         void publishInfo() {
00065 
00066                 std::ostringstream oss;
00067                 oss << nh->getNamespace() << "/info";
00068                 ros::Publisher publisher = nh->advertise<ros_rt_wmp_msgs::WMPInfo>(oss.str(), 1);
00069 
00070                 while (ros::ok()) {
00071                         ros_rt_wmp_msgs::WMPInfo pm;
00072                         pm.serial = wmpGetSerial();
00073                         pm.loop_id = wmpGetLoopId();
00074                         pm.connected = wmpIsNetworkConnected();
00075 
00076                         for (int i = 0; i<wmpGetNumOfNodes(); i++){
00077                                 for (int j = 0; j<wmpGetNumOfNodes(); j++){
00078                                         pm.lqm.push_back(lqm_get_val(i,j));
00079                                 }
00080                         }
00081 
00082                         int size = wmpGetNumOfNodes()*wmpGetNumOfNodes();
00083                         char distances[size];
00084                         wmpGetLatestDistances(distances);
00085                         for (int i = 0; i < size; i++) {
00086                                 pm.dist.push_back(distances[i]);
00087                         }
00088 
00089                         publisher.publish(pm);
00090                         usleep(250000);
00091                 }
00092         }
00093 
00094         void addManager(std::string p, Manager * v) {
00095                 stuffs[p] = v;
00096                 v->startRX();
00097         }
00098 
00099         bool manage(ros_rt_wmp_msgs::WMPControl::Request &req,
00100                         ros_rt_wmp_msgs::WMPControl::Response &res) {
00101                 if (req.dest != wmpGetNodeId()) {
00102                         ROS_ERROR("This service is local only, returning local value");
00103                 }
00104                 ROSWMP_DEBUG(stderr, "Local service called with command %d, %s\n",req.command, req.param3.c_str());
00105                 //fprintf (stderr, "Local service called with command %d, %s node %d \n",req.command, req.param3.c_str(), wmpGetNodeId());
00106 
00107                 if (stuffs.find(req.param3) == stuffs.end()) {
00108                         res.result = -1;
00109                         res.info = "ERROR: Inexistent topic/service";
00110                         return true;
00111                 }
00112                 if (req.dest != wmpGetNodeId()) {
00113                         res.result = -1;
00114                         res.info = "ERROR: Wrong destination parameter";
00115                         return true;
00116                 }
00117                 Manager * m = stuffs[req.param3];
00118                 if (!m->isHost()){
00119                         res.result = -1;
00120                         res.info = "ERROR: The specified node is not source/host of this topic/service";
00121                         return true;
00122                 }
00123 
00124                 if (req.command == TOPIC_STOP) {
00125                         m->stop();
00126                 } else if (req.command == TOPIC_START) {
00127                         m->start();
00128                 }else if (req.command == TOPIC_DECIMATE) {
00129                         m->setDecimation(req.param1);
00130                 }else if (req.command == TOPIC_JUSTONE) {
00131                         m->justOne();
00132                 }else if (req.command == SET_PRIORITY) {
00133                         m->setPriority(req.param1);
00134                 }else if (req.command == GET_PRIORITY) {
00135                         res.result = m->getPriority();
00136                 }
00137                 res.info = "OK";
00138                 return true;
00139         }
00140 };
00141 
00142 
00143 #endif /* WMPNODEMANAGER_H_ */


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Fri Jan 3 2014 12:07:56