Revision: 6336
http://playerstage.svn.sourceforge.net/playerstage/?rev=6336&view=rev
Author: gerkey
Date: 2008-04-16 10:23:26 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
debugging controller
Modified Paths:
--------------
code/player/trunk/server/drivers/planner/wavefront/plan_control.c
code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan_control.c
2008-04-16 09:10:27 UTC (rev 6335)
+++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c
2008-04-16 17:23:26 UTC (rev 6336)
@@ -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/trunk/server/drivers/planner/wavefront/wavefront.cc
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2008-04-16 09:10:27 UTC (rev 6335)
+++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2008-04-16 17:23:26 UTC (rev 6336)
@@ -645,15 +645,8 @@
t1 = get_time();
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 +668,6 @@
(void*)&pts,0,NULL);
free(pts.points);
}
-#endif
}
void
@@ -962,6 +954,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 +985,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);
@@ -1110,6 +1105,27 @@
}
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;
@@ -1123,7 +1139,7 @@
double tvmin = 0.1;
double tvmax = 0.5;
double avmin = DTOR(10.0);
- double avmax = DTOR(45.0);
+ double avmax = DTOR(90.0);
double amin = DTOR(5.0);
double amax = DTOR(20.0);
@@ -1156,7 +1172,10 @@
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 = tvmin + (d / maxd) * (tvmax-tvmin);
+ }
av = avmin + (fabs(ad)/M_PI) * (avmax-avmin);
if(ad < 0)
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
