Commit 30b6f8f9 authored by Leigh B. Stoller's avatar Leigh B. Stoller
Browse files

Have the locpiper save dynamic obstacles (internally) so that when a

new client connects, we can dump the current set to the applet. I'm
still holding off on putting them into the DB (really do not think
this is the right thing to do, but I can be convinced otherwise).

Change battery code in the locpiper so that if the data for a robot is
more then 10 minutes old, stop sending updates for that robot.

For the applet, add a little daemon thread that checks to see if
battery updates have stopped, and if so, change the color of the robot
to blue (cold and stale). This makes it more clear which robots are
working and which are turned off.

Fix collision detection code so that we test for the center of a robot
overlapping with an obstacle, rather then radius of the area it sweeps
(Tim said this was the correct thing to do).
parent dcd8d01f
......@@ -18,7 +18,7 @@ use Time::HiRes qw(gettimeofday);
sub usage()
{
print(STDOUT
"Usage: locpiper [-d] [-k] [-n] [-f] pid eid\n" .
"Usage: locpiper [-d] [-v] [-k] [-n] [-f] pid eid\n" .
"switches and arguments:\n".
"-k - Kill running locpiper for experiment pid/eid (swapout)\n".
"-d - Debug mode, turns on lots of output.\n".
......@@ -28,7 +28,7 @@ sub usage()
"<eid> - The experiment name (id)\n");
exit(-1);
}
my $optlist = "dkfn";
my $optlist = "dvkfn";
my $debug = 0;
my $impotent = 0;
my $daemon = 1;
......@@ -65,6 +65,7 @@ $libdb::DBQUERY_MAXTRIES = 0;
my $logfile;
my %nodeids = (); # Map vnames to nodeid to avoid lookup later.
my %clients = ();
my %obstacles = ();
my $PPM = 100.0; # XXX Pixels Per Meter.
my $loop_count = 0;
my $query_result;
......@@ -83,6 +84,9 @@ if (! getopts($optlist, \%options)) {
if (defined($options{"d"})) {
$debug++;
}
if (defined($options{"v"})) {
$debug++;
}
if (defined($options{"n"})) {
$impotent = 1;
}
......@@ -249,7 +253,7 @@ if (!event_subscribe($handle, \&callbackFunc, $tuple)) {
#
$sock = IO::Socket::INET->new(Listen => 10,
LocalAddr => 'localhost',
LocalPort => 9006,
LocalPort => 9005,
Reuse => 1,
Proto => 'tcp');
fatal("Could not create socket!")
......@@ -279,8 +283,10 @@ while (1) {
# We need to *something* to brand new connections to keep the client
# from timing out (happens on my desktop).
#
SendBatteryData()
if ($newconn);
if ($newconn) {
SendBatteryData(1);
SendDynObstacles();
}
event_poll_blocking($handle, 1000);
......@@ -292,7 +298,7 @@ while (1) {
#
$loop_count++;
if ($loop_count > 60) {
SendBatteryData();
SendBatteryData(0);
$loop_count = 0;
}
}
......@@ -321,6 +327,7 @@ sub callbackFunc($$$) {
if ($objtype eq "TOPOGRAPHY") {
my $args = event_notification_get_arguments($handle, $notification);
my @sets = ();
my @event = ();
my $id;
......@@ -355,6 +362,16 @@ sub callbackFunc($$$) {
if (@event);
ForwardEvent("$seconds:$microseconds", $evstr);
#
# Store the obstacle info for later (to give to new clients)
#
if ($evtype eq "CREATE" || $evtype eq "MODIFY") {
$obstacles{"$id"} = $evstr;
}
elsif ($evtype eq "CLEAR") {
delete($obstacles{"$id"});
}
return;
}
......@@ -469,11 +486,15 @@ sub callbackFunc($$$) {
#
# Send battery data.
#
sub SendBatteryData()
sub SendBatteryData($)
{
my ($nostore) = @_;
my $query_result =
DBQueryWarn("select r.node_id,n.battery_voltage, ".
" n.battery_percentage ".
" n.battery_percentage, ".
" UNIX_TIMESTAMP(now()) - n.battery_timestamp ".
" as age ".
" from reserved as r ".
"left join nodes as n on n.node_id=r.node_id ".
"where ".
......@@ -490,21 +511,41 @@ sub SendBatteryData()
}
$seconds -= $starttime;
while (my ($nodeid,$voltage,$percentage) =
while (my ($nodeid,$voltage,$percentage,$age) =
$query_result->fetchrow_array()) {
#
# If the battery data is more then 10 minutes old, do not
# send it; its most certainly stale and not worth anything.
#
next
if ($age > (10 * 60));
ForwardEvent("$seconds:$microseconds",
"TYPE=NODE,ID=$nodeid,BATV=$voltage,BAT%=$percentage");
"TYPE=NODE,ID=$nodeid,BATV=$voltage,BAT%=$percentage",
$nostore);
}
}
}
#
# Send dynamic obstacles
#
sub SendDynObstacles()
{
foreach my $id (keys(%obstacles)) {
my $evstr = $obstacles{"$id"};
ForwardEvent("", $evstr, 1);
}
}
#
# Send the translated event to all of the clients that are listening.
#
sub ForwardEvent($$)
sub ForwardEvent($$;$)
{
my ($stamp, $event) = @_;
my ($stamp, $event, $nostore) = @_;
print "Forwarding event: '$stamp $event'\n"
if ($debug > 1);
......@@ -522,7 +563,8 @@ sub ForwardEvent($$)
#
# Now dump it to the file.
#
print EVFILE "$stamp $event\n";
print EVFILE "$stamp $event\n"
if (!defined($nostore) || $nostore == 0);
}
#
......
......@@ -379,7 +379,7 @@ public class RoboTrack extends JApplet {
private BufferedImage scalebar_bimg;
private class Map extends JPanel implements Runnable {
private Thread thread;
private Thread thread, daemon;
public Map() {
/*
......@@ -562,6 +562,11 @@ public class RoboTrack extends JApplet {
// Store these as strings for easy display.
robbie.battery_voltage =
FORMATTER.format(Float.parseFloat(val));
// This causes a robot to look alive.
robbie.last_update = System.currentTimeMillis();
now.setTime(robbie.last_update);
robbie.update_string = TIME_FORMAT.format(now);
}
else if (key.equals("BAT%")) {
// Store these as strings for easy display.
......@@ -569,9 +574,6 @@ public class RoboTrack extends JApplet {
FORMATTER.format(Float.parseFloat(val));
}
}
robbie.last_update = System.currentTimeMillis();
now.setTime(robbie.last_update);
robbie.update_string = TIME_FORMAT.format(now);
}
/*
......@@ -713,14 +715,18 @@ public class RoboTrack extends JApplet {
if (!robbie.dragging)
continue;
// Bounding box.
int rx1 = robbie.drag_x - (robbie.radius);
int ry1 = robbie.drag_y - (robbie.radius);
int rx2 = robbie.drag_x + (robbie.radius);
int ry2 = robbie.drag_y + (robbie.radius);
/*
* Tim says that we only want to look to see if the
* center of the circle the robot sweeps, overlaps
* with an exclusion zone around an obstacle.
*/
int rx1 = robbie.drag_x;
int ry1 = robbie.drag_y;
int rx2 = robbie.drag_x;
int ry2 = robbie.drag_y;
//System.out.println("CheckforObstacles: " + rx1 + "," +
// ry1 + "," + rx2 + "," + ry2);
System.out.println("CheckforObstacles: " + rx1 + "," +
ry1 + "," + rx2 + "," + ry2);
/*
* Check for overlap of this robot with each obstacle.
......@@ -760,8 +766,8 @@ public class RoboTrack extends JApplet {
int ox2 = obstacle.x2 + OBSTACLE_BUFFER;
int oy2 = obstacle.y2 + OBSTACLE_BUFFER;
//System.out.println(" " + ox1 + "," +
// oy1 + "," + ox2 + "," + oy2);
System.out.println(" " + ox1 + "," +
oy1 + "," + ox2 + "," + oy2);
if (! (oy2 < ry1 ||
ry2 < oy1 ||
......@@ -930,13 +936,17 @@ public class RoboTrack extends JApplet {
public void drawRobot(Graphics2D g2,
int x, int y, double or, String label,
int dot_radius, int which) {
int dot_radius, int which, boolean stale) {
/*
* An allocated robot is a filled circle.
* A destination is an unfilled circle. So is a dragging robot.
*/
if (which == DRAWROBOT_LOC) {
g2.setColor(Color.green);
if (stale)
g2.setColor(Color.blue);
else
g2.setColor(Color.green);
g2.fillOval(x - dot_radius, y - dot_radius,
dot_radius * 2, dot_radius * 2);
}
......@@ -1077,9 +1087,10 @@ public class RoboTrack extends JApplet {
int x = robbie.x;
int y = robbie.y;
boolean stale = (robbie.last_update == 0);
drawRobot(g2, x, y, robbie.or, robbie.pname,
(int) (robbie.size/2), DRAWROBOT_LOC);
(int) (robbie.size/2), DRAWROBOT_LOC, stale);
/*
* Okay, if the robot has a destination, draw that too
......@@ -1090,7 +1101,7 @@ public class RoboTrack extends JApplet {
int dy = robbie.dy;
drawRobot(g2, dx, dy, robbie.dor, robbie.pname,
robbie.size/2, DRAWROBOT_DST);
robbie.size/2, DRAWROBOT_DST, false);
/*
* And draw a light grey line from source to dest.
......@@ -1103,7 +1114,7 @@ public class RoboTrack extends JApplet {
int dy = robbie.drag_y;
drawRobot(g2, dx, dy, robbie.drag_or, robbie.pname,
robbie.size/2, DRAWROBOT_DRAG);
robbie.size/2, DRAWROBOT_DRAG, false);
/*
* And draw a red line from source to drag.
......@@ -1133,18 +1144,58 @@ public class RoboTrack extends JApplet {
}
public void start() {
daemon = new Thread(this);
daemon.start();
thread = new Thread(this);
thread.start();
}
public synchronized void stop() {
thread = null;
daemon = null;
}
public void Daemon() {
Thread me = Thread.currentThread();
System.out.println("Daemon Starting");
while (me == daemon) {
try {
me.sleep(1000);
}
catch (InterruptedException e)
{
break;
}
long now = System.currentTimeMillis();
Enumeration e = robots.elements();
while (e.hasMoreElements()) {
Robot robbie = (Robot)e.nextElement();
if (robbie.last_update == 0)
continue;
if (now - robbie.last_update >= 10000) {
System.out.println(robbie.pname + " has gone stale");
robbie.last_update = 0;
}
}
repaint();
maptable.repaint(10);
}
System.out.println("Daemon Stopping");
}
public void run() {
Thread me = Thread.currentThread();
String str;
long start_time = System.currentTimeMillis();
// See if we are the daemon thread.
if (me == daemon) {
Daemon();
return;
}
if (! shelled) {
System.out.println("Opening URL: " + robopipeurl.toString());
......@@ -1170,11 +1221,6 @@ public class RoboTrack extends JApplet {
try
{
while (null != ((str = input.readLine()))) {
long now = System.currentTimeMillis();
long diff = (now - start_time) / 1000;
//System.out.println("" + diff);
parseEvent(str);
repaint();
maptable.repaint(10);
......@@ -1188,7 +1234,7 @@ public class RoboTrack extends JApplet {
break;
}
try {
thread.sleep(1000);
me.sleep(1000);
}
catch (InterruptedException e)
{
......
......@@ -83,7 +83,7 @@ function SPEWCLEANUP()
set_time_limit(0);
register_shutdown_function("SPEWCLEANUP");
$socket = fsockopen("localhost", 9006);
$socket = fsockopen("localhost", 9005);
if (!$socket) {
TBERROR("Error opening locpiper socket - $errstr",1);
}
......
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