Revision: 7236
http://playerstage.svn.sourceforge.net/playerstage/?rev=7236&view=rev
Author: thjc
Date: 2009-01-04 01:57:44 +0000 (Sun, 04 Jan 2009)
Log Message:
-----------
applied patch 2187145 - Added wheelbase option to position "car" model
fixed a few compiler warnings
Modified Paths:
--------------
code/stage/branches/release-2-1-patches/src/model_bumper.c
code/stage/branches/release-2-1-patches/src/model_position.c
code/stage/branches/release-2-1-patches/src/raytrace.c
code/stage/branches/release-2-1-patches/src/stage.c
code/stage/branches/release-2-1-patches/src/stage.h
code/stage/branches/release-2-1-patches/src/typetable.c
Modified: code/stage/branches/release-2-1-patches/src/model_bumper.c
===================================================================
--- code/stage/branches/release-2-1-patches/src/model_bumper.c 2009-01-04
01:45:19 UTC (rev 7235)
+++ code/stage/branches/release-2-1-patches/src/model_bumper.c 2009-01-04
01:57:44 UTC (rev 7236)
@@ -303,7 +303,7 @@
}
else
if( dlen > 0 )
- PRINT_WARN2( "data size doesn't match configuation (%l/%l bytes)",
+ PRINT_WARN2( "data size doesn't match configuation (%ld/%ld bytes)",
(int)dlen, (int)(rcount * sizeof(stg_bumper_sample_t) ));
return 0; // keep running
Modified: code/stage/branches/release-2-1-patches/src/model_position.c
===================================================================
--- code/stage/branches/release-2-1-patches/src/model_position.c
2009-01-04 01:45:19 UTC (rev 7235)
+++ code/stage/branches/release-2-1-patches/src/model_position.c
2009-01-04 01:57:44 UTC (rev 7236)
@@ -217,6 +217,8 @@
data.watchdog_timeout = -1;
+ data.wheelbase = 1.0;
+
stg_model_set_data( mod, &data, sizeof(data));
#if ! INCLUDE_GNOME
@@ -324,6 +326,13 @@
data->max_speed.a = wf_read_tuple_angle(mod->id, "max_speed", 2,
data->max_speed.a );
}
+ // did the user specify a wheelbase
+ if( wf_property_exists( mod->id, "wheelbase" ) )
+ {
+ data->wheelbase =
+ wf_read_float( mod->id, "wheelbase", 1.0 );
+ }
+
// odometry model parameters
if( wf_property_exists( mod->id, "odom_error" ) )
{
@@ -490,7 +499,7 @@
// car like steering model based on speed and turning angle
vel->x = cmd->x * cos(cmd->a);
vel->y = 0;
- vel->a = cmd->x * sin(cmd->a)/1.0; // here 1.0 is the wheel
base, this should be a config option
+ vel->a = cmd->x * sin(cmd->a)/data->wheelbase;
break;
default:
Modified: code/stage/branches/release-2-1-patches/src/raytrace.c
===================================================================
--- code/stage/branches/release-2-1-patches/src/raytrace.c 2009-01-04
01:45:19 UTC (rev 7235)
+++ code/stage/branches/release-2-1-patches/src/raytrace.c 2009-01-04
01:57:44 UTC (rev 7236)
@@ -5,13 +5,13 @@
extern stg_rtk_fig_t* fig_debug_rays;
-
+#if 0
/* useful debug */
static void print_thing( char* prefix, stg_cell_t* cell, double x, double y )
{
- printf( "%s %p x[%.7f %.7f %.7f] y[%.7f %.7f %.7f] (x %s xmin x %s xmax) (y
%s ymin y %s ymax)\n",
+ printf( "%s %p x[%.7f %.7f %.7f] y[%.7f %.7f %.7f] (x %s xmin x %s xmax) (y
%s ymin y %s ymax)\n",
prefix,
- cell,
+ cell,
cell->xmin, x, cell->xmax,
cell->ymin, y, cell->ymax,
GTE(x,cell->xmin) ? ">=" : "<",
@@ -33,20 +33,20 @@
else
printf( "null array\n" );
}
+#endif
-
-itl_t* itl_create( double x, double y, double a, double b,
+itl_t* itl_create( double x, double y, double a, double b,
stg_matrix_t* matrix, itl_mode_t pmode )
-{
+{
itl_t* itl = calloc( sizeof(itl_t), 1 );
-
+
itl->matrix = matrix;
itl->x = x;
itl->y = y;
itl->models = NULL;
itl->index = 0;
- itl->range = 0;
+ itl->range = 0;
itl->incr = NULL;
switch( pmode )
@@ -54,7 +54,7 @@
case PointToBearingRange:
{
double range = b;
- double bearing = a;
+ double bearing = a;
itl->a = NORMALIZE(bearing);
itl->max_range = range;
}
@@ -62,7 +62,7 @@
case PointToPoint:
{
double x1 = a;
- double y1 = b;
+ double y1 = b;
itl->a = atan2( y1-y, x1-x );
itl->max_range = hypot( x1-x, y1-y );
}
@@ -70,14 +70,14 @@
default:
puts( "Stage Warning: unknown LineIterator mode" );
}
-
+
//printf( "a = %.2f remaining_range = %.2f\n", itl->a,
//remaining_range ); fflush( stdout );
-
+
itl->cosa = cos( itl->a );
itl->sina = sin( itl->a );
itl->tana = tan( itl->a );
-
+
return itl;
};
@@ -85,15 +85,15 @@
{
if( itl )
{
- if( itl->incr )
+ if( itl->incr )
free( itl->incr );
free( itl );
}
}
// returns the first model in the array that matches, else NULL.
-static stg_model_t* gslist_first_matching( GSList* list,
- stg_itl_test_func_t func,
+static stg_model_t* gslist_first_matching( GSList* list,
+ stg_itl_test_func_t func,
stg_model_t* finder )
{
for( ; list ; list=list->next )
@@ -101,7 +101,7 @@
if( (*func)( finder, (stg_model_t*)(list->data) ) )
return (stg_model_t*)(list->data);
}
-
+
return NULL; // nothing in the array matched
}
@@ -112,31 +112,31 @@
// start by going up the tree until the cell contains the point
// if x,y is NOT contained in the cell we jump to its parent
- while( !( GTE(x,cell->xmin) &&
- LT(x,cell->xmax) &&
- GTE(y,cell->ymin) &&
+ while( !( GTE(x,cell->xmin) &&
+ LT(x,cell->xmax) &&
+ GTE(y,cell->ymin) &&
LT(y,cell->ymax) ))
{
//print_thing( "ascending", cell, x, y );
-
+
if( cell->parent )
cell = cell->parent;
else
return NULL; // the point is outside the root node!
}
-
+
// now we know that the point is contained in this cell, we go down
// the tree to the leaf node that contains the point
-
+
// if we have children, we must jump down into the child
while( cell->children[0] )
{
- // choose the right quadrant
+ // choose the right quadrant
int index;
if( LT(x,cell->x) )
- index = LT(y,cell->y) ? 0 : 2;
+ index = LT(y,cell->y) ? 0 : 2;
else
- index = LT(y,cell->y) ? 1 : 3;
+ index = LT(y,cell->y) ? 1 : 3;
cell = cell->children[index];
}
@@ -145,58 +145,58 @@
return cell;
}
-stg_model_t* itl_first_matching( itl_t* itl,
- stg_itl_test_func_t func,
+stg_model_t* itl_first_matching( itl_t* itl,
+ stg_itl_test_func_t func,
stg_model_t* finder )
{
itl->index = 0;
- itl->models = NULL;
+ itl->models = NULL;
stg_cell_t* cell = itl->matrix->root;
while( LT(itl->range,itl->max_range) )
{
// locate the leaf cell at X,Y
- cell = stg_cell_locate( cell, itl->x, itl->y );
-
+ cell = stg_cell_locate( cell, itl->x, itl->y );
+
// the cell is null iff the point was outside the root
if( cell == NULL )
{
itl->range = itl->max_range; // stop the ray here
return NULL;
}
-
+
if( fig_debug_rays ) // draw the cell rectangle
stg_rtk_fig_rectangle( fig_debug_rays,
- cell->x, cell->y, 0,
- cell->size, cell->size, 0 );
- if( cell->data )
- {
- stg_model_t* hitmod =
+ cell->x, cell->y, 0,
+ cell->size, cell->size, 0 );
+ if( cell->data )
+ {
+ stg_model_t* hitmod =
gslist_first_matching( (GSList*)cell->data, func, finder );
-
- if( hitmod )
- return hitmod; // done!
+
+ if( hitmod )
+ return hitmod; // done!
}
-
+
double c = itl->y - itl->tana * itl->x; // line offset
-
+
double xleave = itl->x;
double yleave = itl->y;
-
+
if( GT(itl->a,0) ) // up
{
// ray could leave through the top edge
- // solve x for known y
+ // solve x for known y
yleave = cell->ymax; // top edge
xleave = (yleave - c) / itl->tana;
-
- // if the edge crossing was not in cell bounds
+
+ // if the edge crossing was not in cell bounds
if( !(GTE(xleave,cell->xmin) && LT(xleave,cell->xmax)) )
{
- // it must have left the cell on the left or right instead
+ // it must have left the cell on the left or right instead
// solve y for known x
-
+
if( GT(itl->a,M_PI/2.0) ) // left side
{
xleave = cell->xmin-0.00001;
@@ -205,22 +205,22 @@
{
xleave = cell->xmax;
}
-
+
yleave = itl->tana * xleave + c;
}
- }
- else
+ }
+ else
{
// ray could leave through the bottom edge
- // solve x for known y
+ // solve x for known y
yleave = cell->ymin; // bottom edge
xleave = (yleave - c) / itl->tana;
-
- // if the edge crossing was not in cell bounds
+
+ // if the edge crossing was not in cell bounds
if( !(GTE(xleave,cell->xmin) && LT(xleave,cell->xmax)) )
{
- // it must have left the cell on the left or right instead
- // solve y for known x
+ // it must have left the cell on the left or right instead
+ // solve y for known x
if( LT(itl->a,-M_PI/2.0) ) // left side
{
xleave = cell->xmin-0.00001;
@@ -229,26 +229,26 @@
{
xleave = cell->xmax;
}
-
+
yleave = itl->tana * xleave + c;
}
else
yleave-=0.00001;
}
-
+
if( fig_debug_rays ) // draw the cell rectangle
{
stg_rtk_fig_color_rgb32( fig_debug_rays, 0xFFBBBB );
- stg_rtk_fig_arrow_ex( fig_debug_rays,
+ stg_rtk_fig_arrow_ex( fig_debug_rays,
itl->x, itl->y, xleave, yleave, 0.01 );
stg_rtk_fig_color_rgb32( fig_debug_rays, 0xFF0000 );
}
// jump to the leave point
itl->range += hypot( yleave - itl->y, xleave - itl->x );
-
+
itl->x = xleave;
- itl->y = yleave;
+ itl->y = yleave;
}
return NULL; // we didn't find anything
@@ -273,14 +273,14 @@
stg_model_t* last_model = NULL; // What model we just left.
double justoutm = 0; // "When" we just left this model.
-
+
while (LT(itl->range, itl->max_range)) {
// Locate the leaf cell at X, Y
cell = stg_cell_locate( cell, itl->x, itl->y );
if ( cell == NULL ) {
- // done.
+ // done.
itl->range = itl->max_range;
if (hitting != NULL) {}
return through_dist;
@@ -290,25 +290,25 @@
stg_rtk_fig_rectangle( fig_debug_rays, cell->x, cell->y, 0, cell->size,
cell->size, 0);
if (cell->data) { // not empty
- stg_model_t* hitmod = gslist_first_matching( (GSList*) cell->data, func,
finder );
+ stg_model_t* hitmod = gslist_first_matching( (GSList*) cell->data, func,
finder );
if (!stg_model_is_related(otherrobot,hitmod)) {
// We hit something which matched // we should check if its the robot
- if (hitmod == hitting) {
+ if (hitmod == hitting) {
//printf("Coming out: %s %f\n", hitmod->token, through_dist);
// Coming out of the object
last_model = hitting;
justoutm = 0;
- hitting = NULL;
- } else if (hitting == NULL) {
- if ((last_model == hitmod) && (justoutm < 0.1)) {}
+ hitting = NULL;
+ } else if (hitting == NULL) {
+ if ((last_model == hitmod) && (justoutm < 0.1)) {}
else {
hitting = hitmod;
}
}
}
- }
-
+ }
+
double c = itl->y - itl->tana * itl->x;
double xleave = itl->x;
@@ -357,15 +357,15 @@
// jump to the leave point
double distance = hypot( yleave - itl->y, xleave - itl->x );
itl->range += distance;
- itl->x = xleave;
+ itl->x = xleave;
itl->y = yleave;
if (hitting != NULL) {
through_dist += distance;
- } else {
+ } else {
justoutm += distance;
}
}
-
+
if (hitting != NULL) {
//printf("Finished");
}
Modified: code/stage/branches/release-2-1-patches/src/stage.c
===================================================================
--- code/stage/branches/release-2-1-patches/src/stage.c 2009-01-04 01:45:19 UTC
(rev 7235)
+++ code/stage/branches/release-2-1-patches/src/stage.c 2009-01-04 01:57:44 UTC
(rev 7236)
@@ -109,15 +109,15 @@
{
struct timeval tv;
static stg_msec_t starttime = 0;
-
+
gettimeofday( &tv, NULL );
-
+
stg_msec_t timenow = (stg_msec_t)( tv.tv_sec*1000 + tv.tv_usec/1000 );
-
-
+
+
if( starttime == 0 )
starttime = timenow;
-
+
return( timenow - starttime );
}
@@ -145,16 +145,16 @@
{
if( name == NULL ) // no string?
return 0; // black
-
+
if( strcmp( name, "" ) == 0 ) // empty string?
return 0; // black
-
+
static FILE *file = NULL;
static GHashTable* table = NULL;
if( table == NULL )
table = g_hash_table_new( g_str_hash, g_str_equal );
-
+
if( file == NULL )
{
char* searchfiles[] = {
@@ -164,7 +164,7 @@
#endif
"../rgb.txt",
NULL };
-
+
for( int i=0;
searchfiles[i];
i++ )
@@ -174,52 +174,52 @@
if( (file = fopen( filename, "r")) )
break; // opened a file ok - jump out of for loop
}
-
+
if( file == NULL )
{
-
+
PRINT_ERR1("unable to open color database: %s",
strerror(errno));
fclose(file);
exit(0);
}
-
+
PRINT_DEBUG( "Success!" );
- // load the file into the hash table
+ // load the file into the hash table
while (TRUE)
{
char line[1024];
if (!fgets(line, sizeof(line), file))
break;
-
+
// it's a macro or comment line - ignore the line
- if (line[0] == '!' || line[0] == '#' || line[0] == '%')
+ if (line[0] == '!' || line[0] == '#' || line[0] == '%')
continue;
// Trim the trailing space
while (strchr(" \t\n", line[strlen(line)-1]))
line[strlen(line)-1] = 0;
-
+
// Read the color
int r, g, b;
int chars_matched = 0;
sscanf( line, "%d %d %d %n", &r, &g, &b, &chars_matched );
-
+
stg_color_t col = ( 0xFF000000 | (r << 16) | (g << 8) | b);
// Read the name
char* colorname = strdup( line + chars_matched );
-
+
// map the name to the color in the table
g_hash_table_insert( table, (gpointer)colorname, (gpointer)col );
-
+
}
fclose(file);
}
- // look up the colorname in the database
+ // look up the colorname in the database
return (stg_color_t)g_hash_table_lookup( table, name );
}
@@ -232,28 +232,28 @@
double minx, miny, maxx, maxy;
minx = miny = BILLION;
maxx = maxy = -BILLION;
-
+
int l;
for( l=0; l<num; l++ )
{
// find the bounding rectangle
if( lines[l].x1 < minx ) minx = lines[l].x1;
- if( lines[l].y1 < miny ) miny = lines[l].y1;
- if( lines[l].x1 > maxx ) maxx = lines[l].x1;
+ if( lines[l].y1 < miny ) miny = lines[l].y1;
+ if( lines[l].x1 > maxx ) maxx = lines[l].x1;
if( lines[l].y1 > maxy ) maxy = lines[l].y1;
if( lines[l].x2 < minx ) minx = lines[l].x2;
- if( lines[l].y2 < miny ) miny = lines[l].y2;
- if( lines[l].x2 > maxx ) maxx = lines[l].x2;
+ if( lines[l].y2 < miny ) miny = lines[l].y2;
+ if( lines[l].x2 > maxx ) maxx = lines[l].x2;
if( lines[l].y2 > maxy ) maxy = lines[l].y2;
}
-
+
// now normalize all lengths so that the lines all fit inside
// rectangle from 0,0 to 1,1
double scalex = maxx - minx;
double scaley = maxy - miny;
for( l=0; l<num; l++ )
- {
+ {
lines[l].x1 = (lines[l].x1 - minx) / scalex;
lines[l].y1 = (lines[l].y1 - miny) / scaley;
lines[l].x2 = (lines[l].x2 - minx) / scalex;
@@ -293,43 +293,43 @@
double minx, miny, maxx, maxy;
minx = miny = BILLION;
maxx = maxy = -BILLION;
-
+
int r;
for( r=0; r<num; r++ )
{
// test the origin of the rect
if( rects[r].pose.x < minx ) minx = rects[r].pose.x;
- if( rects[r].pose.y < miny ) miny = rects[r].pose.y;
- if( rects[r].pose.x > maxx ) maxx = rects[r].pose.x;
+ if( rects[r].pose.y < miny ) miny = rects[r].pose.y;
+ if( rects[r].pose.x > maxx ) maxx = rects[r].pose.x;
if( rects[r].pose.y > maxy ) maxy = rects[r].pose.y;
// test the extremes of the rect
- if( (rects[r].pose.x+rects[r].size.x) < minx )
+ if( (rects[r].pose.x+rects[r].size.x) < minx )
minx = (rects[r].pose.x+rects[r].size.x);
-
- if( (rects[r].pose.y+rects[r].size.y) < miny )
+
+ if( (rects[r].pose.y+rects[r].size.y) < miny )
miny = (rects[r].pose.y+rects[r].size.y);
-
- if( (rects[r].pose.x+rects[r].size.x) > maxx )
+
+ if( (rects[r].pose.x+rects[r].size.x) > maxx )
maxx = (rects[r].pose.x+rects[r].size.x);
-
- if( (rects[r].pose.y+rects[r].size.y) > maxy )
+
+ if( (rects[r].pose.y+rects[r].size.y) > maxy )
maxy = (rects[r].pose.y+rects[r].size.y);
}
-
+
// now normalize all lengths so that the rects all fit inside
// rectangle from 0,0 to 1,1
double scalex = maxx - minx;
double scaley = maxy - miny;
for( r=0; r<num; r++ )
- {
+ {
rects[r].pose.x = (rects[r].pose.x - minx) / scalex;
rects[r].pose.y = (rects[r].pose.y - miny) / scaley;
rects[r].size.x = rects[r].size.x / scalex;
rects[r].size.y = rects[r].size.y / scaley;
}
-}
+}
// returns an array of 4 * num_rects stg_line_t's
stg_line_t* stg_rotrects_to_lines( stg_rotrect_t* rects, int num_rects )
@@ -337,7 +337,7 @@
// convert rects to an array of lines
int num_lines = 4 * num_rects;
stg_line_t* lines = (stg_line_t*)calloc( sizeof(stg_line_t), num_lines );
-
+
int r;
for( r=0; r<num_rects; r++ )
{
@@ -345,23 +345,23 @@
lines[4*r].y1 = rects[r].pose.y;
lines[4*r].x2 = rects[r].pose.x + rects[r].size.x;
lines[4*r].y2 = rects[r].pose.y;
-
+
lines[4*r+1].x1 = rects[r].pose.x + rects[r].size.x;;
lines[4*r+1].y1 = rects[r].pose.y;
lines[4*r+1].x2 = rects[r].pose.x + rects[r].size.x;
lines[4*r+1].y2 = rects[r].pose.y + rects[r].size.y;
-
+
lines[4*r+2].x1 = rects[r].pose.x + rects[r].size.x;;
lines[4*r+2].y1 = rects[r].pose.y + rects[r].size.y;;
lines[4*r+2].x2 = rects[r].pose.x;
lines[4*r+2].y2 = rects[r].pose.y + rects[r].size.y;
-
+
lines[4*r+3].x1 = rects[r].pose.x;
lines[4*r+3].y1 = rects[r].pose.y + rects[r].size.y;
lines[4*r+3].x2 = rects[r].pose.x;
lines[4*r+3].y2 = rects[r].pose.y;
}
-
+
return lines;
}
@@ -371,10 +371,10 @@
{
stg_polygon_t* polys = stg_polygons_create( count );
stg_point_t pts[4];
-
+
size_t r;
for( r=0; r<count; r++ )
- {
+ {
pts[0].x = rects[r].pose.x;
pts[0].y = rects[r].pose.y;
pts[1].x = rects[r].pose.x + rects[r].size.x;
@@ -383,7 +383,7 @@
pts[2].y = rects[r].pose.y + rects[r].size.y;
pts[3].x = rects[r].pose.x;
pts[3].y = rects[r].pose.y + rects[r].size.y;
-
+
// copy these points in the polygon
stg_polygon_set_points( &polys[r], pts, 4 );
@@ -391,7 +391,7 @@
polys[r].bbox.x = width;
polys[r].bbox.y = height;
}
-
+
return polys;
}
@@ -400,11 +400,11 @@
{
double cosa = cos(p1->a);
double sina = sin(p1->a);
-
+
double tx = p1->x + p2->x * cosa - p2->y * sina;
double ty = p1->y + p2->x * sina + p2->y * cosa;
double ta = p1->a + p2->a;
-
+
result->x = tx;
result->y = ty;
result->a = ta;
@@ -438,23 +438,21 @@
PRINT_WARN4( "pb_set_pixel coordinate %d,%d out of range (image dimensions
%d by %d)", x, y, width, height );
}
-// set all the pixels in a rectangle
+// set all the pixels in a rectangle
void pb_set_rect( GdkPixbuf* pb, int x, int y, int width, int height, uint8_t
val )
{
- int pbwidth = gdk_pixbuf_get_width(pb);
- int pbheight = gdk_pixbuf_get_height(pb);
int bytes_per_sample = gdk_pixbuf_get_bits_per_sample (pb) / 8;
int num_samples = gdk_pixbuf_get_n_channels(pb);
int a, b;
for( a = y; a < y+height; a++ )
for( b = x; b < x+width; b++ )
- {
+ {
// zeroing
guchar* pix = pb_get_pixel( pb, b, a );
memset( pix, val, num_samples * bytes_per_sample );
}
-}
+}
// returns TRUE if any channel in the pixel is non-zero
gboolean pb_pixel_is_set( GdkPixbuf* pb, int x, int y, int threshold )
@@ -470,7 +468,7 @@
}
-stg_polygon_t* stg_polygons_from_image_file( const char* filename,
+stg_polygon_t* stg_polygons_from_image_file( const char* filename,
size_t* count )
{
stg_polygon_t* polys;
@@ -478,13 +476,13 @@
int rect_count = 0;
int width, height;
- if( stg_rotrects_from_image_file( filename,
+ if( stg_rotrects_from_image_file( filename,
&rects,
&rect_count,
&width, &height ) )
{
PRINT_ERR1( "failed to load rects from image file \"%s\"",
- filename );
+ filename );
return NULL;
}
@@ -497,46 +495,43 @@
return(polys);
}
-stg_polyline_t* stg_polylines_from_image_file( const char* filename,
+stg_polyline_t* stg_polylines_from_image_file( const char* filename,
size_t* num )
{
- // TODO: make this a parameter
- const int threshold = 127;
-
GError* err = NULL;
GdkPixbuf* pb = gdk_pixbuf_new_from_file( filename, &err );
if( err )
{
fprintf( stderr, "\nError loading bitmap: %s\n", err->message );
- return 1; // error
+ return (stg_polyline_t*)1; // error
}
-
+
// this should be ok as no error was reported
assert( pb );
stg_polyline_t* lines = NULL;
size_t lines_count = 0;
-
+
int img_width = gdk_pixbuf_get_width(pb);
int img_height = gdk_pixbuf_get_height(pb);
-
+
int y, x;
for(y = 0; y < img_height; y++)
for(x = 0; x < img_width; x++)
{
// TODO!
- }
-
+ }
+
// free the image data
gdk_pixbuf_unref( pb );
-
+
if( num ) *num = lines_count;
return lines;
-}
+}
-int stg_rotrects_from_image_file( const char* filename,
- stg_rotrect_t** rects,
+int stg_rotrects_from_image_file( const char* filename,
+ stg_rotrect_t** rects,
int* rect_count,
int* widthp, int* heightp )
{
@@ -551,19 +546,19 @@
fprintf( stderr, "\nError loading bitmap: %s\n", err->message );
return 1; // error
}
-
+
// this should be ok as no error was reported
assert( pb );
-
-
+
+
#ifdef DEBUG
printf( "image \"%s\" channels:%d bits:%d alpha:%d "
"width:%d height:%d rowstride:%d pixels:%p\n",
-
+
filename,
gdk_pixbuf_get_n_channels(pb),
gdk_pixbuf_get_bits_per_sample(pb),
- gdk_pixbuf_get_has_alpha(pb),
+ gdk_pixbuf_get_has_alpha(pb),
gdk_pixbuf_get_width(pb),
gdk_pixbuf_get_height(pb),
gdk_pixbuf_get_rowstride(pb),
@@ -572,14 +567,14 @@
*rect_count = 0;
*rects = NULL;
-
+
int img_width = gdk_pixbuf_get_width(pb);
int img_height = gdk_pixbuf_get_height(pb);
-
+
// if the caller wanted to know the dimensions
if( widthp ) *widthp = img_width;
if( heightp ) *heightp = img_height;
-
+
int y, x;
for(y = 0; y < img_height; y++)
{
@@ -588,7 +583,7 @@
// skip blank (white) pixels
if( pb_pixel_is_set( pb,x,y, threshold) )
continue;
-
+
// a rectangle starts from this point
int startx = x;
int starty = y;
@@ -598,7 +593,7 @@
for( ; x < img_width && ! pb_pixel_is_set(pb,x,y,threshold); x++ )
{
// handle horizontal cropping
- //double ppx = x * sx;
+ //double ppx = x * sx;
//if (ppx < this->crop_ax || ppx > this->crop_bx)
//continue;
@@ -620,26 +615,26 @@
// whiten the pixels we have used in this rect
pb_set_rect( pb, startx, starty, x-startx, height, 0xFF );
-
+
// add this rectangle to the array
(*rect_count)++;
*rects = (stg_rotrect_t*)
realloc( *rects, *rect_count * sizeof(stg_rotrect_t) );
-
+
stg_rotrect_t *latest = &(*rects)[(*rect_count)-1];
latest->pose.x = startx;
latest->pose.y = starty;
latest->pose.a = 0.0;
latest->size.x = x - startx;
latest->size.y = height;
-
- //printf( "rect %d (%.2f %.2f %.2f %.2f %.2f\n",
- // *rect_count,
- // latest->x, latest->y, latest->a, latest->w, latest->h );
-
+
+ //printf( "rect %d (%.2f %.2f %.2f %.2f %.2f\n",
+ // *rect_count,
+ // latest->x, latest->y, latest->a, latest->w, latest->h );
+
}
}
-
+
// free the image data
gdk_pixbuf_unref( pb );
@@ -649,12 +644,12 @@
int r;
for( r=0; r< *rect_count; r++ )
{
- stg_rotrect_t *rect = &(*rects)[r];
+ stg_rotrect_t *rect = &(*rects)[r];
rect->pose.y = img_height - rect->pose.y;
rect->size.y = -rect->size.y;
}
-
+
return 0; // ok
}
@@ -676,7 +671,7 @@
stg_polygon_t* stg_polygons_create( int count )
{
stg_polygon_t* polys = (stg_polygon_t*)calloc( count, sizeof(stg_polygon_t));
-
+
// each polygon contains an array of points
int p;
for( p=0; p<count; p++ )
@@ -697,8 +692,8 @@
for( c=0; c<count; c++ )
if( p[c].points )
g_array_free( p[c].points, TRUE );
-
- free( p );
+
+ free( p );
}
stg_polygon_t* stg_unit_polygon_create( void )
@@ -711,17 +706,17 @@
pts[2].x = 1;
pts[2].y = 1;
pts[3].x = 0;
- pts[3].y = 1;
-
+ pts[3].y = 1;
+
stg_polygon_t* poly = stg_polygons_create(1);
- stg_polygon_set_points( poly, pts, 4 );
+ stg_polygon_set_points( poly, pts, 4 );
return poly;
}
//////////////////////////////////////////////////////////////////////////
// scale an array of polygons so they fit in a rectangle of size
// [width] by [height], with the origin in the center of the rectangle
-void stg_polygons_normalize( stg_polygon_t* polys, int num,
+void stg_polygons_normalize( stg_polygon_t* polys, int num,
double width, double height )
{
if( num == 0 )
@@ -731,7 +726,7 @@
double minx, miny, maxx, maxy;
minx = miny = BILLION;
maxx = maxy = -BILLION;
-
+
int l;
for( l=0; l<num; l++ ) // examine all the polygons
{
@@ -749,7 +744,7 @@
assert( ! isnan( pt->y ) );
}
}
-
+
//minx = 0;
//miny = 0;
// maxx = polys[0].bbox.x;
@@ -762,15 +757,15 @@
//double scalex = polys[0].bbox.x;
//double scaley = polys[0].bbox.y;
-
+
for( int l=0; l<num; l++ ) // scale each polygon
- {
+ {
// scale all the points in the polygon
int p;
for( p=0; p<polys[l].points->len; p++ )
{
stg_point_t* pt = &g_array_index( polys[l].points, stg_point_t, p);
-
+
pt->x = ((pt->x - minx) / scalex * width) - width/2.0;
pt->y = ((pt->y - miny) / scaley * height) - height/2.0;
@@ -783,7 +778,7 @@
void stg_polygon_print( stg_polygon_t* poly )
{
printf( "polygon: %d pts : ", poly->points->len );
-
+
int i;
for(i=0;i<poly->points->len;i++)
{
@@ -796,11 +791,11 @@
void stg_polygons_print( stg_polygon_t* polys, unsigned int count )
{
printf( "polygon array (%d polys)\n", count );
-
+
int i;
for( i=0; i<count; i++ )
{
- printf( "[%d] ", i );
+ printf( "[%d] ", i );
stg_polygon_print( &polys[i] );
}
}
@@ -826,7 +821,7 @@
void stg_polygon_set_points( stg_polygon_t* poly, stg_point_t* pts, size_t
count )
{
assert( poly );
-
+
g_array_set_size( poly->points, 0 );
g_array_append_vals( poly->points, pts, count );
}
Modified: code/stage/branches/release-2-1-patches/src/stage.h
===================================================================
--- code/stage/branches/release-2-1-patches/src/stage.h 2009-01-04 01:45:19 UTC
(rev 7235)
+++ code/stage/branches/release-2-1-patches/src/stage.h 2009-01-04 01:57:44 UTC
(rev 7236)
@@ -1047,6 +1047,7 @@
//stg_bool_t stall; ///< TRUE iff the robot can't move due to a collision
stg_position_localization_mode_t localization; ///< global or local mode
stg_msec_t watchdog_timeout; ///< time since last command after which we
stop
+ stg_meters_t wheelbase; ///< wheelbase length (for "car" mode) in meters
} stg_position_data_t;
/** "position_cfg" property */
Modified: code/stage/branches/release-2-1-patches/src/typetable.c
===================================================================
--- code/stage/branches/release-2-1-patches/src/typetable.c 2009-01-04
01:45:19 UTC (rev 7235)
+++ code/stage/branches/release-2-1-patches/src/typetable.c 2009-01-04
01:57:44 UTC (rev 7236)
@@ -18,22 +18,22 @@
int indicator_init( stg_model_t* mod );
// map worldfile keywords onto initialization functions
-stg_type_record_t typetable[] =
- {
+stg_type_record_t typetable[] =
+ {
{ "model", NULL },
{ "position", position_init },
{ "ranger", ranger_init },
{ "laser", laser_init },
{ "blobfinder", blobfinder_init },
{ "fiducialfinder", fiducial_init },
- { "gripper", gripper_init },
- //{ "power", energy_init },
- { "ptz", ptz_init },
- { "wifi", wifi_init },
- { "speech", speech_init },
- { "audio", audio_init },
+ { "gripper", gripper_init },
+ //{ "power", energy_init },
+ { "ptz", ptz_init },
+ { "wifi", wifi_init },
+ { "speech", speech_init },
+ { "audio", audio_init },
{ "indicator", indicator_init },
- { "bumper", bumper_init },
- { NULL, 0, NULL } // this must be the last entry
+ { "bumper", bumper_init },
+ { NULL, NULL } // this must be the last entry
};
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
