Revision: 8312
http://playerstage.svn.sourceforge.net/playerstage/?rev=8312&view=rev
Author: thjc
Date: 2009-10-18 15:31:25 +0000 (Sun, 18 Oct 2009)
Log Message:
-----------
applied patch 2880788: Player SVN trunk: collective ranger patch
Modified Paths:
--------------
code/player/trunk/client_libs/libplayerc/dev_ranger.c
code/player/trunk/server/drivers/ranger/lasertoranger.cc
code/player/trunk/server/drivers/ranger/sonartoranger.cc
code/player/trunk/server/drivers/ranger/toranger.cc
code/player/trunk/utils/playerv/pv_dev_ranger.c
Modified: code/player/trunk/client_libs/libplayerc/dev_ranger.c
===================================================================
--- code/player/trunk/client_libs/libplayerc/dev_ranger.c 2009-10-18
15:26:36 UTC (rev 8311)
+++ code/player/trunk/client_libs/libplayerc/dev_ranger.c 2009-10-18
15:31:25 UTC (rev 8312)
@@ -122,20 +122,23 @@
return;
}
- if (device->element_count == 1)
+ if (device->bearings_count >= device->element_count)
{
- b = device->min_angle;
- for (ii = 0; ii < device->bearings_count; ii++)
+ if (device->element_count == 1)
{
- device->bearings[ii] = b + device->device_pose.pyaw;
- b += device->angular_res;
+ b = device->min_angle;
+ for (ii = 0; ii < device->bearings_count; ii++)
+ {
+ device->bearings[ii] = b + device->device_pose.pyaw;
+ b += device->angular_res;
+ }
}
- }
- else
- {
- for (ii = 0; ii < device->element_count; ii++)
+ else
{
- device->bearings[ii] = device->element_poses[ii].pyaw;
+ for (ii = 0; ii < device->element_count; ii++)
+ {
+ device->bearings[ii] = device->element_poses[ii].pyaw;
+ }
}
}
}
@@ -145,7 +148,7 @@
// Calculate scan points
void playerc_ranger_calculate_points(playerc_ranger_t *device)
{
- double b;
+ double b, r, s;
uint32_t ii;
device->points_count = device->ranges_count;
@@ -163,27 +166,30 @@
return;
}
- if (device->element_count == 1)
+ if (device->points_count >= device->element_count)
{
- b = device->min_angle;
- for (ii = 0; ii < device->points_count; ii++)
+ if (device->element_count == 1)
{
- double r = device->ranges[ii];
- device->points[ii].px = r * cos(b);
- device->points[ii].py = r * sin(b);
- device->points[ii].pz = 0.0;
- b += device->angular_res;
+ b = device->min_angle;
+ for (ii = 0; ii < device->points_count; ii++)
+ {
+ r = device->ranges[ii];
+ device->points[ii].px = r * cos(b);
+ device->points[ii].py = r * sin(b);
+ device->points[ii].pz = 0.0;
+ b += device->angular_res;
+ }
}
- }
- else
- {
- for (ii = 0; ii < device->element_count; ii++)
+ else
{
- double r = device->ranges[ii];
- double s = r * cos(device->element_poses[ii].ppitch);
- device->points[ii].px = s * cos(device->element_poses[ii].pyaw) +
device->element_poses[ii].px;
- device->points[ii].py = s * sin(device->element_poses[ii].pyaw) +
device->element_poses[ii].py;
- device->points[ii].pz = r * sin(device->element_poses[ii].ppitch) +
device->element_poses[ii].pz;
+ for (ii = 0; ii < device->element_count; ii++)
+ {
+ r = device->ranges[ii];
+ s = r * cos(device->element_poses[ii].ppitch);
+ device->points[ii].px = s * cos(device->element_poses[ii].pyaw) +
device->element_poses[ii].px;
+ device->points[ii].py = s * sin(device->element_poses[ii].pyaw) +
device->element_poses[ii].py;
+ device->points[ii].pz = r * sin(device->element_poses[ii].ppitch) +
device->element_poses[ii].pz;
+ }
}
}
}
@@ -204,7 +210,7 @@
}
}
// Copy the range data
- memcpy(device->ranges, data->ranges, data->ranges_count *
sizeof(data->ranges[0]));
+ if (data->ranges_count > 0) memcpy(device->ranges, data->ranges,
data->ranges_count * sizeof(data->ranges[0]));
device->ranges_count = data->ranges_count;
}
@@ -223,7 +229,7 @@
}
}
// Copy the range data
- memcpy(device->intensities, data->intensities, data->intensities_count *
sizeof(data->intensities[0]));
+ if (data->intensities_count > 0) memcpy(device->intensities,
data->intensities, data->intensities_count * sizeof(data->intensities[0]));
device->intensities_count = data->intensities_count;
}
Modified: code/player/trunk/server/drivers/ranger/lasertoranger.cc
===================================================================
--- code/player/trunk/server/drivers/ranger/lasertoranger.cc 2009-10-18
15:26:36 UTC (rev 8311)
+++ code/player/trunk/server/drivers/ranger/lasertoranger.cc 2009-10-18
15:31:25 UTC (rev 8312)
@@ -57,8 +57,18 @@
@par Configuration file options
- - None
+- buggy_geom (integer)
+ - Default: 0
+ - If set to 1, the pz, proll and ppitch fields will be set to 0.0
+ - This option is added to provide compatibility with old buggy laser drivers
+ that do not clean unused geometry structure fields
+- force_config (float tuple)
+ - Default: not set
+ - If set, it will override configuration values obtained from buggy laser
drivers
+ - values: [min_angle max_angle angular_res max_range range_res]
+ - units: [deg deg deg m m]
+
@par Example
@verbatim
@@ -114,6 +124,13 @@
uint8_t lastReqType; // Last request
type on the ranger interface
bool startupComplete; // True once
this driver is ready to go
+ int buggy_geom;
+ int force_config;
+ float force_min_angle;
+ float force_max_angle;
+ float force_angular_res;
+ float force_max_range;
+ float force_range_res;
};
// Initialisation function
@@ -140,9 +157,28 @@
// Need a laser device as input
if (cf->ReadDeviceAddr(&inputDeviceAddr, section, "requires",
PLAYER_LASER_CODE, -1, NULL) != 0)
{
- SetError (-1);
+ this->SetError(-1);
return;
}
+ this->buggy_geom = cf->ReadInt(section, "buggy_geom", 0);
+ this->force_config = 0;
+ switch (cf->GetTupleCount(section, "force_config"))
+ {
+ case 0:
+ break;
+ case 5:
+ this->force_config = !0;
+ this->force_min_angle = cf->ReadTupleAngle(section,
"force_config", 0, 0.0);
+ this->force_max_angle = cf->ReadTupleAngle(section,
"force_config", 1, 0.0);
+ this->force_angular_res = cf->ReadTupleAngle(section,
"force_config", 2, 0.0);
+ this->force_max_range = cf->ReadTupleFloat(section,
"force_config", 3, 0.0);
+ this->force_range_res = cf->ReadTupleFloat(section,
"force_config", 4, 0.0);
+ break;
+ default:
+ PLAYER_ERROR("Invalid number of forced settings");
+ this->SetError(-1);
+ return;
+ }
}
// Setup function
@@ -240,10 +276,19 @@
if (data != NULL)
{
// Update the properties from the data
- laserConfig.min_angle = scanData->min_angle;
- laserConfig.max_angle = scanData->max_angle;
- laserConfig.resolution = scanData->resolution;
- laserConfig.max_range = scanData->max_range;
+ if (this->force_config)
+ {
+ laserConfig.min_angle = this->force_min_angle;
+ laserConfig.max_angle = this->force_max_angle;
+ laserConfig.resolution = this->force_angular_res;
+ laserConfig.max_range = this->force_max_range;
+ } else
+ {
+ laserConfig.min_angle = scanData->min_angle;
+ laserConfig.max_angle = scanData->max_angle;
+ laserConfig.resolution = scanData->resolution;
+ laserConfig.max_range = scanData->max_range;
+ }
// Copy out the range data
if (scanData->ranges_count > 0)
@@ -306,6 +351,12 @@
deviceGeom.size.sw = data->size.sw;
deviceGeom.size.sl = data->size.sl;
deviceGeom.size.sh = data->size.sh;
+ if (this->buggy_geom)
+ {
+ deviceGeom.pose.pz = 0.0;
+ deviceGeom.pose.proll = 0.0;
+ deviceGeom.pose.ppitch = 0.0;
+ }
*(deviceGeom.element_poses) = deviceGeom.pose;
*(deviceGeom.element_sizes) = deviceGeom.size;
@@ -318,11 +369,21 @@
// Save a copy of the laser config, we need it to set intensity and/or
config later
memcpy (&laserConfig, data, sizeof (player_laser_config_t));
// Copy it into the destination
- dest->min_angle = data->min_angle;
- dest->max_angle = data->max_angle;
- dest->angular_res = data->resolution;
- dest->max_range = data->max_range;
- dest->range_res = data->range_res;
+ if (this->force_config)
+ {
+ dest->min_angle = this->force_min_angle;
+ dest->max_angle = this->force_max_angle;
+ dest->angular_res = this->force_angular_res;
+ dest->max_range = this->force_max_range;
+ dest->range_res = this->force_range_res;
+ } else
+ {
+ dest->min_angle = data->min_angle;
+ dest->max_angle = data->max_angle;
+ dest->angular_res = data->resolution;
+ dest->max_range = data->max_range;
+ dest->range_res = data->range_res;
+ }
dest->frequency = data->scanning_frequency;
}
Modified: code/player/trunk/server/drivers/ranger/sonartoranger.cc
===================================================================
--- code/player/trunk/server/drivers/ranger/sonartoranger.cc 2009-10-18
15:26:36 UTC (rev 8311)
+++ code/player/trunk/server/drivers/ranger/sonartoranger.cc 2009-10-18
15:31:25 UTC (rev 8312)
@@ -105,6 +105,8 @@
int ConvertGeom (player_sonar_geom_t *geom);
// Convert sonar data to ranger data
int ConvertData (player_sonar_data_t *data);
+ private:
+ int skip_rq;
};
// Initialisation function
@@ -128,6 +130,7 @@
SonarToRanger::SonarToRanger( ConfigFile* cf, int section)
: ToRanger (cf, section)
{
+ this->skip_rq = 0;
// Need a sonar device as input
if (cf->ReadDeviceAddr(&inputDeviceAddr, section, "requires",
PLAYER_SONAR_CODE, -1, NULL) != 0)
{
@@ -158,6 +161,7 @@
// Request the sonar geometry
inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_SONAR_REQ_GET_GEOM, NULL, 0, NULL);
+ this->skip_rq = !0;
return 0;
}
@@ -314,12 +318,14 @@
// Geometry request - need to manage the info we just got
if (ConvertGeom (reinterpret_cast<player_sonar_geom_t*> (data))
== 0)
{
- Publish (device_addr, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_RANGER_REQ_GET_GEOM, reinterpret_cast<void*> (&deviceGeom), sizeof
(deviceGeom), NULL);
+ if (!(this->skip_rq)) Publish (device_addr,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_GEOM, reinterpret_cast<void*>
(&deviceGeom), sizeof (deviceGeom), NULL);
+ this->skip_rq = 0;
return 0;
}
else
{
- Publish (device_addr, ret_queue,
PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_GEOM, NULL, 0, NULL);
+ if (!(this->skip_rq)) Publish (device_addr, ret_queue,
PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_GEOM, NULL, 0, NULL);
+ this->skip_rq = 0;
return 0;
}
}
@@ -334,7 +340,8 @@
else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_SONAR_REQ_GET_GEOM, inputDeviceAddr))
{
// Geometry request
- Publish (device_addr, ret_queue, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_RANGER_REQ_GET_GEOM, NULL, 0, NULL);
+ if (!(this->skip_rq)) Publish (device_addr, ret_queue,
PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_GEOM, NULL, 0, NULL);
+ this->skip_rq = 0;
return 0;
}
Modified: code/player/trunk/server/drivers/ranger/toranger.cc
===================================================================
--- code/player/trunk/server/drivers/ranger/toranger.cc 2009-10-18 15:26:36 UTC
(rev 8311)
+++ code/player/trunk/server/drivers/ranger/toranger.cc 2009-10-18 15:31:25 UTC
(rev 8312)
@@ -38,6 +38,9 @@
ToRanger::ToRanger (ConfigFile* cf, int section)
: Driver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_RANGER_CODE)
{
+ memset (&deviceGeom, 0, sizeof (deviceGeom));
+ deviceGeom.element_poses = NULL;
+ deviceGeom.element_sizes = NULL;
inputDevice = NULL;
}
@@ -56,6 +59,8 @@
{
// Clean output
memset (&deviceGeom, 0, sizeof (deviceGeom));
+ deviceGeom.element_poses = NULL;
+ deviceGeom.element_sizes = NULL;
return 0;
}
Modified: code/player/trunk/utils/playerv/pv_dev_ranger.c
===================================================================
--- code/player/trunk/utils/playerv/pv_dev_ranger.c 2009-10-18 15:26:36 UTC
(rev 8311)
+++ code/player/trunk/utils/playerv/pv_dev_ranger.c 2009-10-18 15:31:25 UTC
(rev 8312)
@@ -19,6 +19,7 @@
*/
#include <assert.h>
+#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
@@ -199,6 +200,7 @@
double point1[2], point2[2];
double (*points)[2];
double temp = 0.0;
+ double b, range;
// Drawing type depends on the assumed sensor type
// Singular sensors (e.g. sonar sensors):
@@ -221,7 +223,7 @@
// Assume the range is straight ahead (ignore min_angle and angular_res
properties)
points[0][0] = 0.0f;
points[0][1] = 0.0f;
- double range = ranger->proxy->ranges[ii];
+ range = ranger->proxy->ranges[ii];
if (range < ranger->proxy->min_range)
range = ranger->proxy->max_range;
points[1][0] = range * cos(-temp);
@@ -252,10 +254,10 @@
// Draw empty space
points[0][0] = 0.0;
points[0][1] = 0.0;
- double b = ranger->proxy->min_angle;
+ b = ranger->proxy->min_angle;
for (ii = 0; ii < ranger->proxy->ranges_count; ii++)
{
- double range = ranger->proxy->ranges[ii];
+ range = ranger->proxy->ranges[ii];
if (range < ranger->proxy->min_range) {
points[ii + 1][0] = ranger->proxy->max_range * cos(b);
points[ii + 1][1] = ranger->proxy->max_range * sin(b);
@@ -292,9 +294,9 @@
rtk_fig_color_rgb32(ranger->scan_fig[0], COLOR_LASER_OCC);
// Get the first point
- double b = ranger->proxy->min_angle;
+ b = ranger->proxy->min_angle;
- double range = ranger->proxy->ranges[0];
+ range = ranger->proxy->ranges[0];
if (range < ranger->proxy->min_range) {
point1[0] = ranger->proxy->max_range * cos(b);
point1[1] = ranger->proxy->max_range * sin(b);
@@ -326,14 +328,14 @@
if (rtk_menuitem_ischecked(ranger->intns_item))
{
// Draw an intensity scan
- double b = ranger->proxy->min_angle;
+ b = ranger->proxy->min_angle;
if (ranger->proxy->intensities_count > 0)
{
for (ii = 0; ii < ranger->proxy->intensities_count; ii++)
{
if (ranger->proxy->intensities[ii] != 0)
{
- double range = ranger->proxy->ranges[ii];
+ range = ranger->proxy->ranges[ii];
if (range < ranger->proxy->min_range) {
point1[0] = ranger->proxy->max_range * cos(b);
point1[1] = ranger->proxy->max_range * sin(b);
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Come build with us! The BlackBerry(R) Developer Conference in SF, CA
is the only developer event you need to attend this year. Jumpstart your
developing skills, take BlackBerry mobile applications to market and stay
ahead of the curve. Join us from November 9 - 12, 2009. Register now!
http://p.sf.net/sfu/devconference
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
