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 #include <ros/ros.h>
00038 #include <sstream>
00039 #include <boost/thread.hpp>
00040 #include <vector>
00041 #include <ostream>
00042 #include "config.h"
00043 #include "Manager.h"
00044 #include "TopicManager.h"
00045 #include "ServiceManager.h"
00046 #include "TFManager.h"
00047 #include "WMPNodeManager.h"
00048 #include "argon.h"
00049
00050 extern "C" {
00051 #include "core/include/frames.h"
00052
00053 }
00054 #define FRAME_INFO
00055
00056 static ros::Publisher publisher;
00057 void received(int *rtnCode, wmpFrame *p, wmpFrame*q) {
00058 static ros_rt_wmp_msgs::WMPFrames v;
00059 v.rssi = q->hdr.rssi;
00060 v.noise = q->hdr.noise;
00061 v.serial = q->hdr.serial;
00062 v.loop_id = q->hdr.loop_id;
00063 v.header.stamp = ros::Time::now();
00064 v.source = q->hdr.from;
00065 publisher.publish(v);
00066 }
00067
00068 int main(int argc, char** argv) {
00069 char ns[32], name[32];
00070 int nnodes, node_id, ans;
00071 argo_setCommentId(argo_addString(name, STR("node-name"), STR("")),
00072 STR("Specify the name of the node"));
00073 argo_setCommentId(argo_addString(ns, STR("namespace"), STR("")),
00074 STR("Specify the namespace"));
00075 argo_setCommentId(argo_addInt(&ans, STR("auto-namespace"), 0, 0),
00076 STR("Obtain the namespace from the RT-WMP node_id"));
00077
00078 if (!wmpIsKernelSpace()){
00079 argo_setCommentId(argo_addIntMandatory(&node_id, STR("node-id"), 0, 1),
00080 STR("Specify node WMP address"));
00081 argo_setCommentId(argo_addIntMandatory(&nnodes, STR("num-nodes"), 2, 1),
00082 STR("Specify number of nodes"));
00083 argo_setExample(argv[0],STR("--node-id 0 --num-nodes 3 --auto-namespace"));
00084 } else {
00085 argo_setExample(argv[0],STR("--auto-namespace"));
00086 }
00087 argo_doProcess(argc, argv, 0);
00088
00089 wmpSetup(node_id, nnodes);
00090
00091 if (strcmp(name,"")==0){
00092 snprintf(name,31,"R%d",wmpGetNodeId());
00093 }
00094
00095 ros::init(argc, argv, name);
00096
00097 if (ans){
00098 snprintf(ns,31,"R%d",wmpGetNodeId());
00099 }
00100
00101 ros::NodeHandle n(ns);
00102
00103 WMPNodeManager w(&n, 2, std::string("wmp_control"));
00104
00105 define_objects(&n,&w);
00106
00107 wmpRunBG();
00108 if (wmpIsKernelSpace()){
00109 printf("Node %s with id %d of %d is running.\n",name, wmpGetNodeId(), wmpGetNumOfNodes());
00110 }
00111 ros::spin();
00112 return 0;
00113
00114 }
00115