Update of /cvsroot/playerstage/code/player/server/drivers/laser
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv16319
Modified Files:
urg_laser.cc urg_laser.h urglaserdriver.cc
Log Message:
added TOP-URG support
Index: urglaserdriver.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/laser/urglaserdriver.cc,v
retrieving revision 1.15
retrieving revision 1.16
diff -C2 -d -r1.15 -r1.16
*** urglaserdriver.cc 7 Feb 2008 02:23:00 -0000 1.15
--- urglaserdriver.cc 7 Mar 2008 02:01:22 -0000 1.16
***************
*** 223,239 ****
}
Laser.GetSensorConfig (&Conf);
// Solve the min/max angle problem
! min_i = static_cast<int> (round (384.0 + Conf.min_angle/Conf.resolution));
! max_i = static_cast<int> (round (384.0 + Conf.max_angle/Conf.resolution));
// For ancient firmware versions, set some hard limits on the min/max angle
capabilities
! if (min_i < URG04_MIN_STEP)
! min_i = URG04_MIN_STEP;
! if (max_i > URG04_MAX_STEP)
! max_i = URG04_MAX_STEP;
! int user_min_i = static_cast<int> (round (384.0 +
user_min_angle/Conf.resolution));
! int user_max_i = static_cast<int> (round (384.0 +
user_max_angle/Conf.resolution));
if (user_min_i > user_max_i)
--- 223,244 ----
}
Laser.GetSensorConfig (&Conf);
+
+ int half_idx = Laser.GetNumRanges() / 2;
// Solve the min/max angle problem
! min_i = static_cast<int> (round (half_idx +
Conf.min_angle/Conf.resolution));
! max_i = static_cast<int> (round (half_idx +
Conf.max_angle/Conf.resolution));
// For ancient firmware versions, set some hard limits on the min/max angle
capabilities
! if(Laser.GetSCIPVersion() < 3)
! {
! if (min_i < URG04_MIN_STEP)
! min_i = URG04_MIN_STEP;
! if (max_i > URG04_MAX_STEP)
! max_i = URG04_MAX_STEP;
! }
! int user_min_i = static_cast<int> (round (half_idx +
user_min_angle/Conf.resolution));
! int user_max_i = static_cast<int> (round (half_idx +
user_max_angle/Conf.resolution));
if (user_min_i > user_max_i)
***************
*** 255,260 ****
max_i = user_max_i;
! Conf.min_angle = (min_i - 384) * Conf.resolution;
! Conf.max_angle = (max_i - 384) * Conf.resolution;
// Start the device thread; spawns a new thread and executes
--- 260,265 ----
max_i = user_max_i;
! Conf.min_angle = (min_i - half_idx) * Conf.resolution;
! Conf.max_angle = (max_i - half_idx) * Conf.resolution;
// Start the device thread; spawns a new thread and executes
***************
*** 338,341 ****
--- 343,348 ----
Data.ranges_count = (max_i - min_i) + 1;
Data.ranges = new float [Data.ranges_count];
+ // TODO: look into getting intensity data
+ Data.intensity_count = 0;
for (unsigned int i = 0; i < Data.ranges_count; ++i)
Index: urg_laser.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/laser/urg_laser.cc,v
retrieving revision 1.6
retrieving revision 1.7
diff -C2 -d -r1.6 -r1.7
*** urg_laser.cc 4 Feb 2008 19:40:27 -0000 1.6
--- urg_laser.cc 7 Mar 2008 02:01:22 -0000 1.7
***************
*** 37,41 ****
// range is 5.6 meters (hey!)
int
! urg_laser::GetSCIPVersion ()
{
unsigned char Buffer [18];
--- 37,41 ----
// range is 5.6 meters (hey!)
int
! urg_laser::QuerySCIPVersion ()
{
unsigned char Buffer [18];
***************
*** 67,71 ****
if (strncmp ((const char *) Buffer, "VV\n00P\n", 7) != 0)
{
! printf ("> E: GetSCIPVersion: Error reading after VV command. Answer:
%s\n", Buffer);
return (-1);
}
--- 67,71 ----
if (strncmp ((const char *) Buffer, "VV\n00P\n", 7) != 0)
{
! printf ("> E: QuerySCIPVersion: Error reading after VV command. Answer:
%s\n", Buffer);
return (-1);
}
***************
*** 87,91 ****
if (strncmp ((const char *) Buffer, "FIRM:", 5) != 0)
! printf ("> W: GetSCIPVersion: Warning, 'FIRM:' is not where it is
supposed to be!\n");
// Read the firmware version major value
--- 87,98 ----
if (strncmp ((const char *) Buffer, "FIRM:", 5) != 0)
! {
! //printf ("> W: QuerySCIPVersion: Warning, 'FIRM:' is not where it is
supposed to be!\n");
! // HACK: assume that we're talking to a TOP-URG
! tcflush (fileno (laser_port), TCIFLUSH);
! this->SCIP_Version = 3;
! this->num_ranges = 1128;
! return(0);
! }
// Read the firmware version major value
***************
*** 141,144 ****
--- 148,154 ----
urg_laser::GetSensorConfig (player_laser_config_t *cfg)
{
+ // TODO: look into getting intensity data
+ cfg->intensity = 0;
+
if (SCIP_Version == 1)
{
***************
*** 228,232 ****
tcflush (fileno(laser_port), TCIFLUSH);
}
! else // SCIP_Version = 2
{
// ask hokuyo: PP
--- 238,242 ----
tcflush (fileno(laser_port), TCIFLUSH);
}
! else if(SCIP_Version == 2)
{
// ask hokuyo: PP
***************
*** 304,307 ****
--- 314,328 ----
RTOD (cfg->min_angle), RTOD (cfg->max_angle), RTOD
(cfg->resolution), cfg->max_range);
}
+ else // SCIP_Version = 3 (TOP-URG)
+ {
+ // HACK: should ask the device, but for now just hardcode it:
+ cfg->min_angle = DTOR(-135.0);
+ cfg->max_angle = DTOR(135.0);
+ cfg->resolution = DTOR(270.0/1128.0);
+ cfg->max_range = 30.0;
+
+ printf ("> I: TOP-URG specifications: [min_angle, max_angle, resolution,
max_range] = [%f, %f, %f, %f]\n",
+ RTOD (cfg->min_angle), RTOD (cfg->max_angle), RTOD
(cfg->resolution), cfg->max_range);
+ }
return (0);
}
***************
*** 354,357 ****
--- 375,379 ----
// Defaults to SCIP version 1
SCIP_Version = 1;
+ num_ranges = 769;
laser_port = NULL;
}
***************
*** 558,562 ****
tcsetattr (fd, TCSANOW, &newtio);
usleep (200000);
! GetSCIPVersion ();
tcflush (fd, TCIOFLUSH);
}
--- 580,584 ----
tcsetattr (fd, TCSANOW, &newtio);
usleep (200000);
! QuerySCIPVersion ();
tcflush (fd, TCIOFLUSH);
}
***************
*** 641,645 ****
}
}
! else // SCIP_Version == 2
{
tcflush (fileno (laser_port), TCIFLUSH);
--- 663,667 ----
}
}
! else if(SCIP_Version == 2)
{
tcflush (fileno (laser_port), TCIFLUSH);
***************
*** 712,715 ****
--- 734,812 ----
}
}
+ else // SCIP_Version == 3 (TOP-URG)
+ {
+ tcflush (fileno (laser_port), TCIFLUSH);
+ // send the command
+ fprintf (laser_port, "GD0000112700\n");
+
+ int file = fileno (laser_port);
+
+ // check the returned command
+ ReadUntil (file, Buffer, 13, -1);
+
+ if (strncmp ((const char *) Buffer, "GD0000112700", 12) != 0)
+ {
+ printf ("> E: GetReadings: Error reading command result: %s\n", Buffer);
+ tcflush (fileno (laser_port), TCIFLUSH);
+ return (-1);
+ }
+
+ // check the returned status
+ ReadUntil (file, Buffer, 3, -1);
+ Buffer[2] = 0;
+ if (Buffer[0] != '0' || Buffer[1] != '0')
+ return (Buffer[0] - '0')*10 + (Buffer[1] - '0');
+
+ ReadUntil_nthOccurence (file, 2, (char)0xa);
+
+ // NOTE: This only works for 769 requested samples.. (64 data bytes
+ // blocks are not the best choice for 3-byte values...)
+
+ for (int i = 0; ; ++i)
+ {
+ ReadUntil (file, Buffer, 3, -1);
+
+ //printf ("[%d of %d] 0x%x 0x%x 0x%x\n", i, MAX_READINGS, Buffer[0],
Buffer[1], Buffer [2]);
+
+ if ((Buffer[1] == '\n') && (Buffer[2] == '\n'))
+ break;
+ else if (Buffer[2] == '\n')
+ {
+ if (ReadUntil(file, &Buffer[1], 2, -1) < 0)
+ return (-1);
+ }
+ else if (Buffer[0] == '\n')
+ {
+ if (i <= MAX_READINGS)
+ {
+ readings->Readings[i - 1] = ((readings->Readings[i - 1] &
0xFFC0) | (Buffer[1]-0x30));
+ Buffer [0] = Buffer [2];
+ if (ReadUntil (file, &Buffer[1], 2, -1) < 0)
+ return (-1);
+ }
+ else
+ printf ("> E: Got too many readings! %d\n",i);
+ }
+ else if (Buffer[1] == '\n')
+ {
+ Buffer[0] = Buffer[2];
+ if (ReadUntil (file, &Buffer[1], 2, -1) < 0)
+ return (-1);
+ }
+
+ if (i < MAX_READINGS)
+ {
+ readings->Readings[i] = ((Buffer[0]-0x30) << 12) | ((Buffer[1]-0x30)
<< 6) | (Buffer[2]-0x30);
+ // > 50000 seems to be an error code for when an object is too close
+ if(readings->Readings[i] >= 50000)
+ readings->Readings[i] = 0;
+ if ((readings->Readings[i] > 30000) && (i >= min_i) && (i <= max_i))
+ printf ("> W: [%d] read error: %i is bigger than 30.0 meters\n", i,
readings->Readings[i]);
+ }
+ else
+ printf ("> E: Got too many readings! %d\n",i);
+ }
+ }
+
return (0);
}
Index: urg_laser.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/laser/urg_laser.h,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** urg_laser.h 7 Feb 2007 18:02:33 -0000 1.4
--- urg_laser.h 7 Mar 2008 02:01:22 -0000 1.5
***************
*** 2,6 ****
#include <libplayercore/playercore.h>
! #define MAX_READINGS 769
typedef struct urg_laser_readings
--- 2,6 ----
#include <libplayercore/playercore.h>
! #define MAX_READINGS 1128
typedef struct urg_laser_readings
***************
*** 28,35 ****
float GetMaxRange ();
int GetSensorConfig (player_laser_config_t *cfg);
! int GetSCIPVersion ();
private:
int SCIP_Version;
FILE * laser_port;
};
--- 28,38 ----
float GetMaxRange ();
int GetSensorConfig (player_laser_config_t *cfg);
! int GetSCIPVersion() { return(this->SCIP_Version); }
! int GetNumRanges() { return(this->num_ranges); }
private:
+ int QuerySCIPVersion ();
int SCIP_Version;
+ int num_ranges;
FILE * laser_port;
};
-------------------------------------------------------------------------
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
