diff --git a/libstageplugin/p_map.cc b/libstageplugin/p_map.cc new file mode 100644 index 000000000..0f706088b --- /dev/null +++ b/libstageplugin/p_map.cc @@ -0,0 +1,267 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2004, 2005 Richard Vaughan + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* + * Desc: A plugin driver for Player that gives access to Stage devices. + * Author: Richard Vaughan + * Date: 10 December 2004 + * CVS: $Id$ + */ + +#include "p_driver.h" + +#include +using namespace std; + +// DOCUMENTATION ------------------------------------------------------------ + +/** @addtogroup player +@par Map interface +- PLAYER_MAP_REQ_GET_INFO +- PLAYER_MAP_REQ_GET_DATA +*/ + + +InterfaceMap::InterfaceMap( player_devaddr_t addr, + StgDriver* driver, + ConfigFile* cf, + int section ) + : InterfaceModel( addr, driver, cf, section, NULL ) +{ + // nothing to do... +} + +// Handle map info request - adapted from Player's mapfile driver by +// Brian Gerkey +int InterfaceMap::HandleMsgReqInfo( QueuePointer & resp_queue, + player_msghdr * hdr, + void * data ) +{ + printf( "Stage: device \"%s\" received map info request\n", + this->mod->Token()); + + + + // create and render a map for this model + Stg::Geom geom; + geom = this->mod->GetGeom(); + + + double mres = this->mod->GetMapResolution(); + + // size_t sz=0; +// polygon_t* polys = (polygon_t*) +// model_get_property( this->mod, "polygons", &sz); +// size_t count = sz / sizeof(polygon_t); + + + // prepare the map info for the client + player_map_info_t info; + + info.scale = mres; + + // size in pixels + info.width = (uint32_t)(geom.size.x / mres); + info.height = (uint32_t)(geom.size.y / mres); + + // origin of map center in global coords + Stg::Pose global; + memcpy( &global, &geom.pose, sizeof(global)); + + global = this->mod->LocalToGlobal( &geom.pose ); + + // get real-world pose of lower-left corner of map (this is what Player + // calls the 'origin' + info.origin.px = geom.pose.x - geom.size.x / 2.0; + info.origin.py = geom.pose.y - geom.size.y / 2.0; + info.origin.pa = geom.pose.a; + + printf( "Stage: creating map for model \"%s\" of %d by %d cells at res %.2f\n", + this->mod->Token(), + info.width, info.height, info.scale ); + + this->driver->Publish(this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_MAP_REQ_GET_INFO, + (void*)&info, sizeof(info), NULL); + return 0; +} + + +void render_line( int8_t* buf, + int w, int h, + int x1, int y1, + int x2, int y2, + int8_t val ) +{ + // if both ends of the line are out of bounds, there's nothing to do + if( x1<0 && x1>w && y1<0 && y1>h && + x2<0 && x2>w && y2<0 && y2>h ) + return; + + // if the two ends are adjacent, fill in the pixels + if( abs(x2-x1) <= 1 && abs(y2-y1) <= 1 ) + { + if( x1 >= 0 && x1 < w && y1 >=0 && y1 < h ) + { + if( x1 == w ) x1 = w-1; + if( y1 == h ) y1 = h-1; + + buf[ y1 * w + x1] = val; + } + + + if( x2 >= 0 && x2 <= w && y2 >=0 && y2 <= h ) + { + if( x2 == w ) x2 = w-1; + if( y2 == h ) y2 = h-1; + + buf[ y2 * w + x2] = val; + } + } + else // recursively draw two half-lines + { + int xm = x1+(x2-x1)/2; + int ym = y1+(y2-y1)/2; + render_line( buf, w, h, x1, y1, xm, ym, val ); + render_line( buf, w, h, xm, ym, x2, y2, val ); + } +} + + +// Handle map data request +int InterfaceMap::HandleMsgReqData( QueuePointer & resp_queue, + player_msghdr * hdr, + void * data ) +{ + // printf( "device %s received map data request\n", this->mod->token ); + + Stg::Geom geom; + geom = this->mod->GetGeom( ); + + double mres = this->mod->GetMapResolution(); + + // request packet + player_map_data_t* mapreq = (player_map_data_t*)data; + size_t mapsize = sizeof(player_map_data_t); + + // response packet + player_map_data_t* mapresp = (player_map_data_t*)calloc(1,mapsize); + assert(mapresp); + + unsigned int oi, oj, si, sj; + oi = mapresp->col = mapreq->col; + oj = mapresp->row = mapreq->row; + si = mapresp->width = mapreq->width; + sj = mapresp->height = mapreq->height; + + // initiall all cells are 'empty' + memset( mapresp->data, -1, sizeof(uint8_t) * mapresp->width * mapresp->height ); + + printf( "Stage computing map tile (%d,%d)(%d,%d)...", + oi, oj, si, sj); + fflush(stdout); + + // render the polygons directly into the outgoing message buffer. fast! outrageous! + + GList* plist; + for( plist = model_get_polygons( mod ); + plist; + plist = plist->next ) + { + polygon_t* p = (polygon_t*)plist->data; + + // draw each line in the poly + int line_count = (int)p->points->len; + for( int l=0; lpoints, point_t, l ); + point_t* pt2 = &g_array_index( p->points, point_t, (l+1) % line_count ); + + render_line( mapresp->data, + mapresp->width, mapresp->height, + (int)((pt1->x+geom.size.x/2.0) / mres) -oi, + (int)((pt1->y+geom.size.y/2.0) / mres) -oj, + (int)((pt2->x+geom.size.x/2.0) / mres) -oi, + (int)((pt2->y+geom.size.y/2.0) / mres) -oj, + 1 ); + } + } + + // if the model has a boundary, that's in the map too + + // TODO: need to think about this a little. not vital for now.... + +// bool_t* boundary = (bool_t*) +// model_get_property_fixed( mod, "boundary", sizeof(bool_t)); + +// if( boundary && *boundary ) +// { +// int right = (int)(geom.size.x/mres - oi); +// int top = (int)(geom.size.y/mres - oj); + +// render_line( mapresp->data, mapresp->width, mapresp->height, +// 0,0, 0, top, 1 ); +// render_line( mapresp->data, mapresp->width, mapresp->height, +// 0, top, right, top, 1 ); +// render_line( mapresp->data, mapresp->width, mapresp->height, +// right, top, right, 0, 1 ); +// render_line( mapresp->data, mapresp->width, mapresp->height, +// right, 0, 0,0, 1 ); +// } + + mapresp->data_count = mapresp->width * mapresp->height; + + //printf( "Stage publishing map data %d bytes\n", + // (int)mapsize ); + + this->driver->Publish(this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_MAP_REQ_GET_DATA, + (void*)mapresp, mapsize, NULL); + free(mapresp); + + puts( " done." ); + + return(0); +} + +int InterfaceMap::ProcessMessage(QueuePointer & resp_queue, + player_msghdr * hdr, + void * data ) +{ + // printf( "Stage map interface processing message\n" ); + + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_MAP_REQ_GET_DATA, + this->addr)) + return( this->HandleMsgReqData( resp_queue, hdr, data )); + + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_MAP_REQ_GET_INFO, + this->addr)) + return( this->HandleMsgReqInfo( resp_queue, hdr, data )); + + PLAYER_WARN2("stage map doesn't support message %d:%d.", + hdr->type, hdr->subtype ); + return -1; +} + diff --git a/libstageplugin/p_power.cc.TODO b/libstageplugin/p_power.cc similarity index 100% rename from libstageplugin/p_power.cc.TODO rename to libstageplugin/p_power.cc