From 6cce17626efe1cdecef280efb615b0639640aad8 Mon Sep 17 00:00:00 2001
From: Timothy Stack <stack@flux.utah.edu>
Date: Thu, 31 Mar 2005 20:43:24 +0000
Subject: [PATCH] Pass camera local to world offsets through vmcd.
* event/sched/rpc.cc: Add the camera fixed_x,fixed_y values to the
emcd.config file.
* robots/emc/emcd.c: Pickup the cameras fixed_x,fixed_y values
from the config file. Add supports for setting the speed of the
robot.
* robots/mtp/mtp.h, robots/mtp/mtp.c: Add support for setting
speed in GOTO packets and constructing vmc-client config packets.
* robots/mtp/mtp.x: Add a vmc-client config packet that contains
the cameras origin in world coordinates.
* robots/mtp/mtp_dump.c: Dump the min/max values for x and y,
handy for figuring out the bounds of the camera.
* robots/vmcd/visionTrack.c, robots/vmcd/visionTrack.h: Handle
cameras that are overlapping on two axes.
* robots/vmcd/vmc-client.c: Accept a vmc-client config packet that
tells us where the camera is in world coordinates. Add an
orientation flag that specifies how the camera is oriented wrt to
the world coordinate system.
* robots/vmcd/vmcd.c: Send vmc-client config packets out. Fix a
bug that left unknown tracks in the previous frame, which could
confuse the matching algorithm causing it to match unknown tracks
instead of valid known tracks.
---
event/sched/rpc.cc | 9 +-
robots/emc/emcd.c | 26 ++-
robots/mtp/mtp.c | 50 +++++-
robots/mtp/mtp.h | 1 +
robots/mtp/mtp.x | 14 ++
robots/vmcd/visionTrack.c | 323 ++++++++++++++------------------------
robots/vmcd/visionTrack.h | 59 ++++++-
robots/vmcd/vmc-client.c | 92 +++++++----
robots/vmcd/vmcd.c | 43 +++--
9 files changed, 346 insertions(+), 271 deletions(-)
diff --git a/event/sched/rpc.cc b/event/sched/rpc.cc
index fefa85be02..65e4fd40f0 100644
--- a/event/sched/rpc.cc
+++ b/event/sched/rpc.cc
@@ -375,14 +375,16 @@ RPC_cameralist(FILE *emcd_config, char *area)
tmp = attr.getMember("hostname");
port = ((ulxr::Integer)attr.getMember("port")).getInteger();
fprintf(emcd_config,
- "camera %s %s %d %f %f %f %f\n",
+ "camera %s %s %d %f %f %f %f %f %f\n",
area,
tmp.getString().c_str(),
port,
((ulxr::Double)attr.getMember("loc_x")).getDouble(),
((ulxr::Double)attr.getMember("loc_y")).getDouble(),
((ulxr::Double)attr.getMember("width")).getDouble(),
- ((ulxr::Double)attr.getMember("height")).getDouble());
+ ((ulxr::Double)attr.getMember("height")).getDouble(),
+ ((ulxr::Double)attr.getMember("fixed_x")).getDouble(),
+ ((ulxr::Double)attr.getMember("fixed_y")).getDouble());
}
return 0;
@@ -459,7 +461,8 @@ RPC_waitforrobots(event_handle_t handle, char *pid, char *eid)
info("info: waiting for robots\n");
if ((emcd_config = fopen("tbdata/emcd.config", "w")) == NULL) {
- fprintf(stderr, "Cannot create emcd.config!\n");
+ error("Cannot create emcd.config!\n");
+ return -1;
}
for (lpc = 0; lpc < locs.size(); lpc++) {
diff --git a/robots/emc/emcd.c b/robots/emc/emcd.c
index 68f3c8ac93..f05155ec8a 100755
--- a/robots/emc/emcd.c
+++ b/robots/emc/emcd.c
@@ -75,7 +75,6 @@ static char *progname;
static int debug;
static int port = EMC_SERVER_PORT;
-static int command_seq_no = 0;
/* initial data pulled from the config file */
/* it's somewhat non-intuitive to use lists here -- since we're doing maps,
@@ -202,7 +201,6 @@ static void dump_info(void)
while ((mup = (struct mtp_update_position *)
robot_list_enum_next_element(e)) != NULL) {
struct emc_robot_config *erc;
- struct mtp_packet mp;
erc = robot_list_search(hostname_list, mup->robot_id);
info(" %s: %.2f %.2f %.2f\n",
@@ -631,11 +629,13 @@ void parse_config_file(char *config_file) {
}
else if (strcmp(directive, "camera") == 0) {
char area[32], hostname[MAXHOSTNAMELEN];
- float x, y, width, height;
+ float x, y, width, height, fixed_x, fixed_y;
int port;
if (sscanf(line,
- "%16s %32s %" __XSTRING(MAXHOSTNAMELEN) "s %d %f %f %f %f",
+ "%16s %32s %"
+ __XSTRING(MAXHOSTNAMELEN)
+ "s %d %f %f %f %f %f %f",
directive,
area,
hostname,
@@ -643,7 +643,9 @@ void parse_config_file(char *config_file) {
&x,
&y,
&width,
- &height) != 8) {
+ &height,
+ &fixed_x,
+ &fixed_y) != 10) {
fprintf(stderr,
"error:%d: syntax error in config file - %s\n",
line_no,
@@ -663,6 +665,8 @@ void parse_config_file(char *config_file) {
cc[cc_size].y = y;
cc[cc_size].width = width;
cc[cc_size].height = height;
+ cc[cc_size].fixed_x = fixed_x;
+ cc[cc_size].fixed_y = fixed_y;
cc_size += 1;
}
}
@@ -873,8 +877,8 @@ void ev_callback(event_handle_t handle,
error("no match for host\n");
}
else {
+ float x, y, orientation = 0.0, speed = 0.1;
char *value, args[BUFSIZ];
- float x, y, orientation;
struct mtp_packet mp;
event_notification_get_arguments(handle, notification, args, sizeof(args));
@@ -901,6 +905,13 @@ void ev_callback(event_handle_t handle,
}
}
+ if (event_arg_get(args, "SPEED", &value) > 0) {
+ if (sscanf(value, "%f", &speed) != 1) {
+ error("SPEED argument in event is not a float: %s\n", value);
+ return;
+ }
+ }
+
event_notification_get_int32(handle, notification,
"TOKEN", (int32_t *)&match->token);
@@ -922,6 +933,7 @@ void ev_callback(event_handle_t handle,
MA_X, x,
MA_Y, y,
MA_Theta, orientation,
+ MA_Speed, speed,
MA_TAG_DONE);
if (rmc_data.handle != NULL) {
@@ -1559,7 +1571,7 @@ int vmc_callback(elvin_io_handler_t handler,
double best_pose_delta = POSE_THRESHOLD;
int robot_id = -1;
- double dx,dy,dtheta,ddist;
+ double dx = 0.0, dy = 0.0, dtheta, ddist;
struct mtp_packet mp_reply;
diff --git a/robots/mtp/mtp.c b/robots/mtp/mtp.c
index 025116d266..e9a5f5cbb3 100755
--- a/robots/mtp/mtp.c
+++ b/robots/mtp/mtp.c
@@ -474,6 +474,10 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->data.mtp_payload_u.command_goto.position.x =
va_arg(args, double);
break;
+ case MTP_CONFIG_VMC_CLIENT:
+ mp->data.mtp_payload_u.config_vmc_client.fixed_x =
+ va_arg(args, double);
+ break;
default:
assert(0);
break;
@@ -493,6 +497,10 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->data.mtp_payload_u.command_goto.position.y =
va_arg(args, double);
break;
+ case MTP_CONFIG_VMC_CLIENT:
+ mp->data.mtp_payload_u.config_vmc_client.fixed_y =
+ va_arg(args, double);
+ break;
default:
assert(0);
break;
@@ -591,6 +599,17 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->data.mtp_payload_u.contact_report.points[lpc] = cp[lpc];
}
break;
+ case MA_Speed:
+ switch (mp->data.opcode) {
+ case MTP_COMMAND_GOTO:
+ mp->data.mtp_payload_u.command_goto.speed =
+ va_arg(args, double);
+ break;
+ default:
+ assert(0);
+ break;
+ }
+ break;
}
tag = va_arg(args, mtp_tag_t);
@@ -905,13 +924,28 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
struct mtp_config_vmc *mcv = &mp->data.mtp_payload_u.config_vmc;
fprintf(file,
- " camera[%d]:\t%s:%d\n",
+ " camera[%d]:\t%s:%d %.2f %.2f %.2f %.2f -- %.2f %.2f\n",
lpc,
mcv->cameras.cameras_val[lpc].hostname,
- mcv->cameras.cameras_val[lpc].port);
+ mcv->cameras.cameras_val[lpc].port,
+ mcv->cameras.cameras_val[lpc].x,
+ mcv->cameras.cameras_val[lpc].y,
+ mcv->cameras.cameras_val[lpc].width,
+ mcv->cameras.cameras_val[lpc].height,
+ mcv->cameras.cameras_val[lpc].fixed_x,
+ mcv->cameras.cameras_val[lpc].fixed_y);
}
break;
+ case MTP_CONFIG_VMC_CLIENT:
+ fprintf(file,
+ " opcode:\tconfig-vmc-client\n"
+ " fixed_x:\t\t%.2f\n"
+ " fixed_y:\t\t%.2f\n",
+ mp->data.mtp_payload_u.config_vmc_client.fixed_x,
+ mp->data.mtp_payload_u.config_vmc_client.fixed_y);
+ break;
+
case MTP_CONFIG_RMC:
fprintf(file,
" opcode:\tconfig-rmc\n"
@@ -1019,13 +1053,15 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
" x:\t\t%f\n"
" y:\t\t%f\n"
" theta:\t%f\n"
- " timestamp:\t%f\n",
+ " timestamp:\t%f\n"
+ " speed:\t%.2f\n",
mp->data.mtp_payload_u.command_goto.command_id,
mp->data.mtp_payload_u.command_goto.robot_id,
mp->data.mtp_payload_u.command_goto.position.x,
mp->data.mtp_payload_u.command_goto.position.y,
mp->data.mtp_payload_u.command_goto.position.theta,
- mp->data.mtp_payload_u.command_goto.position.timestamp);
+ mp->data.mtp_payload_u.command_goto.position.timestamp,
+ mp->data.mtp_payload_u.command_goto.speed);
break;
case MTP_COMMAND_STOP:
@@ -1070,7 +1106,8 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
" speed:\t\t%f\n"
" status:\t\t%d\n"
" user_button:\t\t%d\n"
- " user_led:\t\t%d\n",
+ " user_led:\t\t%d\n"
+ " stall_contact:\t\t%d\n",
mgt->battery_level,
mgt->battery_voltage,
mgt->battery_misses,
@@ -1096,7 +1133,8 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
mgt->speed,
mgt->status,
mgt->user_button,
- mgt->user_led);
+ mgt->user_led,
+ mgt->stall_contact);
break;
}
break;
diff --git a/robots/mtp/mtp.h b/robots/mtp/mtp.h
index f20e5492ca..95d234e148 100755
--- a/robots/mtp/mtp.h
+++ b/robots/mtp/mtp.h
@@ -149,6 +149,7 @@ typedef enum {
MA_WiggleType, /*< (mtp_wiggle_t) */
MA_ContactPointCount, /*< (int) */
MA_ContactPoints, /*< (contact_point *) */
+ MA_Speed, /*< (float) */
MA_TAG_MAX
} mtp_tag_t;
diff --git a/robots/mtp/mtp.x b/robots/mtp/mtp.x
index 8febc49ad7..a7d57dd7eb 100644
--- a/robots/mtp/mtp.x
+++ b/robots/mtp/mtp.x
@@ -64,6 +64,11 @@ enum mtp_opcode_t {
*/
MTP_CONFIG_RMC = 21,
+ /**
+ * VMC client configuration packet.
+ */
+ MTP_CONFIG_VMC_CLIENT = 22,
+
/**
* Request the current position of a given robot.
@@ -172,6 +177,8 @@ struct camera_config {
float y;
float width;
float height;
+ float fixed_x;
+ float fixed_y;
};
struct obstacle_config {
@@ -202,6 +209,11 @@ struct mtp_config_vmc {
camera_config cameras<>;
};
+struct mtp_config_vmc_client {
+ float fixed_x;
+ float fixed_y;
+};
+
struct mtp_request_position {
int robot_id;
};
@@ -234,6 +246,7 @@ struct mtp_command_goto {
int command_id;
int robot_id;
robot_position position;
+ float speed;
};
struct mtp_command_stop {
@@ -324,6 +337,7 @@ union mtp_payload switch (mtp_opcode_t opcode) {
case MTP_CONTROL_CLOSE: mtp_control close;
case MTP_CONFIG_RMC: mtp_config_rmc config_rmc;
case MTP_CONFIG_VMC: mtp_config_vmc config_vmc;
+ case MTP_CONFIG_VMC_CLIENT: mtp_config_vmc_client config_vmc_client;
case MTP_REQUEST_POSITION: mtp_request_position request_position;
case MTP_REQUEST_ID: mtp_request_id request_id;
case MTP_UPDATE_POSITION: mtp_update_position update_position;
diff --git a/robots/vmcd/visionTrack.c b/robots/vmcd/visionTrack.c
index 79b45fddd9..281134f81c 100644
--- a/robots/vmcd/visionTrack.c
+++ b/robots/vmcd/visionTrack.c
@@ -1,3 +1,8 @@
+/*
+ * EMULAB-COPYRIGHT
+ * Copyright (c) 2005 University of Utah and the Flux Group.
+ * All rights reserved.
+ */
#include "config.h"
@@ -26,6 +31,7 @@ static float curvy(float x)
* will continue the search until it reaches the end of the list.
* @param distance_out Reference to a float where the minimum distance found
* should be stored.
+ * @return The closest track found to the given track.
*/
static struct vision_track *vtFindMin(struct vision_track *vt,
struct vision_track *curr,
@@ -46,14 +52,6 @@ static struct vision_track *vtFindMin(struct vision_track *vt,
distance = hypotf(vt->vt_position.x - curr->vt_position.x,
vt->vt_position.y - curr->vt_position.y);
-#if 0
- printf(" min %f %f - %f %f = %f\n",
- vt->vt_position.x,
- vt->vt_position.y,
- curr->vt_position.x,
- curr->vt_position.y,
- distance);
-#endif
if (distance < *distance_out) {
retval = curr;
*distance_out = distance;
@@ -61,32 +59,86 @@ static struct vision_track *vtFindMin(struct vision_track *vt,
curr = (struct vision_track *)curr->vt_link.ln_Succ;
}
+
+ assert((retval == NULL) || (*distance_out != FLT_MAX));
+
+ return retval;
+}
+
+/**
+ * Extract tracks from a frame that are near the given track.
+ *
+ * @param list_out The list of tracks found to be within the distance
+ * tolerance.
+ * @param vt The track to compare other tracks against.
+ * @param curr The track in a list where the search should start. The function
+ * will continue the search until it reaches the end of the list.
+ * @param tolerance The maximum distance that other tracks have to be within
+ * before they are considered "near."
+ * @return The number of tracks in "list_out."
+ */
+static int vtExtractNear(struct lnMinList *list_out,
+ struct vision_track *vt,
+ struct vision_track *curr,
+ float tolerance)
+{
+ int retval = 0;
+ assert(list_out != NULL);
+ assert(vt != NULL);
+ assert(tolerance > 0.0);
+
+ if (curr == NULL)
+ return retval;
+
+ while (curr->vt_link.ln_Succ != NULL) {
+ struct vision_track *vt_next;
+ float distance;
+
+ vt_next = (struct vision_track *)curr->vt_link.ln_Succ;
+
+ distance = hypotf(vt->vt_position.x - curr->vt_position.x,
+ vt->vt_position.y - curr->vt_position.y);
+ if (distance <= tolerance) {
+ lnRemove(&curr->vt_link);
+ lnAddTail(list_out, &curr->vt_link);
+ retval += 1;
+ }
+
+ curr = vt_next;
+ }
+
+ assert(lnEmptyList(list_out) || (retval > 0));
+
return retval;
}
+/**
+ * Scan a frame for the next set of tracks that are in a different camera.
+ *
+ * @param vt The track in a list where the search should start.
+ * @return The first vision track in the remainder of the list that was
+ * detected by a different camera than the given track or NULL if there are no
+ * more tracks in different cameras.
+ */
static struct vision_track *vtNextCamera(struct vision_track *vt)
{
struct vision_track *retval = NULL;
struct vmc_client *vc;
+ assert(vt != NULL);
+
vc = vt->vt_client;
- while (vt->vt_link.ln_Succ != NULL && vt->vt_client == vc) {
+ while ((vt->vt_link.ln_Succ != NULL) && (vt->vt_client == vc)) {
vt = (struct vision_track *)vt->vt_link.ln_Succ;
}
- if (vt->vt_link.ln_Succ != NULL) {
+ if (vt->vt_link.ln_Succ != NULL)
retval = vt;
- }
return retval;
}
-#ifdef RECORD_FRAMES
-static FILE *xdat_file, *ydat_file, *tdat_file;
-static unsigned int frame_count;
-#endif
-
int vtUpdate(struct lnMinList *now,
struct vmc_client *vc,
struct mtp_packet *mp,
@@ -97,21 +149,8 @@ int vtUpdate(struct lnMinList *now,
assert(now != NULL);
assert(mp != NULL);
-#ifdef RECORD_FRAMES
- if (xdat_file == NULL) {
- xdat_file = fopen("xframes.dat", "w");
- ydat_file = fopen("yframes.dat", "w");
- tdat_file = fopen("tframes.dat", "w");
- }
-#endif
-
switch (mp->data.opcode) {
case MTP_CONTROL_ERROR:
-#if 0
- fprintf(dat_file,
- "# vc %d -- empty\n",
- vc->vc_port);
-#endif
retval = 1;
break;
case MTP_UPDATE_POSITION:
@@ -129,7 +168,6 @@ int vtUpdate(struct lnMinList *now,
vt->vt_userdata = NULL;
lnAddTail(now, &vt->vt_link);
-#if 1
/* Adjust the cameras viewable area based on this track. */
if (mup->position.x < vc->vc_left)
vc->vc_left = mup->position.x;
@@ -139,15 +177,7 @@ int vtUpdate(struct lnMinList *now,
vc->vc_top = mup->position.y;
if (mup->position.y > vc->vc_bottom)
vc->vc_bottom = mup->position.y;
-#endif
-#if 0
- fprintf(dat_file,
- "# vc %d -- %.2f %.2f\n",
- vc->vc_port,
- mup->position.x,
- mup->position.y);
-#endif
retval = (mup->status == MTP_POSITION_STATUS_CYCLE_COMPLETE);
}
break;
@@ -169,13 +199,13 @@ void vtAgeTracks(struct lnMinList *now,
assert(old != NULL);
while ((vt = (struct vision_track *)lnRemHead(old)) != NULL) {
- if (vt->vt_age < 5) {
+ if (vt->vt_age < MAX_TRACK_AGE) {
float distance;
if ((vtFindMin(vt,
(struct vision_track *)now->lh_Head,
&distance) == NULL) ||
- (distance > 0.02)) {
+ (distance > MERGE_TOLERANCE)) {
vt->vt_age += 1;
lnAddHead(now, &vt->vt_link);
vt = NULL;
@@ -219,12 +249,6 @@ void vtCoalesce(struct lnMinList *extra,
assert(vc != NULL);
assert(vc_len > 0);
- // fprintf(dat_file, "# coalesce -- %d\n", lnEmptyList(now));
-
-#ifdef RECORD_FRAMES
- frame_count += 1;
-#endif
-
vt = (struct vision_track *)now->lh_Head;
while (vt->vt_link.ln_Succ != NULL) {
int in_camera_count = 0, lpc;
@@ -234,15 +258,6 @@ void vtCoalesce(struct lnMinList *extra,
* can coalesce them.
*/
for (lpc = 0; lpc < vc_len; lpc++) {
-#if 0
- printf("vc2 %f %f -- %f %f %f %f\n",
- vt->vt_position.x,
- vt->vt_position.y,
- vc[lpc].vc_left,
- vc[lpc].vc_right,
- vc[lpc].vc_top,
- vc[lpc].vc_bottom);
-#endif
if ((vt->vt_position.x >= vc[lpc].vc_left) &&
(vt->vt_position.x <= vc[lpc].vc_right) &&
(vt->vt_position.y >= vc[lpc].vc_top) &&
@@ -276,161 +291,45 @@ void vtCoalesce(struct lnMinList *extra,
*/
if (in_camera_count > 1) {
- struct vision_track *vt_extra;
- float distance;
-
- while ((in_camera_count > 1) &&
- ((vt_extra = vtFindMin(vt,
- (struct vision_track *)
- vtNextCamera(vt),
- &distance)) != NULL) &&
- (distance < 0.35)) {
- float curcam_dx, nextcam_dx, overlap, curweight, nextweight;
- struct robot_position rp;
+ struct lnMinList near;
+ int rc;
+
+ lnNewList(&near);
+ if ((rc = vtExtractNear(&near,
+ vt,
+ vtNextCamera(vt),
+ COALESCE_TOLERANCE)) > 0) {
+ struct robot_position rp = vt->vt_position;
+ struct vision_track *vt_near;
- if (debug > 1) {
- info("coalesce %.2f %.2f %.2f %d -- %.2f %.2f %.2f %d\n",
- vt->vt_position.x, vt->vt_position.y,
- vt->vt_position.theta, vt->vt_client->vc_port,
- vt_extra->vt_position.x, vt_extra->vt_position.y,
- vt->vt_position.theta, vt_extra->vt_client->vc_port);
+ if (debug > 2) {
+ printf("start %.2f %.2f\n", vt->vt_position.x,
+ vt->vt_position.y);
}
+ vt_near = (struct vision_track *)near.lh_Head;
+ while (vt_near->vt_link.ln_Succ != NULL) {
+ if (debug > 2) {
+ printf("merge %.2f %.2f\n", vt_near->vt_position.x,
+ vt_near->vt_position.y);
+ }
+ rp.x += vt_near->vt_position.x;
+ rp.y += vt_near->vt_position.y;
+ // rp.theta += vt_near->vt_position.theta;
+ vt->vt_age = min(vt->vt_age, vt_near->vt_age);
+
+ vt_near = (struct vision_track *)vt_near->vt_link.ln_Succ;
+ }
+ lnAppendList(extra, &near);
- //midpoint = (vt->vt_client->vc_right +
- // vt_extra->vt_client->vc_left) / 2.0f;
-
- //curcam_dx = midpoint - vt->vt_position.x;
- //nextcam_dx = vt_extra->vt_position.x - midpoint;
-
- overlap = vt->vt_client->vc_right -
- vt_extra->vt_client->vc_left;
- curcam_dx = vt->vt_client->vc_right - vt->vt_position.x;
- nextcam_dx = vt_extra->vt_position.x -
- vt_extra->vt_client->vc_left;
-
- curweight = curvy(1.0 - curcam_dx / overlap);
- nextweight = curvy(1.0 - nextcam_dx / overlap);
-
-#if 0
- info(" cwt %.2f / %.2f -> %.2f ~ %.2f -- %d\n",
- curcam_dx, overlap, curcam_dx / overlap, curweight,
- vt->vt_client->vc_port);
- info(" nwt %.2f / %.2f -> %.2f ~ %.2f -- %d\n",
- nextcam_dx, overlap, nextcam_dx / overlap, nextweight,
- vt_extra->vt_client->vc_port);
-#endif
-
- rp = vt->vt_position;
- rp.x = ((curweight * vt->vt_position.x) +
- (nextweight * vt_extra->vt_position.x)) /
- (curweight + nextweight);
- rp.y = ((curweight * vt->vt_position.y) +
- (nextweight * vt_extra->vt_position.y)) /
- (curweight + nextweight);
- rp.theta = ((curweight * vt->vt_position.theta) +
- (nextweight * vt_extra->vt_position.theta)) /
- (curweight + nextweight);
-
-#if 0
- info(" wa %.2f %.2f -> %.2f -- %.2f %.2f -> %.2f\n",
- vt->vt_position.x,
- vt_extra->vt_position.x,
- rp.x,
- vt->vt_position.y,
- vt_extra->vt_position.y,
- rp.y);
-#endif
-
-
-#ifdef RECORD_FRAMES
- fprintf(xdat_file,
- "%d %.2f %.2f # coalesce -- %.2f %.2f -- %d %d\n",
- frame_count,
- rp.x,
- (nextcam_dx > curcam_dx) ?
- vt_extra->vt_position.x : vt->vt_position.x,
- vt->vt_position.x,
- vt_extra->vt_position.x,
- vt->vt_client->vc_port,
- vt_extra->vt_client->vc_port);
-
- fprintf(ydat_file,
- "%d %.2f %.2f # coalesce -- %.2f %.2f -- %d %d\n",
- frame_count,
- rp.y,
- (nextcam_dx > curcam_dx) ?
- vt_extra->vt_position.y : vt->vt_position.y,
- vt->vt_position.y,
- vt_extra->vt_position.y,
- vt->vt_client->vc_port,
- vt_extra->vt_client->vc_port);
-
- fprintf(tdat_file,
- "%d %.2f %.2f # coalesce -- %.2f %.2f -- %d %d\n",
- frame_count,
- rp.theta,
- (nextcam_dx > curcam_dx) ?
- vt_extra->vt_position.theta : vt->vt_position.theta,
- vt->vt_position.theta,
- vt_extra->vt_position.theta,
- vt->vt_client->vc_port,
- vt_extra->vt_client->vc_port);
-#endif
-
+ rp.x /= rc + 1;
+ rp.y /= rc + 1;
+ if (debug > 2) {
+ printf("final %.2f %.2f\n", rp.x, rp.y);
+ }
+ // rp.theta /= rc;
vt->vt_position = rp;
- vt->vt_age = min(vt->vt_age, vt_extra->vt_age);
-
- lnRemove(&vt_extra->vt_link);
- lnAddHead(extra, &vt_extra->vt_link);
-
- in_camera_count -= 1;
- }
-
- if (in_camera_count > 1) {
-#ifdef RECORD_FRAMES
- fprintf(xdat_file,
- "%d %.2f %.2f # 2 cam %d\n",
- frame_count,
- vt->vt_position.x,
- vt->vt_position.x,
- vt->vt_client->vc_port);
- fprintf(ydat_file,
- "%d %.2f %.2f # 2 cam %d\n",
- frame_count,
- vt->vt_position.y,
- vt->vt_position.y,
- vt->vt_client->vc_port);
- fprintf(tdat_file,
- "%d %.2f %.2f # 2 cam %d\n",
- frame_count,
- vt->vt_position.theta,
- vt->vt_position.theta,
- vt->vt_client->vc_port);
-#endif
}
}
- else {
-#ifdef RECORD_FRAMES
- fprintf(xdat_file,
- "%d %.2f %.2f # %d\n",
- frame_count,
- vt->vt_position.x,
- vt->vt_position.x,
- vt->vt_client->vc_port);
- fprintf(ydat_file,
- "%d %.2f %.2f # %d\n",
- frame_count,
- vt->vt_position.y,
- vt->vt_position.y,
- vt->vt_client->vc_port);
- fprintf(tdat_file,
- "%d %.2f %.2f # %d\n",
- frame_count,
- vt->vt_position.theta,
- vt->vt_position.theta,
- vt->vt_client->vc_port);
-#endif
- }
vt = (struct vision_track *)vt->vt_link.ln_Succ;
}
@@ -454,15 +353,29 @@ void vtMatch(struct lnMinList *pool,
while (vt->vt_link.ln_Succ != NULL) {
struct vision_track *vt_prev;
float distance;
+ void dump_vision_list(struct lnMinList *list);
if ((vt_prev = vtFindMin(vt,
(struct vision_track *)prev->lh_Head,
&distance)) != NULL) {
- if (distance < 0.30) {
+ if (distance <= 0.06) {
vt->vt_userdata = vt_prev->vt_userdata;
lnRemove(&vt_prev->vt_link);
lnAddHead(pool, &vt_prev->vt_link);
}
+ else {
+#if 0
+ info("too far %.2f\n", distance);
+#endif
+ }
+ }
+ else {
+#if 0
+ info("no match %.2f %.2f\n", vt->vt_position.x, vt->vt_position.y);
+ dump_vision_list(now);
+ info("==\n");
+ dump_vision_list(prev);
+#endif
}
vt = (struct vision_track *)vt->vt_link.ln_Succ;
diff --git a/robots/vmcd/visionTrack.h b/robots/vmcd/visionTrack.h
index 3e0aa209ba..a939964a41 100644
--- a/robots/vmcd/visionTrack.h
+++ b/robots/vmcd/visionTrack.h
@@ -1,3 +1,8 @@
+/*
+ * EMULAB-COPYRIGHT
+ * Copyright (c) 2005 University of Utah and the Flux Group.
+ * All rights reserved.
+ */
#ifndef _vision_track_h
#define _vision_track_h
@@ -7,17 +12,39 @@
#include "vmcd.h"
+/**
+ * The maximum distance in meters for which tracks from different cameras will
+ * be considered the same and coalesced.
+ */
+#define COALESCE_TOLERANCE 0.075
+
+/**
+ * The maximum distance in meters for which a track is considered the same from
+ * one frame to the next.
+ */
+#define MERGE_TOLERANCE 0.03
+
+/**
+ * The maximum number of frames that a track can miss before it is considered
+ * lost.
+ */
+#define MAX_TRACK_AGE 5
+
+/**
+ * Structure used to manage fiducials detected by the vision system.
+ */
struct vision_track {
- struct lnMinNode vt_link;
- struct robot_position vt_position;
- struct vmc_client *vt_client;
- int vt_age;
- void *vt_userdata;
+ struct lnMinNode vt_link; /*< Linked list header. */
+ struct robot_position vt_position; /*< Fiducial position/orientation. */
+ struct vmc_client *vt_client; /*< Camera that detected the track. */
+ int vt_age; /*< Age of the track (lower=younger) */
+ void *vt_userdata; /*< Data attached to the track. */
};
/**
* Update the list of tracks for the current frame with the tracks from a given
- * vmc-client.
+ * vmc-client. The dimensions of the camera will also be adjusted to ensure
+ * the track's position falls within the bounds.
*
* @param now The track list for the current frame, any new tracks will be
* added to this.
@@ -32,15 +59,33 @@ int vtUpdate(struct lnMinList *now,
struct mtp_packet *mp,
struct lnMinList *pool);
+/**
+ * Move tracks from previous frames into the current frame if they are still
+ * young and missing from the current frame. Tracks that are too old or match
+ * a track in the current frame are returned to the pool.
+ *
+ * @param now The list of tracks in the current frame.
+ * @param old The list of tracks from the previous frame.
+ * @param pool The list to return dead tracks to.
+ */
void vtAgeTracks(struct lnMinList *now,
struct lnMinList *old,
struct lnMinList *pool);
+
+/**
+ * Copy tracks in the source frame to the destination frame.
+ *
+ * @param dst The list to add new tracks to.
+ * @param src The list of tracks to duplicate.
+ * @param pool The list of free nodes to draw from.
+ */
void vtCopyTracks(struct lnMinList *dst,
struct lnMinList *src,
struct lnMinList *pool);
/**
- * Coalesce tracks that are in an area where cameras are overlapping.
+ * Coalesce tracks that are in an area where cameras are overlapping. This
+ * function will produce a frame with the canonical set of tracks.
*
* @param extra The list to add any duplicate tracks found in the current
* frame.
diff --git a/robots/vmcd/vmc-client.c b/robots/vmcd/vmc-client.c
index 59b8c6517e..4a69fd195a 100644
--- a/robots/vmcd/vmc-client.c
+++ b/robots/vmcd/vmc-client.c
@@ -24,6 +24,8 @@
#include "mtp.h"
#include <math.h>
+#define MAX_CLIENT_COUNT 10
+
#if defined(HAVE_MEZZANINE)
# include "mezz.h"
#else
@@ -81,13 +83,9 @@ static volatile unsigned int mezz_frame_count = 0;
/**
* X offset from the real world X == 0 to our local camera X == 0.
- */
-static double x_offset = 0.0;
-
-/**
* Y offset from the real world Y == 0 to our local camera Y == 0.
*/
-static double y_offset = 0.0;
+static struct robot_position offsets;
static double z_offset = 0.0;
@@ -112,6 +110,7 @@ static void usage(void)
" -x offset\tx offset from real world x = 0 to our local x = 0\n"
" -y offset\ty offset from real world y = 0 to our local y = 0\n"
" -z offset\tz offset from\n"
+ " -o orientation\tcamera orientation\n"
#if !defined(HAVE_MEZZANINE)
" -f file\tFile to read simulated positions from.\n"
#endif
@@ -180,13 +179,17 @@ void radial_trans(struct robot_position *p_inout)
*/
void local2global_posit_trans(struct robot_position *p_inout)
{
- float old_x = p_inout->x;
+ struct robot_position rp;
+ float ct, st;
assert(p_inout != NULL);
-
- p_inout->x = p_inout->y + x_offset;
- p_inout->y = old_x + y_offset;
- //p_inout->theta -= M_PI_2;
+
+ ct = cosf(offsets.theta);
+ st = sinf(offsets.theta);
+ rp.x = ct * p_inout->x + st * -p_inout->y + offsets.x;
+ rp.y = ct * -p_inout->y + st * -p_inout->x + offsets.y;
+ rp.theta = mtp_theta(p_inout->theta + offsets.theta + M_PI_2);
+ *p_inout = rp;
}
/**
@@ -341,7 +344,7 @@ int main(int argc, char *argv[])
xdrrec_create(&xdr, 0, 0, NULL, NULL, mem_write);
- while ((c = getopt(argc, argv, "hdp:l:i:f:x:y:z:")) != -1) {
+ while ((c = getopt(argc, argv, "hdp:l:i:f:x:y:z:o:")) != -1) {
switch (c) {
case 'h':
usage();
@@ -373,14 +376,14 @@ int main(int argc, char *argv[])
#endif
break;
case 'x':
- if (sscanf(optarg, "%lf", &x_offset) != 1) {
+ if (sscanf(optarg, "%f", &offsets.x) != 1) {
error("error: -x option is not a number: %s\n", optarg);
usage();
exit(1);
}
break;
case 'y':
- if (sscanf(optarg, "%lf", &y_offset) != 1) {
+ if (sscanf(optarg, "%f", &offsets.y) != 1) {
error("error: -y option is not a number: %s\n", optarg);
usage();
exit(1);
@@ -393,6 +396,13 @@ int main(int argc, char *argv[])
exit(1);
}
break;
+ case 'o':
+ if (sscanf(optarg, "%f", &offsets.theta) != 1) {
+ error("error: -o option is not a number: %s\n", optarg);
+ usage();
+ exit(1);
+ }
+ break;
default:
break;
}
@@ -499,6 +509,7 @@ int main(int argc, char *argv[])
errorc("listen");
}
else {
+ mtp_handle_t mh[MAX_CLIENT_COUNT];
fd_set readfds, clientfds;
int last_mezz_frame = 0;
@@ -535,7 +546,13 @@ int main(int argc, char *argv[])
&slen)) == -1) {
errorc("accept");
}
- else {
+ else if (fd >= MAX_CLIENT_COUNT) {
+ error("too many clients connected\n");
+ }
+ else if ((mh[fd] = mtp_create_handle(fd)) == NULL) {
+ error("unable to allocate handle\n");
+ }
+ else {
if (debug) {
info("vmc-client: connect from %s:%d (fd=%d)\n",
inet_ntoa(peer_sin.sin_addr),
@@ -552,35 +569,44 @@ int main(int argc, char *argv[])
}
FD_SET(fd, &readfds);
FD_SET(fd, &clientfds);
+
+ fd = -1;
}
+
+ if (fd != -1)
+ close(fd);
}
/*
* We assume that if somebody connected to us tries to write
* us some data, they're screwing up; we're just a data source.
*/
- for (lpc = 0; lpc < FD_SETSIZE; lpc++) {
+ for (lpc = 0; lpc < MAX_CLIENT_COUNT; lpc++) {
if ((lpc != serv_sock) && FD_ISSET(lpc, &rreadyfds)) {
- if (debug) {
- char buffer[512];
- int rc;
-
- rc = read(lpc, buffer, sizeof(buffer));
- if (rc == 0) {
- info("vmc-client: fd %d disconnected\n", lpc);
- }
- else {
- info("vmc-client: fd %d send %d bytes -- "
- "disconnecting\n",
- lpc,
- rc);
+ struct mtp_packet mp;
+ int good = 0;
+
+ if (((rc = mtp_receive_packet(mh[lpc], &mp)) !=
+ MTP_PP_SUCCESS) ||
+ (mp.vers != MTP_VERSION)) {
+ error("invalid client %d %p\n", rc, mp);
+ }
+ else {
+ if (mp.data.opcode == MTP_CONFIG_VMC_CLIENT) {
+ offsets.x = mp.data.mtp_payload_u.
+ config_vmc_client.fixed_x;
+ offsets.y = mp.data.mtp_payload_u.
+ config_vmc_client.fixed_y;
}
+ good = 1;
}
-
- close(lpc);
- FD_CLR(lpc, &readfds);
- FD_CLR(lpc, &clientfds);
- }
+
+ if (!good) {
+ close(lpc);
+ FD_CLR(lpc, &readfds);
+ FD_CLR(lpc, &clientfds);
+ }
+ }
}
}
else if (rc == -1) {
diff --git a/robots/vmcd/vmcd.c b/robots/vmcd/vmcd.c
index 15c0c42185..99ce9f7215 100644
--- a/robots/vmcd/vmcd.c
+++ b/robots/vmcd/vmcd.c
@@ -1,3 +1,10 @@
+/*
+ * EMULAB-COPYRIGHT
+ * Copyright (c) 2005 University of Utah and the Flux Group.
+ * All rights reserved.
+ */
+
+#include "config.h"
#include <math.h>
#include <float.h>
@@ -178,7 +185,7 @@ static void dump_vision_track(struct vision_track *vt)
((struct robot_object *)vt->vt_userdata)->ro_name);
}
-static void dump_vision_list(struct lnMinList *list)
+ void dump_vision_list(struct lnMinList *list)
{
struct vision_track *vt;
@@ -462,6 +469,18 @@ int main(int argc, char *argv[])
NULL)) == NULL) {
pfatal("mtp_create_handle");
}
+ else {
+ struct mtp_packet mp;
+
+ mtp_init_packet(&mp,
+ MA_Opcode, MTP_CONFIG_VMC_CLIENT,
+ MA_Role, MTP_ROLE_VMC,
+ MA_X, vmc_config.cameras.cameras_val[lpc].fixed_x,
+ MA_Y, vmc_config.cameras.cameras_val[lpc].fixed_y,
+ MA_TAG_DONE);
+ if (mtp_send_packet(vc->vc_handle, &mp) != MTP_PP_SUCCESS)
+ pfatal("could not configure vmc-client");
+ }
}
/*
@@ -511,8 +530,7 @@ int main(int argc, char *argv[])
#endif
if (current_client == vmc_client_count) {
- struct vision_track *vt, *vt_unknown = NULL;
- int unknown_track_count = 0;
+ struct vision_track *vt;
struct robot_object *ro;
/* We've got all of the camera tracks, start processing. */
@@ -523,10 +541,14 @@ int main(int argc, char *argv[])
vmc_clients,
vmc_client_count); // Get rid of duplicates
- /*
- * Match the current frame to the last frame and throw any old
- * frames (vt_age > 5) in the pool.
+ /**
+ * Remove any unknown tracks before doing the match so that they
+ * are not candidates for matching against tracks in the current
+ * frame.
*/
+ vtUnknownTracks(&vt_pool, &last_frame);
+
+ /* Match the current frame to the last frame. */
vtMatch(&vt_pool, &last_frame, ¤t_frame);
/* Reset the list of known/unknown bots. */
@@ -553,10 +575,6 @@ int main(int argc, char *argv[])
MA_TAG_DONE);
mtp_send_packet(emc_handle, &ump);
}
- else {
- unknown_track_count += 1;
- vt_unknown = vt;
- }
if (debug > 2) {
printf("track %f %f %f - %d - %p %s\n",
@@ -731,6 +749,11 @@ int main(int argc, char *argv[])
*/
if ((vt = vtFindWiggle(&wiggle_frame,
&last_frame)) == NULL) {
+ if (wiggle_bot == NULL) {
+ lnAppendList(&vt_pool, &wiggle_frame);
+ break;
+ }
+
/*
* Couldn't locate the robot, add him to
* the list of "lost" robots which we'll
--
GitLab