Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
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