Menu
▾
▴
[Playerstage-commit] SF.net SVN: playerstage: [6362] code/player/branches/cmake
|
From: <gb...@us...> - 2008-04-18 01:17:47
|
Revision: 6362
http://playerstage.svn.sourceforge.net/playerstage/?rev=6362&view=rev
Author: gbiggs
Date: 2008-04-18 01:17:51 -0700 (Fri, 18 Apr 2008)
Log Message:
-----------
Merging changes 6302:6312 from trunk
Modified Paths:
--------------
code/player/branches/cmake/client_libs/libplayerc++/logproxy.cc
code/player/branches/cmake/client_libs/libplayerc++/playerc++.h
code/player/branches/cmake/server/drivers/joystick/linuxjoy.cc
code/player/branches/cmake/server/drivers/laser/CMakeLists.txt
code/player/branches/cmake/server/drivers/laser/sicklms200.cc
code/player/branches/cmake/server/drivers/position/nav200/sicknav200.cc
code/player/branches/cmake/server/drivers/shell/readlog.cc
code/player/branches/cmake/server/libplayerdrivers/driverregistry.cc
Removed Paths:
-------------
code/player/branches/cmake/server/drivers/laser/sickpls.cc
Modified: code/player/branches/cmake/client_libs/libplayerc++/logproxy.cc
===================================================================
--- code/player/branches/cmake/client_libs/libplayerc++/logproxy.cc 2008-04-18 08:10:03 UTC (rev 6361)
+++ code/player/branches/cmake/client_libs/libplayerc++/logproxy.cc 2008-04-18 08:17:51 UTC (rev 6362)
@@ -82,7 +82,38 @@
mDevice = NULL;
}
+void LogProxy::QueryState()
+{
+ scoped_lock_t lock(mPc->mMutex);
+
+ if (0 != playerc_log_get_state(mDevice))
+ throw PlayerError("LogProxy::QueryState()", "error querying state");
+ return;
+}
+
void
+LogProxy::SetState(int aState)
+{
+ scoped_lock_t lock(mPc->mMutex);
+
+ if (mDevice->type == 0) {
+ if (0 != playerc_log_get_state(mDevice))
+ throw PlayerError("LogProxy::SetState()", "error querying type");
+ }
+
+ if (mDevice->type == PLAYER_LOG_TYPE_READ) {
+ if (0 != playerc_log_set_read_state(mDevice,aState))
+ throw PlayerError("LogProxy::SetState()", "error setting read");
+ } else if(mDevice->type == PLAYER_LOG_TYPE_WRITE) {
+ if (0 != playerc_log_set_write_state(mDevice,aState))
+ throw PlayerError("LogProxy::SetState()", "error setting write");
+ } else {
+ // unknown type
+ }
+ return;
+}
+
+void
LogProxy::SetWriteState(int aState)
{
scoped_lock_t lock(mPc->mMutex);
Modified: code/player/branches/cmake/client_libs/libplayerc++/playerc++.h
===================================================================
--- code/player/branches/cmake/client_libs/libplayerc++/playerc++.h 2008-04-18 08:10:03 UTC (rev 6361)
+++ code/player/branches/cmake/client_libs/libplayerc++/playerc++.h 2008-04-18 08:17:51 UTC (rev 6362)
@@ -1318,12 +1318,19 @@
~LogProxy();
/// What kind of log device is this? Either PLAYER_LOG_TYPE_READ or
- /// PLAYER_LOG_TYPE_WRITE. Call GetState() to fill it.
+ /// PLAYER_LOG_TYPE_WRITE. Call QueryState() to fill it.
int GetType() const { return GetVar(mDevice->type); };
- /// Is logging/playback enabled? Call GetState() to fill it.
+ /// Is logging/playback enabled? Call QueryState() to fill it.
int GetState() const { return GetVar(mDevice->state); };
+ /// Query the server for type and state info.
+ void QueryState();
+
+ /// Start/stop (1/0) reading from or writing to the log file.
+ /// If the type of interface (reader/writer) is unknown, a query package is sent first.
+ void SetState(int aState);
+
/// Start/stop (1/0) writing to the log file.
void SetWriteState(int aState);
Modified: code/player/branches/cmake/server/drivers/joystick/linuxjoy.cc
===================================================================
--- code/player/branches/cmake/server/drivers/joystick/linuxjoy.cc 2008-04-18 08:10:03 UTC (rev 6361)
+++ code/player/branches/cmake/server/drivers/joystick/linuxjoy.cc 2008-04-18 08:17:51 UTC (rev 6362)
@@ -86,6 +86,9 @@
- The maximum absolute X and Y translational and rotational velocities to be
used when commanding a position device. (Y is only revelant for
holonomous robot)
+- scale_pos (float tuple)
+ - Default: [1.0 1.0 1.0]
+ - Position2d readings scale
- timeout (float)
- Default: 5.0
- Time (in seconds) since receiving a new joystick event after which
@@ -115,14 +118,15 @@
)
@endverbatim
-Provide a position interface, instead of a joystick interface.
+Provide a position2d interface, instead of a joystick interface.
@verbatim
driver
(
name "linuxjoystick"
- provides ["position:0"]
+ provides ["position2d:0"]
port "/dev/js0"
+ scale_pos [ 0.0001 0.0001 0.0001 ]
)
@endverbatim
@@ -274,6 +278,7 @@
private: uint16_t buttons;
private: int axes_max[3];
private: int axes_min[3];
+ private: double scale_pos[3];
private: double timeout;
private: struct timeval lastread;
@@ -358,6 +363,9 @@
this->axes_min[0] = cf->ReadTupleInt(section, "axes_minima", 0, XAXIS_MIN);
this->axes_min[1] = cf->ReadTupleInt(section, "axes_minima", 1, YAXIS_MIN);
this->axes_min[2] = cf->ReadTupleInt(section, "axes_minima", 2, YAWAXIS_MIN);
+ this->scale_pos[0] = cf->ReadTupleFloat(section, "scale_pos", 0, 1.0);
+ this->scale_pos[1] = cf->ReadTupleFloat(section, "scale_pos", 1, 1.0);
+ this->scale_pos[2] = cf->ReadTupleFloat(section, "scale_pos", 2, 1.0);
// Do we talk to a position device?
if(cf->GetTupleCount(section, "requires"))
@@ -466,6 +474,9 @@
// test if we are supposed to cancel
pthread_testcancel();
+ // Process incoming messages
+ this->ProcessMessages();
+
// Run and process output
this->ReadJoy();
@@ -492,7 +503,6 @@
}
}
}
- return;
}
@@ -588,9 +598,9 @@
if(this->position_addr.interf)
{
memset(&(this->pos_data),0,sizeof(player_position2d_data_t));
- this->pos_data.pos.px = this->pos[0];
- this->pos_data.pos.py = this->pos[1];
- this->pos_data.pos.pa = this->pos[2];
+ this->pos_data.pos.px = this->pos[0] * this->scale_pos[0];
+ this->pos_data.pos.py = this->pos[1] * this->scale_pos[1];
+ this->pos_data.pos.pa = this->pos[2] * this->scale_pos[2];
this->Publish(this->position_addr,
PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
(void*)&this->pos_data, sizeof(this->pos_data), NULL);
Modified: code/player/branches/cmake/server/drivers/laser/CMakeLists.txt
===================================================================
--- code/player/branches/cmake/server/drivers/laser/CMakeLists.txt 2008-04-18 08:10:03 UTC (rev 6361)
+++ code/player/branches/cmake/server/drivers/laser/CMakeLists.txt 2008-04-18 08:17:51 UTC (rev 6362)
@@ -12,10 +12,6 @@
PLAYERDRIVER_REQUIRE_HEADER (sicks3000 build_sicks3000 linux/serial.h)
PLAYERDRIVER_ADD_DRIVER_SIMPLE (sicks3000 build_sicks3000 sicks3000.cc)
-PLAYERDRIVER_OPTION (sickpls build_sickpls ON)
-PLAYERDRIVER_REQUIRE_HEADER (sickpls build_sickpls linux/serial.h)
-PLAYERDRIVER_ADD_DRIVER_SIMPLE (sickpls build_sickpls sickpls.cc)
-
PLAYERDRIVER_OPTION (laserposeinterpolator build_laserposeinterpolator ON)
PLAYERDRIVER_ADD_DRIVER_SIMPLE (laserposeinterpolator build_laserposeinterpolator laserposeinterpolator.cc)
Modified: code/player/branches/cmake/server/drivers/laser/sicklms200.cc
===================================================================
--- code/player/branches/cmake/server/drivers/laser/sicklms200.cc 2008-04-18 08:10:03 UTC (rev 6361)
+++ code/player/branches/cmake/server/drivers/laser/sicklms200.cc 2008-04-18 08:17:51 UTC (rev 6362)
@@ -29,10 +29,10 @@
/** @ingroup drivers Drivers */
/** @{ */
/** @defgroup driver_sicklms200 sicklms200
- * @brief SICK LMS 200 laser range-finder
+ * @brief SICK LMS 200 / PLS laser range-finder
-The sicklms200 driver controls the SICK LMS 200 scanning laser range-finder.
+The sicklms200 driver controls the SICK LMS 200 and PLS scanning laser range-finders.
@note LMS200 lasers may take several seconds to start up. You may want to set the 'alwayson' option for sicklms200 to '1' in your configuration file in order start the laser when player starts. Otherwise, your client may experience a timeout in trying to subscribe to this device.
@@ -73,6 +73,21 @@
- Baud rate. Valid values are 9600, 38400 (RS232 or RS422) and
500000 (RS422 only).
+- serial_high_speed_mode (integer)
+ - Default: 0 (FTDI)
+ - Method to achieve high speed (RS422) communication
+ 0: FTDI RS422 to USB, using Linux High Speed Serial
+ 1: CP210x RS422 to USB (And maybe others). This chipset has
+ the hardware remap some normal baudrate (230400 for
+ example) to 500000, so the host machine doesn't need to
+ know it's running at 500000 (Works on Mac OS X).
+
+- serial_high_speed_baudremap (integer)
+ - Default: 38400 (Needed for FTDI)
+ - The fake baud rate to use after 500Kbps is achieved. In
+ high_speed_mode 1, this is the baud rate that the hardware
+ is remapping.
+
- retry (integer)
- Default: 0
- If the initial connection to the laser fails, retry this many times before
@@ -114,14 +129,50 @@
@par Example
@verbatim
+#linux config
driver
(
name "sicklms200"
- provides ["laser:0"]
- port "/dev/ttyS0"
- resolution 100 # Angular resolution 1 degree (181 readings @ 10Hz)
- range_res 10 # Range resolution 1 cm (maximum range 81.92m)
+ provides [ "laser:0" ]
+ port "/dev/ttyUSB0"
+ resolution 50
+ serial_high_speed_mode 1
+ serial_high_speed_baudremap 230400
+ connect_rate [ 9600 500000 38400]
+ transfer_rate 38400
+ retry 2
+ alwayson 1
)
+
+# MAC config
+driver
+(
+ name "sicklms200"
+ provides [ "laser:0" ]
+ port "/dev/cu.SLAB_USBtoUART"
+ resolution 50
+ serial_high_speed_mode 1
+ serial_high_speed_baudremap 230400
+ connect_rate [ 9600 500000 38400]
+ transfer_rate 38400
+ retry 2
+ alwayson 1
+)
+
+#FTDI Config
+driver
+(
+ name "sicklms200"
+ provides [ "laser:0" ]
+ port "/dev/ttyUSB0"
+ resolution 50
+ serial_high_speed_mode 0
+ connect_rate [ 9600 500000 38400]
+ transfer_rate 38400
+ retry 2
+ alwayson 1
+)
+
@endverbatim
@author Andrew Howard, Richard Vaughan, Kasper Stoy
@@ -131,7 +182,9 @@
-#include <config.h>
+#if HAVE_CONFIG_H
+ #include <config.h>
+#endif
#include <assert.h>
#include <math.h>
@@ -157,7 +210,7 @@
#include <libplayercore/playercore.h>
#include <libplayerxdr/playerxdr.h>
-#include <replace/replace.h>
+//#include <replace/replace.h>
extern PlayerTime* GlobalTime;
#define DEFAULT_LASER_PORT "/dev/ttyS1"
@@ -226,6 +279,11 @@
// Change the resolution of the laser
int SetLaserRes(int angle, int res);
+
+ // RequestLaserStopStream()
+ // Returns 0 on success
+ int RequestLaserStopStream();
+
// Request data from the laser
// Returns 0 on success
int RequestLaserData(int min_segment, int max_segment);
@@ -237,7 +295,7 @@
ssize_t WriteToLaser(uint8_t *data, ssize_t len);
// Read a packet from the laser
- ssize_t ReadFromLaser(uint8_t *data, ssize_t maxlen, bool ack = false, int timeout = -1);
+ ssize_t ReadFromLaser(uint8_t *data, ssize_t maxlen, bool ack = false, int timeout = -1, int timeout_header = -1);
// Calculates CRC for a telegram
unsigned short CreateCRC(uint8_t *data, ssize_t len);
@@ -260,6 +318,10 @@
// Starup delay
int startup_delay;
+
+ // Type of laser (from GetLaserType)
+ char laser_type[64];
+
// Number of time to try connecting
int retry_limit;
@@ -284,14 +346,17 @@
// readings)
int invert;
- bool can_do_hi_speed;
int connect_rates[MAX_CONNECT_RATES]; // Ordered list of connection rates
int num_connect_rates; // Length of connect_rates
int connect_rate; // The final connection rate that we settle on
int transfer_rate; // Desired rate for operation
int current_rate; // Current rate
+ int serial_high_speed_mode; // Which method to use for 500k mode
+ int serial_high_speed_baudremap; // Baud rate to fake as 500k
int scan_id;
+
+
player_laser_data_t data;
#ifdef HAVE_HI_SPEED_SERIAL
@@ -309,8 +374,10 @@
void sicklms200_Register(DriverTable* table)
{
table->AddDriver("sicklms200", SickLMS200_Init);
+ table->AddDriver("sickpls", SickLMS200_Init);
}
+
////////////////////////////////////////////////////////////////////////////////
// Device codes
@@ -366,19 +433,39 @@
this->current_rate = 0;
this->retry_limit = cf->ReadInt(section, "retry", 0) + 1;
+
+
#ifdef HAVE_HI_SPEED_SERIAL
- this->can_do_hi_speed = true;
+ this->serial_high_speed_mode = cf->ReadInt(section, "serial_high_speed_mode", 0);
#else
- this->can_do_hi_speed = false;
+ this->serial_high_speed_mode = cf->ReadInt(section, "serial_high_speed_mode", 1);
#endif
+ this->serial_high_speed_baudremap = cf->ReadInt(section, "serial_high_speed_baudremap", 38400);
- if (!this->can_do_hi_speed && this->transfer_rate > 38400)
+ switch(this->serial_high_speed_baudremap)
{
- PLAYER_ERROR1("sicklms200: requested hi speed serial, but no support compiled in. Defaulting to %d bps.",
- DEFAULT_LASER_TRANSFER_RATE);
- this->transfer_rate = DEFAULT_LASER_TRANSFER_RATE;
+ case 9600:
+ this->serial_high_speed_baudremap = B9600;
+ break;
+ case 38400:
+ this->serial_high_speed_baudremap = B38400;
+ break;
+ case 57600:
+ this->serial_high_speed_baudremap = B57600;
+ break;
+ case 115200:
+ this->serial_high_speed_baudremap = B115200;
+ break;
+ case 230400:
+ this->serial_high_speed_baudremap = B230400;
+ break;
+ default:
+ printf("Unknown baud rate [%d] defaulting to B38400\n", this->serial_high_speed_baudremap);
+ this->serial_high_speed_baudremap = B38400;
+ break;
}
+
// Set default configuration
this->startup_delay = cf->ReadInt(section, "delay", 0);
this->scan_width = 180;
@@ -420,29 +507,25 @@
// for the laser to initialized
sleep(this->startup_delay);
- // Try connecting at each rate, in order
- for(int j=0;j<this->num_connect_rates;j++)
+ for(int i=0;i<this->retry_limit;i++)
{
- this->connect_rate = this->connect_rates[j];
-
- if (!this->can_do_hi_speed && this->connect_rate > 38400)
+ PLAYER_MSG1(2, "Connection attempt #%d", i);
+ // Try connecting at each rate, in order
+ for(int j=0;j<this->num_connect_rates;j++)
{
- PLAYER_ERROR1("sicklms200: requested hi speed serial, but no support compiled in. Defaulting to %d bps.",
- DEFAULT_LASER_CONNECT_RATE);
- this->connect_rate = DEFAULT_LASER_CONNECT_RATE;
- }
+ this->connect_rate = this->connect_rates[j];
- for(int i=0;i<this->retry_limit;i++)
- {
- // Try connecting at the given rate
PLAYER_MSG1(2, "connecting at %d", this->connect_rate);
if (ChangeTermSpeed(this->connect_rate))
continue;
- if (SetLaserMode() == 0)
+ if (RequestLaserStopStream() == 0)
this->current_rate = this->connect_rate;
if(this->current_rate != 0)
+ {
+ PLAYER_MSG1(2, "connected at %d!", this->current_rate);
break;
+ }
}
if(this->current_rate != 0)
break;
@@ -455,30 +538,17 @@
return 1;
}
- // Jump up to 38400 rate
- if (this->current_rate != this->transfer_rate && this->transfer_rate == 38400)
+ sleep(1);
+ if (this->current_rate != this->transfer_rate && (this->transfer_rate == 38400 || this->transfer_rate == 500000) )
{
PLAYER_MSG2(2, "laser operating at %d; changing to %d", this->current_rate, this->transfer_rate);
if (SetLaserSpeed(this->transfer_rate))
return 1;
- sleep(1);
- if (ChangeTermSpeed(this->transfer_rate))
- return 1;
- sleep(1);
- }
- // Jump up to 500000
- else if (this->current_rate != 500000 && this->transfer_rate == 500000 && this->can_do_hi_speed)
- {
- PLAYER_MSG2(2, "laser operating at %d; changing to %d", this->current_rate, this->transfer_rate);
- if (SetLaserSpeed(this->transfer_rate))
- return 1;
- sleep(1);
if (ChangeTermSpeed(this->transfer_rate))
return 1;
sleep(1);
}
-
// Dont know this rate
else if (this->current_rate != this->transfer_rate)
{
@@ -487,12 +557,16 @@
}
// Display the laser type
- char type[64];
- memset(type,0,sizeof(type));
- if (GetLaserType(type, sizeof(type)))
+// char type[64];
+ memset(this->laser_type,0,sizeof(this->laser_type));
+ if (GetLaserType(this->laser_type, sizeof(this->laser_type)))
return 1;
- PLAYER_MSG3(2, "SICK laser type [%s] at [%s:%d]", type, this->device_name, this->transfer_rate);
+ PLAYER_MSG3(2, "SICK laser type [%s] at [%s:%d]", this->laser_type, this->device_name, this->transfer_rate);
+ if (SetLaserMode())
+ return 1;
+
+
// Configure the laser
if (SetLaserRes(this->scan_width, this->scan_res))
return 1;
@@ -545,11 +619,14 @@
{
player_laser_config_t * config =
reinterpret_cast<player_laser_config_t *> (data);
+ int old_scan_width, old_scan_res;
+
this->intensity = config->intensity;
this->scan_res = (int) rint(RTOD(config->resolution)*100);
this->min_angle = (int)rint(RTOD(config->min_angle)*100);
this->max_angle = (int)rint(RTOD(config->max_angle)*100);
- this->range_res = (int)config->range_res*1000;
+ this->range_res = (int) (config->range_res*1000);
+ printf("range_res: %f %d\n", config->range_res, this->range_res);
if(this->CheckScanConfig() != 0)
{
@@ -560,12 +637,14 @@
PLAYER_ERROR("request for config mode failed");
else
{
- if (SetLaserRes(this->scan_width, this->scan_res) != 0)
- PLAYER_ERROR("failed setting resolution");
- /* This call fails for me, but I've only tested with one laser - BPG
- * */
+ if(old_scan_width != this->scan_width || old_scan_res != this->scan_res) {
+ if (SetLaserRes(this->scan_width, this->scan_res) != 0)
+ PLAYER_ERROR("failed setting resolution [SetLaserRes()]");
+ /* This call fails for me, but I've only tested with one laser - BPG
+ * */
+ }
if(SetLaserConfig(this->intensity) != 0)
- PLAYER_ERROR("failed setting intensity");
+ PLAYER_ERROR("failed setting intensity [SetLaserConfig()]");
}
// Issue a new request for data
@@ -667,7 +746,7 @@
{
if (first)
{
- PLAYER_MSG0(2, "receiving data");
+ PLAYER_MSG0(2, "SickLMS200: receiving data");
first = false;
}
@@ -702,7 +781,12 @@
}
for (int i = 0; i < this->scan_max_segment - this->scan_min_segment + 1; i++)
{
- data.intensity[i] = ((mm_ranges[i] >> 13) & 0x0007);
+ /* Not sure about this */
+ if(strncmp(this->laser_type, "PLS", 3) == 0)
+ data.intensity[i] = ((mm_ranges[i] >> 13) & 0x000E);
+ else
+ data.intensity[i] = ((mm_ranges[i] >> 13) & 0x0007);
+
data.ranges[i] = (mm_ranges[i] & 0x1FFF) * this->range_res / 1e3;
}
@@ -856,27 +940,30 @@
#ifdef HAVE_HI_SPEED_SERIAL
struct serial_struct serial;
+ if(this->serial_high_speed_mode == 0)
+ {
- // we should check and reset the AYSNC_SPD_CUST flag
- // since if it's set and we request 38400, we're likely
- // to get another baud rate instead (based on custom_divisor)
- // this way even if the previous player doesn't reset the
- // port correctly, we'll end up with the right speed we want
- if (ioctl(this->laser_fd, TIOCGSERIAL, &serial) < 0)
- {
- //RETURN_ERROR(1, "error on TIOCGSERIAL in beginning");
- PLAYER_WARN("ioctl() failed while trying to get serial port info");
+ // we should check and reset the AYSNC_SPD_CUST flag
+ // since if it's set and we request 38400, we're likely
+ // to get another baud rate instead (based on custom_divisor)
+ // this way even if the previous player doesn't reset the
+ // port correctly, we'll end up with the right speed we want
+ if (ioctl(this->laser_fd, TIOCGSERIAL, &serial) < 0)
+ {
+ //RETURN_ERROR(1, "error on TIOCGSERIAL in beginning");
+ PLAYER_WARN("ioctl() failed while trying to get serial port info");
+ }
+ else
+ {
+ serial.flags &= ~ASYNC_SPD_CUST;
+ serial.custom_divisor = 0;
+ if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0)
+ {
+ //RETURN_ERROR(1, "error on TIOCSSERIAL in beginning");
+ PLAYER_WARN("ioctl() failed while trying to set serial port info");
+ }
+ }
}
- else
- {
- serial.flags &= ~ASYNC_SPD_CUST;
- serial.custom_divisor = 0;
- if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0)
- {
- //RETURN_ERROR(1, "error on TIOCSSERIAL in beginning");
- PLAYER_WARN("ioctl() failed while trying to set serial port info");
- }
- }
#endif
//printf("LASER: change TERM speed: %d\n", speed);
@@ -911,41 +998,63 @@
case 500000:
//PLAYER_MSG0(2, "terminal speed to 500000");
+ if(this->serial_high_speed_mode == 0)
+ {
+ #ifdef HAVE_HI_SPEED_SERIAL
+ if (ioctl(this->laser_fd, TIOCGSERIAL, &this->old_serial) < 0) {
+ RETURN_ERROR(1, "error on TIOCGSERIAL ioctl");
+ }
-#ifdef HAVE_HI_SPEED_SERIAL
- if (ioctl(this->laser_fd, TIOCGSERIAL, &this->old_serial) < 0) {
- RETURN_ERROR(1, "error on TIOCGSERIAL ioctl");
- }
+ serial = this->old_serial;
- serial = this->old_serial;
+ serial.flags |= ASYNC_SPD_CUST;
+ serial.custom_divisor = 240/5; // for FTDI USB/serial converter divisor is 240/5
- serial.flags |= ASYNC_SPD_CUST;
- serial.custom_divisor = 48; // for FTDI USB/serial converter divisor is 240/5
+ if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0) {
+ RETURN_ERROR(1, "error on TIOCSSERIAL ioctl");
+ }
- if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0) {
- RETURN_ERROR(1, "error on TIOCSSERIAL ioctl");
- }
+ #else
+ fprintf(stderr, "sicklms200: Trying to change to 500kbps in serial_high_speed_mode = 0, but no support compiled in, defaulting to 38.4kbps.\n");
+ #endif
-#else
- fprintf(stderr, "sicklms200: Trying to change to 500kbps, but no support compiled in, defaulting to 38.4kbps.\n");
-#endif
+ // even if we are doing 500kbps, we have to set the speed to 38400...
+ // the FTDI will know we want 500000 instead.
- // even if we are doing 500kbps, we have to set the speed to 38400...
- // the driver will know we want 500000 instead.
+ if( tcgetattr( this->laser_fd, &term ) < 0 )
+ RETURN_ERROR(1, "unable to get device attributes");
- if( tcgetattr( this->laser_fd, &term ) < 0 )
- RETURN_ERROR(1, "unable to get device attributes");
+ cfmakeraw( &term );
+ cfsetispeed( &term, B38400 );
+ cfsetospeed( &term, B38400 );
- cfmakeraw( &term );
- cfsetispeed( &term, B38400 );
- cfsetospeed( &term, B38400 );
+ if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
+ RETURN_ERROR(1, "unable to set device attributes");
+ }
+ else if(this->serial_high_speed_mode == 1)
+ {
+ tcflush(this->laser_fd, TCIFLUSH);
+ close(this->laser_fd);
+ usleep(1000000);
+ this->laser_fd = ::open(this->device_name, O_RDWR | O_NOCTTY | O_SYNC);
+ if(this->laser_fd < 0)
+ RETURN_ERROR(1, "error opening");
- if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
- RETURN_ERROR(1, "unable to set device attributes");
+ if( tcgetattr( this->laser_fd, &term ) < 0 )
+ RETURN_ERROR(1, "unable to get device attributes");
- break;
- default:
- PLAYER_ERROR1("unknown speed %d", speed);
+ term.c_cflag = this->serial_high_speed_baudremap | CS8 | CLOCAL | CREAD;
+ cfmakeraw( &term );
+ cfsetispeed( &term, this->serial_high_speed_baudremap );
+ cfsetospeed( &term, this->serial_high_speed_baudremap );
+ tcflush(this->laser_fd, TCIFLUSH);
+ tcsetattr(this->laser_fd, TCSANOW, &term);
+ tcflush(this->laser_fd, TCIFLUSH);
+ }
+ break;
+
+ default:
+ PLAYER_ERROR1("unknown speed %d", speed);
}
return 0;
}
@@ -971,12 +1080,18 @@
packet[4] = 0x43; // C
packet[5] = 0x4B; // K
packet[6] = 0x5F; // _
+/*
packet[7] = 0x4C; // L
packet[8] = 0x4D; // M
packet[9] = 0x53; // S
+*/
+ packet[7] = this->laser_type[0];
+ packet[8] = this->laser_type[1];
+ packet[9] = this->laser_type[2];
+
len = 10;
- PLAYER_MSG0(2, "sending configuration mode request to laser");
+ PLAYER_MSG0(2, "sending configuration mode request to laser [SetLaserMode()]");
if (WriteToLaser(packet, len) < 0)
return 1;
@@ -984,22 +1099,22 @@
// This could take a while...
//
PLAYER_MSG0(2, "waiting for acknowledge");
- len = ReadFromLaser(packet, sizeof(packet), true, 2000);
+ len = ReadFromLaser(packet, sizeof(packet), true, 2500, 250);
if (len < 0)
return 1;
else if (len < 1)
{
- PLAYER_WARN("timeout");
+ PLAYER_WARN("SetLaserMode(): timeout in ReadFromLaser");
continue;
}
else if (packet[0] == NACK)
{
- PLAYER_ERROR("request denied by laser");
+ PLAYER_ERROR("SetLaserMode(): request denied by laser");
return 1;
}
else if (packet[0] != ACK)
{
- PLAYER_ERROR("unexpected packet type");
+ PLAYER_ERROR("SetLaserMode(): unexpected packet type");
return 1;
}
break;
@@ -1031,22 +1146,22 @@
// Wait for laser to return ack
//PLAYER_MSG0(2, "waiting for acknowledge");
- len = ReadFromLaser(packet, sizeof(packet), true, 10000);
+ len = ReadFromLaser(packet, sizeof(packet), true, 20000, 5000);
if (len < 0)
return 1;
else if (len < 1)
{
- PLAYER_ERROR("no reply from laser");
+ PLAYER_ERROR("SetLaserSpeed(): no reply from laser");
return 1;
}
else if (packet[0] == NACK)
{
- PLAYER_ERROR("request denied by laser");
+ PLAYER_ERROR("SetLaserSpeed(): request denied by laser");
return 1;
}
else if (packet[0] != ACK)
{
- PLAYER_ERROR("unexpected packet type");
+ PLAYER_ERROR("SetLaserSpeed(): unexpected packet type");
return 1;
}
break;
@@ -1075,21 +1190,22 @@
// Wait for laser to return data
len = ReadFromLaser(packet, sizeof(packet), false, 2000);
+
if (len < 0)
return 1;
else if (len < 1)
{
- PLAYER_WARN("timeout");
+ PLAYER_WARN("GetLaserType(): timeout in ReadFromLaser()");
continue;
}
else if (packet[0] == NACK)
{
- PLAYER_ERROR("request denied by laser");
+ PLAYER_ERROR("GetLaserType(): request denied by laser");
return 1;
}
else if (packet[0] != 0xBA)
{
- PLAYER_ERROR("unexpected packet type");
+ PLAYER_ERROR("GetLaserType(): unexpected packet type");
return 1;
}
@@ -1129,22 +1245,22 @@
// Wait for laser to return data
//PLAYER_MSG0(2, "waiting for reply");
- len = ReadFromLaser(packet, sizeof(packet), false, 2000);
+ len = ReadFromLaser(packet, sizeof(packet), false, 25000, 5000);
if (len < 0)
return 1;
else if (len < 1)
{
- PLAYER_WARN("timeout");
+ PLAYER_WARN("SetLaserConfig(): timeout in ReadFromLaser() [1]");
continue;
}
else if (packet[0] == NACK)
{
- PLAYER_ERROR("request denied by laser");
+ PLAYER_ERROR("SetLaserConfig(): request denied by laser [1]");
return 1;
}
else if (packet[0] != 0xF4)
{
- PLAYER_ERROR("unexpected packet type");
+ PLAYER_ERROR("SetLaserConfig(): unexpected packet type [1]");
return 1;
}
break;
@@ -1179,22 +1295,22 @@
// Wait for the change to "take"
//PLAYER_MSG0(2, "waiting for acknowledge");
- len = ReadFromLaser(packet, sizeof(packet), false, 2000);
+ len = ReadFromLaser(packet, sizeof(packet), false, 25000, 10000);
if (len < 0)
return 1;
else if (len < 1)
{
- PLAYER_ERROR("timeout");
+ PLAYER_WARN("SetLaserConfig(): timeout in ReadFromLaser() [2]");
continue;
}
else if (packet[0] == NACK)
{
- PLAYER_ERROR("request denied by laser");
+ PLAYER_ERROR("SetLaserConfig(): request denied by laser [2]");
return 1;
}
else if (packet[0] != 0xF7)
{
- PLAYER_ERROR("unexpected packet type");
+ PLAYER_ERROR("SetLaserConfig(): unexpected packet type [2]");
return 1;
}
@@ -1266,6 +1382,52 @@
////////////////////////////////////////////////////////////////////////////////
+// Request status from the laser
+// Returns 0 on success
+//
+int SickLMS200::RequestLaserStopStream()
+{
+ int tries;
+ ssize_t len;
+ uint8_t packet[20];
+
+ for (tries = 0; tries < DEFAULT_LASER_RETRIES; tries++)
+ {
+ packet[0] = 0x20;
+ packet[1] = 0x25;
+ len = 2;
+ PLAYER_MSG0(2, "sending LMS stop continuous mode [RequestLaserStopStream()]");
+ if (WriteToLaser(packet, len) < 0)
+ return 1;
+
+ // Wait for laser to return ack
+ // This could take a while...
+ //
+ PLAYER_MSG0(2, "waiting for acknowledge");
+ len = ReadFromLaser(packet, sizeof(packet), true, 2500, 500);
+ if (len < 0)
+ return 1;
+ else if (len < 1)
+ {
+ PLAYER_WARN("RequestLaserStopStream(): timeout in ReadFromLaser");
+ continue;
+ }
+ else if (packet[0] == NACK)
+ {
+ PLAYER_ERROR("RequestLaserStopStream(): request denied by laser");
+ return 1;
+ }
+ else if (packet[0] != ACK)
+ {
+ PLAYER_ERROR("RequestLaserStopStream(): unexpected packet type");
+ return 1;
+ }
+ break;
+ }
+ return (tries >= DEFAULT_LASER_RETRIES);
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Request data from the laser
// Returns 0 on success
//
@@ -1426,48 +1588,53 @@
tcflush(this->laser_fd, TCIOFLUSH);
ssize_t bytes = 0;
+#if 0
+ printf("WriteToLaser: [ ");
+ for(int i = 0; i < len + 6; i++)
+ printf("%02x ", buffer[i]);
+ printf("]\n");
+#endif
#ifdef HAVE_HI_SPEED_SERIAL
- struct timeval start, end;
- // have to write one char at a time, because if we're
- // high speed, then must take no longer than 55 us between
- // chars
+ if(this->serial_high_speed_mode == 0 && current_rate > 38400)
+ {
+ struct timeval start, end;
+ // have to write one char at a time, because if we're
+ // high speed, then must take no longer than 55 us between
+ // chars
- int ret;
- if (current_rate > 38400) {
- //printf("LASER: writing %d bytes\n", 6+len);
- for (int i =0; i < 6 + len; i++) {
- do {
- gettimeofday(&start, NULL);
- ret = ::write(this->laser_fd, buffer + i, 1);
- } while (!ret);
- if (ret > 0) {
- bytes += ret;
- }
+ int ret;
+ //printf("LASER: writing %d bytes\n", 6+len);
+ for (int i =0; i < 6 + len; i++) {
+ do {
+ gettimeofday(&start, NULL);
+ ret = ::write(this->laser_fd, buffer + i, 1);
+ } while (!ret);
+ if (ret > 0) {
+ bytes += ret;
+ }
- // need to do this sort of busy wait to ensure the right timing
- // although I've noticed you will get some anamolies that are
- // in the ms range; this could be a problem...
- int usecs;
- do {
- gettimeofday(&end, NULL);
- usecs= (end.tv_sec - start.tv_sec)*1000000 +
- (end.tv_usec - start.tv_usec);
- } while (usecs < 60);
+ // need to do this sort of busy wait to ensure the right timing
+ // although I've noticed you will get some anamolies that are
+ // in the ms range; this could be a problem...
+ int usecs;
+ do {
+ gettimeofday(&end, NULL);
+ usecs= (end.tv_sec - start.tv_sec)*1000000 +
+ (end.tv_usec - start.tv_usec);
+ } while (usecs < 60);
- //printf("usecs: %d bytes=%02X\n", (end.tv_sec - start.tv_sec)*1000000 +
- // (end.tv_usec - start.tv_usec), *(buffer + i));
-
+ //printf("usecs: %d bytes=%02X\n", (end.tv_sec - start.tv_sec)*1000000 +
+ // (end.tv_usec - start.tv_usec), *(buffer + i));
}
} else {
- bytes = ::write( this->laser_fd, buffer, 4 + len + 2);
+ /* Notice for high_speed_mode != 0 but 500k we still use this method */
+ bytes = ::write( this->laser_fd, buffer, 4 + len + 2);
}
#else
-
- // Write the data to the port
- //
bytes = ::write( this->laser_fd, buffer, 4 + len + 2);
#endif
+
// Make sure the queue is drained
// Synchronous IO doesnt always work
//
@@ -1485,8 +1652,12 @@
// Set timeout to -1 to make this blocking, otherwise it will return in timeout ms.
// Returns the packet length (0 if timeout occurs)
//
-ssize_t SickLMS200::ReadFromLaser(uint8_t *data, ssize_t maxlen, bool ack, int timeout)
+ssize_t SickLMS200::ReadFromLaser(uint8_t *data, ssize_t maxlen, bool ack, int timeout, int timeout_header)
{
+
+ if(timeout_header == -1)
+ timeout_header = timeout;
+
// If the timeout is infinite,
// go to blocking io
//
@@ -1526,6 +1697,7 @@
int64_t start_time = GetTime();
int64_t stop_time = start_time + timeout;
+ int64_t stop_time_header = start_time + timeout_header;
//PLAYER_MSG2(2, "%Ld %Ld", start_time, stop_time);
@@ -1553,10 +1725,10 @@
break;
}
memmove(header, header + 1, sizeof(header) - 1);
- if (timeout >= 0 && GetTime() >= stop_time)
+ if (timeout >= 0 && GetTime() >= stop_time_header)
{
//PLAYER_MSG2(2, "%Ld %Ld", GetTime(), stop_time);
- //PLAYER_MSG0(2, "timeout on read (1)");
+ PLAYER_MSG0(2, "timeout on read (1)");
return 0;
}
}
Deleted: code/player/branches/cmake/server/drivers/laser/sickpls.cc
===================================================================
--- code/player/branches/cmake/server/drivers/laser/sickpls.cc 2008-04-18 08:10:03 UTC (rev 6361)
+++ code/player/branches/cmake/server/drivers/laser/sickpls.cc 2008-04-18 08:17:51 UTC (rev 6362)
@@ -1,1536 +0,0 @@
-/*
- * Player - One Hell of a Robot Server
- * Copyright (C) 2000
- * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
- *
- *
- * 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
- *
- */
-
-///////////////////////////////////////////////////////////////////////////
-//
-// File: laserdevice.cc
-// Author: Andrew Howard
-// Date: 7 Nov 2000
-// Desc: Driver for the SICK laser
-//
-// CVS info:
-// $Source$
-// $Author$
-// $Revision$
-//
-// Usage:
-// (empty)
-//
-// Theory of operation:
-// (empty)
-//
-// Known bugs:
-// (empty)
-//
-// Possible enhancements:
-// (empty)
-//
-///////////////////////////////////////////////////////////////////////////
-
-/** @ingroup drivers */
-/** @{ */
-/** @defgroup driver_sickpls sickpls
- * @brief SICK PLS laser range-finder
-
-The sickpls driver controls the SICK PLS scanning laser range-finder.
-This driver will likely be merged into the @ref driver_sicklms200
-driver (eventually).
-
-@par Compile-time dependencies
-
-- none
-
-@par Provides
-
-- @ref interface_laser
-
-@par Requires
-
-- none
-
-@par Configuration requests
-
-- PLAYER_LASER_REQ_GET_GEOM
-- PLAYER_LASER_REQ_GET_CONFIG
-- PLAYER_LASER_REQ_SET_CONFIG
-
-@par Configuration file options
-
-- port (string)
- - Default: "/dev/ttyS1"
- - Serial port to which laser is attached. If you are using a
- USB/232 or USB/422 converter, this will be "/dev/ttyUSBx".
-
-- rate (integer)
- - Default: 9600
- - Baud rate. Valid values are 9600, 38400 (RS232 or RS422) and
- 500000 (RS422 only).
-
-- delay (integer)
- - Default: 0
- - Delay (in seconds) before laser is initialized (set this to 35 if
- you have a newer generation Pioneer whose laser is switched on
- when the serial port is open).
-
-- resolution (integer)
- - Default: 50
- - Angular resolution. Valid values are:
- - resolution 50 : 0.5 degree increments, 361 readings @ 5Hz (38400) or 32Hz (500000).
- - resolution 100 : 1 degree increments, 181 readings @ 10Hz (38400) or 75Hz (500000).
-
-- invert (integer)
- - Default: 0
- - Is the laser physically inverted (i.e., upside-down)? Is so, scan data
- will be reversed accordingly.
-
-- pose (length tuple)
- - Default: [0.0 0.0 0.0]
- - Pose (x,y,theta) of the laser, relative to its parent object (e.g.,
- the robot to which the laser is attached).
-
-- autodetect_rate (integer)
- - Default: 1
- - Set to 0 to avoid baud rate autodetection, which fails on some lasers.
-
-- ignore_errors (integer)
- - Default: 0
- - Ignore errors during initialization of the laser.
-
-@par Example
-
-@verbatim
-driver
-(
- name "sickpls"
- provides ["laser:0"]
- port "/dev/ttyS0"
-)
-@endverbatim
-
-@author Yannick Brosseau, Andrew Howard
-
-*/
-/** @} */
-
-#include <config.h>
-
-#include <assert.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <string.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <termios.h>
-#include <unistd.h>
-//#include <netinet/in.h> /* for struct sockaddr_in, htons(3) */
-#include <sys/ioctl.h>
-#include <math.h>
-
-#undef HAVE_HI_SPEED_SERIAL
-#ifdef HAVE_LINUX_SERIAL_H
- #ifndef DISABLE_HIGHSPEEDSICK
- #include <linux/serial.h>
- #define HAVE_HI_SPEED_SERIAL
- #endif
-#endif
-
-#define PLAYER_ENABLE_MSG 0
-#define PLAYER_ENABLE_TRACE 0
-
-#include <libplayercore/playercore.h>
-#include <libplayerxdr/playerxdr.h>
-#include <replace/replace.h>
-
-#define DEFAULT_LASER_PORT "/dev/ttyS1"
-#define DEFAULT_LASER_PORT_RATE 9600
-
-// The laser device class.
-class SickPLS : public Driver
-{
- public:
-
- // Constructor
- SickPLS( ConfigFile* cf, int section);
-
- int Setup();
- int Shutdown();
-
- // MessageHandler
- int ProcessMessage(QueuePointer & resp_queue,
- player_msghdr * hdr,
- void * data);
- private:
-
- // Main function for device thread.
- virtual void Main();
-
- // Process configuration requests. Returns 1 if the configuration
- // has changed.
- //int UpdateConfig();
-
- // Compute the start and end scan segments based on the current resolution and
- // scan angles. Returns 0 if the configuration is valid.
- int CheckScanConfig();
-
- // Open the terminal
- // Returns 0 on success
- int OpenTerm();
-
- // Close the terminal
- // Returns 0 on success
- int CloseTerm();
-
- // Set the terminal speed
- // Valid values are 9600 and 38400
- // Returns 0 on success
- int ChangeTermSpeed(int speed);
-
- // Get the laser type
- int GetLaserType(char *buffer, size_t bufflen);
-
- // Put the laser into configuration mode
- int SetLaserMode();
-
- // Set the laser data rate
- // Valid values are 9600 and 38400
- // Returns 0 on success
- int SetLaserSpeed(int speed);
-
- // Set the laser configuration
- // Returns 0 on success
- int SetLaserConfig(bool intensity);
-
- // Request data from the laser
- // Returns 0 on success
- int RequestLaserData(int min_segment, int max_segment);
-
- // Read range data from laser
- int ReadLaserData(uint16_t *data, size_t datalen);
-
- // Write a packet to the laser
- ssize_t WriteToLaser(uint8_t *data, ssize_t len);
-
- // Read a packet from the laser
- ssize_t ReadFromLaser(uint8_t *data, ssize_t maxlen, bool ack = false, int timeout = -1);
-
- // Calculates CRC for a telegram
- unsigned short CreateCRC(uint8_t *data, ssize_t len);
-
- // Get the time (in ms)
- int64_t GetTime();
-
- protected:
-
- // Laser pose in robot cs.
- double pose[3];
- double size[2];
-
- // Name of device used to communicate with the laser
- const char *device_name;
-
- // laser device file descriptor
- int laser_fd;
-
- // Starup delay
- int startup_delay;
-
- // Scan width and resolution.
- int scan_width, scan_res;
-
- // Start and end scan angles (for restricted scan). These are in
- // units of 0.01 degrees.
- int min_angle, max_angle;
-
- // Start and end scan segments (for restricted scan). These are
- // the values used by the laser.
- int scan_min_segment, scan_max_segment;
-
- // Range resolution (1 = 1mm, 10 = 1cm, 100 = 10cm).
- int range_res;
-
- // Turn intensity data on/off
- bool intensity;
-
- // Is the laser upside-down? (if so, we'll reverse the ordering of the
- // readings)
- int invert;
-
- bool can_do_hi_speed;
- int port_rate;
-
- // Allow the autodetect mechanism for the rate - MB
- int autodetect_rate;
-
- // Ignore errors in initialization
- int ignore_errors;
-
- int type;
-
-#ifdef HAVE_HI_SPEED_SERIAL
- struct serial_struct old_serial;
-#endif
-};
-
-// a factory creation function
-Driver* SickPLS_Init( ConfigFile* cf, int section)
-{
- return((Driver*)(new SickPLS( cf, section)));
-}
-
-// a driver registration function
-void sickpls_Register(DriverTable* table)
-{
- table->AddDriver("sickpls", SickPLS_Init);
-}
-
-////////////////////////////////////////////////////////////////////////////////
-// Device codes
-
-#define STX 0x02
-#define ACK 0xA0
-#define NACK 0x92
-#define CRC16_GEN_POL 0x8005
-#define MAX_RETRIES 5
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Error macros
-#define RETURN_ERROR(erc, m) {PLAYER_ERROR(m); return erc;}
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Constructor
-SickPLS::SickPLS( ConfigFile* cf, int section)
- : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_LASER_CODE)
-{
- // Laser geometry.
- this->pose[0] = cf->ReadTupleLength(section, "pose", 0, 0.0);
- this->pose[1] = cf->ReadTupleLength(section, "pose", 1, 0.0);;
- this->pose[2] = cf->ReadTupleLength(section, "pose", 2, 0.0);;
- this->size[0] = 0.15;
- this->size[1] = 0.15;
-
- // Default serial port
- this->device_name = cf->ReadString(section, "port", DEFAULT_LASER_PORT);
-
- // Set default configuration
- this->startup_delay = cf->ReadInt(section, "delay", 0);
- this->scan_width = 180;
- this->scan_res = cf->ReadInt(section, "resolution", 50);
- this->min_angle = -9000;
- this->max_angle = +9000;
- this->scan_min_segment = 0;
- this->scan_max_segment = 360;
- this->intensity = true;
- this->range_res = 10;
- this->invert = cf->ReadInt(section, "invert", 0);
-
- this->port_rate = cf->ReadInt(section, "rate", DEFAULT_LASER_PORT_RATE);
- this->autodetect_rate = cf->ReadInt(section, "autodetect_rate", 1);
- this->ignore_errors = cf->ReadInt(section, "ignore_errors", 0);
-
-#ifdef HAVE_HI_SPEED_SERIAL
- this->can_do_hi_speed = true;
-#else
- this->can_do_hi_speed = false;
-#endif
-
- //TBM: need to validation the speed capacity of the PLS
- if (!this->can_do_hi_speed && this->port_rate > 38400) {
- fprintf(stderr, "sickpls: requested hi speed serial, but no support compiled in. Defaulting to 38400 bps.\n");
- this->port_rate = 9600;
- }
-
- if (this->CheckScanConfig() != 0)
- PLAYER_ERROR("invalid scan configuration");
-
- return;
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Set up the device
-int SickPLS::Setup()
-{
- printf("Laser initialising (%s)\n", this->device_name);
-
- // Open the terminal
- if (OpenTerm())
- return 1;
-
- // Some Pioneers only power laser after the terminal is opened; wait
- // for the laser to initialized
- sleep(this->startup_delay);
-
- //////// MB
- // The autodetect mechanism for speed might not work for all (older) PLS lasers
- // we are then forced to use the rate given in the configuration file and *not* change it.
- if(!autodetect_rate) {
- if (ChangeTermSpeed(port_rate))
- return 1;
- if (RequestLaserData(0,360) != 0)
- {
- PLAYER_ERROR("connection failed");
- return 1;
- }
- puts("laser ready");
-
- // Start the device thread
- StartThread();
-
- return 0;
- }
- //////////
-
-
- // Start out at 38400 with non-blocking io
- if (ChangeTermSpeed(38400))
- return 1;
-
- PLAYER_MSG0(2, "connecting at 38400");
- if (RequestLaserData(0,360) != 0)
- {
-
- PLAYER_MSG0(2, "connect at 38400 failed, trying 9600");
- if (ChangeTermSpeed(9600))
- return 1;
- if (RequestLaserData(0,360) != 0)
- {
- PLAYER_ERROR("connection failed");
- return 1;
- }
- PLAYER_MSG0(2, "laser operating at 9600; changing to 38400");
-
- if (SetLaserSpeed(38400))
- return 1;
- if (ChangeTermSpeed(38400))
- return 1;
- }
-
- puts("laser ready");
-
- // Start the device thread
- StartThread();
-
- return 0;
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Shutdown the device
-int SickPLS::Shutdown()
-{
- // shutdown laser device
- StopThread();
-
- if (port_rate > 38400) {
- SetLaserSpeed(9600);
- }
-
- CloseTerm();
- puts("Laser has been shutdown");
-
- return(0);
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Main function for device thread
-void SickPLS::Main()
-{
- float tmp;
- // Ask the laser to send data
-
- for (int retry = 0; retry < MAX_RETRIES; retry++)
- {
- if (RequestLaserData(this->scan_min_segment, this->scan_max_segment) == 0)
- break;
- else if (retry >= MAX_RETRIES)
- {
- PLAYER_ERROR("laser not responding; exiting laser thread");
- return;
- }
- }
-
- while (true)
- {
- // test if we are supposed to cancel
- pthread_testcancel();
-
- // Update the configuration.
-/* if (UpdateConfig())
- {
- if (SetLaserMode() != 0)
- PLAYER_ERROR("request for config mode failed");
- else
- {
- //TBM: Check the configurability of the PLS before enabling this
- //if (SetLaserConfig(this->intensity) != 0)
- // PLAYER_ERROR("failed setting intensity");
- }
-
- // Issue a new request for data
- if (RequestLaserData(this->scan_min_segment, this->scan_max_segment))
- PLAYER_ERROR("request for laser data failed");
- }
-*/
- // Get the time at which we started reading
- // This will be a pretty good estimate of when the phenomena occured
- struct timeval time;
- GlobalTime->GetTime(&time);
-
- // Process incoming data
- player_laser_data_t data;
- data.ranges_count = (this->scan_max_segment - this->scan_min_segment + 1);
- data.intensity_count = data.ranges_count;
- data.ranges = new float [data.ranges_count];
- data.intensity = new uint8_t [data.intensity_count];
- uint16_t * TempData = new uint16_t[data.ranges_count];
- if (ReadLaserData(TempData, data.ranges_count) == 0)
- {
- // Prepare packet
- data.min_angle = (this->scan_min_segment * this->scan_res - this->scan_width * 50);
- data.max_angle = (this->scan_max_segment * this->scan_res - this->scan_width * 50);
- data.resolution = (this->scan_res);
-// data.range_res = (this->range_res);
- for (int i = 0; i < this->scan_max_segment - this->scan_min_segment + 1; i++)
- {
- data.intensity[i] = ((TempData[i] >> 13) & 0x000E);
- data.ranges[i] = ((TempData[i] & 0x1FFF));
- }
-
- // if the laser is upside-down, reverse the data and intensity
- // arrays, in place. this could probably be made more efficient by
- // burying it in a lower-level loop where the data is being read, but
- // i can't be bothered to figure out where.
- if(this->invert)
- {
- for (int i = 0;
- i < (this->scan_max_segment - this->scan_min_segment + 1)/2;
- i++)
- {
- tmp=data.ranges[i];
- data.ranges[i]=data.ranges[this->scan_max_segment-this->scan_min_segment-i];
- data.ranges[this->scan_max_segment-this->scan_min_segment-i] = tmp;
- uint8_t tmp_intensity=data.intensity[i];
- data.intensity[i]=data.intensity[this->scan_max_segment-this->scan_min_segment-i];
- data.intensity[this->scan_max_segment-this->scan_min_segment-i] = tmp_intensity;
- }
- }
-
- // Make data available
- this->Publish(this->device_addr,
- PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN,
- (void*)&data);
- }
- delete TempData;
- player_laser_data_t_cleanup(&data);
- }
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Process an incoming message
-int
-SickPLS::ProcessMessage(QueuePointer & resp_queue,
- player_msghdr * hdr,
- void * data)
-{
- assert(hdr);
- assert(data);
-
- if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_LASER_REQ_SET_CONFIG,
- this->device_addr))
- {
- assert(hdr->size == sizeof(player_laser_config_t));
- player_laser_config_t * l_cfg = reinterpret_cast<player_laser_config_t *> (data);
- Lock();
- this->intensity = l_cfg->intensity;
- this->scan_res = (int)rint(RTOD(l_cfg->resolution)*100);
- this->min_angle = (int)rint(RTOD(l_cfg->min_angle)*100);
- this->max_angle = (int)rint(RTOD(l_cfg->max_angle)*100);
- this->range_res = (int)l_cfg->range_res*1000;
- Unlock();
- if (this->CheckScanConfig() == 0)
- {
- if (SetLaserMode() != 0)
- PLAYER_ERROR("request for config mode failed");
- else
- {
- //TBM: Check the configurability of the PLS before enabling this
- //if (SetLaserConfig(this->intensity) != 0)
- // PLAYER_ERROR("failed setting intensity");
- }
-
- // Issue a new request for data
- if (RequestLaserData(this->scan_min_segment, this->scan_max_segment))
- PLAYER_ERROR("request for laser data failed");
- return PLAYER_MSGTYPE_RESP_ACK;
- }
- else
- return PLAYER_MSGTYPE_RESP_NACK;
- }
-
- if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_LASER_REQ_GET_CONFIG,
- this->device_addr))
- {
- assert(hdr->size == 0);
- player_laser_config_t lcfg;
-
- lcfg.intensity = this->intensity;
- lcfg.resolution = (this->scan_res);
- lcfg.min_angle = ((short) this->min_angle);
- lcfg.max_angle = ((short) this->max_angle);
- lcfg.range_res = (this->range_res);
- this->Publish(this->device_addr,
- resp_queue,
- PLAYER_MSGTYPE_RESP_ACK,
- PLAYER_LASER_REQ_GET_CONFIG,
- (void*)&lcfg, sizeof(lcfg), NULL);
- return 0;
- }
-
- if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_LASER_REQ_GET_GEOM,
- this->device_addr))
- {
- player_laser_geom_t geom;
- memset(&geom, 0, sizeof(geom));
- geom.pose.px = pose[0];
- geom.pose.py = pose[1];
- geom.pose.pyaw = pose[2];
- geom.size.sl = size[0];
- geom.size.sw = size[1];
-
- this->Publish(this->device_addr,
- resp_queue,
- PLAYER_MSGTYPE_RESP_ACK,
- PLAYER_LASER_REQ_GET_GEOM,
- (void*)&geom, sizeof(geom), NULL);
- return(0);
- }
- return -1;
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Process configuration requests. Returns 1 if the configuration has changed.
-/*int SickPLS::UpdateConfig()
-{
- int len;
- void *client;
- char buffer[PLAYER_MAX_REQREP_SIZE];
- player_laser_config_t config;
- player_laser_geom_t geom;
-
- while ((len = GetConfig(&client, &buffer, sizeof(buffer), NULL)) > 0)
- {
- switch (buffer[0])
- {
- case PLAYER_LASER_REQ_SET_CONFIG:
- {
- if (len != sizeof(player_laser_config_t))
- {
- PLAYER_ERROR2("config request len is invalid (%d != %d)", len, sizeof(config));
- if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- continue;
- }
-
- memcpy(&config, buffer, sizeof(config));
- this->intensity = config.intensity;
- this->scan_res = ntohs(config.resolution);
- this->min_angle = (short) ntohs(config.min_angle);
- this->max_angle = (short) ntohs(config.max_angle);
- this->range_res = ntohs(config.range_res);
-
- if (this->CheckScanConfig() == 0)
- {
- if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,
- &config, sizeof(config),NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- return 1;
- }
- else
- {
- if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- }
- break;
- }
-
- case PLAYER_LASER_REQ_GET_CONFIG:
- {
- if (len != 1)
- {
- PLAYER_ERROR2("config request len is invalid (%d != %d)", len, 1);
- if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- continue;
- }
-
- config.intensity = this->intensity;
- config.resolution = htons(this->scan_res);
- config.min_angle = htons((short) this->min_angle);
- config.max_angle = htons((short) this->max_angle);
- config.range_res = htons(this->range_res);
-
- if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,
- &config, sizeof(config), NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- break;
- }
-
- case PLAYER_LASER_REQ_GET_GEOM:
- {
- if (len != 1)
- {
- PLAYER_ERROR2("config request len is invalid (%d != %d)", len, 1);
- if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- continue;
- }
-
- geom.pose[0] = htons((short) (this->pose[0] * 1000));
- geom.pose[1] = htons((short) (this->pose[1] * 1000));
- geom.pose[2] = htons((short) (this->pose[2] * 180/M_PI));
- geom.size[0] = htons((short) (this->size[0] * 1000));
- geom.size[1] = htons((short) (this->size[1] * 1000));
-
- if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,
- &geom, sizeof(geom),NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- break;
- }
-
- default:
- {
- if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- break;
- }
- }
- }
- return 0;
-}
-*/
-
-////////////////////////////////////////////////////////////////////////////////
-// Compute the start and end scan segments based on the current resolution and
-// scan angles. Returns 0 if the configuration is valid.
-int SickPLS::CheckScanConfig()
-{
- if (this->scan_res == 50 || this->scan_res == 100)
- {
- this->scan_width = 180;
- this->scan_min_segment = (this->min_angle + 9000) / this->scan_res;
- this->scan_max_segment = (this->max_angle + 9000) / this->scan_res;
-
- if (this->scan_min_segment < 0)
- this->scan_min_segment = 0;
- if (this->scan_min_segment > 360)
- this->scan_min_segment = 360;
-
- if (this->scan_max_segment < 0)
- this->scan_max_segment = 0;
- if (this->scan_max_segment > 360)
- this->scan_max_segment = 360;
-
- return 0;
- }
-
-
- return 0;
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Open the terminal
-// Returns 0 on success
-int SickPLS::OpenTerm()
-{
- this->laser_fd = ::open(this->device_name, O_RDWR | O_SYNC , S_IRUSR | S_IWUSR );
- if (this->laser_fd < 0)
- {
- PLAYER_ERROR2("unable to open serial port [%s]; [%s]",
- (char*) this->device_name, strerror(errno));
- return 1;
- }
-
- // set the serial port speed to 9600 to match the laser
- // later we can ramp the speed up to the SICK's 38K
- //
- struct termios term;
- if( tcgetattr( this->laser_fd, &term ) < 0 )
- RETURN_ERROR(1, "Unable to get serial port attributes");
-
- cfmakeraw( &term );
-
- // Set to even parity
- term.c_iflag |= INPCK;
- term.c_iflag &= ~IXOFF;
- term.c_cflag |= PARENB;
-
-
- cfsetispeed( &term, B9600 );
- cfsetospeed( &term, B9600 );
-
- tcflush( this->laser_fd, TCIFLUSH);
- if( tcsetattr( this->laser_fd, TCSANOW, &term ) < 0 )
- RETURN_ERROR(1, "Unable to set serial port attributes");
-
- // Make sure queue is empty
- //
- tcflush(this->laser_fd, TCIOFLUSH);
-
- return 0;
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Close the terminal
-// Returns 0 on success
-//
-int SickPLS::CloseTerm()
-{
-#ifdef HAVE_HI_SPEED_SERIAL
- if (ioctl(this->laser_fd, TIOCSSERIAL, &this->old_serial) < 0) {
- RETURN_ERROR(1, "error trying to reset serial to old state");
- }
-#endif
-
- ::close(this->laser_fd);
- return 0;
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-// Set the terminal speed
-// Valid values are 9600 and 38400
-// Returns 0 on success
-//
-int SickPLS::ChangeTermSpeed(int speed)
-{
- struct termios term;
-
-#ifdef HAVE_HI_SPEED_SERIAL
- struct serial_struct serial;
-
- // we should check and reset the AYSNC_SPD_CUST flag
- // since if it's set and we request 38400, we're likely
- // to get another baud rate instead (based on custom_divisor)
- // this way even if the previous player doesn't reset the
- // port correctly, we'll end up with the right speed we want
- if (ioctl(this->laser_fd, TIOCGSERIAL, &serial) < 0) {
- RETURN_ERROR(1, "error on TIOCGSERIAL in beginning");
- }
-
- serial.flags &= ~ASYNC_SPD_CUST;
- serial.custom_divisor = 0;
- if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0) {
- RETURN_ERROR(1, "error on TIOCSSERIAL in beginning");
- }
-#endif
-
- printf("LASER: change TERM speed: %d\n", speed);
-
- switch(speed) {
- case 9600:
- PLAYER_MSG0(2, "terminal speed to 9600");
- if( tcgetattr( this->laser_fd, &term ) < 0 )
- RETURN_ERROR(1, "unable to get device attributes");
-
- cfmakeraw( &term );
-
- term.c_iflag |= INPCK;
- term.c_iflag &= ~IXOFF;
- term.c_cflag |= PARENB;
-
- cfsetispeed( &term, B9600 );
- cfsetospeed( &term, B9600 );
-
- tcflush( this->laser_fd, TCIFLUSH);
- if( tcsetattr( this->laser_fd, TCSANOW, &term ) < 0 )
- RETURN_ERROR(1, "unable to set device attributes");
- break;
-
- case 38400:
- PLAYER_MSG0(2, "terminal speed to 38400");
- if( tcgetattr( this->laser_fd, &term ) < 0 )
- RETURN_ERROR(1, "unable to get device attributes");
-
- cfmakeraw( &term );
- term.c_iflag |= INPCK;
- term.c_iflag &= ~IXOFF;
- term.c_cflag |= PARENB;
-
- cfsetispeed( &term, B38400 );
- cfsetospeed( &term, B38400 );
-
- tcflush( this->laser_fd, TCIFLUSH);
- if( tcsetattr( this->laser_fd, TCSANOW, &term ) < 0 )
- // if( tcsetattr( this->laser_fd, TCSAFLUSH, &term ) < 0 )
- RETURN_ERROR(1, "unable to set device attributes");
- break;
-
- case 500000:
- PLAYER_MSG0(2, "terminal speed to 500000");
-
-#ifdef HAVE_HI_SPEED_SERIAL
- if (ioctl(this->laser_fd, TIOCGSERIAL, &this->old_serial) < 0) {
- RETURN_ERROR(1, "error on TIOCGSERIAL ioctl");
- }
-
- serial = this->old_serial;
-
- serial.flags |= ASYNC_SPD_CUST;
- serial.custom_divisor = 48; // for FTDI USB/serial converter divisor is 240/5
-
- if (ioctl(this->laser_fd, TIOCSSERIAL, &serial) < 0) {
- RETURN_ERROR(1, "error on TIOCSSERIAL ioctl");
- }
-
-#else
- fprintf(stderr, "sicklms200: Trying to change to 500kbps, but no support compiled in, defaulting to 38.4kbps.\n");
-#endif
-
- // even if we are doing 500kbps, we have to set the speed to 38400...
- // the driver will know we want 500000 instead.
-
- if( tcgetattr( this->laser_fd, &term ) < 0 )
- RETURN_ERROR(1, "unable to get device attributes");
-
- cfmakeraw( &term );
-
- term....
[truncated message content] |