Commit e778a07f authored by David Johnson's avatar David Johnson

This is mostly bounds-checking code (so that robots can't run out of

vision system bounds).
parent 7cd87904
......@@ -90,10 +90,40 @@ static elvin_error_t elvin_error;
static struct mtp_packet config_rmc;
static struct mtp_packet config_vmc;
/*
* these are for some global bounds checking on user-requested
* positions
*/
static struct camera_config *g_camera_config;
int g_camera_config_size = 0;
static struct obstacle_config *g_obstacle_config;
int g_obstacle_config_size = 0;
static struct rmc_client rmc_data;
static struct vmc_client vmc_data;
static mtp_handle_t emulab_handle = NULL;
/*
* returns 1 if x,y are inside the union of the boxes defined
* in the camera_config struct; 0 if not.
*/
static int position_in_camera(struct camera_config *cc,
int cc_size,
float x,
float y
);
/*
* returns 0 if the position is not in a known obstacle; 1 if it is.
*/
static int position_in_obstacle(struct obstacle_config *oc,
int oc_size,
float x,
float y
);
static void ev_callback(event_handle_t handle,
event_notification_t notification,
void *data);
......@@ -293,6 +323,9 @@ int main(int argc, char *argv[])
hostname_list = robot_list_create();
initial_position_list = robot_list_create();
position_queue = robot_list_create();
g_camera_config = NULL;
g_obstacle_config = NULL;
/* read config file into the above lists*/
parse_config_file(config_file);
......@@ -417,6 +450,44 @@ int main(int argc, char *argv[])
return retval;
}
static int position_in_camera(struct camera_config *cc,
int cc_size,
float x,
float y
) {
int i;
int retval = 0;
for (i = 0; i < cc_size; ++i) {
if (x >= cc[i].x && y >= cc[i].y &&
x <= (cc[i].x + cc[i].width) && y <= (cc[i].y + cc[i].height)) {
retval = 1;
break;
}
}
return retval;
}
static int position_in_obstacle(struct obstacle_config *oc,
int oc_size,
float x,
float y
) {
int i;
int retval = 0;
for (i = 0; i < oc_size; ++i) {
if (x >= oc[i].xmin && y >= oc[i].ymin &&
x <= oc[i].xmax && y <= oc[i].ymax) {
retval = 1;
break;
}
}
return retval;
}
int have_camera_config(char *hostname, int port,
struct camera_config *cc, int cc_size)
{
......@@ -586,6 +657,8 @@ void parse_config_file(char *config_file) {
{
struct robot_config *robot_val, *rc;
struct robot_list_enum *e;
struct box *boxes_val;
int boxes_len;
int lpc = 0;
robot_val = malloc(sizeof(robot_config) * hostname_list->item_count);
......@@ -596,12 +669,23 @@ void parse_config_file(char *config_file) {
lpc += 1;
}
robot_list_enum_destroy(e);
boxes_len = cc_size;
boxes_val = (struct box *)malloc(sizeof(struct box) * boxes_len);
for (lpc = 0; lpc < boxes_len; ++lpc) {
boxes_val[lpc].x = cc[lpc].x;
boxes_val[lpc].y = cc[lpc].y;
boxes_val[lpc].width = cc[lpc].width;
boxes_val[lpc].height = cc[lpc].height;
}
mtp_init_packet(&config_rmc,
MA_Opcode, MTP_CONFIG_RMC,
MA_Role, MTP_ROLE_EMC,
MA_RobotLen, hostname_list->item_count,
MA_RobotVal, robot_val,
MA_BoundsLen, boxes_len,
MA_BoundsVal, boxes_val,
MA_ObstacleLen, oc_size,
MA_ObstacleVal, oc,
MA_TAG_DONE);
......@@ -613,6 +697,11 @@ void parse_config_file(char *config_file) {
MA_CameraLen, cc_size,
MA_CameraVal, cc,
MA_TAG_DONE);
/* don't free cc or oc! */
g_camera_config = cc;
g_camera_config_size = cc_size;
g_obstacle_config = oc;
g_obstacle_config_size = oc_size;
}
}
......@@ -701,6 +790,7 @@ void ev_callback(event_handle_t handle,
struct emc_robot_config *match = NULL;
char objname[TBDB_FLEN_EVOBJNAME];
robot_list_item_t *rli;
int in_obstacle, in_camera;
if (! event_notification_get_objname(handle, notification,
objname, sizeof(objname))) {
......@@ -754,35 +844,65 @@ void ev_callback(event_handle_t handle,
orientation = orientation * M_PI / 180.0;
mtp_init_packet(&mp,
MA_Opcode, MTP_COMMAND_GOTO,
MA_Role, MTP_ROLE_EMC,
MA_CommandID, 1,
MA_RobotID, match->id,
MA_X, x,
MA_Y, y,
MA_Theta, orientation,
MA_TAG_DONE);
if (rmc_data.handle != NULL) {
mtp_send_packet(rmc_data.handle, &mp);
if ((in_camera = position_in_camera(g_camera_config,g_camera_config_size,
x,y)) &&
!(in_obstacle = position_in_obstacle(g_obstacle_config,
g_obstacle_config_size,
x,y))
) {
mtp_init_packet(&mp,
MA_Opcode, MTP_COMMAND_GOTO,
MA_Role, MTP_ROLE_EMC,
MA_CommandID, 1,
MA_RobotID, match->id,
MA_X, x,
MA_Y, y,
MA_Theta, orientation,
MA_TAG_DONE);
if (rmc_data.handle != NULL) {
mtp_send_packet(rmc_data.handle, &mp);
}
else {
mtp_print_packet(stdout, &mp);
#if defined(TBDB_EVENTTYPE_COMPLETE)
event_do(handle,
EA_Experiment, pideid,
EA_Type, TBDB_OBJECTTYPE_NODE,
EA_Name, match->vname,
EA_Event, TBDB_EVENTTYPE_COMPLETE,
EA_ArgInteger, "ERROR", 0,
EA_ArgInteger, "CTOKEN", match->token,
EA_TAG_DONE);
#endif
}
mtp_free_packet(&mp);
}
else {
mtp_print_packet(stdout, &mp);
int which_err_num;
which_err_num = (in_camera == 0)?1:2;
info("requested position either outside camera bounds or inside"
" an obstacle!\n"
);
#if defined(TBDB_EVENTTYPE_COMPLETE)
event_do(handle,
EA_Experiment, pideid,
EA_Type, TBDB_OBJECTTYPE_NODE,
EA_Name, match->vname,
EA_Event, TBDB_EVENTTYPE_COMPLETE,
EA_ArgInteger, "ERROR", 0,
EA_ArgInteger, "CTOKEN", match->token,
EA_TAG_DONE);
event_do(handle,
EA_Experiment, pideid,
EA_Type, TBDB_OBJECTTYPE_NODE,
EA_Name, match->vname,
EA_Event, TBDB_EVENTTYPE_COMPLETE,
EA_ArgInteger, "ERROR", which_err_num,
EA_ArgInteger, "CTOKEN", match->token,
EA_TAG_DONE
);
#endif
}
mtp_free_packet(&mp);
// XXX What to do with the data?
......@@ -1165,17 +1285,33 @@ int emulab_callback(elvin_io_handler_t handler,
switch (mp->data.opcode) {
case MTP_COMMAND_GOTO:
if (rmc_data.handle == NULL) {
error("no rmcd yet\n");
}
else if (mtp_send_packet(rmc_data.handle, mp) != MTP_PP_SUCCESS) {
error("could not forward packet to rmcd\n");
}
else {
info("forwarded goto\n");
retval = 1;
}
if (position_in_camera(g_camera_config,g_camera_config_size,
mp->data.mtp_payload_u.command_goto.position.x,
mp->data.mtp_payload_u.command_goto.position.y
) &&
!position_in_obstacle(g_obstacle_config,g_obstacle_config_size,
mp->data.mtp_payload_u.command_goto.position.x,
mp->data.mtp_payload_u.command_goto.position.y)
) {
/* forward the packet on to rmc... */
if (rmc_data.handle == NULL) {
error("no rmcd yet\n");
}
else if (mtp_send_packet(rmc_data.handle, mp) != MTP_PP_SUCCESS) {
error("could not forward packet to rmcd\n");
}
else {
info("forwarded goto\n");
retval = 1;
}
}
else {
error("invalid position request (outside camera bounds or in"
" an obstacle\n"
);
}
break;
case MTP_REQUEST_POSITION:
{
......@@ -1284,7 +1420,7 @@ int vmc_callback(elvin_io_handler_t handler,
error("invalid client %p\n", mp);
}
else {
if (debug) {
if (debug > 1) {
fprintf(stderr, "vmc_callback: ");
mtp_print_packet(stderr, mp);
}
......
......@@ -340,6 +340,7 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
case MA_Message:
mp->data.mtp_payload_u.error.msg = va_arg(args, char *);
break;
#if 0
case MA_Horizontal:
mp->data.mtp_payload_u.config_rmc.box.horizontal =
va_arg(args, double);
......@@ -348,6 +349,7 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->data.mtp_payload_u.config_rmc.box.vertical =
va_arg(args, double);
break;
#endif
case MA_RobotLen:
mp->data.mtp_payload_u.config_rmc.robots.robots_len =
va_arg(args, int);
......@@ -364,6 +366,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_BoundsVal:
mp->data.mtp_payload_u.config_rmc.bounds.bounds_val =
va_arg(args, struct box *);
break;
case MA_BoundsLen:
mp->data.mtp_payload_u.config_rmc.bounds.bounds_len =
va_arg(args, int);
break;
case MA_ObstacleLen:
mp->data.mtp_payload_u.config_rmc.obstacles.obstacles_len =
va_arg(args, int);
......@@ -673,12 +683,8 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
case MTP_CONFIG_RMC:
fprintf(file,
" opcode:\tconfig-rmc\n"
" num:\t\t%d\n"
" horiz:\t%f\n"
" vert:\t\t%f\n",
mp->data.mtp_payload_u.config_rmc.robots.robots_len,
mp->data.mtp_payload_u.config_rmc.box.horizontal,
mp->data.mtp_payload_u.config_rmc.box.vertical);
" num:\t\t%d\n",
mp->data.mtp_payload_u.config_rmc.robots.robots_len);
for (lpc = 0;
lpc < mp->data.mtp_payload_u.config_rmc.robots.robots_len;
lpc++) {
......@@ -690,6 +696,21 @@ 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.bounds.bounds_len;
++lpc
) {
struct mtp_config_rmc *mcr = &mp->data.mtp_payload_u.config_rmc;
fprintf(file,
" bound[%d]:\tx=%f,y=%f,width=%f,height=%f\n",
lpc,
mcr->bounds.bounds_val[lpc].x,
mcr->bounds.bounds_val[lpc].y,
mcr->bounds.bounds_val[lpc].width,
mcr->bounds.bounds_val[lpc].height
);
}
for (lpc = 0;
lpc < mp->data.mtp_payload_u.config_rmc.obstacles.obstacles_len;
lpc++) {
......
......@@ -134,6 +134,8 @@ typedef enum {
MA_RobotVal, /*< (robot_config *) */
MA_CameraLen, /*< (int) */
MA_CameraVal, /*< (camera_config *) */
MA_BoundsVal, /*< (box *) */
MA_BoundsLen, /*< (int) */
MA_ObstacleLen, /*< (int) */
MA_ObstacleVal, /*< (obstacle_config *) */
MA_RobotID, /*< (int) */
......
......@@ -184,14 +184,16 @@ struct obstacle_config {
float zmax;
};
struct global_bound {
float horizontal;
float vertical;
struct box {
float x;
float y;
float width;
float height;
};
struct mtp_config_rmc {
robot_config robots<>;
global_bound box;
box bounds<>;
obstacle_config obstacles<>;
};
......
......@@ -256,6 +256,8 @@ void pc_plot_waypoint(struct pilot_connection *pc)
else {
float bearing;
rc_code_t rc;
int in_vision = 0;
int flipped_bearing = 0;
pc->pc_flags |= PCF_WAYPOINT;
......@@ -297,95 +299,145 @@ void pc_plot_waypoint(struct pilot_connection *pc)
info("debug: waypoint bearing %f\n", bearing);
}
if (rl.x0 == oc->xmin && rl.y0 == oc->ymin) {
printf(" top left\n");
if (bearing > -M_PI_4) {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
/*
* we need to make sure the generated waypoint is inside
* the vision system bounds; if it's not, there are a couple
* different approaches we can take (take the n+1st approach
* if the nth approach failed...)
*
* 1. Flip the bearing across M_PI, and try again.
* 2. Stall the bot; hopefully the obstacle will clear up.
* 3. Back up from the obstacle, and move away from the vision
* system bounds; hopefully, this will give the robot
* a better chance at picking a decent waypoint.
* 4. ?
*
* For now, we'll try 1, then 3. I'm not familiar enough with
* the state_change function to chance adding more states to
* control if we've tried stalling.
*/
while (!in_vision) {
if (rl.x0 == oc->xmin && rl.y0 == oc->ymin) {
printf(" top left\n");
if (bearing > -M_PI_4) {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
}
else {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
}
else if (rl.x0 == oc->xmin && rl.y0 == oc->ymax) {
printf(" bottom left\n");
if (bearing > M_PI_4) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
else if (rl.x0 == oc->xmin && rl.y0 == oc->ymax) {
printf(" bottom left\n");
if (bearing > M_PI_4) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
}
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
else if (rl.x0 == oc->xmax && rl.y0 == oc->ymax) {
printf(" bottom right\n");
if (bearing > (M_PI_2 + M_PI_4)) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
}
}
}
else if (rl.x0 == oc->xmax && rl.y0 == oc->ymax) {
printf(" bottom right\n");
if (bearing > (M_PI_2 + M_PI_4)) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
}
}
else if (rl.x0 == oc->xmax && rl.y0 == oc->ymin) {
printf(" top right\n");
if (bearing < -(M_PI_2 + M_PI_4)) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
}
}
else if (rl.x0 == oc->xmin) {
printf(" left\n");
if (bearing > 0.0) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
else if (rl.x0 == oc->xmax && rl.y0 == oc->ymin) {
printf(" top right\n");
if (bearing < -(M_PI_2 + M_PI_4)) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
}
}
else {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
else if (rl.x0 == oc->xmin) {
printf(" left\n");
if (bearing > 0.0) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
}
}
else if (rl.x0 == oc->xmax) {
printf(" right\n");
if (bearing > M_PI_2) {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
else if (rl.x0 == oc->xmax) {
printf(" right\n");
if (bearing > M_PI_2) {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
}
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
else if (rl.y0 == oc->ymin) {
printf(" top\n");
if (bearing < -M_PI_2) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
}
}
}
else if (rl.y0 == oc->ymin) {
printf(" top\n");
if (bearing < -M_PI_2) {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymin;
else if (rl.y0 == oc->ymax) {
printf(" bottom\n");
if (bearing < M_PI_2) {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
}
else {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
}
}
else {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymin;
assert(0);
}
}
else if (rl.y0 == oc->ymax) {
printf(" bottom\n");
if (bearing < M_PI_2) {
pc->pc_waypoint.x = oc->xmax;
pc->pc_waypoint.y = oc->ymax;
if (pc_point_in_bounds(&pc_data,
pc->pc_waypoint.x,
pc->pc_waypoint.y
)) {
in_vision = 1;
info("waypoint in vision\n");
}
else {
pc->pc_waypoint.x = oc->xmin;
pc->pc_waypoint.y = oc->ymax;
if (!flipped_bearing) {
flipped_bearing = 1;
/* flip it */
bearing = -bearing;
info("flipped bearing in waypoint gen\n");
}
else {
/* try moving backwards, and away from vision */
info("could not find a waypoint!\n");
/* XXX: how to find current direction of movement,
* so we know which direction is "backwards" ?
*/
assert(0);
}
}
}
if (debug) {
info("debug: %s waypoint %f %f\n",
pc->pc_robot->hostname,
......@@ -725,6 +777,10 @@ static void pc_handle_update(struct pilot_connection *pc,
pc->pc_state = PS_ARRIVED;
}
break;