Commit 959742d6 authored by Leigh B. Stoller's avatar Leigh B. Stoller

Checkpoint new dynamic obstacle stuff that Tim requested. This is not

well tested, but will get tested now!

The gist is that Tim now generates TOPOGRAPHY events that define an
dynamic obstacle that is drawn in the applet and added to the list of
obstacles to be checked for collisons (when submitting moves).
parent e84b8467
......@@ -18,13 +18,18 @@ use Time::HiRes qw(gettimeofday);
sub usage()
{
print(STDOUT
"Usage: locpiper [-d] [-k] pid eid\n" .
"Usage: locpiper [-d] [-k] [-n] [-f] pid eid\n" .
"switches and arguments:\n".
"-d - Debug mode, use to prevent daemonization\n");
"-k - Kill running locpiper for experiment pid/eid (swapout)\n".
"-d - Debug mode, turns on lots of output.\n".
"-n - Impotent mode, do not update the DB as events arrive.\n".
"-f - Foreground mode, turns off daemonization.\n".
"<pid> - The project the experiment belongs to\n".
"<eid> - The experiment name (id)\n");
exit(-1);
}
my $optlist = "dk";
my $debug = 0;
my $optlist = "dkfn";
my $debug = 3;
my $impotent = 0;
my $daemon = 1;
my $killmode = 0;
......@@ -65,6 +70,7 @@ my $loop_count = 0;
my $query_result;
my $locpiperpid;
my $starttime = 0;
my $sock;
#
# Parse command arguments. Once we return from getopts, all that should be
......@@ -77,6 +83,12 @@ if (! getopts($optlist, \%options)) {
if (defined($options{"d"})) {
$debug++;
}
if (defined($options{"n"})) {
$impotent = 1;
}
if (defined($options{"f"})) {
$daemon = 0;
}
if (defined($options{"k"})) {
$killmode = 1;
}
......@@ -211,6 +223,14 @@ if (!event_subscribe($handle, \&callbackFunc, $tuple)) {
fatal("Could not subscribe to SETDEST,MODIFY events!");
}
%$tuple = ( expt => "$pid/$eid",
objtype => 'TOPOGRAPHY',
eventtype => 'CREATE,MODIFY,CLEAR');
if (!event_subscribe($handle, \&callbackFunc, $tuple)) {
fatal("Could not subscribe to TOPOGRAPHY events!");
}
%$tuple = ( expt => "$pid/$eid",
objtype => 'NODE',
eventtype => 'COMPLETE',
......@@ -227,24 +247,41 @@ if (!event_subscribe($handle, \&callbackFunc, $tuple)) {
# more adhoc. We set the main socket to non-blocking so that we can
# just query it for new connections.
#
my $sock = IO::Socket::INET->new(Listen => 10,
LocalAddr => 'localhost',
LocalPort => 9005,
Reuse => 1,
Proto => 'tcp');
$sock = IO::Socket::INET->new(Listen => 10,
LocalAddr => 'localhost',
LocalPort => 9006,
Reuse => 1,
Proto => 'tcp');
fatal("Could not create socket!")
if (!defined($sock));
DBQueryWarn("update experiments set ".
"locpiper_port='" . $sock->sockport() . "' ".
"where pid='$pid' and eid='$eid'")
or fatal("Cannot update DB with locpiper_port!");
fcntl($sock, F_SETFL, O_NONBLOCK)
or fatal("Cannot set non blocking on socket: $!");
while (1) {
my ($newsock,$peeraddr) = $sock->accept();
if (defined($newsock)) {
my $newsock;
my $newconn = 0;
while (($newsock = $sock->accept())) {
my $tag = $newsock->peerhost . ":" . $newsock->peerport();
print "Connection from $tag at " . TBTimeStamp() . "\n";
$clients{$tag} = $newsock;
$newsock->autoflush(1);
$newconn++;
}
#
# We need to *something* to brand new connections to keep the client
# from timing out (happens on my desktop).
#
SendBatteryData()
if ($newconn);
event_poll_blocking($handle, 1000);
#
......@@ -252,35 +289,10 @@ while (1) {
# and send them as an event. I might end up changing things so that
# tmcd generates a real event when these come in, but sending the
# battery data in real time is not quite so important.
#
#
$loop_count++;
if ($loop_count > 60) {
$query_result =
DBQueryWarn("select r.node_id,n.battery_voltage, ".
" n.battery_percentage ".
" from reserved as r ".
"left join nodes as n on n.node_id=r.node_id ".
"where ".
($debug ? "" :
" r.pid='$pid' and r.eid='$eid' and ") .
" n.battery_voltage is not NULL ");
if ($query_result) {
#
# Get a (relative) timestamp for the event.
#
my ($seconds, $microseconds) = gettimeofday();
if (! $starttime) {
$starttime = $seconds;
}
$seconds -= $starttime;
while (my ($nodeid,$voltage,$percentage) =
$query_result->fetchrow_array()) {
ForwardEvent($nodeid, "$seconds:$microseconds",
"BATV=$voltage,BAT%=$percentage");
}
}
SendBatteryData();
$loop_count = 0;
}
}
......@@ -291,17 +303,13 @@ while (1) {
sub callbackFunc($$$) {
my ($handle,$notification,$data) = @_;
my $vname = event_notification_get_objname($handle, $notification);
my $evtype = event_notification_get_eventtype($handle, $notification);
my $vname = event_notification_get_objname($handle, $notification);
my $objtype = event_notification_get_objtype($handle, $notification);
my $evtype = event_notification_get_eventtype($handle, $notification);
print "Got event for $vname\n"
print "Got event ($objtype,$evtype) for $vname\n"
if ($debug);
return
if (! exists($nodeids{$vname}));
my $nodeid = $nodeids{$vname};
#
# Get a (relative) timestamp for the event.
#
......@@ -311,6 +319,51 @@ sub callbackFunc($$$) {
}
$seconds -= $starttime;
if ($objtype eq "TOPOGRAPHY") {
my $args = event_notification_get_arguments($handle, $notification);
my @event = ();
my $id;
foreach my $keyval (split(' ', $args)) {
my ($key, $val) = split('=', $keyval);
return
if (! ($val =~ /^[-\w\.]+$/));
if ($key eq "ID" || $key eq "is") {
$id = $val;
}
elsif ($key eq "XMIN" || $key eq "xmin") {
$val = int($val * $PPM);
push(@event, "XMIN=$val");
}
elsif ($key eq "XMAX" || $key eq "xmax") {
$val = int($val * $PPM);
push(@event, "XMAX=$val");
}
elsif ($key eq "YMIN" || $key eq "ymin") {
$val = int($val * $PPM);
push(@event, "YMIN=$val");
}
elsif ($key eq "YMAX" || $key eq "ymax") {
$val = int($val * $PPM);
push(@event, "YMAX=$val");
}
}
my $evstr = "TYPE=AREA,ID=$id,ACTION=$evtype";
$evstr .= "," . join("," ,@event)
if (@event);
ForwardEvent("$seconds:$microseconds", $evstr);
return;
}
# Must be for a node then.
return
if (! exists($nodeids{$vname}));
my $nodeid = $nodeids{$vname};
if ($evtype eq "COMPLETE") {
DBQueryWarn("update nodes set ".
" destination_x=NULL,".
......@@ -319,8 +372,8 @@ sub callbackFunc($$$) {
"where node_id='$nodeid'")
if (!$impotent);
ForwardEvent($nodeid, "$seconds:$microseconds",
"DX=NULL,DY=NULL,DOR=NULL");
ForwardEvent("$seconds:$microseconds",
"TYPE=NODE,ID=$nodeid,DX=NULL,DY=NULL,DOR=NULL");
return;
}
if ($evtype eq "SETDEST") {
......@@ -367,7 +420,8 @@ sub callbackFunc($$$) {
"where node_id='$nodeid'")
if (!$impotent);
ForwardEvent($nodeid, "$seconds:$microseconds", join("," ,@event));
ForwardEvent("$seconds:$microseconds",
"TYPE=NODE,ID=$nodeid," . join("," ,@event));
return;
}
if ($evtype eq "MODIFY") {
......@@ -406,26 +460,61 @@ sub callbackFunc($$$) {
"where node_id='$nodeid'")
if (!$impotent);
ForwardEvent($nodeid, "$seconds:$microseconds", join("," ,@event));
ForwardEvent("$seconds:$microseconds",
"TYPE=NODE,ID=$nodeid," . join("," ,@event));
return;
}
}
#
# Send battery data.
#
sub SendBatteryData()
{
my $query_result =
DBQueryWarn("select r.node_id,n.battery_voltage, ".
" n.battery_percentage ".
" from reserved as r ".
"left join nodes as n on n.node_id=r.node_id ".
"where ".
($debug ? "" :
" r.pid='$pid' and r.eid='$eid' and ") .
" n.battery_voltage is not NULL ");
if ($query_result) {
#
# Get a (relative) timestamp for the event.
#
my ($seconds, $microseconds) = gettimeofday();
if (! $starttime) {
$starttime = $seconds;
}
$seconds -= $starttime;
while (my ($nodeid,$voltage,$percentage) =
$query_result->fetchrow_array()) {
ForwardEvent("$seconds:$microseconds",
"TYPE=NODE,ID=$nodeid,BATV=$voltage,BAT%=$percentage");
}
}
}
#
# Send the translated event to all of the clients that are listening.
#
sub ForwardEvent($$$)
sub ForwardEvent($$)
{
my ($nodeid, $stamp, $event) = @_;
$eventstr = "${nodeid} ${event}\n";
my ($stamp, $event) = @_;
print "Forwarding event: '$nodeid $stamp $event'\n"
print "Forwarding event: '$stamp $event'\n"
if ($debug > 1);
$event .= "\n";
foreach my $client (keys(%clients)) {
my $sock = $clients{$client};
if (! $sock->write($eventstr, length($eventstr))) {
if (! $sock->write($event, length($event))) {
print "Dropped $client at " . TBTimeStamp() . "\n";
delete($clients{$client});
}
......@@ -433,7 +522,7 @@ sub ForwardEvent($$$)
#
# Now dump it to the file.
#
print EVFILE "$stamp $nodeid $event\n";
print EVFILE "$stamp $event\n";
}
#
......
......@@ -347,8 +347,14 @@ public class RoboTrack extends JApplet {
int x1, y1; // Upper left x,y coords in pixels.
int x2, y2; // Lower right x,y coords in pixels.
String description;
boolean dynamic = false; // A dynamically created Obstacle.
Rectangle rectangle = null; // Only for dynamic obstacles.
Rectangle exclusion = null; // Ditto, for the grey area.
}
Vector Obstacles = new Vector(10, 10);
Dictionary ObDynMap = new Hashtable(); // Temp Obstacles only.
int ObCount = 0;
int OBSTACLE_BUFFER = 23; // XXX
/*
* A container for camera information.
......@@ -365,6 +371,10 @@ public class RoboTrack extends JApplet {
int FONT_WIDTH = 6;
Font OurFont = null;
// For exclusion zone.
Composite ExclusionComposite;
Stroke ExclusionStroke;
// A little bufferedimage to hold (cache) the scale bar.
private BufferedImage scalebar_bimg;
......@@ -383,6 +393,11 @@ public class RoboTrack extends JApplet {
OurFont = new Font("Arial", Font.PLAIN, 14);
createScale();
ExclusionComposite = AlphaComposite.
getInstance(AlphaComposite.SRC_OVER, 0.15f);
ExclusionStroke = new BasicStroke(1.5f);
}
/*
......@@ -403,30 +418,64 @@ public class RoboTrack extends JApplet {
/*
* Parse the data returned by the web server. This is bogus; we
* should create an XML representation of it!
* should create an XML representation of it, but thats a lot of
* extra overhead, and I anticipate a lot of events!
*/
public void parseRobot(String str) {
StringTokenizer tokens;
String nodeid;
Robot robbie;
int index, ch;
public void parseEvent(String str) {
StringTokenizer tokens;
String id, type, tmp, key;
int ch, delim;
System.out.println(str);
//
// nodeid X=1,Y=2, ...
// TYPE=XXX,ID=YYY,X=1,Y=2, ...
//
ch = str.indexOf(' ');
nodeid = str.substring(0, ch);
str = str.substring(ch+1);
tokens = new StringTokenizer(str, ",");
tmp = tokens.nextToken().trim();
delim = tmp.indexOf('=');
if (delim < 0)
return;
key = tmp.substring(0, delim);
type = tmp.substring(delim+1);
if (! key.equals("TYPE"))
return;
tmp = tokens.nextToken().trim();
delim = tmp.indexOf('=');
if (delim < 0)
return;
key = tmp.substring(0, delim);
id = tmp.substring(delim+1);
if (! key.equals("ID"))
return;
if (type.equals("NODE")) {
parseRobot(id, tokens);
}
else if (type.equals("AREA")) {
parseObstacle(id, tokens);
}
}
/*
* Parse a robot event.
*/
public void parseRobot(String nodeid, StringTokenizer tokens) {
Robot robbie;
int index;
if ((robbie = (Robot) robots.get(nodeid)) == null) {
// For testing from the shell.
if (!shelled)
return;
robbie = new Robot();
robbie = new Robot();
index = robotcount++;
robbie.index = index;
robbie.pname = nodeid;
......@@ -450,8 +499,9 @@ public class RoboTrack extends JApplet {
String tmp = tokens.nextToken().trim();
int delim = tmp.indexOf('=');
if (delim < 0)
if (delim < 0) {
continue;
}
String key = tmp.substring(0, delim);
String val = tmp.substring(delim+1);
......@@ -474,6 +524,10 @@ public class RoboTrack extends JApplet {
if (val.equals("NULL")) {
robbie.dx = 0;
robbie.dx_meters = "";
robbie.dy = 0;
robbie.dy_meters = "";
robbie.dor = 500.0;
robbie.dor_string = "";
robbie.gotdest = false;
}
else {
......@@ -520,6 +574,103 @@ public class RoboTrack extends JApplet {
robbie.update_string = TIME_FORMAT.format(now);
}
/*
* Parse an obstacle event. We create, destroy, and modify temp
* obstacles so that the user notices them.
*/
public void parseObstacle(String obid, StringTokenizer tokens) {
Obstacle oby;
int index;
String action = "";
int x1 = 0, y1 = 0, x2 = 0, y2 = 0, id;
id = Integer.parseInt(obid);
// We either know about it, or we do not ...
oby = (Obstacle) ObDynMap.get(obid);
while (tokens.hasMoreTokens()) {
String tmp = tokens.nextToken().trim();
int delim = tmp.indexOf('=');
if (delim < 0)
continue;
String key = tmp.substring(0, delim);
String val = tmp.substring(delim+1);
if (key.equals("ACTION")) {
action = val;
}
else if (key.equals("XMIN")) {
x1 = Integer.parseInt(val);
}
else if (key.equals("XMAX")) {
x2 = Integer.parseInt(val);
}
else if (key.equals("YMIN")) {
y1 = Integer.parseInt(val);
}
else if (key.equals("YMAX")) {
y2 = Integer.parseInt(val);
}
}
if (action.equals("CREATE")) {
// Is this (left over ID) going to happen? Remove old one.
// We do not want a partly initialized Obstacle in the list!
if (oby != null) {
ObDynMap.remove(obid);
}
// Must delay insert until the new Obstacle is initialized
oby = new Obstacle();
oby.id = id;
oby.x1 = x1;
oby.y1 = y1;
oby.x2 = x2;
oby.y2 = y2;
oby.description = "Dynamic Obstacle";
oby.dynamic = true;
oby.rectangle = new Rectangle(x1, y1, x2-x1, y2-y1);
oby.exclusion = new Rectangle(x1 - OBSTACLE_BUFFER,
y1 - OBSTACLE_BUFFER,
(x2 + OBSTACLE_BUFFER) -
(x1 - OBSTACLE_BUFFER),
(y2 + OBSTACLE_BUFFER) -
(y1 - OBSTACLE_BUFFER));
System.out.println("Creating Obstacle: " + id);
ObDynMap.put(obid, oby);
}
else if (action.equals("CLEAR")) {
if (oby != null) {
System.out.println("Clearing Obstacle: " + id);
ObDynMap.remove(obid);
}
}
else if (action.equals("MODIFY")) {
// Can this happen?
if (oby == null) {
return;
}
oby.x1 = x1;
oby.y1 = y1;
oby.x2 = x2;
oby.y2 = y2;
oby.rectangle = new Rectangle(x1, y1, x2-x1, y2-y1);
oby.exclusion = new Rectangle(x1 - OBSTACLE_BUFFER,
y1 - OBSTACLE_BUFFER,
(x2 + OBSTACLE_BUFFER) -
(x1 - OBSTACLE_BUFFER),
(y2 + OBSTACLE_BUFFER) -
(y1 - OBSTACLE_BUFFER));
System.out.println("Modifying Obstacle: " + id);
}
}
/*
* Given an x,y from a button click, try to map the coords
* to a robot that has been drawn on the screen. There are
......@@ -550,11 +701,9 @@ public class RoboTrack extends JApplet {
*
* XXX I am treating the robot as a square! Easier to calculate.
*
* XXX The value below (OBSTACLE_BUFFER) is hardwired in floormap
* XXX The value of OBSTACLE_BUFFER is hardwired in floormap
* code (where the base image is generated).
*/
int OBSTACLE_BUFFER = 23;
public boolean CheckforObstacles() {
Enumeration robot_enum = robots.elements();
......@@ -585,7 +734,7 @@ public class RoboTrack extends JApplet {
int oy2 = obstacle.y2 + OBSTACLE_BUFFER;
//System.out.println(" " + ox1 + "," +
// oy1 + "," + ox2 + "," + cy2);
// oy1 + "," + ox2 + "," + oy2);
if (! (oy2 < ry1 ||
ry2 < oy1 ||
......@@ -596,6 +745,35 @@ public class RoboTrack extends JApplet {
return true;
}
}
/*
* Check Dynamic obstacles also.
*/
if (ObDynMap.size() > 0) {
Enumeration e = ObDynMap.elements();
while (e.hasMoreElements()) {
Obstacle obstacle = (Obstacle)e.nextElement();
int ox1 = obstacle.x1 - OBSTACLE_BUFFER;
int oy1 = obstacle.y1 - OBSTACLE_BUFFER;
int ox2 = obstacle.x2 + OBSTACLE_BUFFER;
int oy2 = obstacle.y2 + OBSTACLE_BUFFER;
//System.out.println(" " + ox1 + "," +
// oy1 + "," + ox2 + "," + oy2);
if (! (oy2 < ry1 ||
ry2 < oy1 ||
ox2 < rx1 ||
rx2 < ox1)) {
MyDialog("Collision",
robbie.pname +
" overlaps with a dynamic obstacle");
return true;
}
}
}
}
return false;
}
......@@ -859,6 +1037,35 @@ public class RoboTrack extends JApplet {
g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING,
RenderingHints.VALUE_ANTIALIAS_ON);
/*
* Dyamnic obstacles first
*/
if (ObDynMap.size() > 0) {
Enumeration e = ObDynMap.elements();
Composite savedcomposite = g2.getComposite();
Stroke savedstroke = g2.getStroke();
g2.setPaint(Color.black);
g2.setComposite(ExclusionComposite);
g2.setStroke(ExclusionStroke);
g2.setColor(Color.blue);
while (e.hasMoreElements()) {
Obstacle oby = (Obstacle)e.nextElement();
Rectangle rectangle = oby.rectangle;
Rectangle exclusion = oby.exclusion;
if (rectangle != null) {
g2.draw(rectangle);
}
if (exclusion != null) {
g2.fill(exclusion);
}
}
g2.setComposite(savedcomposite);
g2.setStroke(savedstroke);
}
/*
* Then we draw a bunch of stuff on it, like the robots.
......@@ -968,7 +1175,7 @@ public class RoboTrack extends JApplet {
//System.out.println("" + diff);
parseRobot(str);
parseEvent(str);
repaint();
maptable.repaint(10);
if (thread == null)
...</