Commit 0ed6b518 authored by Timothy Stack's avatar Timothy Stack
Browse files

Command line tools for the robot protocol:

	* configure, configure.in: Add the robots GNUmakefiles.

	* robots/mtp/GNUmakefile.in: Add command line tools: mtp_send and
	mtp_recv.

	* robots/mtp/mtp.c: Make readall return an error if all of the
	data wasn't read instead of making the caller check.  Free the
	buffer allocated in mtp_send_packet.  Add an mtp_free_packet.

	* robots/mtp/mtp_recv.c: Faux receiver of mtp packets, it doesn't
	actually follow the protocol, it just prints out whatever it
	receives.

	* robots/mtp/mtp_send.c: Command line tool for sending mtp
	packets.
parent ce5b0c61
......@@ -1582,7 +1582,8 @@ outfiles="$outfiles Makeconf GNUmakefile \
dhcpd/dhcpd.conf.template dhcpd/GNUmakefile \
install/GNUmakefile install/ops-install install/boss-install \
install/newnode_sshkeys/GNUmakefile \
mote/GNUmakefile mote/tbuisp "
mote/GNUmakefile mote/tbuisp \
robots/GNUmakefile robots/mtp/GNUmakefile "
#
# Do this for easy distclean.
......
......@@ -581,6 +581,7 @@ outfiles="$outfiles Makeconf GNUmakefile \
tmcd/linux/sethostname.dhclient \
tmcd/linux9/GNUmakefile tmcd/linux9/supfile \
tmcd/freebsd5/GNUmakefile tmcd/freebsd5/supfile \
tmcd/cygwinxp/GNUmakefile \
tmcd/openbsd/GNUmakefile tmcd/ron/GNUmakefile tmcd/plab/GNUmakefile \
utils/GNUmakefile utils/vlandiff utils/vlansync utils/delay_config \
utils/sshtb utils/create_image utils/node_admin utils/webcreateimage \
......@@ -611,7 +612,8 @@ outfiles="$outfiles Makeconf GNUmakefile \
dhcpd/dhcpd.conf.template dhcpd/GNUmakefile \
install/GNUmakefile install/ops-install install/boss-install \
install/newnode_sshkeys/GNUmakefile \
mote/GNUmakefile mote/tbuisp "
mote/GNUmakefile mote/tbuisp \
robots/GNUmakefile robots/mtp/GNUmakefile "
#
# Do this for easy distclean.
......
......@@ -12,7 +12,7 @@ SUBDIR = robots/mtp
include $(OBJDIR)/Makeconf
MTPLIBS = libmtp.a
MTPPROGS = mtp_test
MTPPROGS = mtp_test mtp_send mtp_recv
TESTS = mtp_test
......@@ -29,10 +29,18 @@ libmtp.a: $(OBJS)
mtp.o: mtp.h
mtp_test.o: mtp.h
mtp_send.o: mtp.h
mtp_recv.o: mtp.h
mtp_test: mtp_test.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_test.o -L. -lmtp
mtp_send: mtp_send.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_send.o -L. -lmtp
mtp_recv: mtp_recv.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_recv.o -L. -lmtp
install: $(addprefix $(INSTALL_LIBDIR), $(MTPLIBS))
clean:
......
#include "config.h"
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
......@@ -23,6 +24,10 @@ static int readall(int fd,char *buffer,unsigned int len)
if (rc < 0) {
retval = rc;
}
else if (off != len) {
errno = EIO;
retval = -1;
}
else {
retval = off;
}
......@@ -40,7 +45,7 @@ int mtp_receive_packet(int fd,struct mtp_packet **packet) {
return MTP_PP_ERROR_ARGS;
}
retval = readall(fd,&clength,4);
retval = readall(fd,(char *)&clength,4);
if (retval == -1) {
return MTP_PP_ERROR_READ;
......@@ -551,6 +556,10 @@ int mtp_send_packet(int fd,struct mtp_packet *packet) {
// now we can write the buffer out the socket.
retval = write(fd,buf,retval);
free(buf);
buf = NULL;
if (retval != -1) {
return MTP_PP_SUCCESS;
}
......@@ -623,6 +632,37 @@ struct mtp_packet *mtp_make_packet( unsigned char opcode,
return retval;
}
void mtp_free_packet(struct mtp_packet *mp) {
if (mp != NULL) {
int lpc;
switch (mp->opcode) {
case MTP_CONTROL_ERROR:
case MTP_CONTROL_NOTIFY:
case MTP_CONTROL_INIT:
case MTP_CONTROL_CLOSE:
free(mp->data.control->msg);
break;
case MTP_CONFIG_VMC:
for (lpc = 0; lpc < mp->data.config_vmc->num_robots; lpc++) {
free(mp->data.config_vmc->robots[lpc].hostname);
}
free(mp->data.config_vmc->robots);
break;
case MTP_CONFIG_RMC:
for (lpc = 0; lpc < mp->data.config_rmc->num_robots; lpc++) {
free(mp->data.config_rmc->robots[lpc].hostname);
}
free(mp->data.config_rmc->robots);
break;
}
free(mp);
mp = NULL;
}
}
int mtp_calc_size(int opcode,void *data) {
int retval = -1;
......
#include "config.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <sys/types.h>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include "mtp.h"
int main(int argc, char *argv[])
{
int fd, port = 0, retval = EXIT_SUCCESS;
if ((argc == 2) && sscanf(argv[1], "%d", &port) != 1) {
fprintf(stderr, "error: port argument is not a number: %s\n", argv[1]);
retval = EXIT_FAILURE;
}
else if ((fd = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
perror("socket");
retval = EXIT_FAILURE;
}
else {
struct sockaddr_in sin;
int on_off = 1;
memset(&sin, 0, sizeof(sin));
sin.sin_len = sizeof(sin);
sin.sin_family = AF_INET;
sin.sin_port = htons(port);
sin.sin_addr.s_addr = INADDR_ANY;
setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &on_off, sizeof(int));
if (bind(fd, (struct sockaddr *)&sin, sizeof(sin)) == -1) {
perror("bind");
retval = EXIT_FAILURE;
}
else if (listen(fd, 5) == -1) {
perror("listen");
retval = EXIT_FAILURE;
}
else {
struct sockaddr_in sin_peer;
socklen_t slen;
int fd_peer;
slen = sizeof(sin_peer);
getsockname(fd, (struct sockaddr *)&sin, &slen);
printf("Listening for mtp packets on port: %d\n",
ntohs(sin.sin_port));
while ((fd_peer = accept(fd,
(struct sockaddr *)&sin_peer,
&slen)) != -1) {
struct mtp_packet *mp;
while (mtp_receive_packet(fd_peer, &mp) == MTP_PP_SUCCESS) {
int lpc;
printf("Packet: length %d; version %d; role %d\n",
mp->length,
mp->version,
mp->role);
switch (mp->opcode) {
case MTP_CONTROL_ERROR:
case MTP_CONTROL_NOTIFY:
case MTP_CONTROL_INIT:
case MTP_CONTROL_CLOSE:
switch (mp->opcode) {
case MTP_CONTROL_ERROR:
printf(" opcode:\terror\n");
break;
case MTP_CONTROL_NOTIFY:
printf(" opcode:\tnotify\n");
break;
case MTP_CONTROL_INIT:
printf(" opcode:\tinit\n");
break;
case MTP_CONTROL_CLOSE:
printf(" opcode:\tclose\n");
break;
}
printf(" id:\t\t%d\n"
" code:\t\t%d\n"
" msg:\t\t%s\n",
mp->data.control->id,
mp->data.control->code,
mp->data.control->msg);
break;
case MTP_CONFIG_VMC:
printf(" opcode:\tconfig-vmc\n"
" num:\t\t%d\n",
mp->data.config_vmc->num_robots);
for (lpc = 0;
lpc < mp->data.config_vmc->num_robots;
lpc++) {
printf(" robot[%d]:\t%d, %s\n",
lpc,
mp->data.config_vmc->robots[lpc].id,
mp->data.config_vmc->robots[lpc].hostname);
}
break;
case MTP_CONFIG_RMC:
printf(" opcode:\tconfig-rmc\n"
" num:\t%d\n"
" horiz:\t%f\n"
" vert:\t%f\n",
mp->data.config_rmc->num_robots,
mp->data.config_rmc->box.horizontal,
mp->data.config_rmc->box.vertical);
for (lpc = 0;
lpc < mp->data.config_rmc->num_robots;
lpc++) {
printf(" robot[%d]:\t%d, %s\n",
lpc,
mp->data.config_rmc->robots[lpc].id,
mp->data.config_rmc->robots[lpc].hostname);
}
break;
case MTP_REQUEST_POSITION:
printf(" opcode:\trequest-position\n"
" id:\t\t%d\n",
mp->data.request_position->robot_id);
break;
case MTP_REQUEST_ID:
printf(" opcode:\trequest-id\n"
" x:\t\t%f\n"
" y:\t\t%f\n"
" theta:\t%f\n"
" timestamp:\t%d\n",
mp->data.request_id->position.x,
mp->data.request_id->position.y,
mp->data.request_id->position.theta,
mp->data.request_id->timestamp);
break;
case MTP_UPDATE_POSITION:
printf(" opcode:\tupdate-position\n"
" id:\t\t%d\n"
" x:\t\t%f\n"
" y:\t\t%f\n"
" theta:\t%f\n"
" status:\t%d\n"
" timestamp:\t%d\n",
mp->data.update_position->robot_id,
mp->data.update_position->position.x,
mp->data.update_position->position.y,
mp->data.update_position->position.theta,
mp->data.update_position->status,
mp->data.update_position->timestamp);
break;
case MTP_UPDATE_ID:
printf(" opcode:\tupdate-id\n"
" id:\t%d\n",
mp->data.update_position->robot_id);
break;
case MTP_COMMAND_GOTO:
printf(" opcode:\tcommand-goto\n"
" commid:\t%d\n"
" id:\t\t%d\n"
" x:\t\t%f\n"
" y:\t\t%f\n"
" theta:\t%f\n",
mp->data.command_goto->command_id,
mp->data.command_goto->robot_id,
mp->data.command_goto->position.x,
mp->data.command_goto->position.y,
mp->data.command_goto->position.theta);
break;
case MTP_COMMAND_STOP:
printf(" opcode:\tupdate-id\n"
" commid:\t%d\n"
" id:\t%d\n",
mp->data.command_stop->command_id,
mp->data.command_stop->robot_id);
break;
default:
assert(0);
break;
}
mtp_free_packet(mp);
mp = NULL;
}
close(fd_peer);
fd_peer = -1;
slen = sizeof(sin_peer);
}
perror("accept");
}
close(fd);
fd = -1;
}
return retval;
}
#include "config.h"
#include <errno.h>
#include <float.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <sys/types.h>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include "mtp.h"
static int debug = 0;
static int mygethostbyname(struct sockaddr_in *host_addr, char *host)
{
struct hostent *host_ent;
int retval = 0;
assert(host_addr != NULL);
assert(host != NULL);
assert(strlen(host) > 0);
if( (host_ent = gethostbyname(host)) != NULL ) {
memcpy((char *)&host_addr->sin_addr.s_addr,
host_ent->h_addr,
host_ent->h_length);
retval = 1;
}
else {
retval = inet_aton(host, &host_addr->sin_addr);
}
return( retval );
}
static void usage(void)
{
fprintf(stderr,
"Usage: mtp_send [OPTIONS] <command>\n"
"Commands:\n"
" error notify init close request-position request-id\n"
" update-position update-id command-goto command-stop\n"
"\n"
"Options:\n"
" -h\t\tPrint this message\n"
" -d\t\tTurn on debugging messages\n"
" -i #\t\tThe robot identifier\n"
" -c #\t\tThe code number for control packets\n"
" -C #\t\tThe command_id for goto/stop\n"
" -n name\tThe robot name\n"
" -x #\t\tThe X coordinate\n"
" -y #\t\tThe Y coordinate\n"
" -o #\t\tThe orientation\n"
" -H #\t\tThe horizontal size\n"
" -V #\t\tThe vertical size\n"
" -t #\t\tThe timestamp\n"
" -s idle|moving|error|complete\n"
" \t\tThe robot status\n"
" -m msg\tThe msg for control packets\n"
" -r vmc|emc|rmc\n"
" \t\tThe role (Default: rmc)\n"
" -P #\t\tThe port the peer listening on\n"
"\n"
"Example:\n"
" $ mtp_send -n garcia1 -P 3000 -i 1 -c 2 -m \"FooBar\" error\n");
}
static void required_option(char *name)
{
assert(name != NULL);
fprintf(stderr,
"error: the -%s option is required for this packet type\n",
name);
usage();
exit(1);
}
int main(int argc, char *argv[])
{
struct {
int id;
int code;
char *hostname;
int command_id;
float x;
float y;
float orientation;
float horizontal;
float vertical;
int timestamp;
int status;
char *message;
int role;
int port;
} args = {
-1, /* id */
-1, /* code */
NULL, /* hostname */
-1, /* command_id */
DBL_MIN, /* x */
DBL_MIN, /* y */
DBL_MIN, /* orientation */
DBL_MIN, /* horizontal */
DBL_MIN, /* vertical */
-1, /* timestamp */
-1, /* status */
NULL, /* message */
MTP_ROLE_RMC, /* role */
-1, /* port */
};
union {
struct mtp_control control;
struct mtp_config_rmc config_rmc;
struct mtp_config_vmc config_vmc;
struct mtp_request_position request_position;
struct mtp_request_id request_id;
struct mtp_update_position update_position;
struct mtp_update_id update_id;
struct mtp_command_goto command_goto;
struct mtp_command_stop command_stop;
} payload;
int c, fd, retval = EXIT_SUCCESS;
mtp_payload_t m_payload;
struct sockaddr_in sin;
mtp_packet_t *mp;
char opcode;
while ((c = getopt(argc, argv, "hdi:c:C:n:x:y:o:H:V:t:s:m:r:P:")) != -1) {
switch (c) {
case 'h':
usage();
exit(0);
break;
case 'd':
debug += 1;
break;
case 'i':
if (sscanf(optarg, "%d", &args.id) != 1) {
fprintf(stderr,
"error: i option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'c':
if (sscanf(optarg, "%d", &args.code) != 1) {
fprintf(stderr,
"error: c option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'C':
if (sscanf(optarg, "%d", &args.command_id) != 1) {
fprintf(stderr,
"error: C option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'n':
if (strlen(optarg) == 0) {
fprintf(stderr, "error: n option is empty\n");
usage();
exit(1);
}
args.hostname = optarg;
break;
case 'x':
if (sscanf(optarg, "%f", &args.x) != 1) {
fprintf(stderr,
"error: x option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'y':
if (sscanf(optarg, "%f", &args.y) != 1) {
fprintf(stderr,
"error: y option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'o':
if (sscanf(optarg, "%f", &args.orientation) != 1) {
fprintf(stderr,
"error: o option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'H':
if (sscanf(optarg, "%f", &args.horizontal) != 1) {
fprintf(stderr,
"error: H option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'V':
if (sscanf(optarg, "%f", &args.vertical) != 1) {
fprintf(stderr,
"error: V option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 't':
if (sscanf(optarg, "%d", &args.timestamp) != 1) {
fprintf(stderr,
"error: t option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 's':
if (strcasecmp(optarg, "idle") == 0) {
args.status = MTP_POSITION_STATUS_IDLE;
}
else if (strcasecmp(optarg, "moving") == 0) {
args.status = MTP_POSITION_STATUS_MOVING;
}
else if (strcasecmp(optarg, "error") == 0) {
args.status = MTP_POSITION_STATUS_ERROR;
}
else if (strcasecmp(optarg, "complete") == 0) {
args.status = MTP_POSITION_STATUS_COMPLETE;
}
else {
fprintf(stderr,
"error: s option must be one of: idle, moving, "
"error, or complete\n");
usage();
exit(1);
}
break;
case 'm':
args.message = optarg;
break;
case 'r':
if (strcasecmp(optarg, "vmc") == 0) {
args.role = MTP_ROLE_VMC;
}
else if (strcasecmp(optarg, "emc") == 0) {
args.role = MTP_ROLE_EMC;
}
else if (strcasecmp(optarg, "rmc") == 0) {
args.role = MTP_ROLE_RMC;
}
else {
fprintf(stderr,
"error: r option must be one of: vmc, emc, or rmc\n");
usage();
exit(1);
}
break;
case 'P':