Update of /cvsroot/playerstage/code/player/server/drivers/localization/amcl
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv10946/server/drivers/localization/amcl
Modified Files:
.cvsignore amcl.cc amcl.h amcl_laser.cc amcl_laser.h
Log Message:
applied Toby's patch to replace fixed-size arrays
Index: amcl_laser.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/localization/amcl/amcl_laser.cc,v
retrieving revision 1.25
retrieving revision 1.26
diff -C2 -d -r1.25 -r1.26
*** amcl_laser.cc 23 Aug 2007 19:58:44 -0000 1.25
--- amcl_laser.cc 1 Nov 2007 22:16:19 -0000 1.26
***************
*** 185,191 ****
// allocate space for map cells
! assert(this->map->cells = (map_cell_t*)malloc(sizeof(map_cell_t) *
! this->map->size_x *
! this->map->size_y));
// now, get the map data
--- 185,192 ----
// allocate space for map cells
! this->map->cells = (map_cell_t*)malloc(sizeof(map_cell_t) *
! this->map->size_x *
! this->map->size_y);
! assert(this->map->cells);
// now, get the map data
***************
*** 197,207 ****
int si,sj;
! reqlen = sizeof(player_map_data_t) - PLAYER_MAP_MAX_TILE_SIZE + 4;
! data_req = (player_map_data_t*)calloc(1, reqlen);
assert(data_req);
! // Tile size
! sy = sx = (int)sqrt(PLAYER_MAP_MAX_TILE_SIZE);
! assert(sx * sy < (int)PLAYER_MAP_MAX_TILE_SIZE);
oi=oj=0;
while((oi < this->map->size_x) && (oj < this->map->size_y))
--- 198,206 ----
int si,sj;
! data_req = (player_map_data_t*) malloc(sizeof(player_map_data_t));
assert(data_req);
! // Tile size, limit to sensible maximum of 640x640 tiles
! sy = sx = 640;
oi=oj=0;
while((oi < this->map->size_x) && (oj < this->map->size_y))
***************
*** 303,307 ****
ndata->range_count = data->ranges_count;
ndata->range_max = data->max_range;
! assert((size_t) ndata->range_count < sizeof(ndata->ranges) /
sizeof(ndata->ranges[0]));
// Read the range data
--- 302,306 ----
ndata->range_count = data->ranges_count;
ndata->range_max = data->max_range;
! ndata->ranges = new double [ndata->range_count][2];
// Read the range data
Index: .cvsignore
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/localization/amcl/.cvsignore,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** .cvsignore 17 Sep 2007 02:18:58 -0000 1.3
--- .cvsignore 1 Nov 2007 22:16:19 -0000 1.4
***************
*** 3,4 ****
--- 3,6 ----
.deps
*.la
+ .libs
+ *.lo
Index: amcl.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/localization/amcl/amcl.cc,v
retrieving revision 1.79
retrieving revision 1.80
diff -C2 -d -r1.79 -r1.80
*** amcl.cc 21 Sep 2007 03:31:50 -0000 1.79
--- amcl.cc 1 Nov 2007 22:16:19 -0000 1.80
***************
*** 463,466 ****
--- 463,468 ----
// Initial hypothesis list
this->hyp_count = 0;
+ this->hyp_alloc = 0;
+ this->hyps = NULL;
pthread_mutex_init(&this->best_hyp_lock,NULL);
***************
*** 483,486 ****
--- 485,489 ----
// Delete sensor data queue
delete[] this->q_data;
+ free(hyps);
// Delete sensors
***************
*** 925,928 ****
--- 928,936 ----
//pf_vector_fprintf(pose_mean, stdout, "%.3f");
+ if (this->hyp_count +1 > this->hyp_alloc)
+ {
+ this->hyp_alloc = this->hyp_count+1;
+ this->hyps = (amcl_hyp_t*)realloc(this->hyps,
sizeof(amcl_hyp_t)*this->hyp_alloc);
+ }
hyp = this->hyps + this->hyp_count++;
hyp->weight = weight;
***************
*** 1005,1009 ****
pf_vector_t pose;
pf_matrix_t pose_cov;
! size_t datalen;
player_localize_data_t data;
--- 1013,1017 ----
pf_vector_t pose;
pf_matrix_t pose_cov;
! //size_t datalen;
player_localize_data_t data;
***************
*** 1014,1017 ****
--- 1022,1026 ----
// Encode the hypotheses
data.hypoths_count = this->hyp_count;
+ data.hypoths = new player_localize_hypoth_t[data.hypoths_count];
for (i = 0; i < this->hyp_count; i++)
{
***************
*** 1049,1061 ****
sizeof(player_localize_hypoth_t),&hypoth_compare);
- // Compute the length of the data packet
- datalen = (sizeof(data) - sizeof(data.hypoths) +
- data.hypoths_count * sizeof(data.hypoths[0]));
-
// Push data out
this->Publish(this->localize_addr,
PLAYER_MSGTYPE_DATA,
PLAYER_LOCALIZE_DATA_HYPOTHS,
! (void*)&data,datalen,&time);
}
--- 1058,1067 ----
sizeof(player_localize_hypoth_t),&hypoth_compare);
// Push data out
this->Publish(this->localize_addr,
PLAYER_MSGTYPE_DATA,
PLAYER_LOCALIZE_DATA_HYPOTHS,
! (void*)&data);
! delete [] data.hypoths;
}
***************
*** 1148,1153 ****
set = this->pf->sets + this->pf->current_set;
! resp.particles_count =
! MIN(set->sample_count,PLAYER_LOCALIZE_PARTICLES_MAX);
// TODO: pick representative particles
--- 1154,1159 ----
set = this->pf->sets + this->pf->current_set;
! resp.particles_count = set->sample_count;
! resp.particles = new player_localize_particle_t [resp.particles_count];
// TODO: pick representative particles
***************
*** 1164,1168 ****
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LOCALIZE_REQ_GET_PARTICLES,
! (void*)&resp, sizeof(resp), NULL);
return(0);
} else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
--- 1170,1175 ----
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LOCALIZE_REQ_GET_PARTICLES,
! (void*)&resp);
! delete [] resp.particles;
return(0);
} else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
Index: amcl_laser.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/localization/amcl/amcl_laser.h,v
retrieving revision 1.12
retrieving revision 1.13
diff -C2 -d -r1.12 -r1.13
*** amcl_laser.h 23 Aug 2007 19:58:44 -0000 1.12
--- amcl_laser.h 1 Nov 2007 22:16:19 -0000 1.13
***************
*** 37,44 ****
class AMCLLaserData : public AMCLSensorData
{
// Laser range data (range, bearing tuples)
public: int range_count;
public: double range_max;
! public: double ranges[PLAYER_LASER_MAX_SAMPLES][2];
};
--- 37,47 ----
class AMCLLaserData : public AMCLSensorData
{
+ public:
+ AMCLLaserData () {ranges=NULL;};
+ ~AMCLLaserData() {delete [] ranges;};
// Laser range data (range, bearing tuples)
public: int range_count;
public: double range_max;
! public: double (*ranges)[2];
};
Index: amcl.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/localization/amcl/amcl.h,v
retrieving revision 1.17
retrieving revision 1.18
diff -C2 -d -r1.17 -r1.18
*** amcl.h 23 Aug 2007 19:58:44 -0000 1.17
--- amcl.h 1 Nov 2007 22:16:19 -0000 1.18
***************
*** 174,178 ****
// Current particle filter pose estimates
private: int hyp_count;
! private: amcl_hyp_t hyps[PLAYER_LOCALIZE_MAX_HYPOTHS];
private: pf_vector_t best_hyp;
private: pthread_mutex_t best_hyp_lock;
--- 174,179 ----
// Current particle filter pose estimates
private: int hyp_count;
! private: int hyp_alloc;
! private: amcl_hyp_t *hyps;
private: pf_vector_t best_hyp;
private: pthread_mutex_t best_hyp_lock;
-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems? Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
