Revision: 6400
http://playerstage.svn.sourceforge.net/playerstage/?rev=6400&view=rev
Author: gerkey
Date: 2008-05-07 19:01:26 -0700 (Wed, 07 May 2008)
Log Message:
-----------
added more to low-level controller
Modified Paths:
--------------
code/player/trunk/server/drivers/planner/wavefront/plan.h
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.h
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-05-07
16:39:01 UTC (rev 6399)
+++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-05-08
02:01:26 UTC (rev 6400)
@@ -150,6 +150,19 @@
double plan_get_carrot(plan_t* plan, double* px, double* py,
double lx, double ly,
double maxdist, double distweight);
+int plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va,
+ int* rotate_dir,
+ double lx, double ly, double la,
+ double gx, double gy, double ga,
+ double goal_d, double goal_a,
+ double maxd, double dweight,
+ double tvmin, double tvmax,
+ double avmin, double avmax,
+ double amin, double amax);
+int plan_check_done(plan_t* plan,
+ double lx, double ly, double la,
+ double gx, double gy, double ga,
+ double goal_d, double goal_a);
void plan_set_obstacles(plan_t* plan, double* obs, size_t num);
Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan_control.c
2008-05-07 16:39:01 UTC (rev 6399)
+++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c
2008-05-08 02:01:26 UTC (rev 6400)
@@ -1,10 +1,85 @@
#include <stdlib.h>
+#include <math.h>
#include <assert.h>
#include "plan.h"
-double _plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g);
+static double _plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g);
+static double _angle_diff(double a, double b);
+int
+plan_check_done(plan_t* plan,
+ double lx, double ly, double la,
+ double gx, double gy, double ga,
+ double goal_d, double goal_a)
+{
+ double dt, da;
+ dt = sqrt((gx-lx)*(gx-ly) + (gy-ly)*(gy-ly));
+ da = fabs(_angle_diff(ga,la));
+
+ if((dt < goal_d) && (da < goal_a))
+ return(1);
+ else
+ return(0);
+}
+
+int
+plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va,
+ int* rotate_dir,
+ double lx, double ly, double la,
+ double gx, double gy, double ga,
+ double goal_d, double goal_a,
+ double maxd, double dweight,
+ double tvmin, double tvmax,
+ double avmin, double avmax,
+ double amin, double amax)
+{
+ double cx, cy;
+ double d,b,av,tv,a,ad;
+
+ // Are we at the goal?
+ if(plan_check_done(plan,lx,ly,la,gx,gy,ga,goal_d,goal_a))
+ {
+ *vx = 0.0;
+ *va = 0.0;
+ return(0);
+ }
+
+ // Are we on top of the goal?
+ d = sqrt((gx-lx)*(gx-lx)+(gy-ly)*(gy-ly));
+ if(d < goal_d)
+ {
+ ad = _angle_diff(ga,la);
+ if(!*rotate_dir)
+ {
+ if(ad < 0)
+ *rotate_dir = -1;
+ else
+ *rotate_dir = 1;
+ }
+ *vx = 0.0;
+ *va = *rotate_dir * (avmin + (fabs(ad)/M_PI) * (avmax-avmin));
+ return(0);
+ }
+
+ // We're away from the goal; compute velocities
+ if(plan_get_carrot(plan, &cx, &cy, lx, ly, maxd, dweight) < 0.0)
+ return(-1);
+
+ d = sqrt((lx-cx)*(lx-cx) + (ly-cy)*(ly-cy));
+ b = atan2(cy - ly, cx - lx);
+ a = amin + (d / maxd) * (amax-amin);
+ ad = _angle_diff(b, la);
+
+ if(fabs(ad) > a)
+ *vx = 0.0;
+ else
+ tv = tvmin + (d / maxd) * (tvmax-tvmin);
+ av = avmin + (fabs(ad)/M_PI) * (avmax-avmin);
+ if(ad < 0)
+ av = -av;
+}
+
double
plan_get_carrot(plan_t* plan, double* px, double* py,
double lx, double ly, double maxdist, double distweight)
@@ -64,7 +139,7 @@
return(bestcost);
}
-double
+static double
_plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g)
{
// Bresenham raytracing
@@ -167,4 +242,20 @@
return(obscost);
}
+#define ANG_NORM(a) atan2(cos((a)),sin((a)))
+static double
+_angle_diff(double a, double b)
+{
+ double d1, d2;
+ a = ANG_NORM(a);
+ b = ANG_NORM(b);
+ d1 = a-b;
+ d2 = 2*M_PI - fabs(d1);
+ if(d1 > 0)
+ d2 *= -1.0;
+ if(fabs(d1) < fabs(d2))
+ return(d1);
+ else
+ return(d2);
+}
Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2008-05-07 16:39:01 UTC (rev 6399)
+++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2008-05-08 02:01:26 UTC (rev 6400)
@@ -225,7 +225,7 @@
#include <sys/time.h>
static double get_time(void);
-extern "C" { void draw_cspace(plan_t* plan, const char* fname); }
+//extern "C" { void draw_cspace(plan_t* plan, const char* fname); }
// TODO: monitor localize timestamps, and slow or stop robot accordingly
@@ -1542,7 +1542,7 @@
plan_init(this->plan);
plan_compute_cspace(this->plan);
- draw_cspace(this->plan,"cspace.png");
+ //draw_cspace(this->plan,"cspace.png");
return(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
