Revision: 6369
http://playerstage.svn.sourceforge.net/playerstage/?rev=6369&view=rev
Author: gbiggs
Date: 2008-04-20 17:52:31 -0700 (Sun, 20 Apr 2008)
Log Message:
-----------
Merging changes 6350:6367 from trunk
Modified Paths:
--------------
code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
code/player/branches/cmake/utils/playerlog2carmenlog.py
Modified:
code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
2008-04-21 00:49:39 UTC (rev 6368)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
2008-04-21 00:52:31 UTC (rev 6369)
@@ -249,7 +249,7 @@
if(!cell->plan_next)
{
- puts("never found start");
+ //puts("never found start");
return(-1);
}
else
Modified:
code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
2008-04-21 00:49:39 UTC (rev 6368)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
2008-04-21 00:52:31 UTC (rev 6369)
@@ -868,7 +868,7 @@
tdiff = MAX(0.0, this->cycletime - (currt-loopstart));
if(tdiff == 0.0)
PLAYER_WARN("Wavefront missed deadline and not sleeping; check machine
load");
- usleep((unsigned int)rint(tdiff));
+ usleep((unsigned int)rint(tdiff*1e6));
}
@@ -885,6 +885,7 @@
bool rotate_waypoint=false;
bool replan;
int rotate_dir=0;
+ bool printed_warning=false;
pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
@@ -1003,14 +1004,23 @@
this->localize_y, this->scan_maxrange) < 0))
{
if(!new_goal && (this->plan->path_count != 0))
- puts("Wavefront: local plan failed");
+ puts("Wavefront: local plan failed");
// Create a global plan
if(plan_do_global(this->plan, this->localize_x, this->localize_y,
this->target_x, this->target_y) < 0)
- puts("Wavefront: global plan failed");
+ {
+ if(!printed_warning)
+ {
+ puts("Wavefront: global plan failed");
+ printed_warning = true;
+ }
+ }
else
+ {
this->new_goal = false;
+ printed_warning = false;
+ }
}
if(this->graphics2d_id.interf && this->plan->lpath_count)
Modified: code/player/branches/cmake/utils/playerlog2carmenlog.py
===================================================================
--- code/player/branches/cmake/utils/playerlog2carmenlog.py 2008-04-21
00:49:39 UTC (rev 6368)
+++ code/player/branches/cmake/utils/playerlog2carmenlog.py 2008-04-21
00:52:31 UTC (rev 6369)
@@ -97,36 +97,77 @@
# we're also interested in laser scans
elif type == 'laser':
- if msgsubtype != 1:
- print 'Skipping laser message with subtype ' + `msgsubtype`
- continue
- header = 'FLASER'
- time = lsplit[0]
- min_angle = lsplit[8]
- max_angle = lsplit[9]
- if (min_angle != '-1.5708') or (max_angle != '+1.5708'):
- print 'Sorry, CARMEN requires 180-degree FOV for laser'
- sys.exit(-1)
- num_readings = int(lsplit[12])
- if (num_readings != 181) and (num_readings != 361):
- print 'Sorry, I can only convert 1-deg and 0.5-deg resolution logs'
- sys.exit(-1)
- i = 0
- scanstr = ''
- while i < (num_readings - 1):
- range = lsplit[13 + 2*i]
- scanstr += range + ' '
- i += num_readings/180
+ if msgsubtype == 1:
+ header = 'FLASER'
+ time = lsplit[0]
+ min_angle = lsplit[8]
+ max_angle = lsplit[9]
+ if (min_angle != '-1.5708') or (max_angle != '+1.5708'):
+ print 'Sorry, CARMEN requires 180-degree FOV for laser'
+ sys.exit(-1)
+ num_readings = int(lsplit[12])
+ if (num_readings != 181) and (num_readings != 361):
+ print 'Sorry, I can only convert 1-deg and 0.5-deg resolution logs'
+ sys.exit(-1)
+ i = 0
+ scanstr = ''
+ while i < (num_readings - 1):
+ range = lsplit[13 + 2*i]
+ scanstr += range + ' '
+ i += num_readings/180
+
+ l_x = x + offset_x*math.cos(theta)
+ l_y = y + offset_x*math.sin(theta)
- l_x = x + offset_x*math.cos(theta)
- l_y = y + offset_x*math.sin(theta)
+ outfile.write(header + ' 180 ' + scanstr +
+ str(l_x) + ' ' + str(l_y) + ' ' + str(theta) + ' ' +
+ str(x) + ' ' + str(y) + ' ' + str(theta) + ' ' +
+ time + ' ' + host + ' ' + time + '\n')
+ elif msgsubtype == 2:
+ # split the pose from the scan out as an ODOM message
+ header = 'ODOM'
+ time = lsplit[0]
+ x = float(lsplit[8])
+ y = float(lsplit[9])
+ theta = float(lsplit[10])
+ tv = '0.000'
+ rv = '0.000'
+ accel = '0.000'
+ outfile.write(header + ' ' + str(x) + ' ' + str(y) + ' ' + str(theta)
+ ' ' + tv + ' ' + rv + ' ' + accel + ' ' + time + ' ' + host + ' ' + time +
'\n')
- outfile.write(header + ' 180 ' + scanstr +
- str(l_x) + ' ' + str(l_y) + ' ' + str(theta) + ' ' +
- str(x) + ' ' + str(y) + ' ' + str(theta) + ' ' +
- time + ' ' + host + ' ' + time + '\n')
+ header = 'FLASER'
+ time = lsplit[0]
+ min_angle = lsplit[11]
+ max_angle = lsplit[12]
+ if (min_angle != '-1.5708') or (max_angle != '+1.5708'):
+ print 'Sorry, CARMEN requires 180-degree FOV for laser'
+ sys.exit(-1)
+ num_readings = int(lsplit[15])
+ if (num_readings != 181) and (num_readings != 361):
+ print 'Sorry, I can only convert 1-deg and 0.5-deg resolution logs'
+ sys.exit(-1)
+ i = 0
+ scanstr = ''
+ while i < (num_readings - 1):
+ range = lsplit[16 + 2*i]
+ scanstr += range + ' '
+ i += num_readings/180
+
+
+
+ l_x = x + offset_x*math.cos(theta)
+ l_y = y + offset_x*math.sin(theta)
+
+ outfile.write(header + ' 180 ' + scanstr +
+ str(l_x) + ' ' + str(l_y) + ' ' + str(theta) + ' ' +
+ str(x) + ' ' + str(y) + ' ' + str(theta) + ' ' +
+ time + ' ' + host + ' ' + time + '\n')
+
+ else:
+ print 'Skipping laser message with subtype ' + `msgsubtype`
+ continue
else:
#print 'Ignoring line of type ' + type
pass
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.net email is sponsored by the 2008 JavaOne(SM) Conference
Don't miss this year's exciting event. There's still time to save $100.
Use priority code J8TL2D2.
http://ad.doubleclick.net/clk;198757673;13503038;p?http://java.sun.com/javaone
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
