Revision: 8062
http://playerstage.svn.sourceforge.net/playerstage/?rev=8062&view=rev
Author: gbiggs
Date: 2009-07-21 03:47:16 +0000 (Tue, 21 Jul 2009)
Log Message:
-----------
Updated playerv to the new ranger interface
Modified Paths:
--------------
code/player/trunk/server/drivers/camera/sphere/sphere_mixed.h
code/player/trunk/server/drivers/camera/v4l/camerav4l.cc
code/player/trunk/server/drivers/gps/gbxgarminacfr.cc
code/player/trunk/server/drivers/joystick/linuxjoy.cc
code/player/trunk/server/drivers/laser/pbs_driver.cc
code/player/trunk/server/drivers/laser/sickLDMRS.cc
code/player/trunk/server/drivers/laser/sicklms200.cc
code/player/trunk/server/drivers/mixed/reb/reb.cc
code/player/trunk/server/drivers/mixed/rflex/rflex.cc
code/player/trunk/server/drivers/position/globalize/globalize.cc
code/player/trunk/server/drivers/position/goto/goto.cc
code/player/trunk/server/drivers/position/motionmind/motionmind.cc
code/player/trunk/server/drivers/power/oceanserver.cc
code/player/trunk/server/drivers/ranger/gbxsickacfr.cc
code/player/trunk/server/drivers/vectormap/robotracker.cc
code/player/trunk/server/drivers/wifi/aodv.cc
code/player/trunk/server/drivers/wifi/iwspy.cc
code/player/trunk/server/drivers/wifi/linuxwifi.cc
code/player/trunk/utils/playerv/playerv.h
code/player/trunk/utils/playerv/pv_dev_ranger.c
Modified: code/player/trunk/server/drivers/camera/sphere/sphere_mixed.h
===================================================================
--- code/player/trunk/server/drivers/camera/sphere/sphere_mixed.h
2009-07-21 01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/camera/sphere/sphere_mixed.h
2009-07-21 03:47:16 UTC (rev 8062)
@@ -28,9 +28,6 @@
#define PLAYER_CAMERA_FORMAT_RGB 6
#define PLAYER_CAMERA_FORMAT_YUV420P 7
-// Time for timestamps
-extern PlayerTime *GlobalTime;
-
////////////////////////////////////////////////////////////////////////////////
// The class for the driver
class SphereDriver : public ThreadedDriver
Modified: code/player/trunk/server/drivers/camera/v4l/camerav4l.cc
===================================================================
--- code/player/trunk/server/drivers/camera/v4l/camerav4l.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/camera/v4l/camerav4l.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -185,9 +185,6 @@
#include "v4lcapture.h" // For Gavin's libfg; should integrate this
#include "ccvt.h" // For YUV420P-->RGB conversion
-// Time for timestamps
-extern PlayerTime *GlobalTime;
-
// Driver for detecting laser retro-reflectors.
class CameraV4L : public ThreadedDriver
{
Modified: code/player/trunk/server/drivers/gps/gbxgarminacfr.cc
===================================================================
--- code/player/trunk/server/drivers/gps/gbxgarminacfr.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/gps/gbxgarminacfr.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -70,12 +70,6 @@
- debug (int)
- Default: 0
- Debugging level of the underlying library to get verbose output.
- - pose (float 6-tuple: (m, m, m, rad, rad, rad))
- - Default: [0.0 0.0 0.0 0.0 0.0 0.0]
- - Pose (x, y, z, roll, pitch, yaw) of the laser relative to its parent
object (e.g. the robot).
- - size (float 3-tuple: (m, m, m))
- - Default: [0.0 0.0 0.0]
- - Size of the laser in metres.
@par Example
@@ -117,10 +111,6 @@
// Configuration parameters
gbxgarminacfr::Config _config;
unsigned int _debug;
- // Geometry
- player_ranger_geom_t _geom;
- player_pose3d_t _sensorPose;
- player_bbox3d_t _sensorSize;
// Data storage
player_gps_data_t _gpsData;
// The hardware device itself
@@ -151,22 +141,6 @@
_config.ignoreUnknown = cf->ReadInt (section, "ignore_unknown", false);
_config.device = cf->ReadString (section, "port", "/dev/ttyS0");
_debug = cf->ReadBool (section, "debug", 0);
- // Set up geometry information
- _geom.pose.px = cf->ReadTupleLength (section, "pose", 0, 0.0f);
- _geom.pose.py = cf->ReadTupleLength (section, "pose", 1, 0.0f);
- _geom.pose.pz = cf->ReadTupleLength (section, "pose", 2, 0.0f);
- _geom.pose.proll = cf->ReadTupleAngle (section, "pose", 3, 0.0f);
- _geom.pose.ppitch = cf->ReadTupleAngle (section, "pose", 4, 0.0f);
- _geom.pose.pyaw = cf->ReadTupleAngle (section, "pose", 5, 0.0f);
- _geom.size.sw = cf->ReadTupleLength (section, "size", 0, 0.0f);
- _geom.size.sl = cf->ReadTupleLength (section, "size", 1, 0.0f);
- _geom.size.sh = cf->ReadTupleLength (section, "size", 2, 0.0f);
- _geom.sensor_poses_count = 1;
- _geom.sensor_poses = &_sensorPose;
- memcpy (_geom.sensor_poses, &_geom.pose, sizeof (_geom.pose));
- _geom.sensor_sizes_count = 1;
- _geom.sensor_sizes = &_sensorSize;
- memcpy (_geom.sensor_sizes, &_geom.size, sizeof (_geom.size));
memset (&_gpsData, 0, sizeof (_gpsData));
}
Modified: code/player/trunk/server/drivers/joystick/linuxjoy.cc
===================================================================
--- code/player/trunk/server/drivers/joystick/linuxjoy.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/joystick/linuxjoy.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -223,8 +223,6 @@
#include <replace/replace.h> // for poll(2)
#include <libplayercore/playercore.h>
-extern PlayerTime *GlobalTime;
-
#define XAXIS 1
#define YAXIS 2
#define YAWAXIS 0
Modified: code/player/trunk/server/drivers/laser/pbs_driver.cc
===================================================================
--- code/player/trunk/server/drivers/laser/pbs_driver.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/laser/pbs_driver.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -93,8 +93,6 @@
#include <libplayercore/globals.h>
-extern PlayerTime* GlobalTime;
-
#define DWORD unsigned int
#define DEFAULT_PBS_PORT "/dev/ttyS0"
#define NUMBER_OF_RANGE_READINGS 121
Modified: code/player/trunk/server/drivers/laser/sickLDMRS.cc
===================================================================
--- code/player/trunk/server/drivers/laser/sickLDMRS.cc 2009-07-21 01:49:26 UTC
(rev 8061)
+++ code/player/trunk/server/drivers/laser/sickLDMRS.cc 2009-07-21 03:47:16 UTC
(rev 8062)
@@ -93,8 +93,6 @@
#define MESSAGE_LEN 38U
-//extern PlayerTime *GlobalTime;
-
///////////////////////////////////////////////////////////////////////////////
// The class for the driver
class sickLDMRS : public ThreadedDriver
Modified: code/player/trunk/server/drivers/laser/sicklms200.cc
===================================================================
--- code/player/trunk/server/drivers/laser/sicklms200.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/laser/sicklms200.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -215,7 +215,6 @@
#include <libplayercore/playercore.h>
#include <libplayerinterface/playerxdr.h>
#include <replace/replace.h>
-extern PlayerTime* GlobalTime;
#define DEFAULT_LASER_PORT "/dev/ttyS1"
#define DEFAULT_LASER_CONNECT_RATE 9600
Modified: code/player/trunk/server/drivers/mixed/reb/reb.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/reb/reb.cc 2009-07-21 01:49:26 UTC
(rev 8061)
+++ code/player/trunk/server/drivers/mixed/reb/reb.cc 2009-07-21 03:47:16 UTC
(rev 8062)
@@ -161,7 +161,6 @@
#include <reb.h>
#include <libplayercore/playercore.h>
-extern PlayerTime* GlobalTime;
// so we can access the deviceTable and extract pointers to the sonar
// and position objects
Modified: code/player/trunk/server/drivers/mixed/rflex/rflex.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/rflex/rflex.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/mixed/rflex/rflex.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -315,7 +315,6 @@
#include <libplayercore/playercore.h>
#include <libplayerinterface/playerxdr.h>
-extern PlayerTime* GlobalTime;
extern int RFLEX::joy_control;
Modified: code/player/trunk/server/drivers/position/globalize/globalize.cc
===================================================================
--- code/player/trunk/server/drivers/position/globalize/globalize.cc
2009-07-21 01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/position/globalize/globalize.cc
2009-07-21 03:47:16 UTC (rev 8062)
@@ -73,8 +73,6 @@
#define RQ_QUEUE_LEN 10
-extern PlayerTime * GlobalTime;
-
class Globalize : public Driver
{
public:
Modified: code/player/trunk/server/drivers/position/goto/goto.cc
===================================================================
--- code/player/trunk/server/drivers/position/goto/goto.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/position/goto/goto.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -127,8 +127,6 @@
#define RQ_QUEUE_LEN 10
-extern PlayerTime * GlobalTime;
-
class Goto : public Driver
{
public:
Modified: code/player/trunk/server/drivers/position/motionmind/motionmind.cc
===================================================================
--- code/player/trunk/server/drivers/position/motionmind/motionmind.cc
2009-07-21 01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/position/motionmind/motionmind.cc
2009-07-21 03:47:16 UTC (rev 8062)
@@ -148,8 +148,6 @@
// Register indexes for WRITE and WRITE STORE commands
#define MM_REG_POSITION 0x00
-extern PlayerTime *GlobalTime;
-
///////////////////////////////////////////////////////////////////////////////
// The class for the driver
class MotionMind : public ThreadedDriver
Modified: code/player/trunk/server/drivers/power/oceanserver.cc
===================================================================
--- code/player/trunk/server/drivers/power/oceanserver.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/power/oceanserver.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -103,10 +103,6 @@
// Configuration parameters
std::string _port;
unsigned int _debug;
- // Geometry
- player_ranger_geom_t _geom;
- player_pose3d_t _sensorPose;
- player_bbox3d_t _sensorSize;
// The hardware device itself
std::auto_ptr<gbxsmartbatteryacfr::OceanServer> _device;
// Objects to handle messages from the driver
@@ -130,22 +126,6 @@
// Setup config object
_port = cf->ReadString (section, "port", "/dev/ttyS0");
_debug = cf->ReadBool (section, "debug", 0);
- // Set up geometry information
- _geom.pose.px = cf->ReadTupleLength (section, "pose", 0, 0.0f);
- _geom.pose.py = cf->ReadTupleLength (section, "pose", 1, 0.0f);
- _geom.pose.pz = cf->ReadTupleLength (section, "pose", 2, 0.0f);
- _geom.pose.proll = cf->ReadTupleAngle (section, "pose", 3, 0.0f);
- _geom.pose.ppitch = cf->ReadTupleAngle (section, "pose", 4, 0.0f);
- _geom.pose.pyaw = cf->ReadTupleAngle (section, "pose", 5, 0.0f);
- _geom.size.sw = cf->ReadTupleLength (section, "size", 0, 0.0f);
- _geom.size.sl = cf->ReadTupleLength (section, "size", 1, 0.0f);
- _geom.size.sh = cf->ReadTupleLength (section, "size", 2, 0.0f);
- _geom.sensor_poses_count = 1;
- _geom.sensor_poses = &_sensorPose;
- memcpy (_geom.sensor_poses, &_geom.pose, sizeof (_geom.pose));
- _geom.sensor_sizes_count = 1;
- _geom.sensor_sizes = &_sensorSize;
- memcpy (_geom.sensor_sizes, &_geom.size, sizeof (_geom.size));
}
OceanServer::~OceanServer (void)
Modified: code/player/trunk/server/drivers/ranger/gbxsickacfr.cc
===================================================================
--- code/player/trunk/server/drivers/ranger/gbxsickacfr.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/ranger/gbxsickacfr.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -138,8 +138,8 @@
unsigned int connectionDelay;
// Geometry
player_ranger_geom_t geom;
- player_pose3d_t sensorPose;
- player_bbox3d_t sensorSize;
+ player_pose3d_t elementPose;
+ player_bbox3d_t elementSize;
// Data storage
float *rawRanges;
double *ranges;
@@ -190,12 +190,12 @@
geom.size.sw = cf->ReadTupleLength (section, "size", 0, 0.0f);
geom.size.sl = cf->ReadTupleLength (section, "size", 1, 0.0f);
geom.size.sh = cf->ReadTupleLength (section, "size", 2, 0.0f);
- geom.sensor_poses_count = 1;
- geom.sensor_poses = &sensorPose;
- memcpy (geom.sensor_poses, &geom.pose, sizeof (geom.pose));
- geom.sensor_sizes_count = 1;
- geom.sensor_sizes = &sensorSize;
- memcpy (geom.sensor_sizes, &geom.size, sizeof (geom.size));
+ geom.element_poses_count = 1;
+ geom.element_poses = &elementPose;
+ memcpy (geom.element_poses, &geom.pose, sizeof (geom.pose));
+ geom.element_sizes_count = 1;
+ geom.element_sizes = &elementSize;
+ memcpy (geom.element_sizes, &geom.size, sizeof (geom.size));
}
GbxSickAcfr::~GbxSickAcfr (void)
@@ -331,7 +331,7 @@
player_ranger_config_t rangerConfig;
rangerConfig.min_angle = config.startAngle;
rangerConfig.max_angle = config.startAngle + config.fieldOfView;
- rangerConfig.resolution = config.fieldOfView / static_cast<double>
(config.numberOfSamples - 1);
+ rangerConfig.angular_res = config.fieldOfView / static_cast<double>
(config.numberOfSamples - 1);
rangerConfig.max_range = config.maxRange;
rangerConfig.range_res = 0.0f;
rangerConfig.frequency = 0.0f;
Modified: code/player/trunk/server/drivers/vectormap/robotracker.cc
===================================================================
--- code/player/trunk/server/drivers/vectormap/robotracker.cc 2009-07-21
01:49:26 UTC (rev 8061)
+++ code/player/trunk/server/drivers/vectormap/robotracker.cc 2009-07-21
03:47:16 UTC (rev 8062)
@@ -130,8 +130,6 @@
#define EPS 0.00000000000001
-extern PlayerTime * GlobalTime;
-
class RoboTracker : Driver
{
public:
Modified: code/player/trunk/server/drivers/wifi/aodv.cc
===================================================================
--- code/player/trunk/server/drivers/wifi/aodv.cc 2009-07-21 01:49:26 UTC
(rev 8061)
+++ code/player/trunk/server/drivers/wifi/aodv.cc 2009-07-21 03:47:16 UTC
(rev 8062)
@@ -77,8 +77,6 @@
#include <libplayercore/playercore.h>
-extern PlayerTime *GlobalTime;
-
#define AODV_INFO_FILE "/proc/aodv/route_table"
class Aodv : public Driver
Modified: code/player/trunk/server/drivers/wifi/iwspy.cc
===================================================================
--- code/player/trunk/server/drivers/wifi/iwspy.cc 2009-07-21 01:49:26 UTC
(rev 8061)
+++ code/player/trunk/server/drivers/wifi/iwspy.cc 2009-07-21 03:47:16 UTC
(rev 8062)
@@ -95,8 +95,6 @@
#include <libplayercore/playercore.h>
-extern PlayerTime *GlobalTime;
-
class Iwspy : public ThreadedDriver
{
public: Iwspy( ConfigFile *cf, int section);
Modified: code/player/trunk/server/drivers/wifi/linuxwifi.cc
===================================================================
--- code/player/trunk/server/drivers/wifi/linuxwifi.cc 2009-07-21 01:49:26 UTC
(rev 8061)
+++ code/player/trunk/server/drivers/wifi/linuxwifi.cc 2009-07-21 03:47:16 UTC
(rev 8062)
@@ -95,8 +95,6 @@
#include <libplayercore/playercore.h>
-extern PlayerTime *GlobalTime;
-
#define WIFI_INFO_FILE "/proc/net/wireless"
#define WIFI_UPDATE_INTERVAL 1000 // in milliseconds
Modified: code/player/trunk/utils/playerv/playerv.h
===================================================================
--- code/player/trunk/utils/playerv/playerv.h 2009-07-21 01:49:26 UTC (rev
8061)
+++ code/player/trunk/utils/playerv/playerv.h 2009-07-21 03:47:16 UTC (rev
8062)
@@ -604,7 +604,6 @@
rtk_menuitem_t *subscribe_item;
rtk_menuitem_t *style_item;
rtk_menuitem_t *intns_item;
- rtk_menuitem_t *device_item;
// Figure for drawing the scan
rtk_fig_t **scan_fig;
// Need to track this for creating figures
@@ -612,7 +611,7 @@
// Properties from the device that may be necessary
double start_angle;
- double resolution;
+ double angular_res;
} ranger_t;
Modified: code/player/trunk/utils/playerv/pv_dev_ranger.c
===================================================================
--- code/player/trunk/utils/playerv/pv_dev_ranger.c 2009-07-21 01:49:26 UTC
(rev 8061)
+++ code/player/trunk/utils/playerv/pv_dev_ranger.c 2009-07-21 03:47:16 UTC
(rev 8062)
@@ -48,9 +48,8 @@
snprintf(label, sizeof(label), "ranger:%d (%s)", index, ranger->drivername);
ranger->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
ranger->subscribe_item = rtk_menuitem_create(ranger->menu, "Subscribe", 1);
- ranger->style_item = rtk_menuitem_create(ranger->menu, "Filled", 0);
+ ranger->style_item = rtk_menuitem_create(ranger->menu, "Filled", 1);
ranger->intns_item = rtk_menuitem_create(ranger->menu, "Draw intensity
data", 1);
- ranger->device_item = rtk_menuitem_create(ranger->menu, "Singular", 1);
// Set the initial menu state
rtk_menuitem_check(ranger->subscribe_item, subscribe);
@@ -70,7 +69,7 @@
if (ranger->scan_fig != NULL)
{
- for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
+ for (ii = 0; ii < ranger->proxy->element_count; ii++)
rtk_fig_destroy(ranger->scan_fig[ii]);
free(ranger->scan_fig);
ranger->scan_fig = NULL;
@@ -90,7 +89,6 @@
rtk_menuitem_destroy(ranger->subscribe_item);
rtk_menuitem_destroy(ranger->style_item);
rtk_menuitem_destroy(ranger->intns_item);
- rtk_menuitem_destroy(ranger->device_item);
rtk_menu_destroy(ranger->menu);
free(ranger->drivername);
@@ -121,29 +119,45 @@
{
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
ranger->start_angle = 0.0f;
- ranger->resolution = 0.0f;
+ ranger->angular_res = 0.0f;
}
else
{
ranger->start_angle = ranger->proxy->min_angle;
- ranger->resolution = ranger->proxy->resolution;
+ ranger->angular_res = ranger->proxy->angular_res;
}
// Delete any current figures
ranger_delete_figures(ranger);
// Create the figures
- if ((ranger->scan_fig = malloc(ranger->proxy->sensor_count *
sizeof(rtk_fig_t*))) == NULL )
+ if (ranger->proxy->element_count == 1)
{
- PRINT_ERR1("Failed to allocate memory for %d figures to display
ranger", ranger->proxy->sensor_count);
- return;
+ if ((ranger->scan_fig = malloc(sizeof(rtk_fig_t*))) == NULL )
+ {
+ PRINT_ERR("Failed to allocate memory for a figure to display
ranger");
+ return;
+ }
+ ranger->scan_fig[0] = rtk_fig_create(ranger->mainwnd->canvas,
ranger->mainwnd->robot_fig, 1);
+ rtk_fig_origin(ranger->scan_fig[0],
+ ranger->proxy->device_pose.px,
+ ranger->proxy->device_pose.py,
+ ranger->proxy->device_pose.pyaw);
}
- for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
+ else
{
- ranger->scan_fig[ii] = rtk_fig_create(ranger->mainwnd->canvas,
ranger->mainwnd->robot_fig, 1);
- rtk_fig_origin(ranger->scan_fig[ii],
- ranger->proxy->sensor_poses[ii].px,
- ranger->proxy->sensor_poses[ii].py,
- ranger->proxy->sensor_poses[ii].pyaw);
+ if ((ranger->scan_fig = malloc(ranger->proxy->element_count *
sizeof(rtk_fig_t*))) == NULL )
+ {
+ PRINT_ERR1("Failed to allocate memory for %d figures to display
ranger", ranger->proxy->element_count);
+ return;
+ }
+ for (ii = 0; ii < ranger->proxy->element_count; ii++)
+ {
+ ranger->scan_fig[ii] = rtk_fig_create(ranger->mainwnd->canvas,
ranger->mainwnd->robot_fig, 1);
+ rtk_fig_origin(ranger->scan_fig[ii],
+ ranger->proxy->element_poses[ii].px,
+ ranger->proxy->element_poses[ii].py,
+ ranger->proxy->element_poses[ii].pyaw);
+ }
}
}
}
@@ -172,67 +186,52 @@
// deleted above. I don't know whether commenting it out is the right
// thing to do - BPG.
/*
- for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
+ for (ii = 0; ii < ranger->proxy->element_count; ii++)
rtk_fig_show(ranger->scan_fig[ii], 0);
*/
}
}
-// Converts a range reading into a point in the CS of the ranger device
-// based on the pose of the sensor the reading is likely to belong to
-void range_to_point(ranger_t *ranger, int index, int sensor_num, double angle,
double *point)
-{
- // Point position = point from range -> rotate by range angle// -> rotate by
sensor yaw -> translate by sensor position
- point[0] = ranger->proxy->ranges[index] * cos(angle);// +
ranger->proxy->sensor_poses[sensor_num].pyaw) +
ranger->proxy->sensor_poses[sensor_num].px;
- point[1] = ranger->proxy->ranges[index] * sin(angle);// +
ranger->proxy->sensor_poses[sensor_num].pyaw) +
ranger->proxy->sensor_poses[sensor_num].py;
-}
-
// Draw the ranger scan
void ranger_draw(ranger_t *ranger)
{
- int ii = 0, jj = 0;
- int point_count;
+ int ii = 0;
double point1[2], point2[2];
double (*points)[2];
- double current_angle = 0.0f, temp = 0.0f;
- unsigned int ranges_per_sensor = 0;
+ double temp = 0.0;
- // Drawing type depends on the selected sensor type
+ // Drawing type depends on the assumed sensor type
// Singular sensors (e.g. sonar sensors):
- // Draw a cone for the first range scan of each sensor
+ // Draw a cone for each range scan
// Non-singular sensors (e.g. laser scanner):
// Draw the edge of the scan and empty space
- // Calculate the number of ranges per sensor
- if (ranger->proxy->sensor_count == 0)
- ranges_per_sensor = ranger->proxy->ranges_count;
- else
- ranges_per_sensor = ranger->proxy->ranges_count /
ranger->proxy->sensor_count;
-
- if (rtk_menuitem_ischecked(ranger->device_item))
+ if (ranger->proxy->element_count > 1)
{
// Draw sonar-like
points = calloc(3, sizeof(double)*2);
temp = 20.0f * M_PI / 180.0f / 2.0f;
- for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
+ for (ii = 0; ii < ranger->proxy->ranges_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_SONAR_SCAN);
- // Draw a cone for the first range for each sensor
- // Assume the range is straight ahead (ignore min_angle and resolution
properties)
+ // Draw a cone for each range
+ // Assume the range is straight ahead (ignore min_angle and angular_res
properties)
points[0][0] = 0.0f;
points[0][1] = 0.0f;
- points[1][0] = ranger->proxy->ranges[ii * ranges_per_sensor] *
cos(-temp);
- points[1][1] = ranger->proxy->ranges[ii * ranges_per_sensor] *
sin(-temp);
- points[2][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(temp);
- points[2][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(temp);
+ points[1][0] = ranger->proxy->ranges[ii] * cos(-temp);
+ points[1][1] = ranger->proxy->ranges[ii] * sin(-temp);
+ points[2][0] = ranger->proxy->ranges[ii] * cos(temp);
+ points[2][1] = ranger->proxy->ranges[ii] * sin(temp);
rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, 3, points, 1);
// Draw the sensor itself
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER);
- rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0,
ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0);
+ rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0,
+ ranger->proxy->element_sizes[ii].sw,
+ ranger->proxy->element_sizes[ii].sl, 0);
}
free(points);
points=NULL;
@@ -243,92 +242,75 @@
if (rtk_menuitem_ischecked(ranger->style_item))
{
// Draw each sensor in turn
- points = calloc(ranger->proxy->sensor_count, sizeof(double)*2);
- for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
- {
- rtk_fig_show(ranger->scan_fig[ii], 1);
- rtk_fig_clear(ranger->scan_fig[ii]);
+ points = calloc(ranger->proxy->ranges_count + 1, sizeof(double)*2);
+ rtk_fig_show(ranger->scan_fig[0], 1);
+ rtk_fig_clear(ranger->scan_fig[0]);
- // Draw empty space
- points[0][0] = ranger->proxy->sensor_poses[ii].px;
- points[0][1] = ranger->proxy->sensor_poses[ii].py;
- point_count = 1;
- current_angle = ranger->start_angle;
- // Loop over the ranges
- for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor;
jj++)
- {
- range_to_point(ranger, jj, ii, current_angle, points[point_count]);
- // Move round to the angle of the next range
- current_angle += ranger->resolution;
- point_count++;
- }
- rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_EMP);
- rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, point_count, points, 1);
-
- // Draw occupied space
- rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
- current_angle = ranger->start_angle;
- for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor;
jj++)
- {
- range_to_point(ranger, jj, ii, current_angle - ranger->resolution,
point1);
- range_to_point(ranger, jj, ii, current_angle + ranger->resolution,
point2);
- rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0],
point2[1]);
- current_angle += ranger->resolution;
- }
+ // Draw empty space
+ points[0][0] = 0.0;
+ points[0][1] = 0.0;
+ for (ii = 0; ii < ranger->proxy->ranges_count; ii++)
+ {
+ points[ii + 1][0] = ranger->proxy->points[ii].px;
+ points[ii + 1][1] = ranger->proxy->points[ii].py;
}
+ rtk_fig_color_rgb32(ranger->scan_fig[0], COLOR_LASER_EMP);
+ rtk_fig_polygon(ranger->scan_fig[0], 0, 0, 0,
+ ranger->proxy->ranges_count + 1, points, 1);
free(points);
points = NULL;
+
+ // Draw occupied space
+ rtk_fig_color_rgb32(ranger->scan_fig[0], COLOR_LASER_OCC);
+ for (ii = 1; ii < ranger->proxy->ranges_count; ii++)
+ {
+ point1[0] = ranger->proxy->points[ii - 1].px;
+ point1[1] = ranger->proxy->points[ii - 1].py;
+ point2[0] = ranger->proxy->points[ii].px;
+ point2[1] = ranger->proxy->points[ii].py;
+ rtk_fig_line(ranger->scan_fig[0], point1[0], point1[1], point2[0],
point2[1]);
+ }
}
else
{
- // Draw a range scan for each individual sensor in the device
- for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
- {
- rtk_fig_show(ranger->scan_fig[ii], 1);
- rtk_fig_clear(ranger->scan_fig[ii]);
+ rtk_fig_show(ranger->scan_fig[0], 1);
+ rtk_fig_clear(ranger->scan_fig[0]);
- rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
- current_angle = ranger->start_angle;
- // Get the first point
- range_to_point(ranger, ii * ranges_per_sensor, ii,
ranger->start_angle, point1);
- // Loop over the rest of the ranges
- for (jj = ii * ranges_per_sensor + 1; jj < (ii + 1) *
ranges_per_sensor; jj++)
- {
- range_to_point(ranger, jj, ii, current_angle, point2);
- // Draw a line from point 1 (previous point) to point 2 (current
point)
- rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0],
point2[1]);
- point1[0] = point2[0];
- point1[1] = point2[1];
- // Move round to the angle of the next range
- current_angle += ranger->resolution;
- }
+ rtk_fig_color_rgb32(ranger->scan_fig[0], COLOR_LASER_OCC);
+ // Get the first point
+ point1[0] = ranger->proxy->points[0].px;
+ point1[1] = ranger->proxy->points[0].py;
+ // Loop over the rest of the ranges
+ for (ii = 1; ii < ranger->proxy->ranges_count; ii++)
+ {
+ point2[0] = ranger->proxy->points[ii].px;
+ point2[1] = ranger->proxy->points[ii].py;
+ // Draw a line from point 1 (previous point) to point 2 (current point)
+ rtk_fig_line(ranger->scan_fig[0], point1[0], point1[1], point2[0],
point2[1]);
+ point1[0] = point2[0];
+ point1[1] = point2[1];
}
}
- // For each sensor...
- for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
+ if (rtk_menuitem_ischecked(ranger->intns_item))
{
- if (rtk_menuitem_ischecked(ranger->intns_item))
+ // Draw an intensity scan
+ if (ranger->proxy->intensities_count > 0)
{
- // Draw an intensity scan
- if (ranger->proxy->intensities_count > 0)
+ for (ii = 0; ii < ranger->proxy->intensities_count; ii++)
{
- current_angle = ranger->start_angle;
- for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor;
jj++)
+ if (ranger->proxy->intensities[ii] != 0)
{
- if (ranger->proxy->intensities[0] != 0)
- {
- range_to_point(ranger, jj, ii, current_angle, point1);
- rtk_fig_rectangle(ranger->scan_fig[ii], point1[0], point1[1], 0,
0.05, 0.05, 1);
- }
- current_angle += ranger->resolution;
+ point1[0] = ranger->proxy->points[ii].px;
+ point1[1] = ranger->proxy->points[ii].py;
+ rtk_fig_rectangle(ranger->scan_fig[0], point1[0], point1[1], 0,
0.05, 0.05, 1);
}
}
}
-
- // Draw the sensor itself
- rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER);
- rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0,
ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0);
}
+
+ // Draw the sensor itself
+ rtk_fig_color_rgb32(ranger->scan_fig[0], COLOR_LASER);
+ rtk_fig_rectangle(ranger->scan_fig[0], 0, 0, 0,
ranger->proxy->device_size.sw, ranger->proxy->device_size.sl, 0);
}
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Enter the BlackBerry Developer Challenge
This is your chance to win up to $100,000 in prizes! For a limited time,
vendors submitting new applications to BlackBerry App World(TM) will have
the opportunity to enter the BlackBerry Developer Challenge. See full prize
details at: http://p.sf.net/sfu/Challenge
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
