Revision: 6339
http://playerstage.svn.sourceforge.net/playerstage/?rev=6339&view=rev
Author: gerkey
Date: 2008-04-16 10:59:37 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
working on timing
Modified Paths:
--------------
code/player/trunk/server/drivers/planner/wavefront/Makefile.am
code/player/trunk/server/drivers/planner/wavefront/plan.c
code/player/trunk/server/drivers/planner/wavefront/plan_control.c
code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
Modified: code/player/trunk/server/drivers/planner/wavefront/Makefile.am
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/Makefile.am
2008-04-16 17:24:32 UTC (rev 6338)
+++ code/player/trunk/server/drivers/planner/wavefront/Makefile.am
2008-04-16 17:59:37 UTC (rev 6339)
@@ -3,7 +3,7 @@
noinst_LTLIBRARIES += libwavefront.la
endif
-AM_CPPFLAGS = -Wall -g -O3 -I$(top_srcdir) `pkg-config --cflags gdk-pixbuf-2.0`
+AM_CPPFLAGS = -Wall -g -I$(top_srcdir) `pkg-config --cflags gdk-pixbuf-2.0` -O3
libwavefront_la_SOURCES = plan.c plan_plan.c plan_waypoint.c plan.h \
plan_control.c wavefront.cc heap.h heap.c
Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-16
17:24:32 UTC (rev 6338)
+++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-16
17:59:37 UTC (rev 6339)
@@ -135,7 +135,7 @@
}
t1 = get_time();
- printf("plan_set_obstacles: %.6lf\n", t1-t0);
+ //printf("plan_set_obstacles: %.6lf\n", t1-t0);
}
void
Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan_control.c
2008-04-16 17:24:32 UTC (rev 6338)
+++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c
2008-04-16 17:59:37 UTC (rev 6339)
@@ -41,8 +41,8 @@
// Check whether the straight-line path is clear
if((cost = _plan_check_path(plan, cell, ncell)) < 0.0)
{
- printf("no path from (%d,%d) to (%d,%d)\n",
- cell->ci, cell->cj, ncell->ci, ncell->cj);
+ //printf("no path from (%d,%d) to (%d,%d)\n",
+ //cell->ci, cell->cj, ncell->ci, ncell->cj);
continue;
}
Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
2008-04-16 17:24:32 UTC (rev 6338)
+++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
2008-04-16 17:59:37 UTC (rev 6339)
@@ -67,7 +67,7 @@
t1 = get_time();
- printf("computed global path: %.6lf\n", t1-t0);
+ //printf("computed global path: %.6lf\n", t1-t0);
return(0);
}
@@ -136,7 +136,7 @@
t1 = get_time();
- printf("computed local path: %.6lf\n", t1-t0);
+ //printf("computed local path: %.6lf\n", t1-t0);
return(0);
}
Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2008-04-16 17:24:32 UTC (rev 6338)
+++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2008-04-16 17:59:37 UTC (rev 6339)
@@ -588,7 +588,7 @@
this->scans_count = MIN(this->scans_count,this->scans_size);
this->scans_idx = (this->scans_idx + 1) % this->scans_size;
- printf("%d scans\n", this->scans_count);
+ //printf("%d scans\n", this->scans_count);
// run through the scans to see how much room we need to store all the
// hitpoints
@@ -639,11 +639,11 @@
}
}
- printf("setting %d hit points\n", this->scan_points_count);
+ //printf("setting %d hit points\n", this->scan_points_count);
plan_set_obstacles(plan, this->scan_points, this->scan_points_count);
t1 = get_time();
- printf("ProcessLaserScan: %.6lf\n", t1-t0);
+ //printf("ProcessLaserScan: %.6lf\n", t1-t0);
if(this->graphics2d_id.interf)
{
@@ -857,6 +857,7 @@
bool rotate_waypoint=false;
bool replan;
int rotate_dir=0;
+ double m0=0.0,m1;
pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
@@ -867,17 +868,25 @@
for(;;)
{
+ GlobalTime->GetTimeDouble(&t);
+
+ m1 = get_time();
+ printf("loop time: %.6lf\n", m1-m0);
+ m0 = get_time();
+
pthread_testcancel();
ProcessMessages();
if(!this->have_map && !this->new_map_available)
{
- usleep(CYCLE_TIME_US);
+ double currt,tdiff;
+ GlobalTime->GetTimeDouble(&currt);
+ tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6);
+ usleep((unsigned int)rint(tdiff));
continue;
}
- GlobalTime->GetTimeDouble(&t);
if((t - last_publish_time) > 0.25)
{
@@ -888,7 +897,11 @@
if(!this->enable)
{
this->StopPosition();
- usleep(CYCLE_TIME_US);
+ double currt,tdiff;
+ GlobalTime->GetTimeDouble(&currt);
+ tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6);
+ printf("sleeping: %u\n", (unsigned int)rint(tdiff));
+ usleep((unsigned int)rint(tdiff));
continue;
}
@@ -1004,8 +1017,29 @@
free(line.points);
}
+ if(this->graphics2d_id.interf && this->plan->path_count)
+ {
+ player_graphics2d_cmd_polyline_t line;
+ line.points_count = this->plan->path_count;
+ line.points =
(player_point_2d_t*)malloc(sizeof(player_point_2d_t)*line.points_count);
+ line.color.alpha = 0;
+ line.color.red = 255;
+ line.color.green = 0;
+ line.color.blue = 0;
+ for(int i=0;i<this->plan->path_count;i++)
+ {
+ line.points[i].px = PLAN_WXGX(this->plan,this->plan->path[i]->ci);
+ line.points[i].py = PLAN_WYGY(this->plan,this->plan->path[i]->cj);
+ }
+ this->graphics2d->PutMsg(this->InQueue,
+ PLAYER_MSGTYPE_CMD,
+ PLAYER_GRAPHICS2D_CMD_POLYLINE,
+ (void*)&line,0,NULL);
+ free(line.points);
+ }
+
t1 = get_time();
- printf("planning: %.6lf\n", t1-t0);
+ //printf("planning: %.6lf\n", t1-t0);
if(!this->velocity_control)
{
@@ -1092,8 +1126,8 @@
double maxd=2.0;
double distweight=10.0;
- printf("pose: (%.3lf,%.3lf,%.3lf)\n",
- this->localize_x, this->localize_y, RTOD(this->localize_a));
+ //printf("pose: (%.3lf,%.3lf,%.3lf)\n",
+ //this->localize_x, this->localize_y, RTOD(this->localize_a));
if(plan_get_carrot(this->plan, &wx, &wy,
this->localize_x, this->localize_y,
maxd, distweight) < 0)
@@ -1143,6 +1177,10 @@
double amin = DTOR(5.0);
double amax = DTOR(20.0);
+ double goald = sqrt((this->localize_x-this->target_x)*
+ (this->localize_x-this->target_x) +
+ (this->localize_y-this->target_y)*
+ (this->localize_y-this->target_y));
double d = sqrt((this->localize_x-wx)*(this->localize_x-wx) +
(this->localize_y-wy)*(this->localize_y-wy));
double b = atan2(wy - this->localize_y, wx - this->localize_x);
@@ -1152,7 +1190,7 @@
double ad = angle_diff(b, this->localize_a);
// Are we on top of the goal?
- if(d < this->dist_eps)
+ if(goald < this->dist_eps)
{
if(!rotate_dir)
{
@@ -1190,7 +1228,7 @@
this->StopPosition();
t1 = get_time();
- printf("control: %.6lf\n", t1-t0);
+ //printf("control: %.6lf\n", t1-t0);
}
else // !velocity_control
{
@@ -1243,7 +1281,10 @@
// no more waypoints, so wait for target achievement
//puts("waiting for goal achievement");
- usleep(CYCLE_TIME_US);
+ double currt,tdiff;
+ GlobalTime->GetTimeDouble(&currt);
+ tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6);
+ usleep((unsigned int)rint(tdiff));
continue;
}
// get next waypoint
@@ -1283,7 +1324,11 @@
}
}
- usleep(CYCLE_TIME_US);
+ double currt,tdiff;
+ GlobalTime->GetTimeDouble(&currt);
+ tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6);
+ usleep((unsigned int)rint(tdiff));
+ printf("sleeping: %u\n", (unsigned int)rint(tdiff));
}
}
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
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
