Update of /cvsroot/playerstage/code/player/server/drivers/position/nav200
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv31331/server/drivers/position/nav200
Modified Files:
sicknav200.cc
Log Message:
These changes were contributed by David Olsen, including:
Extensive NAV200 changes, adding new constallelation configuration options
Fix to memory leak of messages when a player client is destroyed
Fix to laser interface which assumed intensity was provided
Fix to memory leaks in vectormap interface (mainly leaking geoms)
Proper error handling in C++ PlayerClient Peek
Mutex lock in VectorMapProxy GetLayerData
playerv vectormap rendering bug
Index: sicknav200.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/nav200/sicknav200.cc,v
retrieving revision 1.12
retrieving revision 1.13
diff -C2 -d -r1.12 -r1.13
*** sicknav200.cc 20 Dec 2007 23:28:27 -0000 1.12
--- sicknav200.cc 4 Feb 2008 18:59:44 -0000 1.13
***************
*** 35,40 ****
output of the device.
! Currently the driver assumes the nav200 has been correctly initialised and
loaded with the
! reflector layers.
@par Compile-time dependencies
--- 35,45 ----
output of the device.
! By default, the driver will enter positioning mode and use the reflectors
stored on the nav200.
! To map the visible reflectors and store them on the nav200, set the mode
property to mapping.
! To copy the reflector positions from a vectormap onto the nav200, add a
vectormap to the requires
! list of the driver and set the mode property to fetch.
! To get the reflector positions from the nav200 to display using the vectormap
this driver
! provides, set the mode to upload. Note: mapping and fetch also provide the
reflectors positions
! for display, as well as storing them on the nav200.
@par Compile-time dependencies
***************
*** 44,48 ****
@par Provides
! - @ref interface_laser
@par Requires
--- 49,54 ----
@par Provides
! - @ref interface_position2d
! - @ref interface_vectormap
@par Requires
***************
*** 64,67 ****
--- 70,78 ----
- Default: [0.15 0.15]
- Footprint (x,y) of the laser.
+
+ - wheelBase (double)
+ - Default: -1.0
+ - Distance between front and rear wheels. Used to calculate velocities
+ when vehicle movement data is provided.
@par Example
***************
*** 71,76 ****
(
name "sicknav200"
! provides ["position2d:0"]
! provides ["vectormap:0"]
requires ["opaque:0"]
)
--- 82,86 ----
(
name "sicknav200"
! provides ["position2d:0" "vectormap:0"]
requires ["opaque:0"]
)
***************
*** 84,88 ****
@endverbatim
! @author Kathy Fung, Toby Collett, inro technologies
*/
--- 94,98 ----
@endverbatim
! @author Kathy Fung, Toby Collett, David Olsen, inro technologies
*/
***************
*** 138,145 ****
virtual void Main();
! // Add new reflectors.
void UpdateMap();
// Get the reflector positions from the device.
void GetReflectors();
// Build the well known binary view of the reflector positions.
void BuildWKB();
--- 148,159 ----
virtual void Main();
! // Get device to map reflectors.
void UpdateMap();
// Get the reflector positions from the device.
void GetReflectors();
+ // Set the reflector positions.
+ void SetReflectors(player_vectormap_layer_data_t* data);
+ // Fetch the reflector positions from a provided vectormap and download
to device.
+ void FetchReflectors();
// Build the well known binary view of the reflector positions.
void BuildWKB();
***************
*** 151,160 ****
double size[2];
// Reflector positions.
! PositionXY* reflectors;
int numReflectors;
uint8_t* wkbData;
int wkbSize;
// If mode is set to mapping the reflector positions will be mapped,
// and mode will be automatically set back to positioning.
--- 165,177 ----
double size[2];
+ // TODO: Add reflector layer support.
// Reflector positions.
! PositionXY reflectors[32];
int numReflectors;
uint8_t* wkbData;
int wkbSize;
+ DoubleProperty wheelBase;
+
// If mode is set to mapping the reflector positions will be mapped,
// and mode will be automatically set back to positioning.
***************
*** 171,174 ****
--- 188,200 ----
int min_radius, max_radius;
+ // Reflector Map Driver info
+ // Provides reflector positions if not mapped by nav200
+ Device *reflector_map;
+ player_devaddr_t reflector_map_id;
+
+ // Velocity Driver info
+ Device *velocity;
+ player_devaddr_t velocity_id;
+
// Opaque Driver info
Device *opaque;
***************
*** 203,206 ****
--- 229,233 ----
SickNAV200::SickNAV200(ConfigFile* cf, int section)
: Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN),
+ wheelBase ("wheelBase", -1.0, 0),
mode ("mode", DEFAULT_SICKNAV200_MODE, 0)
{
***************
*** 242,246 ****
this->size[1] = 0.15;
- this->reflectors = NULL;
this->numReflectors = 0;
this->wkbData = NULL;
--- 269,272 ----
***************
*** 250,257 ****
//this->device_name = strdup(cf->ReadString(section, "port", DEFAULT_PORT));
! // nav200 parameters, convert to cm
this->min_radius = static_cast<int> (cf->ReadLength(section, "min_radius",
1) * 1000);
this->max_radius = static_cast<int> (cf->ReadLength(section, "max_radius",
30) * 1000);
this->RegisterProperty ("mode", &this->mode, cf, section);
this->opaque = NULL;
--- 276,284 ----
//this->device_name = strdup(cf->ReadString(section, "port", DEFAULT_PORT));
! // nav200 parameters, convert to mm
this->min_radius = static_cast<int> (cf->ReadLength(section, "min_radius",
1) * 1000);
this->max_radius = static_cast<int> (cf->ReadLength(section, "max_radius",
30) * 1000);
this->RegisterProperty ("mode", &this->mode, cf, section);
+ this->RegisterProperty ("wheelbase", &this->wheelBase, cf, section);
this->opaque = NULL;
***************
*** 267,270 ****
--- 294,309 ----
PLAYER_MSG0(2, "reading opaque id now");
+ PLAYER_MSG0(2, "reading velocity id now");
+ this->velocity = NULL;
+ memset(&this->velocity_id, 0, sizeof(this->velocity_id));
+ cf->ReadDeviceAddr(&this->velocity_id, section, "requires",
+ PLAYER_POSITION2D_CODE, -1, NULL);
+
+ PLAYER_MSG0(2, "reading reflector map id now");
+ this->reflector_map = NULL;
+ memset(&this->reflector_map_id, 0, sizeof(this->reflector_map_id));
+ cf->ReadDeviceAddr(&this->reflector_map_id, section, "requires",
+ PLAYER_VECTORMAP_CODE, -1, NULL);
+
return;
}
***************
*** 273,277 ****
{
//free(device_name);
- delete [] reflectors;
delete [] wkbData;
}
--- 312,315 ----
***************
*** 285,289 ****
// Subscribe to the opaque device.
! if(Device::MatchDeviceAddress(this->opaque_id, this->device_addr))
{
PLAYER_ERROR("attempt to subscribe to self");
--- 323,327 ----
// Subscribe to the opaque device.
! if(Device::MatchDeviceAddress(this->opaque_id, this->position_addr) ||
Device::MatchDeviceAddress(this->opaque_id, this->vectormap_addr))
{
PLAYER_ERROR("attempt to subscribe to self");
***************
*** 303,306 ****
--- 341,374 ----
}
+ if (this->velocity_id.interf == PLAYER_POSITION2D_CODE) // Velocity is
provided.
+ {
+ if(!(this->velocity = deviceTable->GetDevice(this->velocity_id)))
+ {
+ PLAYER_ERROR("unable to locate suitable position2d device");
+ return(-1);
+ }
+
+ if(this->velocity->Subscribe(this->InQueue) != 0)
+ {
+ PLAYER_ERROR("unable to subscribe to position2d device");
+ return(-1);
+ }
+ }
+
+ if (this->reflector_map_id.interf == PLAYER_VECTORMAP_CODE) // Reflector
positions are provided.
+ {
+ if(!(this->reflector_map =
deviceTable->GetDevice(this->reflector_map_id)))
+ {
+ PLAYER_ERROR("unable to locate suitable vectormap device");
+ return(-1);
+ }
+
+ if(this->reflector_map->Subscribe(this->InQueue) != 0)
+ {
+ PLAYER_ERROR("unable to subscribe to vectormap device");
+ return(-1);
+ }
+ }
+
// Open the terminal
Laser.Initialise(this, opaque, opaque_id);
***************
*** 364,368 ****
}
! static char layerName[5] = "0"; // Dumb name.
// Request for map info
--- 432,436 ----
}
! char* layerName = "0"; // Dumb name.
// Request for map info
***************
*** 371,374 ****
--- 439,443 ----
this->vectormap_addr))
{
+ // I am not sure if this and the below block of code leak memory.
Based on postgis.cc
player_extent2d_t extent;
if (numReflectors > 0)
***************
*** 395,407 ****
extent.x0 = extent.y0 = extent.x1 = extent.y1 = 0;
! static player_vectormap_layer_info_t layerInfo;
! layerInfo.name = layerName;
! layerInfo.name_count = strlen(layerInfo.name)+1;
! layerInfo.extent = extent;
! player_vectormap_info_t response;
response.srid = 0;
response.layers_count = 1;
! response.layers = &layerInfo;
response.extent = extent;
--- 464,476 ----
extent.x0 = extent.y0 = extent.x1 = extent.y1 = 0;
! player_vectormap_layer_info_t* layerInfo = new
player_vectormap_layer_info_t;
! layerInfo->name = strdup(layerName);
! layerInfo->name_count = strlen(layerInfo->name)+1;
! layerInfo->extent = extent;
! static player_vectormap_info_t response;
response.srid = 0;
response.layers_count = 1;
! response.layers = layerInfo;
response.extent = extent;
***************
*** 418,434 ****
this->vectormap_addr))
{
! static char featureName[6] = "point";
! player_vectormap_feature_data feature;
! memset(&feature,0,sizeof(feature));
! feature.name = featureName;
! feature.name_count = strlen(feature.name)+1;
! feature.wkb = wkbData;
! feature.wkb_count = wkbSize;
! player_vectormap_layer_data_t response;
! response.name = layerName;
response.name_count = strlen(response.name)+1;
! response.features_count = wkbSize > 0; // If we have no data, dont'
publish a feature.
! response.features = &feature;
this->Publish(this->vectormap_addr,
--- 487,505 ----
this->vectormap_addr))
{
! char* featureName = "point";
! player_vectormap_feature_data* feature = new
player_vectormap_feature_data;
! memset(feature,0,sizeof(feature));
! feature->name = strdup(featureName);
! feature->name_count = strlen(feature->name)+1;
! feature->wkb = new uint8_t[wkbSize];
! feature->wkb_count = wkbSize;
! for (uint32_t i=0; i < wkbSize; ++i)
! feature->wkb[i] = wkbData[i];
! static player_vectormap_layer_data_t response;
! response.name = strdup(layerName);
response.name_count = strlen(response.name)+1;
! response.features_count = numReflectors > 0; // If we have no data,
don't publish a feature.
! response.features = numReflectors > 0 ? feature : NULL;
this->Publish(this->vectormap_addr,
***************
*** 440,443 ****
--- 511,535 ----
return(0);
}
+ // Write layer data
+ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_VECTORMAP_REQ_WRITE_LAYER,
+ this->vectormap_addr))
+ {
+ player_vectormap_layer_data_t* layerData =
reinterpret_cast<player_vectormap_layer_data_t*>(data);
+
+ SetReflectors(layerData);
+
+ // Does the ack need the layer data?
+ this->Publish(this->vectormap_addr,
+ resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_VECTORMAP_REQ_WRITE_LAYER,
+ (void*)layerData,
+ sizeof(player_vectormap_layer_data_t),
+ NULL);
+
+ return 0;
+ }
+
if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SET_STRPROP_REQ, this->vectormap_addr) ||
***************
*** 452,456 ****
{
UpdateMap();
! mode.SetValue("positioning"); // Automatically return
to positioning.
}
else if (strncmp(mode, "positioning", 11) == 0)
--- 544,558 ----
{
UpdateMap();
! mode.SetValue("positioning"); // Automatically return
to positioning mode.
! }
! else if (strncmp(mode, "fetch", 5) == 0)
! {
! FetchReflectors(); // Fetch reflectors from database
! mode.SetValue("positioning");
! }
! else if (strncmp(mode, "upload", 5) == 0)
! {
! GetReflectors(); // Get reflectors from NAV200
! mode.SetValue("positioning");
}
else if (strncmp(mode, "positioning", 11) == 0)
***************
*** 503,509 ****
}
! // Download the reflector positions in case the vectormap is subscribed to.
! GetReflectors();
!
LaserPos Reading;
for(;;)
--- 605,610 ----
}
! BuildWKB(); // Build an empty WKB.
!
LaserPos Reading;
for(;;)
***************
*** 515,529 ****
ProcessMessages();
// get update and publish result
! if(Laser.GetPositionAuto(Reading))
{
! double angle = M_PI + Reading.orientation/32768.0*M_PI - pose[2];
! double dx = sin(angle);
! double dy = cos(angle);
! double rx = sin(angle + M_PI/2.0);
! double ry = cos(angle + M_PI/2.0);
! data_packet.pos.pa = atan2(dx, dy);
! data_packet.pos.px = static_cast<double> (Reading.pos.x)/1000 - dx *
pose[1] - rx * pose[0];
! data_packet.pos.py = static_cast<double> (Reading.pos.y)/1000 - dy *
pose[1] - ry * pose[0];
if(Reading.quality==0xFF || Reading.quality==0xFE ||
Reading.quality==0x00)
{
--- 616,667 ----
ProcessMessages();
+ player_position2d_geom_t* vel = NULL;
+ if (velocity)
+ {
+ Message* response = velocity->Request(this->InQueue,
PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, NULL, 0, 0, true);
+ if (response->GetDataSize() == sizeof(vel))
+ vel =
reinterpret_cast<player_position2d_geom_t*>(response->GetPayload());
+ else
+ PLAYER_ERROR("invalid response to velocity request\n");
+ }
+
+ bool gotReading;
+ if (vel)
+ {
+ if (wheelBase < 0)
+ {
+ PLAYER_WARN("vehicle movement data provided but wheelBase not
set. Add wheelbase to the NAV config file");
+ gotReading = Laser.GetPositionAuto(Reading);
+ }
+ else
+ {
+ double steeringAngle = vel->pose.pyaw;
+ double speed = vel->pose.px;
+ // Convert vehicle movement data into NAV200 format and
coordinates.
+ //double arcRadius = wheelBase / tan(steeringAngle);
+ //double angularVelocity = speed / arcRadius;
+ double angularVelocity = speed * tan(steeringAngle) / wheelBase;
+ double velX = speed - pose[1] * angularVelocity;
+ double velY = pose[0] * angularVelocity;
+ double navVelX = velX * cos(pose[2]) + velY * sin(pose[2]);
+ double navVelY = velY * cos(pose[2]) - velX * sin(pose[2]);
+ gotReading = Laser.GetPositionSpeedVelocity(short(navVelX *
1000), short(navVelY * 1000), short(angularVelocity * 32768.0 / M_PI), Reading);
+ }
+ }
+ else
+ gotReading = Laser.GetPositionAuto(Reading);
+
// get update and publish result
! if(gotReading)
{
! // Use NAV200 position and orientation data to determine vehicle
position and orientation.
! double angle = Reading.orientation/32768.0*M_PI - pose[2];
! double forwardx = cos(angle);
! double forwardy = sin(angle);
! double leftx = -sin(angle);
! double lefty = cos(angle);
! data_packet.pos.pa = atan2(forwardy, forwardx);
! data_packet.pos.px = static_cast<double> (Reading.pos.x)/1000 -
forwardx * pose[0] - leftx * pose[1];
! data_packet.pos.py = static_cast<double> (Reading.pos.y)/1000 -
forwardy * pose[0] - lefty * pose[1];
if(Reading.quality==0xFF || Reading.quality==0xFE ||
Reading.quality==0x00)
{
***************
*** 571,574 ****
--- 709,714 ----
numReflectors = Laser.StartMapping(0, 0, 0, 0, 45); // Radius may not
be needed.
PLAYER_MSG1(2, "Mapped %d reflectors.", numReflectors);
+ if (numReflectors > 32)
+ PLAYER_ERROR("More reflectors mapped than NAV200 supports.\n");
if (numReflectors < 0)
***************
*** 578,583 ****
}
- delete [] reflectors; // Clear old reflector positions.
- reflectors = new PositionXY[numReflectors];
for (uint8_t i = 0; i < numReflectors; i++)
{
--- 718,721 ----
***************
*** 630,635 ****
}
- delete [] reflectors; // Clear old reflector positions.
- reflectors = new PositionXY[32]; // Max 32 reflectors in a layer.
numReflectors = 0;
ReflectorData reflector;
--- 768,771 ----
***************
*** 666,674 ****
}
void SickNAV200::BuildWKB()
{
// Encode reflector positions in well known binary format for
publishing.
delete [] wkbData; // Clear old data.
! wkbSize = 9 + 21 * numReflectors;
wkbData = new uint8_t[wkbSize];
wkbData[0] = 1; // Little endian (0 for big endian).
--- 802,927 ----
}
+ const unsigned wkbHeaderSize = 9;
+ const unsigned wkbPointSize = 21;
+
+ void SickNAV200::SetReflectors(player_vectormap_layer_data_t* data)
+ {
+ PLAYER_MSG0(2, "Downloading reflectors.");
+
+ numReflectors = 0;
+ for (unsigned f = 0; f < data->features_count; f++)
+ {
+ player_vectormap_feature_data_t feature = data->features[f];
+ uint8_t* wkb = feature.wkb;
+ if (feature.wkb_count < wkbHeaderSize)
+ {
+ PLAYER_WARN("WKB too small in SetReflectors\n");
+ continue;
+ }
+ if (wkb[0] == 0)
+ {
+ PLAYER_WARN("SetReflectors does not support big endian
wkb data\n");
+ continue;
+ }
+ if (*reinterpret_cast<uint32_t*>(wkb + 1) != 4)
+ {
+ PLAYER_WARN("SetReflectors only supports MultiPoint
data\n");
+ continue;
+ }
+ unsigned reflectorsInFeature = *reinterpret_cast<uint32_t*>(wkb
+ 5);
+ if (!reflectorsInFeature)
+ continue;
+ if (feature.wkb_count != wkbHeaderSize + wkbPointSize *
reflectorsInFeature)
+ {
+ PLAYER_WARN("Unexpected WKB size in SetReflectors\n");
+ continue;
+ }
+
+ // Expand the reflectors array
+ int startIndex = numReflectors;
+ numReflectors += reflectorsInFeature;
+ if (numReflectors > 32)
+ PLAYER_ERROR("More reflectors passed than NAV200
supports\n");
+
+ // Copy in new reflectors
+ for (unsigned r = 0; r < reflectorsInFeature; r++)
+ {
+ uint8_t* pointData = wkb + wkbHeaderSize + wkbPointSize
* r;
+ if (pointData[0] == 0)
+ PLAYER_ERROR("SetReflectors does not support
big endian wkb data, let alone inconsistently\n");
+ if (*reinterpret_cast<uint32_t*>(pointData + 1) != 1)
+ PLAYER_ERROR("Malformed wkb data, expected
point\n");
+ reflectors[startIndex + r].x =
int(*reinterpret_cast<double*>(pointData + 5) * 1000.0);
+ reflectors[startIndex + r].y =
int(*reinterpret_cast<double*>(pointData + 13) * 1000.0);
+ }
+ }
+
+ BuildWKB(); // Might be something odd about the passed wkb, so build it
the usual way.
+
+ // Download the reflectors to the device.
+
+ if (!Laser.EnterStandby())
+ PLAYER_ERROR("Unable to enter standby mode.\n");
+
+ if (!Laser.EnterDownload())
+ {
+ PLAYER_ERROR("Unable to enter download mode.\n");
+ return;
+ }
+
+ for (int r = 0; r < numReflectors; r++)
+ Laser.DownloadReflector(0, r, reflectors[r].x, reflectors[r].y);
+ Laser.DownloadReflector(0, -1, 0, 0); // Let the NAV know that's all of
them.
+
+ if (!Laser.EnterStandby())
+ PLAYER_ERROR("Unable to return to standby mode after getting
reflectors.\n");
+
+ if (!Laser.EnterPositioning())
+ PLAYER_ERROR("Unable to return to positioning mode after
getting reflectors.\n");
+ }
+
+ void SickNAV200::FetchReflectors()
+ {
+ PLAYER_MSG0(2, "Fetching reflectors from vectormap");
+ if (reflector_map)
+ {
+ bool gotReflectors = false;
+
+ Message* mapInfoMessage = reflector_map->Request(this->InQueue,
PLAYER_MSGTYPE_REQ, PLAYER_VECTORMAP_REQ_GET_MAP_INFO, NULL, 0, NULL);
+ if (mapInfoMessage->GetHeader()->type ==
PLAYER_MSGTYPE_RESP_ACK && mapInfoMessage->GetHeader()->subtype ==
PLAYER_VECTORMAP_REQ_GET_MAP_INFO)
+ {
+ player_vectormap_info_t* mapInfo =
reinterpret_cast<player_vectormap_info_t*>(mapInfoMessage->GetPayload());
+
+ if (mapInfo->layers_count == 1)
+ {
+ player_vectormap_layer_info_t layer =
mapInfo->layers[0];
+ player_vectormap_layer_data_t request;
+ memset(&request, 0, sizeof(request));
+ request.name = layer.name;
+ request.name_count = layer.name_count;
+
+ Message* response =
reflector_map->Request(this->InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_VECTORMAP_REQ_GET_LAYER_DATA, (void*)&request, 0, NULL);
+ if (response->GetHeader()->type ==
PLAYER_MSGTYPE_RESP_ACK && response->GetHeader()->subtype ==
PLAYER_VECTORMAP_REQ_GET_LAYER_DATA)
+ {
+ player_vectormap_layer_data_t*
layerData =
reinterpret_cast<player_vectormap_layer_data_t*>(response->GetPayload());
+
+ SetReflectors(layerData);
+ gotReflectors = true;
+ }
+ }
+ }
+
+ if (!gotReflectors)
+ PLAYER_WARN("failed to get reflectors from
vectormap\n");
+ }
+ else
+ PLAYER_WARN("no vectormap provided to fetch reflectors from\n");
+ }
+
void SickNAV200::BuildWKB()
{
// Encode reflector positions in well known binary format for
publishing.
delete [] wkbData; // Clear old data.
! wkbSize = wkbHeaderSize + wkbPointSize * numReflectors;
wkbData = new uint8_t[wkbSize];
wkbData[0] = 1; // Little endian (0 for big endian).
***************
*** 677,687 ****
for (int i = 0; i < numReflectors; i++)
{
! wkbData[9 + 21 * i] = 1; // Still little endian.
uint32_t wkbType = 1; // Point
! *reinterpret_cast<uint32_t*>(wkbData + 9 + 21 * i + 1) =
wkbType;
double x = reflectors[i].x / 1000.0; // mm to metres.
double y = reflectors[i].y / 1000.0;
! *reinterpret_cast<double*>(wkbData + 9 + 21 * i + 5) = x;
! *reinterpret_cast<double*>(wkbData + 9 + 21 * i + 13) = y;
}
}
--- 930,941 ----
for (int i = 0; i < numReflectors; i++)
{
! uint8_t* pointData = wkbData + wkbHeaderSize + wkbPointSize * i;
! pointData[0] = 1; // Still little endian.
uint32_t wkbType = 1; // Point
! *reinterpret_cast<uint32_t*>(pointData + 1) = wkbType;
double x = reflectors[i].x / 1000.0; // mm to metres.
double y = reflectors[i].y / 1000.0;
! *reinterpret_cast<double*>(pointData + 5) = x;
! *reinterpret_cast<double*>(pointData + 13) = y;
}
}
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2008.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
