Revision: 8253
http://playerstage.svn.sourceforge.net/playerstage/?rev=8253&view=rev
Author: gbiggs
Date: 2009-09-08 01:59:32 +0000 (Tue, 08 Sep 2009)
Log Message:
-----------
Applied patch #2847832: Extended map interface
Modified Paths:
--------------
code/player/trunk/client_libs/libplayerc/dev_map.c
code/player/trunk/client_libs/libplayerc/playerc.h
code/player/trunk/client_libs/libplayerc++/playerc++.h
code/player/trunk/libplayerinterface/interfaces/042_map.def
code/player/trunk/server/drivers/localization/amcl/amcl_fiducial.cc
code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc
code/player/trunk/server/drivers/localization/amcl/map/map.h
code/player/trunk/server/drivers/map/mapfile.cc
code/player/trunk/server/drivers/map/maptransform.cc
code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp
code/player/trunk/server/drivers/vectormap/vec2map.cc
code/player/trunk/utils/playerv/pv_dev_map.c
Modified: code/player/trunk/client_libs/libplayerc/dev_map.c
===================================================================
--- code/player/trunk/client_libs/libplayerc/dev_map.c 2009-09-08 01:51:40 UTC
(rev 8252)
+++ code/player/trunk/client_libs/libplayerc/dev_map.c 2009-09-08 01:59:32 UTC
(rev 8253)
@@ -184,6 +184,8 @@
}
#endif
+ device->data_range = data_resp->data_range;
+
// copy the map data
for(j=0;j<sj;j++)
{
Modified: code/player/trunk/client_libs/libplayerc/playerc.h
===================================================================
--- code/player/trunk/client_libs/libplayerc/playerc.h 2009-09-08 01:51:40 UTC
(rev 8252)
+++ code/player/trunk/client_libs/libplayerc/playerc.h 2009-09-08 01:59:32 UTC
(rev 8253)
@@ -2352,9 +2352,12 @@
/** Map origin, in meters (i.e., the real-world coordinates of cell 0,0)*/
double origin[2];
- /** Occupancy for each cell (empty = -1, unknown = 0, occupied = +1) */
+ /** Value for each cell (-range <= EMPTY < 0, unknown = 0, 0 < OCCUPIED <=
range) */
+ int8_t data_range;
+
+ /** Occupancy for each cell */
char* cells;
-
+
/** Vector-based version of the map (call playerc_map_get_vector() to
* fill this in). */
double vminx, vminy, vmaxx, vmaxy;
Modified: code/player/trunk/client_libs/libplayerc++/playerc++.h
===================================================================
--- code/player/trunk/client_libs/libplayerc++/playerc++.h 2009-09-08
01:51:40 UTC (rev 8252)
+++ code/player/trunk/client_libs/libplayerc++/playerc++.h 2009-09-08
01:59:32 UTC (rev 8253)
@@ -1457,7 +1457,10 @@
double GetOriginX() const { return GetVar(mDevice->origin[0]); };
double GetOriginY() const { return GetVar(mDevice->origin[1]); };
- /// Occupancy for each cell (empty = -1, unknown = 0, occupied = +1)
+ /// Range of grid data (default: empty = -1, unknown = 0, occupied = +1)
+ int8_t GetDataRange() const { return GetVar(mDevice->data_range); };
+
+ /// Occupancy for each cell
void GetMap(int8_t* aMap) const
{
return GetVarByRef(reinterpret_cast<int8_t*>(mDevice->cells),
Modified: code/player/trunk/libplayerinterface/interfaces/042_map.def
===================================================================
--- code/player/trunk/libplayerinterface/interfaces/042_map.def 2009-09-08
01:51:40 UTC (rev 8252)
+++ code/player/trunk/libplayerinterface/interfaces/042_map.def 2009-09-08
01:59:32 UTC (rev 8253)
@@ -61,7 +61,9 @@
uint32_t height;
/** The number of cells */
uint32_t data_count;
- /** Cell occupancy value (empty = -1, unknown = 0, occupied = +1). */
+ /** Maximum value for each cell (-range <= EMPTY < 0, unknown = 0, 0 <
OCCUPIED <= range) */
+ int8_t data_range;
+ /** Cell occupancy value */
int8_t *data;
} player_map_data_t;
Modified: code/player/trunk/server/drivers/localization/amcl/amcl_fiducial.cc
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/amcl_fiducial.cc
2009-09-08 01:51:40 UTC (rev 8252)
+++ code/player/trunk/server/drivers/localization/amcl/amcl_fiducial.cc
2009-09-08 01:59:32 UTC (rev 8253)
@@ -215,6 +215,8 @@
this->map->size_x = ntohl(info.width);
this->map->size_y = ntohl(info.height);
+ this->map->data_range = 1;
+
// allocate space for map cells
assert(this->map->cells = (map_cell_t*)malloc(sizeof(map_cell_t) *
this->map->size_x *
Modified: code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc
2009-09-08 01:51:40 UTC (rev 8252)
+++ code/player/trunk/server/drivers/localization/amcl/amcl_laser.cc
2009-09-08 01:59:32 UTC (rev 8253)
@@ -180,6 +180,7 @@
this->map->scale = info->scale;
this->map->size_x = info->width;
this->map->size_y = info->height;
+ this->map->data_range = 1;
delete msg;
Modified: code/player/trunk/server/drivers/localization/amcl/map/map.h
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/map/map.h
2009-09-08 01:51:40 UTC (rev 8252)
+++ code/player/trunk/server/drivers/localization/amcl/map/map.h
2009-09-08 01:59:32 UTC (rev 8253)
@@ -77,6 +77,8 @@
// Map dimensions (number of cells)
int size_x, size_y;
+ int8_t data_range;
+
// The map data, stored as a grid
map_cell_t *cells;
Modified: code/player/trunk/server/drivers/map/mapfile.cc
===================================================================
--- code/player/trunk/server/drivers/map/mapfile.cc 2009-09-08 01:51:40 UTC
(rev 8252)
+++ code/player/trunk/server/drivers/map/mapfile.cc 2009-09-08 01:59:32 UTC
(rev 8253)
@@ -256,6 +256,7 @@
occ = color_avg / 255.0;
else
occ = (255 - color_avg) / 255.0;
+
if(occ > 0.95)
this->mapdata[MAP_IDX(this,i,this->size_y - j - 1)] = +1;
else if(occ < 0.1)
@@ -336,6 +337,7 @@
sj = mapresp->height = mapreq->height;
mapresp->data_count = mapresp->width * mapresp->height;
mapresp->data = new int8_t [mapresp->data_count];
+ mapresp->data_range = 1;
// Grab the pixels from the map
for(j = 0; j < sj; j++)
{
Modified: code/player/trunk/server/drivers/map/maptransform.cc
===================================================================
--- code/player/trunk/server/drivers/map/maptransform.cc 2009-09-08
01:51:40 UTC (rev 8252)
+++ code/player/trunk/server/drivers/map/maptransform.cc 2009-09-08
01:59:32 UTC (rev 8253)
@@ -226,6 +226,7 @@
PLAYER_MSG4(9,"Block Requested is: %d,%d + %d,%d",oi,oj,si,sj);
resp_data.data_count = map_data.width * map_data.height;
resp_data.data = new int8_t [resp_data.data_count];
+ resp_data.data_range = map_data.data_range;
// Grab the pixels from the map
for(j = 0; j < sj; j++)
Modified: code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp
===================================================================
--- code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp
2009-09-08 01:51:40 UTC (rev 8252)
+++ code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp
2009-09-08 01:59:32 UTC (rev 8253)
@@ -1023,6 +1023,7 @@
}
}
mapresp.data_count = mapresp.width * mapresp.height;
+ mapresp.data_range = 1;
cout<<"\n --->>> Columns="<<oi<<" Rows="<<oj<<" width="<<si<<"
height="<<sj;
this->Publish(this->device_addr,
resp_queue,PLAYER_MSGTYPE_RESP_ACK,PLAYER_MAP_REQ_GET_DATA,(void*)(&mapresp),
sizeof(mapresp), NULL);
free(mapresp.data);
Modified: code/player/trunk/server/drivers/vectormap/vec2map.cc
===================================================================
--- code/player/trunk/server/drivers/vectormap/vec2map.cc 2009-09-08
01:51:40 UTC (rev 8252)
+++ code/player/trunk/server/drivers/vectormap/vec2map.cc 2009-09-08
01:59:32 UTC (rev 8253)
@@ -555,6 +555,7 @@
}
for (ii = 0; ii < (map_data_request.height); ii++) memcpy(map_data.data
+ (ii * (map_data_request.width)), cells + (ii * width) + map_data_request.col,
map_data_request.width);
}
+ map_data.data_range=1;
this->Publish(this->map_addr,
resp_queue,
Modified: code/player/trunk/utils/playerv/pv_dev_map.c
===================================================================
--- code/player/trunk/utils/playerv/pv_dev_map.c 2009-09-08 01:51:40 UTC
(rev 8252)
+++ code/player/trunk/utils/playerv/pv_dev_map.c 2009-09-08 01:59:32 UTC
(rev 8253)
@@ -145,14 +145,14 @@
rtk_fig_clear(map->fig);
puts( "map draw" );
+
+ rtk_fig_color(map->fig, 0.5, 0.5, 0.5 );
+ rtk_fig_rectangle(map->fig,
+ 0,0,0,
+ map->proxy->width * scale,
+ map->proxy->height * scale,
+ 1 );
- rtk_fig_color_rgb32(map->fig, 0xFF0000 );
- rtk_fig_rectangle(map->fig,
- 0,0,0,
- map->proxy->width * scale,
- map->proxy->height * scale,
- 0 );
-
// TODO - combine contiguous cells to minimize the number of
// rectangles we have to draw - performance is pretty nasty right
// now on big maps.
@@ -160,36 +160,18 @@
for( x=0; x<map->proxy->width; x++ )
for( y=0; y<map->proxy->height; y++ )
{
- switch( map->proxy->cells[ x + y * map->proxy->width ] )
- {
- case -1:
- // empty: draw nothing
- break;
-
- case 0:
- // unknown: draw grey square
- rtk_fig_color_rgb32(map->fig, 0x808080 );
- rtk_fig_rectangle(map->fig,
- (x - map->proxy->width/2.0) * scale + scale/2.0,
- (y - map->proxy->height/2.0) * scale + scale/2.0,
- 0,
- scale, scale, 1);
- break;
-
- case +1:
- // occupied: draw black square
- rtk_fig_color_rgb32(map->fig, 0x0 );
- rtk_fig_rectangle(map->fig,
- (x - map->proxy->width/2.0) * scale + scale/2.0,
- (y - map->proxy->height/2.0) * scale + scale/2.0,
- 0,
- scale, scale, 1);
- break;
-
- default:
- puts( "Warning: invalid occupancy value." );
- break;
- }
+ int8_t val = map->proxy->cells[ x + y * map->proxy->width ];
+ if (val == 0)
+ continue;
+ double color = (double)val/map->proxy->data_range; // scale to[-1,1]
+ color *= -1; //flip sign for coloring occupied to black
+ color = (color+1)/2.0; // scale to [0,1]
+ rtk_fig_color(map->fig, color, color, color );
+ rtk_fig_rectangle(map->fig,
+ (x - map->proxy->width/2.0) * scale + scale/2.0,
+ (y - map->proxy->height/2.0) * scale + scale/2.0,
+ 0,
+ scale, scale, 1);
}
return;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Let Crystal Reports handle the reporting - Free Crystal Reports 2008 30-Day
trial. Simplify your report design, integration and deployment - and focus on
what you do best, core application coding. Discover what's new with
Crystal Reports now. http://p.sf.net/sfu/bobj-july
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
