Revision: 6364
http://playerstage.svn.sourceforge.net/playerstage/?rev=6364&view=rev
Author: gbiggs
Date: 2008-04-18 01:35:32 -0700 (Fri, 18 Apr 2008)
Log Message:
-----------
Merging changes 6320:6327 from trunk
Modified Paths:
--------------
code/player/branches/cmake/server/drivers/localization/fakelocalize.cc
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_control.c
code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
code/player/branches/cmake/server/drivers/planner/wavefront/test.c
code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
Property Changed:
----------------
code/player/branches/cmake/server/drivers/health/statgrab/
Property changes on: code/player/branches/cmake/server/drivers/health/statgrab
___________________________________________________________________
Name: svn:ignore
- Makefile
Makefile.in
.deps
*.la
+ Makefile
Makefile.in
.deps
*.la
.libs
Modified: code/player/branches/cmake/server/drivers/localization/fakelocalize.cc
===================================================================
--- code/player/branches/cmake/server/drivers/localization/fakelocalize.cc
2008-04-18 08:29:18 UTC (rev 6363)
+++ code/player/branches/cmake/server/drivers/localization/fakelocalize.cc
2008-04-18 08:35:32 UTC (rev 6364)
@@ -48,6 +48,7 @@
@par Provides
- @ref interface_localize
+- @ref interface_position2d (optionally)
@par Requires
@@ -88,23 +89,27 @@
*/
/** @} */
-
-#include <sys/types.h>
-#include <netinet/in.h>
#include <unistd.h>
+#include <cstddef>
+#include <cstdlib>
+#include <ctime>
+#include <cassert>
+#include <cstring>
#include <libplayercore/playercore.h>
-#define SLEEPTIME_US 100000
+#define SLEEPTIME_NS 100000000
-#define min(x,y) (x < y ? x : y)
+#define min(x,y) ((x) < (y) ? (x) : (y))
+using namespace std;
+
// Driver for computing the free c-space from a laser scan.
class FakeLocalize : public Driver
{
// Constructor
public: FakeLocalize( ConfigFile* cf, int section);
- ~FakeLocalize();
+ public: virtual ~FakeLocalize();
// Setup/shutdown/run routines.
public: virtual int Setup();
@@ -114,30 +119,27 @@
player_msghdr * hdr,
void * data);
-
// Simulation stuff.
private: int UpdateData();
private: Device *sim;
private: player_devaddr_t sim_id;
private: player_devaddr_t localize_addr;
+ private: player_devaddr_t position2d_addr;
private: char* model;
};
-
// Initialization function
Driver* FakeLocalize_Init( ConfigFile* cf, int section)
{
return ((Driver*) (new FakeLocalize( cf, section)));
}
-
// a driver registration function
void fakelocalize_Register(DriverTable* table)
{
table->AddDriver("fakelocalize", FakeLocalize_Init);
}
-
////////////////////////////////////////////////////////////////////////////////
// Constructor
FakeLocalize::FakeLocalize( ConfigFile* cf, int section)
@@ -151,8 +153,8 @@
return;
}
this->sim = NULL;
-
- memset(&this->localize_addr, 0, sizeof(player_devaddr_t));
+ memset(&(this->localize_addr), 0, sizeof(player_devaddr_t));
+ memset(&(this->position2d_addr), 0, sizeof(player_devaddr_t));
// Do we create a localize interface?
if(cf->ReadDeviceAddr(&(this->localize_addr), section, "provides",
PLAYER_LOCALIZE_CODE, -1, NULL) == 0)
@@ -163,7 +165,22 @@
return;
}
}
+ // Do we create a position2d interface?
+ if(cf->ReadDeviceAddr(&(this->position2d_addr), section, "provides",
+ PLAYER_POSITION2D_CODE, -1, NULL) == 0)
+ {
+ if(this->AddInterface(this->position2d_addr))
+ {
+ this->SetError(-1);
+ return;
+ }
+ }
+ if (!((this->localize_addr.interf) || (this->position2d_addr.interf)))
+ {
+ this->SetError(-1);
+ return;
+ }
if(!(this->model = strdup(cf->ReadString(section, "model", NULL))))
{
@@ -177,7 +194,7 @@
FakeLocalize::~FakeLocalize()
{
- free (this->model);
+ free(this->model);
}
////////////////////////////////////////////////////////////////////////////////
@@ -188,19 +205,18 @@
if(!(this->sim = deviceTable->GetDevice(this->sim_id)))
{
PLAYER_ERROR("unable to locate suitable simulation device");
- return(-1);
+ return -1;
}
if(this->sim->Subscribe(this->InQueue) != 0)
{
PLAYER_ERROR("unable to subscribe to simulation device");
- return(-1);
+ return -1;
}
this->StartThread();
return 0;
}
-
////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int FakeLocalize::Shutdown()
@@ -217,53 +233,70 @@
FakeLocalize::UpdateData()
{
player_localize_data_t loc_data;
+ player_position2d_data_t pos_data;
player_simulation_pose2d_req_t cfg;
// Request pose
+ memset(&cfg, 0, sizeof cfg);
cfg.name = this->model;
- cfg.name_count = strlen(cfg.name);
+ cfg.name_count = strlen(cfg.name) + 1;
Message * Reply = sim->Request(InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_GET_POSE2D,
- (void *) &cfg, sizeof(cfg), NULL);
+ reinterpret_cast<void *>(&cfg), sizeof cfg, NULL);
- if (Reply && Reply->GetHeader()->type == PLAYER_MSGTYPE_RESP_ACK)
+ if (!Reply) return -1;
+ if (Reply->GetHeader()->type == PLAYER_MSGTYPE_RESP_ACK)
{
- // we got a good reply so update our data
- assert(Reply->GetDataSize() > 0);
- player_simulation_pose2d_req_t * resp =
reinterpret_cast<player_simulation_pose2d_req_t *> (Reply->GetPayload());
+ // we got a good reply so update our data
+ assert(Reply->GetDataSize() > 0);
+ player_simulation_pose2d_req_t * resp =
reinterpret_cast<player_simulation_pose2d_req_t *>(Reply->GetPayload());
+ assert(resp);
+ if (this->localize_addr.interf)
+ {
+ // Fill in loc_data
+ memset(&loc_data, 0, sizeof loc_data);
+ loc_data.pending_count = 0;
+ loc_data.pending_time = Reply->GetHeader()->timestamp;
+ loc_data.hypoths_count = 1;
+ loc_data.hypoths = new player_localize_hypoth_t[1];
- // Fill in loc_data, byteswapping as we go.
- loc_data.pending_count = 0;
- loc_data.pending_time = Reply->GetHeader()->timestamp;
- loc_data.hypoths_count = 1;
- loc_data.hypoths = new player_localize_hypoth_t[1];
+ loc_data.hypoths[0].mean = resp->pose;
- loc_data.hypoths[0].mean = resp->pose;
+ // zero covariance and max weight
+ memset(loc_data.hypoths[0].cov, 0, sizeof loc_data.hypoths[0].cov);
+ loc_data.hypoths[0].alpha = 1e6;
- // zero covariance and max weight
- memset(loc_data.hypoths[0].cov,0,sizeof(loc_data.hypoths[0].cov));
- loc_data.hypoths[0].alpha = 1e6;
-
- Publish(device_addr, PLAYER_MSGTYPE_DATA,
PLAYER_LOCALIZE_DATA_HYPOTHS,&loc_data);
- delete [] loc_data.hypoths;
+ Publish(this->localize_addr, PLAYER_MSGTYPE_DATA,
PLAYER_LOCALIZE_DATA_HYPOTHS, reinterpret_cast<void *>(&loc_data));
+ delete []loc_data.hypoths;
+ }
+ if (this->position2d_addr.interf)
+ {
+ // Fill in pos_data
+ memset(&pos_data, 0, sizeof pos_data);
+ pos_data.pos.px = resp->pose.px;
+ pos_data.pos.py = resp->pose.py;
+ pos_data.pos.pa = resp->pose.pa;
+ this->Publish(this->position2d_addr, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
+ reinterpret_cast<void *>(&pos_data),
sizeof pos_data, NULL);
+ }
delete Reply;
Reply = NULL;
}
else
{
- // we got an error, thats bad
+ // we got an error, that's bad
delete Reply;
return -1;
}
-
- return(0);
+ return 0;
}
-
void
FakeLocalize::Main()
{
+ struct timespec ts;
+
for(;;)
{
if(this->UpdateData() < 0)
@@ -272,8 +305,9 @@
pthread_exit(NULL);
}
this->ProcessMessages();
-
- usleep(SLEEPTIME_US);
+ ts.tv_sec = 0;
+ ts.tv_nsec = SLEEPTIME_NS;
+ nanosleep(&ts, NULL);
}
}
@@ -281,36 +315,37 @@
player_msghdr * hdr,
void * data)
{
- player_localize_set_pose_t* setposereq;
-
// Is it a request to set the filter's pose?
if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LOCALIZE_REQ_SET_POSE,
this->localize_addr))
{
- setposereq = (player_localize_set_pose_t*)data;
player_simulation_pose2d_req_t req;
-
+ player_localize_set_pose_t * setposereq =
(player_localize_set_pose_t*)data;
+ assert(setposereq);
req.pose.px = setposereq->mean.px;
req.pose.py = setposereq->mean.py;
req.pose.pa = setposereq->mean.pa;
- strcpy(req.name, this->model);
+ req.name = this->model;
+ req.name_count = strlen(req.name) + 1;
+
// look up the named model
Message * Reply = sim->Request(InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_SET_POSE2D,
- (void *) &req, sizeof(req), NULL);
- if (Reply && Reply->GetHeader()->type == PLAYER_MSGTYPE_RESP_ACK)
+ reinterpret_cast<void *>(&req), sizeof
req, NULL);
+ if (!Reply) return -1;
+ if (Reply->GetHeader()->type == PLAYER_MSGTYPE_RESP_ACK)
{
// Send an ACK
this->Publish(this->localize_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LOCALIZE_REQ_SET_POSE);
- return(0);
+ return 0;
}
else
{
- delete (Reply);
+ delete Reply;
return -1;
}
}
@@ -323,16 +358,16 @@
// Request pose
cfg.name = this->model;
- cfg.name_count = strlen(cfg.name);
+ cfg.name_count = strlen(cfg.name) + 1;
Message * Reply = sim->Request(InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_GET_POSE2D,
- (void *) &cfg, sizeof(cfg), NULL);
-
- if (Reply && Reply->GetHeader()->type == PLAYER_MSGTYPE_RESP_ACK)
+ reinterpret_cast<void *>(&cfg), sizeof
cfg, NULL);
+ if (!Reply) return -1;
+ if (Reply->GetHeader()->type == PLAYER_MSGTYPE_RESP_ACK)
{
- assert(Reply->GetDataSize() == sizeof(cfg));
+ assert(Reply->GetDataSize() == sizeof cfg);
player_simulation_pose2d_req_t * ans =
reinterpret_cast<player_simulation_pose2d_req_t *> (Reply->GetPayload());
-
+ assert(ans);
player_localize_get_particles_t resp;
resp.mean.px = ans->pose.px;
resp.mean.py = ans->pose.py;
@@ -355,7 +390,7 @@
delete Reply;
Reply = NULL;
- return(0);
+ return 0;
}
else
{
@@ -363,6 +398,5 @@
return -1;
}
}
- return(-1);
+ return -1;
}
-
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:29:18 UTC (rev 6363)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
2008-04-18 08:35:32 UTC (rev 6364)
@@ -34,13 +34,17 @@
#define HASH_LEN (MD5_DIGEST_LENGTH / sizeof(unsigned int))
#endif
+#include <sys/time.h>
+static double get_time(void);
+
#if 0
void draw_cspace(plan_t* plan, const char* fname);
#endif
// Create a planner
plan_t *plan_alloc(double abs_min_radius, double des_min_radius,
- double max_radius, double dist_penalty)
+ double max_radius, double dist_penalty,
+ double hysteresis_factor)
{
plan_t *plan;
@@ -51,6 +55,7 @@
plan->max_radius = max_radius;
plan->dist_penalty = dist_penalty;
+ plan->hysteresis_factor = hysteresis_factor;
plan->heap = heap_alloc(PLAN_DEFAULT_HEAP_SIZE, (heap_free_elt_fn_t)NULL);
assert(plan->heap);
@@ -71,42 +76,40 @@
plan_set_obstacles(plan_t* plan, double* obs, size_t num)
{
size_t i;
+ int j;
int di,dj;
float* p;
plan_cell_t* cell, *ncell;
+ double t0,t1;
- // Remove any previous obstacles
- for(i=0;i<plan->obs_pts_num;i++)
+ t0 = get_time();
+
+ // Start with static obstacle data
+ cell = plan->cells;
+ for(j=0;j<plan->size_y*plan->size_x;j++,cell++)
{
- cell = plan->cells +
- PLAN_INDEX(plan,plan->obs_pts[2*i],plan->obs_pts[2*i+1]);
cell->occ_state_dyn = cell->occ_state;
cell->occ_dist_dyn = cell->occ_dist;
+ cell->mark = 0;
}
- // Do we need more room?
- if(num > plan->obs_pts_size)
+ // Expand around the dynamic obstacle pts
+ for(i=0;i<num;i++)
{
- plan->obs_pts_size = num;
- plan->obs_pts = (unsigned short*)realloc(plan->obs_pts,
- sizeof(unsigned short) * 2 *
- plan->obs_pts_size);
- assert(plan->obs_pts);
- }
-
- // Copy and expand costs around them
- plan->obs_pts_num = num;
- for(i=0;i<plan->obs_pts_num;i++)
- {
// Convert to grid coords
int gx,gy;
gx = PLAN_GXWX(plan, obs[2*i]);
gy = PLAN_GYWY(plan, obs[2*i+1]);
- plan->obs_pts[2*i] = gx;
- plan->obs_pts[2*i+1] = gy;
+ if(!PLAN_VALID(plan,gx,gy))
+ continue;
+
cell = plan->cells + PLAN_INDEX(plan,gx,gy);
+ if(cell->mark)
+ continue;
+
+ cell->mark = 1;
cell->occ_state_dyn = 1;
cell->occ_dist_dyn = 0.0;
@@ -128,6 +131,9 @@
}
}
}
+
+ t1 = get_time();
+ printf("plan_set_obstacles: %.6lf\n", t1-t0);
}
void
@@ -373,10 +379,9 @@
}
}
-#if 0
#include <gdk-pixbuf/gdk-pixbuf.h>
- void
+void
draw_cspace(plan_t* plan, const char* fname)
{
GdkPixbuf* pixbuf;
@@ -394,7 +399,8 @@
for(i=0;i<plan->size_x;i++,p++)
{
paddr = p * 3;
- if(plan->cells[PLAN_INDEX(plan,i,j)].occ_state == 1)
+ //if(plan->cells[PLAN_INDEX(plan,i,j)].occ_state == 1)
+ if(plan->cells[PLAN_INDEX(plan,i,j)].occ_dist < plan->abs_min_radius)
{
pixels[paddr] = 255;
pixels[paddr+1] = 0;
@@ -533,7 +539,6 @@
gdk_pixbuf_unref(pixbuf);
free(pixels);
}
-#endif
// Construct the configuration space from the occupancy grid.
// This treats both occupied and unknown cells as bad.
@@ -709,3 +714,11 @@
MD5_Final((unsigned char*)digest,&c);
}
#endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+
+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/plan.h
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
2008-04-18 08:29:18 UTC (rev 6363)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
2008-04-18 08:35:32 UTC (rev 6364)
@@ -37,6 +37,8 @@
// Mark used in dynamic programming
char mark;
+ // Mark used in path hysterisis
+ char lpathmark;
// The next cell in the plan
struct _plan_cell_t *plan_next;
@@ -68,15 +70,13 @@
// Penalty factor for cells inside the max radius
double dist_penalty;
+
+ // Cost multiplier for cells on the previous local path
+ double hysteresis_factor;
// The grid data
plan_cell_t *cells;
- // Dynamic obstacle hitpoints
- unsigned short* obs_pts;
- size_t obs_pts_size;
- size_t obs_pts_num;
-
// Distance penalty kernel, pre-computed in plan_compute_dist_kernel();
float* dist_kernel;
int dist_kernel_width;
@@ -100,8 +100,11 @@
// Create a planner
-plan_t *plan_alloc(double abs_min_radius, double des_min_radius,
- double max_radius, double dist_penalty);
+plan_t *plan_alloc(double abs_min_radius,
+ double des_min_radius,
+ double max_radius,
+ double dist_penalty,
+ double hysteresis_factor);
void plan_compute_dist_kernel(plan_t* plan);
@@ -148,6 +151,8 @@
double lx, double ly,
double maxdist, double distweight);
+void plan_set_obstacles(plan_t* plan, double* obs, size_t num);
+
#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().
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:29:18 UTC (rev 6363)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan_control.c
2008-04-18 08:35:32 UTC (rev 6364)
@@ -1,4 +1,5 @@
#include <stdlib.h>
+#include <assert.h>
#include "plan.h"
@@ -12,12 +13,21 @@
int li, lj;
double dist, d;
double cost, bestcost;
+ char old_occ_state;
+ float old_occ_dist;
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
cell = plan->cells + PLAN_INDEX(plan,li,lj);
+ // Latch and clear the obstacle state for the cell I'm in
+ cell = plan->cells + PLAN_INDEX(plan, li, lj);
+ old_occ_state = cell->occ_state_dyn;
+ old_occ_dist = cell->occ_dist_dyn;
+ cell->occ_state_dyn = -1;
+ cell->occ_dist_dyn = plan->max_radius;
+
// Step back from maxdist, looking for the best carrot
bestcost = -1.0;
for(dist = maxdist; dist >= plan->scale; dist -= plan->scale)
@@ -30,7 +40,11 @@
// 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);
continue;
+ }
// Weight distance
cost += distweight * (1.0/(dist*dist));
@@ -41,6 +55,11 @@
*py = PLAN_WYGY(plan,ncell->cj);
}
}
+
+ // Restore the obstacle state for the cell I'm in
+ cell = plan->cells + PLAN_INDEX(plan, li, lj);
+ cell->occ_state_dyn = old_occ_state;
+ cell->occ_dist_dyn = old_occ_dist;
return(bestcost);
}
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:29:18 UTC (rev 6363)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
2008-04-18 08:35:32 UTC (rev 6364)
@@ -80,14 +80,15 @@
int xmin,ymin,xmax,ymax;
plan_cell_t* cell;
double t0,t1;
+ int i;
t0 = get_time();
// Set bounds as directed
xmin = PLAN_GXWX(plan, lx - plan_halfwidth);
- ymin = PLAN_GXWX(plan, ly - plan_halfwidth);
+ ymin = PLAN_GYWY(plan, ly - plan_halfwidth);
xmax = PLAN_GXWX(plan, lx + plan_halfwidth);
- ymax = PLAN_GXWX(plan, ly + plan_halfwidth);
+ ymax = PLAN_GYWY(plan, ly + plan_halfwidth);
plan_set_bounds(plan, xmin, ymin, xmax, ymax);
// Reset plan costs (within the local patch)
@@ -112,6 +113,11 @@
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
+ // Reset path marks (TODO: find a smarter place to do this)
+ cell = plan->cells;
+ for(i=0;i<plan->size_x*plan->size_y;i++,cell++)
+ cell->lpathmark = 0;
+
// Cache the path
for(cell = plan->cells + PLAN_INDEX(plan,li,lj);
cell;
@@ -125,6 +131,7 @@
assert(plan->lpath);
}
plan->lpath[plan->lpath_count++] = cell;
+ cell->lpathmark = 1;
}
t1 = get_time();
@@ -142,6 +149,8 @@
int gi, gj, li,lj;
float cost;
plan_cell_t *cell, *ncell;
+ char old_occ_state;
+ float old_occ_dist;
// Reset the queue
heap_reset(plan->heap);
@@ -153,7 +162,7 @@
// Initialize the start cell
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
-
+
//printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj);
if(!PLAN_VALID_BOUNDS(plan, gi, gj))
@@ -168,6 +177,13 @@
return(-1);
}
+ // Latch and clear the obstacle state for the cell I'm in
+ cell = plan->cells + PLAN_INDEX(plan, li, lj);
+ old_occ_state = cell->occ_state_dyn;
+ old_occ_dist = cell->occ_dist_dyn;
+ cell->occ_state_dyn = -1;
+ cell->occ_dist_dyn = plan->max_radius;
+
cell = plan->cells + PLAN_INDEX(plan, gi, gj);
cell->plan_cost = 0;
plan_push(plan, cell);
@@ -206,7 +222,11 @@
if (ncell->occ_dist_dyn < plan->abs_min_radius)
continue;
- cost = cell->plan_cost + *p;
+ cost = cell->plan_cost;
+ if(ncell->lpathmark)
+ cost += (*p) * plan->hysteresis_factor;
+ else
+ cost += *p;
if(ncell->occ_dist_dyn < plan->max_radius)
cost += plan->dist_penalty * (plan->max_radius -
ncell->occ_dist_dyn);
@@ -222,7 +242,11 @@
}
}
+ // Restore the obstacle state for the cell I'm in
cell = plan->cells + PLAN_INDEX(plan, li, lj);
+ cell->occ_state_dyn = old_occ_state;
+ cell->occ_dist_dyn = old_occ_dist;
+
if(!cell->plan_next)
{
puts("never found start");
@@ -253,6 +277,8 @@
li = PLAN_GXWX(plan, lx);
lj = PLAN_GYWY(plan, ly);
+ assert(PLAN_VALID_BOUNDS(plan,li,lj));
+
// Find the closest place to jump on the global path
squared_d_min = DBL_MAX;
c_min = -1;
Modified: code/player/branches/cmake/server/drivers/planner/wavefront/test.c
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/test.c
2008-04-18 08:29:18 UTC (rev 6363)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/test.c
2008-04-18 08:35:32 UTC (rev 6364)
@@ -33,9 +33,9 @@
int i, j;
plan_t* plan;
- double robot_radius=0.25;
- double safety_dist=0.2;
- double max_radius=1.0;
+ double robot_radius=0.16;
+ double safety_dist=0.05;
+ double max_radius=0.25;
double dist_penalty=1.0;;
double plan_halfwidth = 5.0;
@@ -76,9 +76,15 @@
}
}
free(mapdata);
+
+ plan->scale = res;
+ plan->size_x = sx;
+ plan->size_y = sy;
+ plan->origin_x = 0.0;
+ plan->origin_y = 0.0;
// Do initialization
- plan_init(plan, res, sx, sy, 0.0, 0.0);
+ plan_init(plan);
t_c0 = get_time();
plan_compute_cspace(plan);
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:29:18 UTC (rev 6363)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
2008-04-18 08:35:32 UTC (rev 6364)
@@ -219,9 +219,14 @@
#include <assert.h>
#include <math.h>
+
#include <libplayercore/playercore.h>
#include "plan.h"
+#include <sys/time.h>
+static double get_time(void);
+extern "C" { void draw_cspace(plan_t* plan, const char* fname); }
+
// TODO: monitor localize timestamps, and slow or stop robot accordingly
// time to sleep between loops (us)
@@ -237,6 +242,8 @@
player_devaddr_t position_id;
player_devaddr_t localize_id;
player_devaddr_t map_id;
+ player_devaddr_t laser_id;
+ player_devaddr_t graphics2d_id;
double map_res;
double robot_radius;
double safety_dist;
@@ -253,6 +260,8 @@
Device* position;
Device* localize;
Device* mapdevice;
+ Device* laser;
+ Device* graphics2d;
// are we disabled?
bool enable;
@@ -299,19 +308,35 @@
int force_map_refresh;
// Should we do velocity control, or position control?
bool velocity_control;
+ // How many laser scans should we buffer?
+ int scans_size;
+ // How far out do we insert obstacles?
+ double scan_maxrange;
+ // The scan buffer
+ player_laser_data_scanpose_t* scans;
+ int scans_count;
+ int scans_idx;
+ double* scan_points;
+ int scan_points_size;
+ int scan_points_count;
// methods for internal use
int SetupLocalize();
+ int SetupLaser();
int SetupPosition();
int SetupMap();
+ int SetupGraphics2d();
int GetMap(bool threaded);
int GetMapInfo(bool threaded);
int ShutdownPosition();
int ShutdownLocalize();
+ int ShutdownLaser();
int ShutdownMap();
+ int ShutdownGraphics2d();
double angle_diff(double a, double b);
void ProcessCommand(player_planner_cmd_t* cmd);
+ void ProcessLaserScan(player_laser_data_scanpose_t* data);
void ProcessLocalizeData(player_position2d_data_t* data);
void ProcessPositionData(player_position2d_data_t* data);
void ProcessMapInfo(player_map_info_t* info);
@@ -378,6 +403,17 @@
this->SetError(-1);
return;
}
+
+ // Can use a laser device
+ memset(&this->laser_id,0,sizeof(player_devaddr_t));
+ cf->ReadDeviceAddr(&this->laser_id, section, "requires",
+ PLAYER_LASER_CODE, -1, NULL);
+
+ // Can use a graphics2d device
+ memset(&this->graphics2d_id,0,sizeof(player_devaddr_t));
+ cf->ReadDeviceAddr(&this->graphics2d_id, section, "requires",
+ PLAYER_GRAPHICS2D_CODE, -1, NULL);
+
this->safety_dist = cf->ReadLength(section,"safety_dist", 0.25);
this->max_radius = cf->ReadLength(section,"max_radius",1.0);
this->dist_penalty = cf->ReadFloat(section,"dist_penalty",1.0);
@@ -391,6 +427,18 @@
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);
+ if(this->laser_id.interf)
+ {
+ this->scans_size = cf->ReadInt(section, "laser_buffer_size", 10);
+ if(this->scans_size < 1)
+ {
+ PLAYER_WARN("must buffer at least one laser scan");
+ this->scans_size = 1;
+ }
+ this->scan_maxrange = cf->ReadLength(section, "laser_maxrange", 6.0);
+ }
+ else
+ this->scans_size = 0;
}
@@ -425,7 +473,8 @@
if(!(this->plan = plan_alloc(this->robot_radius+this->safety_dist,
this->robot_radius+this->safety_dist,
this->max_radius,
- this->dist_penalty)))
+ this->dist_penalty,
+ 0.5)))
{
PLAYER_ERROR("failed to allocate plan");
return(-1);
@@ -436,6 +485,28 @@
if(SetupLocalize() < 0)
return(-1);
+ if(this->laser_id.interf)
+ {
+ if(SetupLaser() < 0)
+ return(-1);
+
+ this->scans =
+ (player_laser_data_scanpose_t*)malloc(this->scans_size *
+
sizeof(player_laser_data_scanpose_t));
+ assert(this->scans);
+ this->scans_idx = 0;
+ this->scans_count = 0;
+ this->scan_points = NULL;
+ this->scan_points_size = 0;
+ this->scan_points_count = 0;
+ }
+
+ if(this->graphics2d_id.interf)
+ {
+ if(SetupGraphics2d() < 0)
+ return(-1);
+ }
+
// Start the driver thread.
this->StartThread();
return 0;
@@ -458,6 +529,10 @@
ShutdownPosition();
ShutdownLocalize();
ShutdownMap();
+ if(this->laser_id.interf)
+ ShutdownLaser();
+ if(this->graphics2d_id.interf)
+ ShutdownGraphics2d();
return 0;
}
@@ -489,6 +564,120 @@
#endif
}
+void
+Wavefront::ProcessLaserScan(player_laser_data_scanpose_t* data)
+{
+ double t0,t1;
+
+ t0 = get_time();
+
+ // free up the old scan, if we're replacing one
+ if(this->scans_idx < this->scans_count)
+ playerxdr_cleanup_message(this->scans+this->scans_idx,
+ PLAYER_LASER_CODE, PLAYER_MSGTYPE_DATA,
+ PLAYER_LASER_DATA_SCANPOSE);
+ // copy in the new scan
+ playerxdr_deepcopy_message(data, this->scans+this->scans_idx,
+ PLAYER_LASER_CODE, PLAYER_MSGTYPE_DATA,
+ PLAYER_LASER_DATA_SCANPOSE);
+ //memcpy(this->scans+this->scans_idx, data,
+ //sizeof(player_laser_data_scanpose_t));
+
+ // update counters
+ this->scans_count++;
+ 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);
+
+ // run through the scans to see how much room we need to store all the
+ // hitpoints
+ int hitpt_cnt=0;
+ player_laser_data_scanpose_t* scan = this->scans;
+ for(int i=0;i<this->scans_count;i++,scan++)
+ hitpt_cnt += scan->scan.ranges_count*2;
+
+ // allocate more space as necessary
+ if(this->scan_points_size < hitpt_cnt)
+ {
+ this->scan_points_size = hitpt_cnt;
+ this->scan_points = (double*)realloc(this->scan_points,
+
this->scan_points_size*sizeof(double));
+ assert(this->scan_points);
+ }
+
+ // project hit points from each scan
+ scan = this->scans;
+ double* pts = this->scan_points;
+ this->scan_points_count = 0;
+ for(int i=0;i<this->scans_count;i++,scan++)
+ {
+ float b=scan->scan.min_angle;
+ float* r=scan->scan.ranges;
+ for(unsigned int j=0;
+ j<scan->scan.ranges_count;
+ j++,r++,b+=scan->scan.resolution)
+ {
+ if(((*r) >= this->scan_maxrange) || ((*r) >= scan->scan.max_range))
+ continue;
+ //double rx, ry;
+ //rx = (*r)*cos(b);
+ //ry = (*r)*sin(b);
+
+ double cs,sn;
+ cs = cos(scan->pose.pa+b);
+ sn = sin(scan->pose.pa+b);
+
+ double lx,ly;
+ lx = scan->pose.px + (*r)*cs;
+ ly = scan->pose.py + (*r)*sn;
+
+ assert(this->scan_points_count*2 < this->scan_points_size);
+ *(pts++) = lx;
+ *(pts++) = ly;
+ 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);
+
+#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)*
+ hitpt_cnt/2));
+ pts.points_count = hitpt_cnt/2;
+ pts.color.alpha = 0;
+ pts.color.red = 255;
+ pts.color.blue = 0;
+ pts.color.green = 0;
+ for(int i=0;i<hitpt_cnt/2;i++)
+ {
+ pts.points[i].px = this->scan_points[2*i];
+ pts.points[i].py = this->scan_points[2*i+1];
+ }
+
+ this->graphics2d->PutMsg(this->InQueue,
+ PLAYER_MSGTYPE_CMD,
+ PLAYER_GRAPHICS2D_CMD_POINTS,
+ (void*)&pts,0,NULL);
+ free(pts.points);
+ }
+#endif
+}
+
void
Wavefront::ProcessLocalizeData(player_position2d_data_t* data)
{
@@ -704,6 +893,13 @@
this->PutPlannerData();
}
+ if(!this->enable)
+ {
+ this->StopPosition();
+ usleep(CYCLE_TIME_US);
+ continue;
+ }
+
// Is it time to replan?
replan_timediff = t - last_replan_time;
replan_dist = sqrt(((this->localize_x - last_replan_lx) *
@@ -766,12 +962,15 @@
this->new_map = false;
}
#endif
+ double t0,t1;
+ t0 = get_time();
+
// compute costs to the new goal. Try local plan first
if(new_goal ||
(this->plan->path_count == 0) ||
(plan_do_local(this->plan, this->localize_x,
- this->localize_y, 5.0) < 0))
+ this->localize_y, this->scan_maxrange) < 0))
{
if(!new_goal && (this->plan->path_count != 0))
puts("Wavefront: local plan failed");
@@ -784,6 +983,35 @@
this->new_goal = false;
}
+ 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);
+ line.color.alpha = 0;
+ line.color.red = 0;
+ line.color.green = 255;
+ line.color.blue = 0;
+ for(int i=0;i<this->plan->lpath_count;i++)
+ {
+ line.points[i].px = PLAN_WXGX(this->plan,this->plan->lpath[i]->ci);
+ line.points[i].py = PLAN_WYGY(this->plan,this->plan->lpath[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);
+
if(!this->velocity_control)
{
// extract waypoints along the path to the goal from the current
position
@@ -842,16 +1070,11 @@
}
}
- if(!this->enable)
- {
- this->StopPosition();
- usleep(CYCLE_TIME_US);
- continue;
- }
-
if(this->velocity_control)
{
+ double t0, t1;
+ t0 = get_time();
if(this->plan->path_count && !this->atgoal)
{
// Check doneness
@@ -874,12 +1097,16 @@
double maxd=2.0;
double distweight=10.0;
+ 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");
this->StopPosition();
+ exit(-1);
}
else
{
@@ -942,6 +1169,9 @@
}
else
this->StopPosition();
+
+ t1 = get_time();
+ printf("control: %.6lf\n", t1-t0);
}
else // !velocity_control
{
@@ -1090,15 +1320,56 @@
// take the bigger of the two dimensions, convert to meters, and halve
// to get a radius
- this->robot_radius = MAX(geom->size.sl, geom->size.sw);
+ //this->robot_radius = MAX(geom->size.sl, geom->size.sw);
+ this->robot_radius = geom->size.sw;
this->robot_radius /= 2.0;
+ printf("robot radius: %.3lf\n", this->robot_radius);
+
delete msg;
return 0;
}
////////////////////////////////////////////////////////////////////////////////
+// Set up the underlying laser device.
+int
+Wavefront::SetupLaser()
+{
+ // Subscribe to the laser device.
+ if(!(this->laser = deviceTable->GetDevice(this->laser_id)))
+ {
+ PLAYER_ERROR("unable to locate suitable laser device");
+ return(-1);
+ }
+ if(this->laser->Subscribe(this->InQueue) != 0)
+ {
+ PLAYER_ERROR("unable to subscribe to laser device");
+ return(-1);
+ }
+
+ return(0);
+}
+
+int
+Wavefront::SetupGraphics2d()
+{
+ // Subscribe to the graphics2d device.
+ if(!(this->graphics2d = deviceTable->GetDevice(this->graphics2d_id)))
+ {
+ PLAYER_ERROR("unable to locate suitable graphics2d device");
+ return(-1);
+ }
+ if(this->graphics2d->Subscribe(this->InQueue) != 0)
+ {
+ PLAYER_ERROR("unable to subscribe to graphics2d device");
+ return(-1);
+ }
+
+ return(0);
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Set up the underlying localize device.
int
Wavefront::SetupLocalize()
@@ -1194,6 +1465,7 @@
plan_init(this->plan);
plan_compute_cspace(this->plan);
+ draw_cspace(this->plan,"cspace.png");
return(0);
}
@@ -1283,6 +1555,18 @@
}
int
+Wavefront::ShutdownLaser()
+{
+ return(this->laser->Unsubscribe(this->InQueue));
+}
+
+int
+Wavefront::ShutdownGraphics2d()
+{
+ return(this->graphics2d->Unsubscribe(this->InQueue));
+}
+
+int
Wavefront::ShutdownMap()
{
return(this->mapdevice->Unsubscribe(this->InQueue));
@@ -1330,7 +1614,8 @@
this->plan = plan_alloc(this->robot_radius+this->safety_dist,
this->robot_radius+this->safety_dist,
this->max_radius,
- this->dist_penalty);
+ this->dist_penalty,
+ 0.5);
assert(this->plan);
// Fill in the map structure
@@ -1412,6 +1697,15 @@
this->new_map_available = true;
return(0);
}
+ // Is it a pose-stamped laser scan?
+ else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
+ PLAYER_LASER_DATA_SCANPOSE,
+ this->laser_id))
+ {
+ player_laser_data_scanpose_t* pdata = (player_laser_data_scanpose_t*)data;
+ this->ProcessLaserScan(pdata);
+ return(0);
+ }
else
return(-1);
}
@@ -1432,3 +1726,11 @@
else
return(d2);
}
+
+double
+static get_time(void)
+{
+ struct timeval curr;
+ gettimeofday(&curr,NULL);
+ return(curr.tv_sec + curr.tv_usec / 1e6);
+}
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
