All new accounts created on Gitlab now require administrator approval. If you invite any collaborators, please let Flux staff know so they can approve the accounts.

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