Revision: 6361
http://playerstage.svn.sourceforge.net/playerstage/?rev=6361&view=rev
Author: gbiggs
Date: 2008-04-18 01:10:03 -0700 (Fri, 18 Apr 2008)
Log Message:
-----------
Merging changes 6287:6302 from trunk
Modified Paths:
--------------
code/player/branches/cmake/libplayercore/driver.h
code/player/branches/cmake/server/drivers/planner/wavefront/CMakeLists.txt
code/player/branches/cmake/server/drivers/planner/wavefront/Makefile.test
code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
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/server/drivers/shell/passthrough.cc
Added Paths:
-----------
code/player/branches/cmake/server/drivers/planner/wavefront/plan_control.c
Modified: code/player/branches/cmake/libplayercore/driver.h
===================================================================
--- code/player/branches/cmake/libplayercore/driver.h 2008-04-18 08:07:57 UTC
(rev 6360)
+++ code/player/branches/cmake/libplayercore/driver.h 2008-04-18 08:10:03 UTC
(rev 6361)
@@ -189,7 +189,7 @@
@param timestamp Timestamp for the message body (if NULL, then the
current time will be filled in)
@param copy if set to false the data will be claimed and the caller should
no longer use or free it */
- void Publish(player_devaddr_t addr,
+ virtual void Publish(player_devaddr_t addr,
QueuePointer &queue,
uint8_t type,
uint8_t subtype,
@@ -211,7 +211,7 @@
@param timestamp Timestamp for the message body (if NULL, then the
current time will be filled in)
@param copy if set to false the data will be claimed and the caller
should no longer use or free it */
- void Publish(player_devaddr_t addr,
+ virtual void Publish(player_devaddr_t addr,
uint8_t type,
uint8_t subtype,
void* src=NULL,
@@ -229,7 +229,7 @@
@param hdr The message header
@param src The message body
@param copy if set to false the data will be claimed and the caller should
no longer use or free it */
- void Publish(QueuePointer &queue,
+ virtual void Publish(QueuePointer &queue,
player_msghdr_t* hdr,
void* src,
bool copy = true);
@@ -241,7 +241,7 @@
@param hdr The message header
@param src The message body
@param copy if set to false the data will be claimed and the caller should
no longer use or free it */
- void Publish(player_msghdr_t* hdr,
+ virtual void Publish(player_msghdr_t* hdr,
void* src,
bool copy = true);
Modified:
code/player/branches/cmake/server/drivers/planner/wavefront/CMakeLists.txt
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/CMakeLists.txt
2008-04-18 08:07:57 UTC (rev 6360)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/CMakeLists.txt
2008-04-18 08:10:03 UTC (rev 6361)
@@ -3,4 +3,4 @@
wavefront_includeDir wavefront_libDir wavefront_linkFlags wavefront_cFlags)
PLAYERDRIVER_ADD_DRIVER (wavefront build_wavefront
"${wavefront_includeDir}" "${wavefront_libDir}" "${wavefront_linkFlags}"
"${wavefront_cFlags}"
- plan.c plan_plan.c plan_waypoint.c wavefront.cc heap.c)
\ No newline at end of file
+ plan.c plan_plan.c plan_waypoint.c wavefront.cc heap.c plan_control.c)
\ No newline at end of file
Modified:
code/player/branches/cmake/server/drivers/planner/wavefront/Makefile.test
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/Makefile.test
2008-04-18 08:07:57 UTC (rev 6360)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/Makefile.test
2008-04-18 08:10:03 UTC (rev 6361)
@@ -2,7 +2,7 @@
CFLAGS = -I. -I../../../.. -Wall -Werror -g `pkg-config --cflags
gdk-pixbuf-2.0`
LIBS = -L../../../../libplayercore/.libs -lplayererror `pkg-config --libs
gdk-pixbuf-2.0`
-SRCS = test.c plan.c plan_plan.c plan_waypoint.c heap.c
+SRCS = test.c plan.c plan_plan.c plan_waypoint.c heap.c plan_control.c
test: $(SRCS) plan.h heap.h
gcc $(CFLAGS) -o $@ $(SRCS) $(LIBS)
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:07:57 UTC (rev 6360)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
2008-04-18 08:10:03 UTC (rev 6361)
@@ -180,18 +180,11 @@
}
// Initialize the plan
-void plan_init(plan_t *plan,
- double res, double sx, double sy, double ox, double oy)
+void plan_init(plan_t *plan)
{
int i, j;
plan_cell_t *cell;
- plan->scale = res;
- plan->size_x = sx;
- plan->size_y = sy;
- plan->origin_x = ox;
- plan->origin_y = oy;
-
cell = plan->cells;
for (j = 0; j < plan->size_y; j++)
{
@@ -224,9 +217,9 @@
for (j = plan->min_y; j <= plan->max_y; j++)
{
- cell = plan->cells + PLAN_INDEX(plan,plan->min_x,j);
- for (i = plan->min_x; i <= plan->max_x; i++, cell++)
+ for (i = plan->min_x; i <= plan->max_x; i++)
{
+ cell = plan->cells + PLAN_INDEX(plan,i,j);
cell->plan_cost = PLAN_MAX_COST;
cell->plan_next = NULL;
cell->mark = 0;
@@ -380,7 +373,7 @@
}
}
-#if 1
+#if 0
#include <gdk-pixbuf/gdk-pixbuf.h>
void
Modified: code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
2008-04-18 08:07:57 UTC (rev 6360)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
2008-04-18 08:10:03 UTC (rev 6361)
@@ -109,8 +109,7 @@
void plan_free(plan_t *plan);
// Initialize the plan
-void plan_init(plan_t *plan,
- double res, double sx, double sy, double ox, double oy);
+void plan_init(plan_t *plan);
// Reset the plan
void plan_reset(plan_t *plan);
@@ -145,6 +144,10 @@
void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint,
double *px, double *py);
+double plan_get_carrot(plan_t* plan, double* px, double* py,
+ double lx, double ly,
+ double maxdist, double distweight);
+
#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
// Write the cspace occupancy distance values to a file, one per line.
// Read them back in with plan_read_cspace().
Copied:
code/player/branches/cmake/server/drivers/planner/wavefront/plan_control.c
(from rev 6302,
code/player/trunk/server/drivers/planner/wavefront/plan_control.c)
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/plan_control.c
(rev 0)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan_control.c
2008-04-18 08:10:03 UTC (rev 6361)
@@ -0,0 +1,151 @@
+#include <stdlib.h>
+
+#include "plan.h"
+
+double _plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g);
+
+double
+plan_get_carrot(plan_t* plan, double* px, double* py,
+ double lx, double ly, double maxdist, double distweight)
+{
+ plan_cell_t* cell, *ncell;
+ int li, lj;
+ double dist, d;
+ double cost, bestcost;
+
+ li = PLAN_GXWX(plan, lx);
+ lj = PLAN_GYWY(plan, ly);
+
+ cell = plan->cells + PLAN_INDEX(plan,li,lj);
+
+ // Step back from maxdist, looking for the best carrot
+ bestcost = -1.0;
+ for(dist = maxdist; dist >= plan->scale; dist -= plan->scale)
+ {
+ // Find a point the required distance ahead, following the cost gradient
+ d=plan->scale;
+ for(ncell = cell;
+ (ncell->plan_next && (d < dist));
+ ncell = ncell->plan_next, d+=plan->scale);
+
+ // Check whether the straight-line path is clear
+ if((cost = _plan_check_path(plan, cell, ncell)) < 0.0)
+ continue;
+
+ // Weight distance
+ cost += distweight * (1.0/(dist*dist));
+ if((bestcost < 0.0) || (cost < bestcost))
+ {
+ bestcost = cost;
+ *px = PLAN_WXGX(plan,ncell->ci);
+ *py = PLAN_WYGY(plan,ncell->cj);
+ }
+ }
+
+ return(bestcost);
+}
+
+double
+_plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g)
+{
+ // Bresenham raytracing
+ int x0,x1,y0,y1;
+ int x,y;
+ int xstep, ystep;
+ char steep;
+ int tmp;
+ int deltax, deltay, error, deltaerr;
+ int obscost=0;
+
+ x0 = s->ci;
+ y0 = s->cj;
+
+ x1 = g->ci;
+ y1 = g->cj;
+
+ if(abs(y1-y0) > abs(x1-x0))
+ steep = 1;
+ else
+ steep = 0;
+
+ if(steep)
+ {
+ tmp = x0;
+ x0 = y0;
+ y0 = tmp;
+
+ tmp = x1;
+ x1 = y1;
+ y1 = tmp;
+ }
+
+ deltax = abs(x1-x0);
+ deltay = abs(y1-y0);
+ error = 0;
+ deltaerr = deltay;
+
+ x = x0;
+ y = y0;
+
+ if(x0 < x1)
+ xstep = 1;
+ else
+ xstep = -1;
+ if(y0 < y1)
+ ystep = 1;
+ else
+ ystep = -1;
+
+ if(steep)
+ {
+ if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->abs_min_radius)
+ return -1;
+ else
+ obscost += plan->dist_penalty *
+ (plan->abs_min_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
+ obscost += plan->dist_penalty *
+ (plan->abs_min_radius -
+ plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn);
+ }
+
+ while(x != (x1 + xstep * 1))
+ {
+ x += xstep;
+ error += deltaerr;
+ if(2*error >= deltax)
+ {
+ y += ystep;
+ error -= deltax;
+ }
+
+ if(steep)
+ {
+ if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->abs_min_radius)
+ return -1;
+ else
+ obscost += plan->dist_penalty *
+ (plan->abs_min_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
+ obscost += plan->dist_penalty *
+ (plan->abs_min_radius -
+ plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn);
+ }
+ }
+
+ return(obscost);
+}
+
+
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:07:57 UTC (rev 6360)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
2008-04-18 08:10:03 UTC (rev 6361)
@@ -13,6 +13,9 @@
#include <limits.h>
#include <float.h>
+#include <sys/time.h>
+static double get_time(void);
+
#include "plan.h"
// Plan queue stuff
@@ -27,7 +30,10 @@
{
plan_cell_t* cell;
int li, lj;
+ double t0,t1;
+ t0 = get_time();
+
// Set bounds to look over the entire grid
plan_set_bounds(plan, 0, 0, plan->size_x - 1, plan->size_y - 1);
@@ -35,7 +41,7 @@
plan_reset(plan);
plan->path_count = 0;
- if(_plan_update_plan(plan, lx, ly, gx, gy) != 0)
+ if(_plan_update_plan(plan, lx, ly, gx, gy) < 0)
{
// no path
return(-1);
@@ -59,6 +65,10 @@
plan->path[plan->path_count++] = cell;
}
+ t1 = get_time();
+
+ printf("computed global path: %.6lf\n", t1-t0);
+
return(0);
}
@@ -69,7 +79,10 @@
int li, lj;
int xmin,ymin,xmax,ymax;
plan_cell_t* cell;
+ double t0,t1;
+ t0 = get_time();
+
// Set bounds as directed
xmin = PLAN_GXWX(plan, lx - plan_halfwidth);
ymin = PLAN_GXWX(plan, ly - plan_halfwidth);
@@ -87,8 +100,9 @@
return(-1);
}
- printf("local goal: %.3lf, %.3lf\n", gx, gy);
+ //printf("local goal: %.3lf, %.3lf\n", gx, gy);
+ plan->lpath_count = 0;
if(_plan_update_plan(plan, lx, ly, gx, gy) != 0)
{
puts("local plan update failed");
@@ -112,6 +126,10 @@
}
plan->lpath[plan->lpath_count++] = cell;
}
+
+ t1 = get_time();
+
+ printf("computed local path: %.6lf\n", t1-t0);
return(0);
}
@@ -124,7 +142,6 @@
int gi, gj, li,lj;
float cost;
plan_cell_t *cell, *ncell;
- int reached_start=0;
// Reset the queue
heap_reset(plan->heap);
@@ -137,7 +154,7 @@
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
- printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj);
+ //printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj);
if(!PLAN_VALID_BOUNDS(plan, gi, gj))
{
@@ -199,24 +216,19 @@
ncell->plan_cost = cost;
ncell->plan_next = cell;
- // Are we done?
- if((ncell->ci == li) && (ncell->cj == lj))
- reached_start=1;
-
plan_push(plan, ncell);
}
}
}
}
- /*
- if(!reached_start)
+ cell = plan->cells + PLAN_INDEX(plan, li, lj);
+ if(!cell->plan_next)
{
puts("never found start");
return(-1);
}
else
- */
return(0);
}
@@ -314,3 +326,11 @@
else
return(heap_extract_max(plan->heap));
}
+
+double
+static get_time(void)
+{
+ struct timeval curr;
+ gettimeofday(&curr,NULL);
+ return(curr.tv_sec + curr.tv_usec / 1e6);
+}
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:07:57 UTC (rev 6360)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
2008-04-18 08:10:03 UTC (rev 6361)
@@ -297,6 +297,8 @@
bool always_insert_rotational_waypoints;
// Should map be updated on every new goal?
int force_map_refresh;
+ // Should we do velocity control, or position control?
+ bool velocity_control;
// methods for internal use
int SetupLocalize();
@@ -388,6 +390,7 @@
this->always_insert_rotational_waypoints =
cf->ReadInt(section, "add_rotational_waypoints", 1);
this->force_map_refresh = cf->ReadInt(section, "force_map_refresh", 0);
+ this->velocity_control = cf->ReadInt(section, "velocity_control", 0);
}
@@ -412,8 +415,9 @@
this->new_goal = false;
this->waypoint_count = 0;
- this->waypoints = NULL;
- this->waypoints_allocated = 0;
+ this->waypoints_allocated = 8;
+ this->waypoints = (double (*)[2])malloc(this->waypoints_allocated *
+ sizeof(this->waypoints[0]));
if(SetupPosition() < 0)
return(-1);
@@ -507,14 +511,10 @@
// Got new map info pushed to us. We'll save this info and get the new
// map.
this->plan->scale = info->scale;
- plan_compute_dist_kernel(this->plan);
this->plan->size_x = info->width;
this->plan->size_y = info->height;
this->plan->origin_x = info->origin.px;
this->plan->origin_y = info->origin.py;
- // Set the bounds to search the whole grid.
- plan_set_bounds(this->plan,
- 0, 0, this->plan->size_x - 1, this->plan->size_y - 1);
// Now get the map data, possibly in separate tiles.
if(this->GetMap(true) < 0)
@@ -608,6 +608,7 @@
(void*)&vel_cmd,sizeof(vel_cmd),NULL);
}
+ this->stopped = false;
}
void
@@ -659,8 +660,6 @@
this->waypoint_odom_x = wx_odom;
this->waypoint_odom_y = wy_odom;
this->waypoint_odom_a = wa_odom;
-
- this->stopped = false;
}
@@ -676,6 +675,7 @@
double replan_timediff, replan_dist;
bool rotate_waypoint=false;
bool replan;
+ int rotate_dir=0;
pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
@@ -717,7 +717,7 @@
!this->atgoal;
// Did we get a new goal, or is it time to replan?
- if(this->new_goal || replan)
+ if(this->new_goal || replan || (this->velocity_control && !this->atgoal))
{
#if 0
// Should we get a new map?
@@ -768,101 +768,78 @@
#endif
// compute costs to the new goal. Try local plan first
- if(plan_do_local(this->plan, this->localize_x, this->localize_y, 5.0) <
0)
+ if(new_goal ||
+ (this->plan->path_count == 0) ||
+ (plan_do_local(this->plan, this->localize_x,
+ this->localize_y, 5.0) < 0))
{
- puts("Wavefront: local plan failed");
+ if(!new_goal && (this->plan->path_count != 0))
+ puts("Wavefront: local plan failed");
- if(plan_do_global(this->plan, this->localize_x, this->localize_x,
+ // 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");
+ else
+ this->new_goal = false;
}
-
- // extract waypoints along the path to the goal from the current position
- plan_update_waypoints(this->plan, this->localize_x, this->localize_y);
-#if 0
- if(this->plan->waypoint_count == 0)
+ if(!this->velocity_control)
{
- // No path. If we only searched a bounding box last time, grow the
- // bounds to encompass the whole map and try again.
- if((plan->min_x > 0) || (plan->max_x < (plan->size_x - 1)) ||
- (plan->min_y > 0) || (plan->max_y < (plan->size_y - 1)))
- {
- // Unfortunately, this computation can take a while (e.g., 1-2
- // seconds). So we'll stop the robot while it thinks.
- this->StopPosition();
+ // extract waypoints along the path to the goal from the current
position
+ plan_update_waypoints(this->plan, this->localize_x, this->localize_y);
- // Set the bounds to search the whole grid.
- plan_set_bounds(this->plan, 0, 0,
- this->plan->size_x - 1,
- this->plan->size_y - 1);
-
- struct timeval t0, t1;
- gettimeofday(&t0, NULL);
- plan_update_cspace(this->plan,this->cspace_fname);
- gettimeofday(&t1, NULL);
- printf("time to update: %f\n",
- (t1.tv_sec + t1.tv_usec/1e6) -
- (t0.tv_sec + t0.tv_usec/1e6));
-
- // compute costs to the new goal
- plan_update_plan(this->plan, this->target_x, this->target_y);
-
- // compute a path to the goal from the current position
- plan_update_waypoints(this->plan,
- this->localize_x,
- this->localize_y);
- }
- }
-#endif
-
- if(this->plan->waypoint_count == 0)
- {
- fprintf(stderr, "Wavefront (port %d):\n "
- "No path from (%.3lf,%.3lf,%.3lf) to (%.3lf,%.3lf,%.3lf)\n",
- this->device_addr.robot,
- this->localize_x,
- this->localize_y,
- RTOD(this->localize_a),
- this->target_x,
- this->target_y,
- RTOD(this->target_a));
- // Only fail here if this is our first try at making a plan;
- // if we're replanning and don't find a path then we'll just stick
- // with the old plan.
- if(this->curr_waypoint < 0)
+ if(this->plan->waypoint_count == 0)
{
- //this->curr_waypoint = -1;
- this->new_goal=false;
- this->waypoint_count = 0;
+ fprintf(stderr, "Wavefront (port %d):\n "
+ "No path from (%.3lf,%.3lf,%.3lf) to (%.3lf,%.3lf,%.3lf)\n",
+ this->device_addr.robot,
+ this->localize_x,
+ this->localize_y,
+ RTOD(this->localize_a),
+ this->target_x,
+ this->target_y,
+ RTOD(this->target_a));
+ // Only fail here if this is our first try at making a plan;
+ // if we're replanning and don't find a path then we'll just stick
+ // with the old plan.
+ if(this->curr_waypoint < 0)
+ {
+ //this->curr_waypoint = -1;
+ this->new_goal=false;
+ this->waypoint_count = 0;
+ }
}
- }
- else
- if (this->plan->waypoint_count > this->waypoints_allocated)
+ else
{
- this->waypoints = (double (*)[2])realloc(this->waypoints,
sizeof(this->waypoints[0])*this->plan->waypoint_count);
- this->waypoints_allocated = this->plan->waypoint_count;
+ if (this->plan->waypoint_count > this->waypoints_allocated)
+ {
+ this->waypoints = (double (*)[2])realloc(this->waypoints,
sizeof(this->waypoints[0])*this->plan->waypoint_count);
+ this->waypoints_allocated = this->plan->waypoint_count;
+ }
+ this->waypoint_count = this->plan->waypoint_count;
}
- this->waypoint_count = this->plan->waypoint_count;
- if(this->plan->waypoint_count > 0)
- {
- for(int i=0;i<this->plan->waypoint_count;i++)
+ if(this->plan->waypoint_count > 0)
{
- double wx, wy;
- plan_convert_waypoint(this->plan,
- this->plan->waypoints[i],
- &wx, &wy);
- this->waypoints[i][0] = wx;
- this->waypoints[i][1] = wy;
+ for(int i=0;i<this->plan->waypoint_count;i++)
+ {
+ double wx, wy;
+ plan_convert_waypoint(this->plan,
+ this->plan->waypoints[i],
+ &wx, &wy);
+ this->waypoints[i][0] = wx;
+ this->waypoints[i][1] = wy;
+ }
+
+ this->curr_waypoint = 0;
+ // Why is this here?
+ this->new_goal = true;
}
-
- this->curr_waypoint = 0;
- this->new_goal = true;
+ last_replan_time = t;
+ last_replan_lx = this->localize_x;
+ last_replan_ly = this->localize_y;
}
- last_replan_time = t;
- last_replan_lx = this->localize_x;
- last_replan_ly = this->localize_y;
}
if(!this->enable)
@@ -872,95 +849,191 @@
continue;
}
- bool going_for_target = (this->curr_waypoint ==
this->plan->waypoint_count);
- dist = 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)));
- // Note that we compare the current heading and waypoint heading in the
- // *odometric* frame. We do this because comparing the current
- // heading and waypoint heading in the localization frame is unreliable
- // when making small adjustments to achieve a desired heading (i.e., the
- // robot gets there and VFH stops, but here we don't realize we're done
- // because the localization heading hasn't changed sufficiently).
- angle = fabs(this->angle_diff(this->waypoint_odom_a,this->position_a));
- if(going_for_target && dist < this->dist_eps && angle < this->ang_eps)
+
+ if(this->velocity_control)
{
- // we're at the final target, so stop
- StopPosition();
- this->curr_waypoint = -1;
- this->new_goal = false;
- this->atgoal = true;
+ if(this->plan->path_count && !this->atgoal)
+ {
+ // Check doneness
+ dist = 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)));
+ angle = fabs(this->angle_diff(this->target_a,this->localize_a));
+ if((dist < this->dist_eps) && (angle < this->ang_eps))
+ {
+ this->StopPosition();
+ this->new_goal = false;
+ this->curr_waypoint = -1;
+ this->atgoal = true;
+ }
+ else
+ {
+ // Compute velocities
+ double wx, wy;
+ double maxd=2.0;
+ double distweight=10.0;
+
+ if(plan_get_carrot(this->plan, &wx, &wy,
+ this->localize_x, this->localize_y,
+ maxd, distweight) < 0)
+ {
+ puts("Failed to find a carrot");
+ this->StopPosition();
+ }
+ else
+ {
+ // Establish fake waypoints, for client-side visualization
+ this->curr_waypoint = 0;
+ this->waypoint_count = 2;
+ this->waypoints[0][0] = this->localize_x;
+ this->waypoints[0][1] = this->localize_y;
+ this->waypoint_x = this->waypoints[1][0] = wx;
+ 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 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 ad = angle_diff(b, this->localize_a);
+
+ // Are we on top of the goal?
+ if(d < this->dist_eps)
+ {
+ if(!rotate_dir)
+ {
+ if(ad < 0)
+ rotate_dir = -1;
+ else
+ rotate_dir = 1;
+ }
+
+ tv = 0.0;
+ av = rotate_dir * (avmin + (fabs(ad)/M_PI) * (avmax-avmin));
+ }
+ else
+ {
+ rotate_dir = 0;
+
+ if(fabs(ad) > a)
+ tv = 0.0;
+ else
+ tv = tvmin + (d / (M_SQRT2 * maxd)) * (tvmax-tvmin);
+
+ av = avmin + (fabs(ad)/M_PI) * (avmax-avmin);
+ if(ad < 0)
+ av = -av;
+ }
+
+ this->PutPositionCommand(tv,0.0,av,0);
+ }
+ }
+ }
+ else
+ this->StopPosition();
}
- else if(this->curr_waypoint < 0)
+ else // !velocity_control
{
- // no more waypoints, so stop
- StopPosition();
- }
- else
- {
- // are we there yet? ignore angle, cause this is just a waypoint
- dist = sqrt(((this->localize_x - this->waypoint_x) *
- (this->localize_x - this->waypoint_x)) +
- ((this->localize_y - this->waypoint_y) *
- (this->localize_y - this->waypoint_y)));
+ bool going_for_target = (this->curr_waypoint ==
this->plan->waypoint_count);
+ dist = 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)));
// Note that we compare the current heading and waypoint heading in the
// *odometric* frame. We do this because comparing the current
// heading and waypoint heading in the localization frame is unreliable
// when making small adjustments to achieve a desired heading (i.e., the
// robot gets there and VFH stops, but here we don't realize we're done
// because the localization heading hasn't changed sufficiently).
- if(this->new_goal ||
- (rotate_waypoint &&
- (fabs(this->angle_diff(this->waypoint_odom_a,this->position_a))
- < M_PI/4.0)) ||
- (!rotate_waypoint && (dist < this->dist_eps)))
+ angle = fabs(this->angle_diff(this->waypoint_odom_a,this->position_a));
+ if(going_for_target && dist < this->dist_eps && angle < this->ang_eps)
{
- if(this->curr_waypoint == this->waypoint_count)
+ // we're at the final target, so stop
+ StopPosition();
+ this->curr_waypoint = -1;
+ this->new_goal = false;
+ this->atgoal = true;
+ }
+ else if(this->curr_waypoint < 0)
+ {
+ // no more waypoints, so stop
+ StopPosition();
+ }
+ else
+ {
+ // are we there yet? ignore angle, cause this is just a waypoint
+ dist = sqrt(((this->localize_x - this->waypoint_x) *
+ (this->localize_x - this->waypoint_x)) +
+ ((this->localize_y - this->waypoint_y) *
+ (this->localize_y - this->waypoint_y)));
+ // Note that we compare the current heading and waypoint heading in the
+ // *odometric* frame. We do this because comparing the current
+ // heading and waypoint heading in the localization frame is unreliable
+ // when making small adjustments to achieve a desired heading (i.e.,
the
+ // robot gets there and VFH stops, but here we don't realize we're done
+ // because the localization heading hasn't changed sufficiently).
+ if(this->new_goal ||
+ (rotate_waypoint &&
+ (fabs(this->angle_diff(this->waypoint_odom_a,this->position_a))
+ < M_PI/4.0)) ||
+ (!rotate_waypoint && (dist < this->dist_eps)))
{
- // no more waypoints, so wait for target achievement
+ if(this->curr_waypoint == this->waypoint_count)
+ {
+ // no more waypoints, so wait for target achievement
- //puts("waiting for goal achievement");
- usleep(CYCLE_TIME_US);
- continue;
- }
- // get next waypoint
- this->waypoint_x = this->waypoints[this->curr_waypoint][0];
- this->waypoint_y = this->waypoints[this->curr_waypoint][1];
- this->curr_waypoint++;
+ //puts("waiting for goal achievement");
+ usleep(CYCLE_TIME_US);
+ continue;
+ }
+ // get next waypoint
+ this->waypoint_x = this->waypoints[this->curr_waypoint][0];
+ this->waypoint_y = this->waypoints[this->curr_waypoint][1];
+ this->curr_waypoint++;
- this->waypoint_a = this->target_a;
- if(this->always_insert_rotational_waypoints ||
- (this->curr_waypoint == 2))
- {
- dist = sqrt((this->waypoint_x - this->localize_x) *
- (this->waypoint_x - this->localize_x) +
- (this->waypoint_y - this->localize_y) *
- (this->waypoint_y - this->localize_y));
- angle = atan2(this->waypoint_y - this->localize_y,
- this->waypoint_x - this->localize_x);
- if((dist > this->dist_eps) &&
- fabs(this->angle_diff(angle,this->localize_a)) > M_PI/4.0)
+ this->waypoint_a = this->target_a;
+ if(this->always_insert_rotational_waypoints ||
+ (this->curr_waypoint == 2))
{
- this->waypoint_x = this->localize_x;
- this->waypoint_y = this->localize_y;
- this->waypoint_a = angle;
- this->curr_waypoint--;
- rotate_waypoint=true;
+ dist = sqrt((this->waypoint_x - this->localize_x) *
+ (this->waypoint_x - this->localize_x) +
+ (this->waypoint_y - this->localize_y) *
+ (this->waypoint_y - this->localize_y));
+ angle = atan2(this->waypoint_y - this->localize_y,
+ this->waypoint_x - this->localize_x);
+ if((dist > this->dist_eps) &&
+ fabs(this->angle_diff(angle,this->localize_a)) > M_PI/4.0)
+ {
+ this->waypoint_x = this->localize_x;
+ this->waypoint_y = this->localize_y;
+ this->waypoint_a = angle;
+ this->curr_waypoint--;
+ rotate_waypoint=true;
+ }
+ else
+ rotate_waypoint=false;
}
else
rotate_waypoint=false;
+
+ this->new_goal = false;
}
- else
- rotate_waypoint=false;
- this->new_goal = false;
+ SetWaypoint(this->waypoint_x, this->waypoint_y, this->waypoint_a);
}
-
- SetWaypoint(this->waypoint_x, this->waypoint_y, this->waypoint_a);
}
-
usleep(CYCLE_TIME_US);
}
}
@@ -1118,6 +1191,10 @@
oj += sj;
}
}
+
+ plan_init(this->plan);
+ plan_compute_cspace(this->plan);
+
return(0);
}
@@ -1132,7 +1209,6 @@
{
PLAYER_WARN("failed to get map info");
this->plan->scale = 0.1;
- plan_compute_dist_kernel(this->plan);
this->plan->size_x = 0;
this->plan->size_y = 0;
this->plan->origin_x = 0.0;
@@ -1144,14 +1220,10 @@
// copy in the map info
this->plan->scale = info->scale;
- plan_compute_dist_kernel(this->plan);
this->plan->size_x = info->width;
this->plan->size_y = info->height;
this->plan->origin_x = info->origin.px;
this->plan->origin_y = info->origin.py;
- // Set the bounds to search the whole grid.
- plan_set_bounds(this->plan,
- 0, 0, this->plan->size_x - 1, this->plan->size_y - 1);
delete msg;
return(0);
@@ -1190,9 +1262,6 @@
if(this->GetMap(false) < 0)
return(-1);
- //plan_update_cspace(this->plan,this->cspace_fname);
- plan_compute_cspace(this->plan);
-
this->have_map = true;
this->new_map = true;
@@ -1271,9 +1340,6 @@
// Now get the map data, possibly in separate tiles.
if(this->GetMap(true) < 0) return -1;
- //plan_update_cspace(this->plan,this->cspace_fname);
- plan_compute_cspace(this->plan);
-
this->have_map = true;
this->new_map = true;
}
Modified: code/player/branches/cmake/server/drivers/shell/passthrough.cc
===================================================================
--- code/player/branches/cmake/server/drivers/shell/passthrough.cc
2008-04-18 08:07:57 UTC (rev 6360)
+++ code/player/branches/cmake/server/drivers/shell/passthrough.cc
2008-04-18 08:10:03 UTC (rev 6361)
@@ -225,7 +225,7 @@
(hdr->type == PLAYER_MSGTYPE_SYNCH) ||
(hdr->type == PLAYER_MSGTYPE_RESP_NACK)))
{
- PLAYER_MSG7(3,"PassThrough: Forwarding SRC->DST Interface code=%d
%d:%d:%d -> %d:%d:%d",hdr->addr.interf, hdr->addr.host,hdr->addr.robot,
hdr->addr.index, dstAddr.host, dstAddr.robot, dstAddr.index);
+ PLAYER_MSG7(8,"PassThrough: Forwarding SRC->DST Interface code=%d
%d:%d:%d -> %d:%d:%d",hdr->addr.interf, hdr->addr.host,hdr->addr.robot,
hdr->addr.index, dstAddr.host, dstAddr.robot, dstAddr.index);
hdr->addr=dstAddr; //will send to my clients, making it seem like it
comes from my DST interface
@@ -236,7 +236,7 @@
if (Device::MatchDeviceAddress(hdr->addr,dstAddr) &&
(hdr->type == PLAYER_MSGTYPE_CMD))
{
- PLAYER_MSG7(3,"PassThrough: Forwarding DST->SRC Interface code=%d
%d:%d:%d -> %d:%d:%d",hdr->addr.interf, hdr->addr.host,hdr->addr.robot,
hdr->addr.index, srcAddr.host, srcAddr.robot, srcAddr.index);
+ PLAYER_MSG7(8,"PassThrough: Forwarding DST->SRC Interface code=%d
%d:%d:%d -> %d:%d:%d",hdr->addr.interf, hdr->addr.host,hdr->addr.robot,
hdr->addr.index, srcAddr.host, srcAddr.robot, srcAddr.index);
hdr->addr=srcAddr; //send to the device to which I subscribed, making
it seem like it comes from my original interface
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
