Revision: 6365
http://playerstage.svn.sourceforge.net/playerstage/?rev=6365&view=rev
Author: gbiggs
Date: 2008-04-18 01:38:29 -0700 (Fri, 18 Apr 2008)
Log Message:
-----------
Merging changes 6335:6344 from trunk
Modified Paths:
--------------
code/player/branches/cmake/server/drivers/map/mapscale.cc
code/player/branches/cmake/server/drivers/map/maptransform.cc
code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
code/player/branches/cmake/server/drivers/planner/wavefront/plan_control.c
code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
Modified: code/player/branches/cmake/server/drivers/map/mapscale.cc
===================================================================
--- code/player/branches/cmake/server/drivers/map/mapscale.cc 2008-04-18
08:35:32 UTC (rev 6364)
+++ code/player/branches/cmake/server/drivers/map/mapscale.cc 2008-04-18
08:38:29 UTC (rev 6365)
@@ -187,13 +187,14 @@
scale_factor = this->source_map.scale / this->new_map.scale;
this->new_map.width = static_cast<unsigned int> (rint(this->source_map.width
* scale_factor));
this->new_map.height = static_cast<unsigned int>
(rint(this->source_map.height * scale_factor));
+ this->new_map.origin = this->source_map.origin;
PLAYER_MSG3(4,"MapScale: New map is %dx%d scale
%f",new_map.width,new_map.height,new_map.scale);
new_pixbuf = gdk_pixbuf_scale_simple(pixbuf,
- this->new_map.width,
- this->new_map.height,
- GDK_INTERP_HYPER);
+ this->new_map.width,
+ this->new_map.height,
+ GDK_INTERP_HYPER);
g_assert(new_pixbuf);
new_map_pixels = gdk_pixbuf_get_pixels(new_pixbuf);
Modified: code/player/branches/cmake/server/drivers/map/maptransform.cc
===================================================================
--- code/player/branches/cmake/server/drivers/map/maptransform.cc
2008-04-18 08:35:32 UTC (rev 6364)
+++ code/player/branches/cmake/server/drivers/map/maptransform.cc
2008-04-18 08:38:29 UTC (rev 6365)
@@ -199,7 +199,8 @@
PLAYER_MSG0(9,"ProcessMessage called for MapTransform Driver");
assert(hdr);
- assert(data);
+ // assert fails on empty messages
+ //assert(data);
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_MAP_REQ_GET_INFO,
device_addr))
{
Modified: code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
2008-04-18 08:35:32 UTC (rev 6364)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
2008-04-18 08:38:29 UTC (rev 6365)
@@ -133,7 +133,7 @@
}
t1 = get_time();
- printf("plan_set_obstacles: %.6lf\n", t1-t0);
+ //printf("plan_set_obstacles: %.6lf\n", t1-t0);
}
void
@@ -191,6 +191,8 @@
int i, j;
plan_cell_t *cell;
+ printf("scale: %.3lf\n", plan->scale);
+
cell = plan->cells;
for (j = 0; j < plan->size_y; j++)
{
Modified:
code/player/branches/cmake/server/drivers/planner/wavefront/plan_control.c
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/plan_control.c
2008-04-18 08:35:32 UTC (rev 6364)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan_control.c
2008-04-18 08:38:29 UTC (rev 6365)
@@ -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;
}
@@ -119,18 +119,18 @@
{
if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->abs_min_radius)
return -1;
- else
+ else if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->max_radius)
obscost += plan->dist_penalty *
- (plan->abs_min_radius -
+ (plan->max_radius -
plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn);
}
else
{
if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->abs_min_radius)
return -1;
- else
+ else if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->max_radius)
obscost += plan->dist_penalty *
- (plan->abs_min_radius -
+ (plan->max_radius -
plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn);
}
@@ -148,18 +148,18 @@
{
if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->abs_min_radius)
return -1;
- else
+ else if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn <
plan->max_radius)
obscost += plan->dist_penalty *
- (plan->abs_min_radius -
+ (plan->max_radius -
plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn);
}
else
{
if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->abs_min_radius)
return -1;
- else
+ else if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn <
plan->max_radius)
obscost += plan->dist_penalty *
- (plan->abs_min_radius -
+ (plan->max_radius -
plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn);
}
}
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-18 08:35:32 UTC (rev 6364)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
2008-04-18 08:38:29 UTC (rev 6365)
@@ -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/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
2008-04-18 08:35:32 UTC (rev 6364)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
2008-04-18 08:38:29 UTC (rev 6365)
@@ -229,15 +229,14 @@
// TODO: monitor localize timestamps, and slow or stop robot accordingly
-// time to sleep between loops (us)
-#define CYCLE_TIME_US 100000
-
class Wavefront : public Driver
{
private:
// Main function for device thread.
virtual void Main();
+ void Sleep(double loopstart);
+
// bookkeeping
player_devaddr_t position_id;
player_devaddr_t localize_id;
@@ -251,7 +250,8 @@
double dist_penalty;
double dist_eps;
double ang_eps;
- const char* cspace_fname;
+ double cycletime;
+ double tvmin, tvmax, avmin, avmax, amin, amax;
// the plan object
plan_t* plan;
@@ -422,11 +422,22 @@
this->replan_dist_thresh = cf->ReadLength(section,"replan_dist_thresh",2.0);
this->replan_min_time = cf->ReadFloat(section,"replan_min_time",2.0);
this->request_map = cf->ReadInt(section,"request_map",1);
- this->cspace_fname = cf->ReadFilename(section,"cspace_file","player.cspace");
this->always_insert_rotational_waypoints =
cf->ReadInt(section, "add_rotational_waypoints", 1);
this->force_map_refresh = cf->ReadInt(section, "force_map_refresh", 0);
+ this->cycletime = 1.0 / cf->ReadFloat(section, "update_rate", 10.0);
+
this->velocity_control = cf->ReadInt(section, "velocity_control", 0);
+ if(this->velocity_control)
+ {
+ this->tvmin = cf->ReadTupleLength(section, "control_tv", 0, 0.1);
+ this->tvmax = cf->ReadTupleLength(section, "control_tv", 1, 0.5);
+ this->avmin = cf->ReadTupleAngle(section, "control_av", 0, DTOR(10.0));
+ this->avmax = cf->ReadTupleAngle(section, "control_av", 1, DTOR(90.0));
+ this->amin = cf->ReadTupleAngle(section, "control_a", 0, DTOR(5.0));
+ this->amax = cf->ReadTupleAngle(section, "control_a", 1, DTOR(20.0));
+ }
+
if(this->laser_id.interf)
{
this->scans_size = cf->ReadInt(section, "laser_buffer_size", 10);
@@ -438,7 +449,11 @@
this->scan_maxrange = cf->ReadLength(section, "laser_maxrange", 6.0);
}
else
+ {
this->scans_size = 0;
+ if(this->velocity_control)
+ PLAYER_WARN("Wavefront doing direct velocity control, but without a
laser for obstacle detection; this is not safe!");
+ }
}
@@ -588,7 +603,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,21 +654,14 @@
}
}
- 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 0
if(this->graphics2d_id.interf)
{
- // Clear the canvas
- this->graphics2d->PutMsg(this->InQueue,
- PLAYER_MSGTYPE_CMD,
- PLAYER_GRAPHICS2D_CMD_CLEAR,
- NULL,0,NULL);
-
// Draw the points
player_graphics2d_cmd_points pts;
assert(pts.points = (player_point_2d_t*)malloc(sizeof(player_point_2d_t)*
@@ -675,7 +683,6 @@
(void*)&pts,0,NULL);
free(pts.points);
}
-#endif
}
void
@@ -851,7 +858,20 @@
this->waypoint_odom_a = wa_odom;
}
+void
+Wavefront::Sleep(double loopstart)
+{
+ double currt,tdiff;
+ //GlobalTime->GetTimeDouble(&currt);
+ currt = get_time();
+ //printf("cycle: %.6lf\n", currt-loopstart);
+ 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));
+}
+
////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void Wavefront::Main()
@@ -875,17 +895,19 @@
for(;;)
{
+ //GlobalTime->GetTimeDouble(&t);
+ t = get_time();
+
pthread_testcancel();
ProcessMessages();
if(!this->have_map && !this->new_map_available)
{
- usleep(CYCLE_TIME_US);
+ this->Sleep(t);
continue;
}
- GlobalTime->GetTimeDouble(&t);
if((t - last_publish_time) > 0.25)
{
@@ -896,7 +918,7 @@
if(!this->enable)
{
this->StopPosition();
- usleep(CYCLE_TIME_US);
+ this->Sleep(t);
continue;
}
@@ -962,6 +984,14 @@
this->new_map = false;
}
#endif
+ if(this->graphics2d_id.interf)
+ {
+ this->graphics2d->PutMsg(this->InQueue,
+ PLAYER_MSGTYPE_CMD,
+ PLAYER_GRAPHICS2D_CMD_CLEAR,
+ NULL,0,NULL);
+ }
+
double t0,t1;
t0 = get_time();
@@ -985,11 +1015,6 @@
if(this->graphics2d_id.interf && this->plan->lpath_count)
{
- this->graphics2d->PutMsg(this->InQueue,
- PLAYER_MSGTYPE_CMD,
- PLAYER_GRAPHICS2D_CMD_CLEAR,
- NULL,0,NULL);
-
player_graphics2d_cmd_polyline_t line;
line.points_count = this->plan->lpath_count;
line.points =
(player_point_2d_t*)malloc(sizeof(player_point_2d_t)*line.points_count);
@@ -1009,8 +1034,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)
{
@@ -1097,19 +1143,40 @@
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)
{
puts("Failed to find a carrot");
- draw_cspace(this->plan, "debug.png");
+ //draw_cspace(this->plan, "debug.png");
this->StopPosition();
- exit(-1);
+ //exit(-1);
}
else
{
+ if(this->graphics2d_id.interf)
+ {
+ player_graphics2d_cmd_polyline_t line;
+ line.points_count = 2;
+ line.points =
(player_point_2d_t*)malloc(sizeof(player_point_2d_t)*line.points_count);
+ line.color.alpha = 0;
+ line.color.red = 0;
+ line.color.green = 0;
+ line.color.blue = 255;
+
+ line.points[0].px = this->localize_x;
+ line.points[0].py = this->localize_y;
+ line.points[1].px = wx;
+ line.points[1].py = wy;
+ this->graphics2d->PutMsg(this->InQueue,
+ PLAYER_MSGTYPE_CMD,
+ PLAYER_GRAPHICS2D_CMD_POLYLINE,
+ (void*)&line,0,NULL);
+ free(line.points);
+ }
+
// Establish fake waypoints, for client-side visualization
this->curr_waypoint = 0;
this->waypoint_count = 2;
@@ -1119,24 +1186,20 @@
this->waypoint_y = this->waypoints[1][1] = wy;
this->waypoint_a = 0.0;
- // TODO: expose these control params in the .cfg file
- double tvmin = 0.1;
- double tvmax = 0.5;
- double avmin = DTOR(10.0);
- double avmax = DTOR(45.0);
- 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);
double av,tv;
- double a = amin + (d / maxd) * (amax-amin);
+ double a = this->amin + (d / maxd) * (this->amax-this->amin);
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)
{
@@ -1147,7 +1210,8 @@
}
tv = 0.0;
- av = rotate_dir * (avmin + (fabs(ad)/M_PI) * (avmax-avmin));
+ av = rotate_dir * (this->avmin + (fabs(ad)/M_PI) *
+ (this->avmax-this->avmin));
}
else
{
@@ -1156,9 +1220,12 @@
if(fabs(ad) > a)
tv = 0.0;
else
- tv = tvmin + (d / (M_SQRT2 * maxd)) * (tvmax-tvmin);
+ {
+ //tv = tvmin + (d / (M_SQRT2 * maxd)) * (tvmax-tvmin);
+ tv = this->tvmin + (d / maxd) * (this->tvmax-this->tvmin);
+ }
- av = avmin + (fabs(ad)/M_PI) * (avmax-avmin);
+ av = this->avmin + (fabs(ad)/M_PI) * (this->avmax-this->avmin);
if(ad < 0)
av = -av;
}
@@ -1171,7 +1238,7 @@
this->StopPosition();
t1 = get_time();
- printf("control: %.6lf\n", t1-t0);
+ //printf("control: %.6lf\n", t1-t0);
}
else // !velocity_control
{
@@ -1224,7 +1291,7 @@
// no more waypoints, so wait for target achievement
//puts("waiting for goal achievement");
- usleep(CYCLE_TIME_US);
+ this->Sleep(t);
continue;
}
// get next waypoint
@@ -1264,7 +1331,7 @@
}
}
- usleep(CYCLE_TIME_US);
+ this->Sleep(t);
}
}
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
