[TeamTalk 210]: [746] trunk/moast-bth/moast: Import from moast.
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Mon Sep 17 11:46:15 EDT 2007
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20070917/9f7e922c/attachment-0001.html
-------------- next part --------------
Added: trunk/moast-bth/moast/doc/papers/INDEX.bib
===================================================================
--- trunk/moast-bth/moast/doc/papers/INDEX.bib (rev 0)
+++ trunk/moast-bth/moast/doc/papers/INDEX.bib 2007-09-17 15:46:15 UTC (rev 746)
@@ -0,0 +1,18 @@
+% INDEX.bib - References to papers written about MOAST.
+% The reference title refers to the file name
+% that the reference pertains to.
+%
+
+ at inproceedings{detc2007-34495,
+ title = {"A Simulation Interface for Integrating Real-Time Vehicle Control with Game Engines"},
+ author = {C. Scrapper and F. Proctor and S. Balakirksy},
+ year = {2007},
+ booktitle = {Proceedings of the ASME 2007 International Design Engineering Technical Conferences & Computers and Information in Engineering Conference (IDETC/CIE 2007)}
+}
+
+ at inproceedings{scrapper_ssrr07,
+ title = {"Stable Navigation Solutions for Robots in Complex Environments"},
+ author = {C. Scrapper and R. Madhavan and S. Balakirksy},
+ year = {2007},
+ booktitle = {Submitted to IEEE International Workshop on Safety, Security, and Rescue Robotics (SSRR2007) }
+}
Added: trunk/moast-bth/moast/doc/papers/detc2007-34495.pdf
===================================================================
(Binary files differ)
Property changes on: trunk/moast-bth/moast/doc/papers/detc2007-34495.pdf
___________________________________________________________________
Name: svn:executable
+ *
Name: svn:mime-type
+ application/octet-stream
Added: trunk/moast-bth/moast/doc/papers/imece2007-43362.pdf
===================================================================
(Binary files differ)
Property changes on: trunk/moast-bth/moast/doc/papers/imece2007-43362.pdf
___________________________________________________________________
Name: svn:mime-type
+ application/octet-stream
Added: trunk/moast-bth/moast/doc/papers/scrapper_ssrr07.pdf
===================================================================
(Binary files differ)
Property changes on: trunk/moast-bth/moast/doc/papers/scrapper_ssrr07.pdf
___________________________________________________________________
Name: svn:executable
+ *
Name: svn:mime-type
+ application/octet-stream
Modified: trunk/moast-bth/moast/doc/refManual_tex/installation/download.tex
===================================================================
--- trunk/moast-bth/moast/doc/refManual_tex/installation/download.tex 2007-09-14 16:32:37 UTC (rev 745)
+++ trunk/moast-bth/moast/doc/refManual_tex/installation/download.tex 2007-09-17 15:46:15 UTC (rev 746)
@@ -3,6 +3,7 @@
The MOAST code may be retrieved from CVS or from an official release tar archive. To retrieve the code from CVS, enter the following commands:\\
\texttt{\$ cvs -d:pserver:anonymous$@$moast.cvs.sourceforge.net:/cvsroot/moast login}\\
+Note that if you are prompted for a password, just type $<$CR$>$\\
\texttt{\$ cvs -z3 -d:pserver:anonymous$@$moast.cvs.sourceforge.net:/cvsroot/moast co -P devel}
Information about accessing this CVS repository may be found in the document titled, ``CVS (Version Control for Source Code)'' \cite{CVS}. Alternatively, the latest snapshot may be retrieved from the release section of the MOAST site.
Modified: trunk/moast-bth/moast/doc/refManual_tex/refManual.tex
===================================================================
--- trunk/moast-bth/moast/doc/refManual_tex/refManual.tex 2007-09-14 16:32:37 UTC (rev 745)
+++ trunk/moast-bth/moast/doc/refManual_tex/refManual.tex 2007-09-17 15:46:15 UTC (rev 746)
@@ -116,7 +116,7 @@
\section{SIMWARE}
\label{S:simware}
-%\input{simware}
+\input{simware}
\section{PRIMITIVE ECHELON}
\label{S:prim}
Added: trunk/moast-bth/moast/doc/refManual_tex/simware/trafficSim.tex
===================================================================
--- trunk/moast-bth/moast/doc/refManual_tex/simware/trafficSim.tex (rev 0)
+++ trunk/moast-bth/moast/doc/refManual_tex/simware/trafficSim.tex 2007-09-17 15:46:15 UTC (rev 746)
@@ -0,0 +1,25 @@
+\subsection{TrafficSim}
+TrafficSim is designed to interface to USARSim through an inferior skin and to MOAST through
+a superior skin. The main difference between it and simWare is that trafficSim opens a single
+command and data buffer for multiple vehicles whereas simWare opens a separate buffer per vehicle.
+In addition, no sensor buffers are established with trafficSim.
+
+The simplist way to run traffic sim is with the runTraffic script located in the bin directory.
+In this file there is a variable named {\it NUM\_ROBOTS} that will define the number of robots that
+trafficSim will spawn. TrafficSIm requires a {\it UTM\_START\_POSE} to be defined for each robot that will
+be spawned. These declarations should be located in the .ini file under the section identified by the
+world that is running in USARSim. In addition, a {\it PLATFORM\_TYPE} statement under the same section
+is also required. This statement specifies the type of robot that will be spawned.
+
+TrafficSim may be controlled by the use of the \texttt{trafficShell} program. Typeing the {\it init} command will initialize the
+traffic simulation and allow it to receive commands. Command and status structures may be found in the
+file \texttt{trafficCtrl.hh} file with the data output may be found in the \texttt{trafficData.hh} file.
+The controller in trafficSim allows for three levels of control:
+\begin{enumerate}
+\item{Servo -} This echelon of control accepts wheel velocites and commands such as {\it TRAFFIC\_CTRL\_CMD\_SKID\_TYPE}.
+\item{Prim -} THis echelon of control accepts the command {\it TRAFFIC\_CTRL\_CMD\_MOVE\_ARC\_SEGMENT\_TYPE} for the input of constant
+curvature arcs.
+\item{AM -} THis echelon of control accepts the command {\it TRAFFIC\_CTRL\_CMD\_MOVE\_WAYPOINE\_TYPE} for the input of waypoints.
+\end{enumerate}
+
+Data on the vehicle's location and status may be read in the texttt{trafficData} buffer.
\ No newline at end of file
Added: trunk/moast-bth/moast/doc/refManual_tex/simware.tex
===================================================================
--- trunk/moast-bth/moast/doc/refManual_tex/simware.tex (rev 0)
+++ trunk/moast-bth/moast/doc/refManual_tex/simware.tex 2007-09-17 15:46:15 UTC (rev 746)
@@ -0,0 +1,2 @@
+Lots more needs to be written on simware. Until that time, I will add a section on trafficSim.
+\input{simware/trafficSim}
Added: trunk/moast-bth/moast/src/tools/linkMonitor/linkMon.cc
===================================================================
--- trunk/moast-bth/moast/src/tools/linkMonitor/linkMon.cc (rev 0)
+++ trunk/moast-bth/moast/src/tools/linkMonitor/linkMon.cc 2007-09-17 15:46:15 UTC (rev 746)
@@ -0,0 +1,755 @@
+/*
+ DISCLAIMER:
+ This software was produced by the National Institute of Standards
+ and Technology (NIST), an agency of the U.S. government, and by statute is
+ not subject to copyright in the United States. Recipients of this software
+ assume all responsibility associated with its operation, modification,
+ maintenance, and subsequent redistribution.
+
+ See NIST Administration Manual 4.09.07 b and Appendix I.
+*/
+
+/*!
+ \file linkMon.cc
+
+ \brief Provide a measure of the link strength between a base station
+ and the robots
+
+ In reality, to have connectivity between a robot and the base station,
+ each robot could communicate directly or determine a route through other
+ robots to get to the base station. The computation of this route may be
+ very complex. Sice we are in simulation, we can cheat and compute these
+ routes very simply. This program computes these routes and provides them
+ over a status channel.
+
+ This program only works with USARSim!
+
+ \code CVS Status:
+ $Author: dr_steveb $
+ $Revision: 1.2 $
+ $Date: 2007/09/11 20:52:00 $
+ \endcode
+
+ \author Stephen Balakirsky
+*/
+#include <unistd.h> // getopt
+#include <stdlib.h> // getenv
+#include <signal.h> // signal
+#include <errno.h> // errno
+//#include "linkMon.hh"
+
+// socket stuff
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h>
+
+#include "rcs.hh"
+#include "moastNmlOffsets.hh" // NAV_DATA_BASE, etc.
+#include "linkMonCtrl.hh"
+#include "trafficCtrl.hh"
+#include "dijkHeapN.hh"
+
+static RCS_CMD_CHANNEL *linkMonCmdNml = NULL;
+static RCS_STAT_CHANNEL *linkMonStatNml = NULL;
+static RCS_STAT_CHANNEL *trafficCtrlSetNml = NULL;
+
+static LinkMonStat linkMonStat;
+static int done = 0;
+static int server_fd = -1; // connection to unreal
+static float radioCutOff = 60;
+
+#define DEFAULT_NML_FILE "../etc/moast.nml"
+#define DEFAULT_INI_FILE "../etc/traffic.ini"
+
+#define THIS_PROCESS "linkMon"
+
+static char *USARSimHostName;
+static int USARSimPort = 5874;
+
+// global variables and defines for searcher
+#define DIJK_MAP_SIZE MAX_TRAFFIC_VEHICLES * MAX_TRAFFIC_VEHICLES
+#define DIJK_PATH_SIZE MAX_TRAFFIC_VEHICLES + 1
+#define MAX_GOALS MAX_TRAFFIC_VEHICLES
+#define MAX_CYCLES 5000
+
+//int searchMap[DIJK_MAP_SIZE];
+DijkHeapNGraph searcherGraph;
+int searcherGoals[MAX_TRAFFIC_VEHICLES];
+DijkHeapNSearcher theSearcher;
+
+
+
+/////////////////////////////////////////////////////////////////////////////////////////
+//// socket code from fred
+/////////////////////////////////////////////////////////////////////////////////////////
+/* sockclient returns the file descriptor of the socket to the server */
+static int sockclient(int port, char *hostname)
+{
+ int sockfd; /* this process' socket fd */
+ struct sockaddr_in server_addr, local_addr;
+ struct hostent *ent;
+ struct in_addr *in_a;
+
+ //*! if (-1 == (sockfd = socket(AF_INET, SOCK_STREAM, 0)))
+ if (-1 == (sockfd = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)))
+ {
+ fprintf(stderr, "can't create socket file descriptor\n");
+ return -1;
+ }
+
+ if (NULL == (ent = gethostbyname(hostname)))
+ {
+ perror("gethostbyname");
+ close(sockfd);
+ sockfd = -1;
+ return -1;
+ }
+ in_a = (struct in_addr *) ent->h_addr_list[0];
+
+ /* fill in server's address */
+ memset(&server_addr, 0, sizeof(struct sockaddr_in));
+ //*! server_addr.sin_family = AF_INET;
+ //server_addr.sin_family = PF_INET;
+ server_addr.sin_family = ent->h_addrtype;
+ server_addr.sin_addr.s_addr = in_a->s_addr;
+ server_addr.sin_port = htons(port);
+
+ /* bind any port number */
+ // localAddr.sin_family = AF_INET;
+ local_addr.sin_family = PF_INET;
+ local_addr.sin_addr.s_addr = htonl(INADDR_ANY);
+ local_addr.sin_port = htons(0);
+
+ int rc;
+ rc = bind(sockfd, (struct sockaddr *) &local_addr, sizeof(local_addr));
+ if(rc<0) {
+ //printf("%s: cannot bind port TCP %u\n",argv[0],SERVER_PORT);
+ perror("error ");
+ return -1;
+ }
+
+
+ /* connect to the server */
+ if (-1 == connect(sockfd, (struct sockaddr *) &server_addr,
+ sizeof(struct sockaddr_in)))
+ {
+ perror("can't connect to server");
+ close(sockfd);
+ sockfd = -1;
+ return -1;
+ }
+
+ return sockfd;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+//// graph search routines from Tom Kramer and me
+/////////////////////////////////////////////////////////////////////////////////////////
+bool computeCosts(int node)
+{
+ DijkHeapNNode * nodes;
+ int i, nn;
+ char msg[128];
+ TrafficCtrlSet * set = (TrafficCtrlSet *) trafficCtrlSetNml->get_address();
+ char *msgPt;
+ float cost;
+
+ nodes = searcherGraph.nodes;
+ for( i=0; i<nodes[node].kidsSize; i++ )
+ {
+ if( nodes[node].kidsCosts[i] == -2 ) // need to get information
+ {
+ sprintf( msg, "getss %s %s;", set->robotInfo[node].robotName,
+ set->robotInfo[nodes[node].kidsIndexes[i]].robotName );
+ // printf("%s: sending %d length message: %s\n", THIS_PROCESS, strlen(msg), msg);
+ nn = send( server_fd, msg, strlen(msg)+1, 0);
+ // send( server_fd, "\r\n", 2, 0);
+ if (nn != (int)strlen(msg)+1)
+ {
+ fprintf(stderr, "Write Failed\n");
+ return false;
+ }
+ //Wait for response
+ nn = recv(server_fd, msg, sizeof(msg)-1, 0);
+ if( nn == -1 )
+ {
+ return false;
+ }
+ msg[nn] = '\0';
+ msgPt = &msg[4];
+ cost = atof(msgPt);
+ if( cost > radioCutOff )
+ cost = -1;
+ nodes[node].kidsCosts[i] = cost;
+ nodes[i].kidsCosts[node] = cost;
+ // printf("%s: message received:%s cost:%f\n", THIS_PROCESS, msg,
+ // nodes[node].kidsCosts[i]);
+
+ }
+ }
+ return true;
+}
+
+void initSearcherGraph()
+{
+ TrafficCtrlSet * set = (TrafficCtrlSet *) trafficCtrlSetNml->get_address();
+ int n;
+ int numGoals = set->robotInfo_length;
+
+ searcherGraph.nodes = new DijkHeapNNode[numGoals + 1];
+ searcherGraph.capacity = (numGoals + 1);
+ searcherGraph.size = (numGoals + 1);
+ for (n = 0; n < numGoals; n++)
+ {
+ searcherGraph.nodes[n].moreInfo = new NodeInfo(n);
+ searcherGraph.nodes[n].kidsSize = 0;
+ // searchMap[n] = searcherGoals[n];
+ // driveMap[driverGoals[n]] = n;
+ }
+ // searchMap[numGoals] = start;
+ // driveMap[start] = numGoals;
+}
+
+
+void setGraph()
+{
+ DijkHeapNNode * nodes;
+ TrafficCtrlSet * set = (TrafficCtrlSet *) trafficCtrlSetNml->get_address();
+ int i,j;
+
+ searcherGraph.size = set->robotInfo_length;
+ searcherGraph.capacity = searcherGraph.size;
+ nodes = new DijkHeapNNode[searcherGraph.size];
+ searcherGraph.nodes = nodes;
+
+ for( i=0; i<searcherGraph.size; i++ )
+ {
+ nodes[i].moreInfo = new NodeInfo(i);
+ nodes[i].kidsSize = set->robotInfo_length;
+ nodes[i].kidsIndexes = new int[nodes[i].kidsSize];
+ nodes[i].kidsCosts = new double[nodes[i].kidsSize];
+ for( j=0; j<searcherGraph.size; j++ )
+ {
+ nodes[i].kidsIndexes[j] = j;
+ if( i == j )
+ nodes[i].kidsCosts[j] = -1; // negative costs invalidates arc
+ else
+ nodes[i].kidsCosts[j] = -2; // my flag that we need a cost
+ }
+ }
+ for( i=1; i<searcherGraph.size; i++ ) // set the goals
+ searcherGoals[i-1] = i;
+}
+
+static void printAnswer( /* ARGUMENTS */
+ int length, /*!< length of the path */
+ int * path, /*!< array of index numbers giving the path */
+ double cost, /*!< cost of the path */
+ LinkMonStat * stat) /*!< status structure */
+{
+ TrafficCtrlSet * set = (TrafficCtrlSet *) trafficCtrlSetNml->get_address();
+ int n;
+
+ for (n = 0; n < length; n++)
+ {
+ // printf(" %s", set->robotInfo[path[n]].robotName);
+ strcpy(stat->route[stat->route_length].name[n], set->robotInfo[path[n]].robotName);
+ }
+ stat->route[stat->route_length].length = length;
+ stat->route[stat->route_length].cost = cost;
+ stat->route_length++;
+ /*
+ printf("\n");
+ printf("With cost: %.4f\n\n", cost);
+ */
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+//// state tables
+/////////////////////////////////////////////////////////////////////////////////////////
+void doCmdNop(LinkMonStat * stat)
+{
+ // ServoMobJACmdNop nopMsg;
+
+ if(state_match(stat, NEW_COMMAND))
+ {
+ state_new(stat);
+ status_next(stat, RCS_DONE);
+ state_next(stat, S0);
+ }
+ else // SO
+ state_default(stat);
+}
+
+void doCmdInit(LinkMonStat * stat)
+{
+ char msg[128];
+ int nn;
+ int i;
+ TrafficCtrlSet * set = (TrafficCtrlSet *) trafficCtrlSetNml->get_address();
+
+ if(state_match(stat, NEW_COMMAND))
+ {
+ printf( "Received init command\n" );
+ state_new(stat);
+ state_next(stat, S0);
+ }
+ if(state_match(stat, S0))
+ {
+ //Set up a socket to connect to the server
+ if (-1 == (server_fd = sockclient(USARSimPort, USARSimHostName)))
+ {
+ fprintf(stderr, "can't connect to server\n");
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ status_next(stat, RCS_EXEC);
+ state_next(stat, S1);
+ }
+ if(state_match(stat, S1))
+ {
+ //Register with the Server
+ for( i=0; i<set->robotInfo_length; i++ )
+ {
+ stat->admin_state = ADMIN_INITIALIZED;
+ sprintf( msg, "register %s %s;", set->robotInfo[i].robotName, "129.6.33.142" );
+ printf("%s: sending %d length message: %s\n", THIS_PROCESS, strlen(msg), msg);
+ nn = send( server_fd, msg, strlen(msg)+1, 0);
+ // send( server_fd, "\r\n", 2, 0);
+ if (nn != (int)strlen(msg)+1)
+ {
+ fprintf(stderr, "Write Failed\n");
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ //Wait for OK response
+ nn = recv(server_fd, msg, sizeof(msg)-1, 0);
+ if( nn == -1 )
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ msg[nn] = '\0';
+ printf("%s: message received:%s\n", THIS_PROCESS, msg);
+ if(msg[0]!='O' || msg[1]!='K')
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ else
+ {
+ state_next(stat, S10);
+
+ }
+ }
+ }
+ // else wait here
+ else
+ state_default(stat);
+}
+
+void doCmdShutdown(LinkMonStat * stat)
+{
+ if(state_match(stat, NEW_COMMAND))
+ {
+ done = 1;
+ stat->admin_state = ADMIN_UNINITIALIZED;
+ status_next(stat, RCS_DONE);
+ state_next(stat, S1);
+ }
+ // else wait here
+ else // S1
+ state_default(stat);
+}
+
+void doCmdHalt(LinkMonStat * stat)
+{
+ if(state_match(stat, NEW_COMMAND))
+ {
+ done = 1;
+ stat->admin_state = ADMIN_UNINITIALIZED;
+ status_next(stat, RCS_DONE);
+ state_next(stat, S1);
+ }
+ // else wait here
+ else // S1
+ state_default(stat);
+}
+
+void doCmdAbort(LinkMonStat * stat)
+{
+ if(state_match(stat, NEW_COMMAND))
+ {
+ done = 1;
+ stat->admin_state = ADMIN_UNINITIALIZED;
+ status_next(stat, RCS_DONE);
+ state_next(stat, S1);
+ }
+ // else wait here
+ else // S1
+ state_default(stat);
+}
+
+void doCmdMonitor( LinkMonStat * stat )
+{
+ int n;
+ TrafficCtrlSet * set = (TrafficCtrlSet *) trafficCtrlSetNml->get_address();
+ int numGoals = set->robotInfo_length -1;
+ int statuses[MAX_TRAFFIC_VEHICLES];
+ double costs[MAX_TRAFFIC_VEHICLES];
+ int searcherGoals[MAX_TRAFFIC_VEHICLES];
+ int goalsFound;
+ int optimaFound;
+ int cycles = 0;
+ int length;
+ int opens; // number of open nodes
+ int path[MAX_TRAFFIC_VEHICLES];
+ int start = 0;
+
+ if(state_match(stat, NEW_COMMAND))
+ {
+ state_new(stat);
+ if(stat->admin_state != ADMIN_INITIALIZED)
+ {
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S2);
+ }
+ else
+ {
+ state_next(stat, S0);
+ status_next(stat, RCS_EXEC);
+ }
+ }
+
+ if(state_match(stat, S0))
+ {
+ setGraph(); // based on readGraph
+ status_next(stat, RCS_EXEC);
+ state_next(stat, S1);
+ }
+ if(state_match(stat, S1))
+ {
+ for( n=0; n<numGoals; n++ )
+ {
+ searcherGoals[n] = n+1;
+ }
+ if(theSearcher.initSearch(&searcherGraph, start, numGoals,
+ searcherGoals, &opens, statuses, costs,
+ &goalsFound, &optimaFound))
+ { // initialization error occurred
+ status_next(stat, RCS_ERROR);
+ state_next(stat, S10);
+ return;
+ }
+ while (opens &&
+ ((statuses[0] != 2) || (cycles < MAX_CYCLES)) &&
+ (optimaFound < numGoals))
+ {
+ n = theSearcher.getNextNode();
+ computeCosts(n);
+ // insertKids(n, searcherGraph.size);
+ theSearcher.cycleSearch();
+ cycles++;
+ }
+ // printf("%d cycles\n", cycles);
+ stat->route_length = 0;
+ for (n = 0; n < numGoals; n++)
+ {
+ if (statuses[n] > 0)
+ {
+ length = theSearcher.reportSearch(searcherGoals[n],
+ DIJK_PATH_SIZE, path);
+ /*
+ printf("%s", ((statuses[n] == 2) ? "Optimal path " : "Path "));
+ printf("from start to node %d:", searcherGoals[n]);
+ */
+ printAnswer(length, path, costs[n], stat);
+ }
+ }
+ theSearcher.closeSearch();
+ status_next(stat, RCS_DONE);
+ state_next(stat, S0);
+ }
+ else
+ {
+ state_default(stat);
+ }
+ return;
+}
+
+//////////////////////////////////////////////////////////////////
+//// end of state tables
+//////////////////////////////////////////////////////////////////
+
+static int cleanupNmlBuffers(void)
+{
+ if(NULL != linkMonCmdNml)
+ delete linkMonCmdNml;
+ if(NULL != linkMonStatNml)
+ delete linkMonStatNml;
+ if (NULL != trafficCtrlSetNml)
+ delete trafficCtrlSetNml;
+
+ linkMonCmdNml = NULL;
+ linkMonStatNml = NULL;
+ trafficCtrlSetNml = NULL;
+ return 0;
+}
+
+static int initNmlBuffers(char *config_file, int bufnum)
+{
+ char chanName[MOAST_NML_BUFFER_NAME_LEN];
+
+ (void) cleanupNmlBuffers();
+
+ sprintf(chanName, "%s%d", LINK_MON_CMD_NAME, bufnum);
+ linkMonCmdNml = new RCS_CMD_CHANNEL(linkMonCtrl_format, chanName,
+ THIS_PROCESS, config_file);
+ if(!linkMonCmdNml->valid())
+ {
+ delete linkMonCmdNml;
+ linkMonCmdNml = 0;
+ return 1;
+ }
+
+ sprintf(chanName, "%s%d", LINK_MON_STAT_NAME, bufnum);
+ linkMonStatNml = new RCS_STAT_CHANNEL(linkMonCtrl_format, chanName,
+ THIS_PROCESS, config_file);
+ if(!linkMonStatNml->valid())
+ {
+ delete linkMonStatNml;
+ linkMonStatNml = 0;
+ return 1;
+ }
+
+ sprintf(chanName, "%s%d", TRAFFIC_CTRL_SET_NAME, bufnum);
+ trafficCtrlSetNml =
+ new RCS_STAT_CHANNEL(trafficCtrl_format, chanName,
+ THIS_PROCESS, config_file);
+ if (!trafficCtrlSetNml->valid()) {
+ delete trafficCtrlSetNml;
+ trafficCtrlSetNml = 0;
+ return 1;
+ }
+
+ nml_start();
+ return 0;
+}
+
+void quit(int sig)
+{
+ done = 1;
+}
+
+int main(int argc, char *argv[])
+{
+ char nml_file_default[] = DEFAULT_NML_FILE;
+ char *nml_file_env;
+ char *NML_FILE = NULL;
+ int bufnum = 1;
+ int retval;
+ NMLTYPE cmd_type;
+ int serial_number;
+ LinkMonCmdNop nopCmd;
+ RCS_TIMER *timer;
+ float cycleTime = 1;
+ // TrafficCtrlSet * set = (TrafficCtrlSet *) trafficCtrlSetBuf->get_address();
+ INIFILE if_d;
+ char *INI_FILE = NULL;
+ char *ini_file_env;
+ char ini_file_default[] = DEFAULT_INI_FILE;
+ const char *strPtr;
+
+ timer = new RCS_TIMER(cycleTime);
+ while (1)
+ {
+ char option = getopt(argc, argv, "n:d:i:");
+ if (option == -1)
+ break;
+ switch (option)
+ {
+ case 'i':
+ INI_FILE = optarg;
+ break;
+ case 'n':
+ NML_FILE = optarg;
+ break;
+ case 'd':
+ if(1 != sscanf(optarg, "%i", &bufnum))
+ {
+ rcs_print_error("%s: bad value for buf number: %s\n", argv[0],
+ optarg);
+ return 1;
+ }
+ break;
+ default:
+ rcs_print_error("%s: bad argument\n", argv[0]);
+ return 1;
+ break;
+ }
+ }
+
+ // get the config file name from arg, if provided, next environment,
+ // if there, else default
+ if(NULL == NML_FILE)
+ {
+ nml_file_env = getenv("CONFIG_NML");
+ if(NULL != nml_file_env)
+ {
+ NML_FILE = nml_file_env;
+ }
+ else
+ {
+ NML_FILE = nml_file_default;
+ }
+ }
+
+ // read in the INI file params
+ if (NULL == INI_FILE)
+ {
+ ini_file_env = getenv("TRAFFIC_INI");
+ if (NULL != ini_file_env)
+ {
+ INI_FILE = ini_file_env;
+ }
+ else
+ {
+ INI_FILE = ini_file_default;
+ }
+ }
+ if (if_d.open (INI_FILE))
+ {
+ printf("** ERROR ** %s: invalid inifile path %s\n",
+ THIS_PROCESS, INI_FILE );
+ if_d.close ();
+ return false;
+ }
+
+ strPtr = if_d.find ("HOST_NAME", "USARSIM.INFSKIN");
+ if ( strPtr == 0 )
+ {
+ strPtr = getenv("USARSimHost");
+ }
+ if (NULL == strPtr)
+ {
+ printf("** ERROR ** %s: 'HOST_NAME' not defined in section 'USARSIM.INFSKIN' in %s\n",
+ THIS_PROCESS, INI_FILE );
+ if_d.close ();
+ return false;
+ }
+ USARSimHostName = (char*)strPtr;
+ strPtr = if_d.find ("CUTOFF", "LINKMON");
+ if(strPtr != 0 )
+ radioCutOff = -atof(strPtr);
+ if_d.close ();
+
+ // connect to my buffers
+ retval = initNmlBuffers(NML_FILE, bufnum);
+ if(retval)
+ {
+ rcs_print_error("%s: can't allocate linkMon NML buffers\n", argv[0]);
+ (void) cleanupNmlBuffers();
+ return 1;
+ }
+
+ // initialize and set the status up so that init command goes through
+ linkMonStat.echo_serial_number = 0;
+ reset_time_tracker(&linkMonStat.tt);
+
+ // stuff a NOP command
+ nopCmd.serial_number = 1;
+ linkMonCmdNml->write(&nopCmd);
+
+ done = 0;
+ signal(SIGINT, quit);
+ while (!done)
+ {
+ // read buffers
+ cmd_type = linkMonCmdNml->read();
+ trafficCtrlSetNml->read();
+
+ serial_number = linkMonCmdNml->get_address()->serial_number;
+ switch (cmd_type)
+ {
+ case 0:
+ // no new command
+ break;
+ case -1:
+ // comm error
+ rcs_print_error("%s: NML command read error\n", argv[0]);
+ break;
+ case LINK_MON_CMD_NOP_TYPE:
+ case LINK_MON_CMD_INIT_TYPE:
+ case LINK_MON_CMD_HALT_TYPE:
+ case LINK_MON_CMD_ABORT_TYPE:
+ case LINK_MON_CMD_MONITOR_TYPE:
+ case LINK_MON_CMD_SHUTDOWN_TYPE:
+ linkMonStat.command_type = cmd_type;
+ if(serial_number != linkMonStat.echo_serial_number)
+ {
+ linkMonStat.echo_serial_number = serial_number;
+ linkMonStat.state = NEW_COMMAND;
+ linkMonStat.status = RCS_EXEC;
+ }
+ break;
+ default:
+ rcs_print_error("%s: unknown cmd: %d\n", argv[0], (int) cmd_type);
+ break;
+ } // switch (cmd_type)
+
+ // handle command state tables
+ switch (linkMonStat.command_type)
+ {
+ case LINK_MON_CMD_NOP_TYPE:
+ doCmdNop(&linkMonStat);
+ break;
+ case LINK_MON_CMD_INIT_TYPE:
+ doCmdInit(&linkMonStat);
+ break;
+ case LINK_MON_CMD_HALT_TYPE:
+ doCmdHalt(&linkMonStat);
+ break;
+ case LINK_MON_CMD_ABORT_TYPE:
+ doCmdAbort(&linkMonStat);
+ break;
+ case LINK_MON_CMD_SHUTDOWN_TYPE:
+ doCmdShutdown(&linkMonStat);
+ break;
+ case LINK_MON_CMD_MONITOR_TYPE:
+ doCmdMonitor(&linkMonStat);
+ break;
+ default:
+ // no command to handle
+ break;
+ } // switch (cmd)
+ cycle_time_tracker(&linkMonStat.tt);
+
+ // update status and settings
+ linkMonStatNml->write(&linkMonStat);
+ timer->wait();
+ }
+ while (!done);
+
+ (void) cleanupNmlBuffers();
+
+ rcs_print("linkMon done\n");
+
+ return 0;
+}
+
+/*
+ Modification history:
+
+ $Log: linkMon.cc,v $
+ Revision 1.2 2007/09/11 20:52:00 dr_steveb
+ Changed default cuttoff to read a negative value
+
+ Revision 1.1 2007/09/11 20:26:53 dr_steveb
+ Initial check-in
+
+*/
+
Added: trunk/moast-bth/moast/src/tools/linkMonitor/linkMonShell.cc
===================================================================
--- trunk/moast-bth/moast/src/tools/linkMonitor/linkMonShell.cc (rev 0)
+++ trunk/moast-bth/moast/src/tools/linkMonitor/linkMonShell.cc 2007-09-17 15:46:15 UTC (rev 746)
@@ -0,0 +1,453 @@
+/*
+ DISCLAIMER:
+ This software was produced by the National Institute of Standards
+ and Technology (NIST), an agency of the U.S. government, and by statute is
+ not subject to copyright in the United States. Recipients of this software
+ assume all responsibility associated with its operation, modification,
+ maintenance, and subsequent redistribution.
+
+ See NIST Administration Manual 4.09.07 b and Appendix I.
+*/
+
+/*!
+ \file linkMonShell.cc
+
+ \brief Command line shell to traffic simulator
+
+ \code CVS Status:
+ $Author: dr_steveb $
+ $Revision: 1.1 $
+ $Date: 2007/09/11 20:26:53 $
+ \endcode
+
+ \author Stephen Balakirsky
+*/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+#include <stdio.h>
+#include <stddef.h> // sizeof(), NULL
+#include <stdlib.h> // getenv()
+#include <ctype.h>
+#ifdef HAVE_GETOPT_H
+#include <getopt.h>
+#else
+#include "getopt.h"
+#endif
+#ifdef HAVE_READLINE_READLINE_H
+#include <readline/readline.h>
+#include <readline/history.h>
+#endif
+#include <rcs.hh>
+#include "moastTypes.hh" // MOAST_NML_BUFFER_NAME_LEN
+#include "moastNmlOffsets.hh" // NAV_DATA_BASE, etc.
+#include "trafficCtrl.hh"
+#include "trafficData.hh"
+#include "linkMonCtrl.hh"
+
+static RCS_CMD_CHANNEL *linkMonCmdNml = NULL;
+static RCS_STAT_CHANNEL *linkMonStatNml = NULL;
+static RCS_STAT_CHANNEL *trafficCtrlSetNml = NULL;
+static NML *trafficDataNml = NULL;
+
+#define DEFAULT_NML_FILE "../etc/moast.nml"
+#define DEFAULT_INI_FILE "../etc/moast.ini"
+
+#define THIS_PROCESS "linkMonShell"
+
+static int cleanupNmlBuffers(void)
+{
+ if (NULL != linkMonCmdNml)
+ delete linkMonCmdNml;
+ if (NULL != linkMonStatNml)
+ delete linkMonStatNml;
+ if (NULL != trafficDataNml)
+ delete trafficDataNml;
+ if (NULL != trafficCtrlSetNml)
+ delete trafficCtrlSetNml;
+
+ linkMonCmdNml = NULL;
+ linkMonStatNml = NULL;
+ trafficDataNml = NULL;
+ trafficCtrlSetNml = NULL;
+
+ return 0;
+}
+
+static int initNmlBuffers(char *config_file, int bufnum)
+{
+ char chanName[MOAST_NML_BUFFER_NAME_LEN];
+
+ (void) cleanupNmlBuffers();
+
+ sprintf(chanName, "%s%d", LINK_MON_CMD_NAME, bufnum);
+ linkMonCmdNml = new RCS_CMD_CHANNEL(linkMonCtrl_format, chanName,
+ THIS_PROCESS, config_file);
+ if(!linkMonCmdNml->valid())
+ {
+ delete linkMonCmdNml;
+ linkMonCmdNml = 0;
+ return 1;
+ }
+
+ sprintf(chanName, "%s%d", LINK_MON_STAT_NAME, bufnum);
+ linkMonStatNml = new RCS_STAT_CHANNEL(linkMonCtrl_format, chanName,
+ THIS_PROCESS, config_file);
+ if(!linkMonStatNml->valid())
+ {
+ delete linkMonStatNml;
+ linkMonStatNml = 0;
+ return 1;
+ }
+
+ sprintf(chanName, "%s%d", TRAFFIC_DATA_NAME, bufnum);
+ trafficDataNml =
+ new NML(trafficData_format, chanName,
+ THIS_PROCESS, config_file);
+ if (!trafficDataNml->valid()) {
+ delete trafficDataNml;
+ trafficDataNml = 0;
+ return 1;
+ }
+
+ sprintf(chanName, "%s%d", TRAFFIC_CTRL_SET_NAME, bufnum);
+ trafficCtrlSetNml =
+ new RCS_STAT_CHANNEL(trafficCtrl_format, chanName,
+ THIS_PROCESS, config_file);
+ if (!trafficCtrlSetNml->valid()) {
+ delete trafficCtrlSetNml;
+ trafficCtrlSetNml = 0;
+ return 1;
+ }
+
+ return 0;
+}
+
+#define rcs_stat_to_string(s) (s) == RCS_DONE ? "RCS_DONE" : (s) == RCS_EXEC ? "RCS_EXEC" : (s) == RCS_ERROR ? "RCS_ERROR" : "?"
+
+static void printStat(void)
+{
+ int count, count2;
+ LinkMonStat * stat = (LinkMonStat *) linkMonStatNml->get_address();
+ TrafficData *data = (TrafficData *) trafficDataNml->get_address();
+ TrafficCtrlSet * set = (TrafficCtrlSet *) trafficCtrlSetNml->get_address();
+
+ printf("command_type: %s\n",
+ linkMonCtrl_symbol_lookup(stat->command_type));
+ printf("echo_serial_number: %d\n", (int) stat->echo_serial_number);
+ printf("status: %s\n", rcs_stat_to_string(stat->status));
+ // printf("state: %s\n", rcsVehStateToString(stat->state));
+ printf("line: %d\n", (int) stat->line);
+ printf("source_line: %d\n", (int) stat->source_line);
+ printf("source_file: %s\n", (char *) stat->source_file);
+ printf("heartbeat: %d\n", (int) stat->tt.count);
+ printf("actual cycleTime: %lf\n",stat->tt.avg);
+ printf("time: %f\n", data->time);
+ printf("data for %d vehicles\n", data->vehicles_length );
+ printf("xLoc:");
+ for( count=0; count<data->vehicles_length; count++ )
+ {
+ printf( "\t%f", data->vehicles[count].tranTrue.x );
+ }
+ printf("\nyLoc:");
+ for( count=0; count<data->vehicles_length; count++ )
+ {
+ printf( "\t%f", data->vehicles[count].tranTrue.y );
+ }
+ printf("\nzLoc:");
+ for( count=0; count<data->vehicles_length; count++ )
+ {
+ printf( "\t%f", data->vehicles[count].tranTrue.z );
+ }
+ printf("\nbotName:");
+ for( count=0; count<set->robotInfo_length; count++ )
+ {
+ printf( "\t%s", set->robotInfo[count].robotName );
+ }
+ printf( "\n" );
+ for( count=0; count<stat->route_length; count++ )
+ {
+ printf( "route: " );
+ for( count2=0; count2<stat->route[count].length; count2++ )
+ {
+ printf( "-> %s ", stat->route[count].name[count2] );
+ }
+ printf( "with cost: %f\n", stat->route[count].cost );
+ }
+ /*
+ printf("\nroll:");
+ for( count=0; count<data->vehicles_length; count++ )
+ {
+ printf( "\t%f", data->vehicles[count].rpyTrue.r );
+ }
+ printf("\npitch:");
+ for( count=0; count<data->vehicles_length; count++ )
+ {
+ printf( "\t%f", data->vehicles[count].rpyTrue.p );
+ }
+ printf("\nyall:");
+ for( count=0; count<data->vehicles_length; count++ )
+ {
+ printf( "\t%f", data->vehicles[count].rpyTrue.y );
+ }
+ printf("\nspeed:");
+ for( count=0; count<data->vehicles_length; count++ )
+ {
+ printf( "\t%f", data->vehicles[count].speed );
+ }
+ printf("\nsteer:");
+ for( count=0; count<data->vehicles_length; count++ )
+ {
+ printf( "\t%f", data->vehicles[count].steeringCurvature );
+ }
+ printf("\npIndex:");
+ for( count=0; count<stat->pathIndex_length; count++ )
+ {
+ printf( "\t%d", stat->pathIndex[count] );
+ }
+ printf("\npState:");
+ for( count=0; count<stat->primState_length; count++ )
+ {
+ printf( "\t%s", rcsPrimStateToString(stat->primState[count]) );
+ }
+ printf("\npStatus:");
+ for( count=0; count<stat->primStatus_length; count++ )
+ {
+ printf( "\t%s", rcs_stat_to_string(stat->primStatus[count]) );
+ }
+ */
+ printf( "\n" );
+}
+
+static int strwcmp(char *test, char *match)
+{
+ while (0 != *test && 0 != *match) {
+ if (*test != *match)
+ return 1;
+ test++, match++;
+ }
+
+ if (0 == *match && (0 == *test || isspace(*test)))
+ return 0;
+ return 1;
+}
+
+static void printHelp(void)
+{
+ printf("q | quit -- quit\n");
+ printf("? | help -- print this help\n");
+ printf("init -- initialize\n");
+ printf("halt -- halt\n");
+ printf("abort -- abort\n");
+ printf("shutdown -- shutdown\n");
+ printf("monitor -- monitor commands\n");
+ /*
+ printf("ct <secs> -- set cycle time to <secs>\n");
+ printf("mvl <x> <y> -- move to <x> <y> in local coordinates\n");
+ printf("mvg <x> <y> -- move to <x> <y> in global coordinates\n");
+ printf("exp <x> <y> -- explore while moving to local waypoint\n");
+ printf("dump -- dump world model and display with imageview\n");
+ printf("debug <0, 1, ...> -- configure debug level\n");
+ */
+}
+
+int main(int argc, char *argv[])
+{
+ int serialNumber = 1;
+ char nml_file_default[] = DEFAULT_NML_FILE;
+ char *nml_file_env;
+ char *NML_FILE = NULL;
+ char ini_file_default[] = DEFAULT_INI_FILE;
+ char *ini_file_env;
+ char *INI_FILE = NULL;
+ int bufnum = 1;
+#ifndef HAVE_READLINE_READLINE_H
+ enum {BUFFERSIZE = 256};
+ char buffer[BUFFERSIZE];
+#endif
+ char *line;
+ char *ptr;
+ int retval;
+ LinkMonCmdNop linkMonCmdNop;
+ LinkMonCmdInit linkMonCmdInit;
+ LinkMonCmdAbort linkMonCmdAbort;
+ LinkMonCmdHalt linkMonCmdHalt;
+ LinkMonCmdShutdown linkMonCmdShutdown;
+ LinkMonCmdMonitor linkMonCmdWaypoint;
+
+#ifdef HAVE_READLINE_READLINE_H
+ using_history();
+#endif
+
+ while (1) {
+ char option = getopt(argc, argv, "n:i:d:");
+ if (option == -1)
+ break;
+
+ switch (option) {
+ case 'n':
+ NML_FILE = optarg;
+ break;
+ case 'i':
+ INI_FILE = optarg;
+ break;
+ case 'd':
+ if (1 != sscanf(optarg, "%i", &bufnum)) {
+ rcs_print_error("%s: bad value for buffer number: %s\n", argv[0], optarg);
+ return 1;
+ }
+ break;
+ default:
+ rcs_print_error("%s: bad argument\n", argv[0]);
+ return 1;
+ break;
+ }
+ }
+
+ // get the config file name from arg, if provided, next environment,
+ // if there, else default
+ if (NULL == NML_FILE) {
+ nml_file_env = getenv("CONFIG_NML");
+ if (NULL != nml_file_env) {
+ NML_FILE = nml_file_env;
+ } else {
+ NML_FILE = nml_file_default;
+ }
+ }
+
+ // ditto for the .ini file
+ if (NULL == INI_FILE) {
+ ini_file_env = getenv("CONFIG_INI");
+ if (NULL != ini_file_env) {
+ INI_FILE = ini_file_env;
+ } else {
+ INI_FILE = ini_file_default;
+ }
+ }
+
+ // initial allocate all the buffers
+ retval = initNmlBuffers(NML_FILE, bufnum);
+
+ if (retval) {
+ fprintf(stderr, "%s: can't allocate NML buffers\n", argv[0]);
+ return 1;
+ }
+
+ while (!feof(stdin)) {
+#ifndef HAVE_READLINE_READLINE_H
+ printf("> ");
+ fflush(stdout);
+ if (NULL == fgets(buffer, BUFFERSIZE, stdin)) {
+ break;
+ }
+ line = buffer;
+#else
+ line = readline("> ");
+ if (NULL == line) {
+ break;
+ }
+ if (*line) {
+ add_history(line);
+ }
+#endif
+
+ linkMonStatNml->read();
+ // linkMonSetNml->read();
+ trafficDataNml->read();
+ trafficCtrlSetNml->read();
+
+ ptr = line;
+ while (isspace(*ptr))
+ ptr++; // skip leading white space
+ if (0 == *ptr) { // blank line
+ printStat();
+ } else if (ptr[0] == 'q' || !strcmp(ptr, "quit")) {
+ break;
+ } else if (ptr[0] == '?' || !strcmp(ptr, "help")) {
+ printHelp();
+ } else if (!strwcmp(ptr, "init")) {
+ linkMonCmdInit.serial_number = serialNumber;
+ serialNumber+=2;
+ linkMonCmdNml->write(&linkMonCmdInit);
+ } else if (!strwcmp(ptr, "nop")) {
+ linkMonCmdNop.serial_number = serialNumber;
+ serialNumber+=2;
+ linkMonCmdNml->write(&linkMonCmdNop);
+ } else if (!strwcmp(ptr, "halt")) {
+ linkMonCmdHalt.serial_number = serialNumber;
+ serialNumber+=2;
+ linkMonCmdNml->write(&linkMonCmdHalt);
+ } else if (!strwcmp(ptr, "abort")) {
+ linkMonCmdAbort.serial_number = serialNumber;
+ serialNumber+=2;
+ linkMonCmdNml->write(&linkMonCmdAbort);
+ } else if (!strwcmp(ptr, "shutdown")) {
+ linkMonCmdShutdown.serial_number = serialNumber;
+ serialNumber+=2;
+ linkMonCmdNml->write(&linkMonCmdShutdown);
+ } else if (!strwcmp(ptr, "monitor")) {
+ linkMonCmdWaypoint.serial_number = serialNumber;
+ serialNumber+=2;
+ linkMonCmdNml->write(&linkMonCmdWaypoint);
+ /*
+ } else if (!strwcmp(ptr, "wp")) {
+ if (1 != sscanf(ptr, "%*s %i",
+ &linkMonCmdWaypoint.numVehicles)) {
+ printf("need number of vehicles and file\n");
+ }else{
+ while (!isspace(*ptr) && 0 != *ptr)
+ ptr++; // skip over word
+ while (isspace(*ptr))
+ ptr++; // skip over space
+ while (!isspace(*ptr) && 0 != *ptr)
+ ptr++; // skip over num vehicles
+ while (isspace(*ptr))
+ ptr++;
+ if (0 == *ptr) {
+ printf("need a data file\n");
+ continue;
+ }
+ retVal = loadWaypoints(ptr,
+ &linkMonCmdWaypoint.waypoint[0][0],
+ &linkMonCmdWaypoint.waypoint_length[0]);
+ if( retVal != 0 ){
+ printf("can't load ``%s''\n", ptr);
+ }else{
+ for( count=1; count<linkMonCmdWaypoint.numVehicles; count++ )
+ {
+ for( count2=0;count2<linkMonCmdWaypoint.waypoint_length[0]; count2++ )
+ {
+ linkMonCmdWaypoint.waypoint[count][count2] =
+ linkMonCmdWaypoint.waypoint[0][count2];
+ }
+ linkMonCmdWaypoint.waypoint_length[count] =
+ linkMonCmdWaypoint.waypoint_length[0];
+ }
+
+ linkMonCmdNml->write(&linkMonCmdWaypoint);
+ }
+ }
+ */
+ } else {
+ printf("?\n");
+ }
+
+#ifdef HAVE_READLINE_READLINE_H
+ free(line);
+#endif
+ }
+
+ return cleanupNmlBuffers();
+}
+
+/*
+ Modification history:
+
+ $Log: linkMonShell.cc,v $
+ Revision 1.1 2007/09/11 20:26:53 dr_steveb
+ Initial check-in
+
+
+*/
+
More information about the TeamTalk-developers
mailing list