Commit 97f36a4e authored by Timothy Stack's avatar Timothy Stack

Add obstacles to emcd's config file and pass them to rmcd

parent a3ca1cb0
......@@ -432,8 +432,9 @@ int have_camera_config(char *hostname, int port,
}
void parse_config_file(char *config_file) {
int oc_size = 0, cc_size = 0, line_no = 0;
struct obstacle_config *oc = NULL;
struct camera_config *cc = NULL;
int cc_size = 0, line_no = 0;
char line[BUFSIZ];
FILE *fp;
......@@ -461,22 +462,23 @@ void parse_config_file(char *config_file) {
sscanf(line, "%16s", directive);
if (strcmp(directive, "robot") == 0) {
char hostname[MAXHOSTNAMELEN], vname[TBDB_FLEN_EVOBJNAME];
char area[32], hostname[MAXHOSTNAMELEN], vname[TBDB_FLEN_EVOBJNAME];
int id;
float init_x;
float init_y;
float init_theta;
if (sscanf(line,
"%16s %d %" __XSTRING(MAXHOSTNAMELEN) "s %f %f %f "
"%16s %32s %d %" __XSTRING(MAXHOSTNAMELEN) "s %f %f %f "
"%" __XSTRING(TBDB_FLEN_EVOBJNAME) "s",
directive,
area,
&id,
hostname,
&init_x,
&init_y,
&init_theta,
vname) != 7) {
vname) != 8) {
fprintf(stderr,
"error:%d: syntax error in config file - %s\n",
line_no,
......@@ -511,14 +513,15 @@ void parse_config_file(char *config_file) {
}
}
else if (strcmp(directive, "camera") == 0) {
char hostname[MAXHOSTNAMELEN];
char area[32], hostname[MAXHOSTNAMELEN];
int port;
if (sscanf(line,
"%16s %" __XSTRING(MAXHOSTNAMELEN) "s %d",
"%16s %32s %" __XSTRING(MAXHOSTNAMELEN) "s %d",
directive,
area,
hostname,
&port) != 3) {
&port) != 4) {
fprintf(stderr,
"error:%d: syntax error in config file - %s\n",
line_no,
......@@ -537,6 +540,39 @@ void parse_config_file(char *config_file) {
cc_size += 1;
}
}
else if (strcmp(directive, "obstacle") == 0) {
float x1, y1, x2, y2;
char area[32];
int id;
if (sscanf(line,
"%16s %32s %d %f %f %f %f",
directive,
area,
&id,
&x1,
&y1,
&x2,
&y2) != 7) {
fprintf(stderr,
"error:%d: syntax error in config file - %s\n",
line_no,
line);
}
else if ((oc = realloc(oc,
(oc_size + 1) *
sizeof(struct camera_config))) == 0) {
fatal("cannot allocate memory\n");
}
else {
oc[oc_size].id = id;
oc[oc_size].x1 = x1;
oc[oc_size].y1 = y1;
oc[oc_size].x2 = x2;
oc[oc_size].y2 = y2;
oc_size += 1;
}
}
// next line!
}
......@@ -559,6 +595,8 @@ void parse_config_file(char *config_file) {
MA_Role, MTP_ROLE_EMC,
MA_RobotLen, hostname_list->item_count,
MA_RobotVal, robot_val,
MA_ObstacleLen, oc_size,
MA_ObstacleVal, oc,
MA_TAG_DONE);
mtp_init_packet(&config_vmc,
MA_Opcode, MTP_CONFIG_VMC,
......
robot 1 garcia1.flux.utah.edu 0 0 0.00 node1
robot 2 garcia2.flux.utah.edu 1 0 0.00 node2
camera junk.flux.utah.edu 6100
robot MEB-ROBOTS 1 garcia1.flux.utah.edu 0 0 0.00 node1
robot MEB-ROBOTS 2 garcia2.flux.utah.edu 1 0 0.00 node2
camera MEB-ROBOTS junk.flux.utah.edu 6100
obstacle MEB-ROBOTS 1 0.35 0.40 0.45 0.50
......@@ -51,6 +51,7 @@ Packet: version 2; role emc
vert: 0.000000
robot[0]: 1, garcia1.flux.utah.edu
robot[1]: 2, garcia2.flux.utah.edu
obstacle[0]: 1 0.350000/0.400000 x 0.000000/0.000000
Packet: version 2; role emc
opcode: error
id: 0
......@@ -83,6 +84,7 @@ Packet: version 2; role emc
vert: 0.000000
robot[0]: 1, garcia1.flux.utah.edu
robot[1]: 2, garcia2.flux.utah.edu
obstacle[0]: 1 0.350000/0.400000 x 0.000000/0.000000
Packet: version 2; role emc
opcode: update-position
id: 1
......
......@@ -16,6 +16,7 @@
#include <stdlib.h>
#include <assert.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
......@@ -223,6 +224,9 @@ int mtp_bind(char *host, int port, char *path)
sizeof(saddr.sin) : sizeof(saddr.sun)) == -1) {
perror("bind");
}
else if ((family == AF_UNIX) && (chmod(path, S_IRWXU) == -1)) {
perror("fchmod");
}
else if (listen(fd, 5) == -1) {
perror("listen");
}
......@@ -356,6 +360,14 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->data.mtp_payload_u.config_vmc.cameras.cameras_val =
va_arg(args, struct camera_config *);
break;
case MA_ObstacleLen:
mp->data.mtp_payload_u.config_rmc.obstacles.obstacles_len =
va_arg(args, int);
break;
case MA_ObstacleVal:
mp->data.mtp_payload_u.config_rmc.obstacles.obstacles_val =
va_arg(args, struct obstacle_config *);
break;
case MA_RobotID:
switch (mp->data.opcode) {
case MTP_REQUEST_POSITION:
......@@ -606,6 +618,20 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
mcr->robots.robots_val[lpc].id,
mcr->robots.robots_val[lpc].hostname);
}
for (lpc = 0;
lpc < mp->data.mtp_payload_u.config_rmc.obstacles.obstacles_len;
lpc++) {
struct mtp_config_rmc *mcr = &mp->data.mtp_payload_u.config_rmc;
fprintf(file,
" obstacle[%d]:\t%d %f/%f x %f/%f\n",
lpc,
mcr->obstacles.obstacles_val[lpc].id,
mcr->obstacles.obstacles_val[lpc].x1,
mcr->obstacles.obstacles_val[lpc].y1,
mcr->obstacles.obstacles_val[lpc].x2,
mcr->obstacles.obstacles_val[lpc].y2);
}
break;
case MTP_REQUEST_POSITION:
......
......@@ -134,6 +134,8 @@ typedef enum {
MA_RobotVal, /*< (robot_config *) */
MA_CameraLen, /*< (int) */
MA_CameraVal, /*< (camera_config *) */
MA_ObstacleLen, /*< (int) */
MA_ObstacleVal, /*< (obstacle_config *) */
MA_RobotID, /*< (int) */
MA_Position, /*< (robot_position *) */
MA_X, /*< (double) */
......
......@@ -156,6 +156,16 @@ struct camera_config {
int port;
};
struct obstacle_config {
int id;
float x1;
float y1;
float z1;
float x2;
float y2;
float z2;
};
struct global_bound {
float horizontal;
float vertical;
......@@ -164,6 +174,7 @@ struct global_bound {
struct mtp_config_rmc {
robot_config robots<>;
global_bound box;
obstacle_config obstacles<>;
};
struct mtp_config_vmc {
......
robot 1 localhost 7 8 0.46 node1
robot MEB-ROBOTS 1 localhost 7 8 0.46 node1
robot 1 garcia1.flux.utah.edu 5 6 0.38 node1
robot MEB-ROBOTS 1 garcia1.flux.utah.edu 5 6 0.38 node1
robot 1 garcia1.flux.utah.edu 5 6 0.38 node1
robot 2 garcia2.flux.utah.edu 20 20 0.0 node2
robot MEB-ROBOTS 1 garcia1.flux.utah.edu 5 6 0.38 node1
robot MEB-ROBOTS 2 garcia2.flux.utah.edu 20 20 0.0 node2
robot 1 garcia1.flux.utah.edu 5 6 0.0 node1
robot MEB-ROBOTS 1 garcia1.flux.utah.edu 5 6 0.0 node1
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment