original code

/**
*******************************************************************************
* @file sweepLaser/sweepLaserInterface.cpp
* @brief This is an example application that interfaces with the sweeping laser
* sensor.
*
* @b PROJECT: sweepLaser
*
* @par ORIGINAL AUTHOR:
* Stephan Roth, September 13, 2011
*
* @par DEPENDENCIES:
* motorInterface, sickLMS115Interface, common/libcommon
*
* @par DESCRIPTION:
* This is an example application that interfaces with the sweeping laser
* sensor. You can modify this program to meet your specific needs.
* The application has the following command line:
* sweepLaserInterface 192.168.2.41 /dev/ttyUSB0
* Here, 192.168.2.41 should be replaced by the Sick laser's IP address
* /dev/ttyUSB0 should be replaced by the serial port where the smart motor is connected.
*
* The program follows this basic structure
*
* @code
* main {
*   Open Sick socket connection
*   Open smart motor serial port
*   Home the smart motor
*   Calculate the offset in (relative) encoder readings between the Sick and smart motor
*   Set the laser in measurement mode
*   while (!done) {
*     Request a single measurement from the laser
*     Receive a single measurement from the laser
*     Log the laser data to file
*   }
*   Stop the smart motor
* }
*
* @endcode
*
* If your processing code is fast enough, you can alternatively set the laser in
* continuous measurement mode. In this mode, you do not need to request individual
* measurements. You set the continuous measurement mode and then in the while loop
* receive the measurements from the laser. You can select this mode by using the
* macro #define CONTINOUS_MEASUREMENT
*
* @par NOTES:
* Use the macro #define CONTINOUS_MEASUREMENT to use continuous measurement mode
*
* @par FUTURE WORK:
* No future additions at this time
*
* @par REFERENCES:
* The Sick LMS1xx family operating instructions .pdf
* The animatics smart motor user guide
*
*
* (c) Copyright 2011 Sensible Machines. All rights reserved
******************************************************************************/

/**
 *@defgroup sweepLaser sweepLaser
 *@brief The Sweep Laser group contains all of the applications and functions
 * necessary to interface with the Sweeping Laser instrument
 *
 * The Sweeping Laser is a sensor useful in an array of applications from
 * surveying to robotics. The sweeping laser system is a laser rangefinder
 * mounted on a motor. This provides the ability to obtain a full 3-D point
 * cloud of the area around the sensor.
 * Libraries and example code are included that interface with all of the
 * system's components including:
 * The Sick LMS 1xx family of laser range finders
 * The Animatics Smart Motor used to point the laser
 */

#include "testSweepLaserInterface/motorInterface.h"
#include "testSweepLaserInterface/sickLMS115Interface.h"
#include <signal.h>
#include <iostream>
#include <fstream>
#include <math.h>

/// Macro for outputting error messages
#define errOut std::cerr << "ERROR!!:" << __FILE__ << ':' << __LINE__ << ' '
/// Macro for outputting warning messages
#define warnOut std::cerr << "WARNING!!:" << __FILE__ << ':' << __LINE__ << ' '
/// Macro for outputting informational messages
#define infoOut std::cout

bool doneG; // Global flag indicating when the application should terminate

/**
*******************************************************************************
* Called whenever the user presses Ctrl-c.  It signals the
* application to stop by setting doneG = true;
*******************************************************************************
* @param[in]      sig   The signal the app received
*******************************************************************************
* @return  none
*******************************************************************************
* @note   sets doneG to true
******************************************************************************/
void mcExitHandler(int sig)
{
  signal(SIGINT, SIG_DFL);
  doneG = true;
}

/**
*******************************************************************************
* This function calculates the offset between the smart motor and Sick
* encoder. Because the Sick laser does not do proper quadrature decoding,
* the Sick encoder readings are 1/4 the smart motor's. Also, because
* the encoder is a relative encoder, there is also an offset between the
* two readings. The function calculates the value that should be subtracted
* from the Sick laser's encoder reading so that a 0 smart motor encoder reading
* corresponds to a 0 Sick laser encoder reading.
*******************************************************************************
* @param[in]      smartMotorEncoder   The value of the smart motor's encoder
* @param[in]            sickEncoder   The value of the Sick's encoder
*******************************************************************************
* @return  The function returns the value that should be subtracted from all
*          subsequent Sick encoder readings
*******************************************************************************
* @note   The Sick laser does not do quadrature decoding.
******************************************************************************/
int calculateSickEncoderTransform(int smartMotorEncoder, int sickEncoder)
{
  // Sick laser does not do quadrature encoding, so its encoder values are 1/4 the smart motor's encoder values
  int sickEncoderTransformVal  = ((sickINT16)(sickEncoder << 2)) - (sickINT16)smartMotorEncoder;///4;
  return sickEncoderTransformVal;
}

/**
*******************************************************************************
* This function takes the raw encoder values from the Sick laser and transforms
* them so that a 0 value on the smart motor encoder corresponds to a 0 value
* on the Sick encoder.
*******************************************************************************
* @param[in]            sickEncoder    The encoder value received from the Sick
* @param[in]   sickEncoderTransform    The value to subtract from the sickEncoder
*******************************************************************************
* @return  The transformed Sick encoder value
*******************************************************************************
* @note   The Sick outputs a 14 bit encoder value.
*******************************************************************************
* @sa calculateSickEncoderTransform
******************************************************************************/
int sickEncoderTransform(int sickEncoder, int sickEncoderTransform)
{
  /* Oddly, it seems that the Sick only outputs a 14 bit encoder value? In this
   * case, the sickEncoder value must be sign extended if there is a 1 in the
   * 14th bit.
   */
  //if (sickEncoder & 0x3000) {
  //  sickEncoder = sickEncoder | 0xFFFFF000;
  //}
  //sickEncoder -= sickEncoderTransform;
  sickINT16 encoder;
  encoder = sickEncoder << 2;
  encoder -= (sickINT16)sickEncoderTransform;
  //sickEncoder = encoder - sickEncoderTransform;
  sickEncoder = encoder;
  return sickEncoder;
}

/**
*******************************************************************************
* This is the main loop for the sweepLaserInterface application. It opens the
* communications with the smart motor and Sick laser, receives the scan data
* from the laser and logs it to file.
*******************************************************************************
* @param[in]      argc   The number of arguments received
* @param[in]      argv   The argument values
*******************************************************************************
* @return  0 on success, -1 on failure
******************************************************************************/
int main(int argc, char *argv[])
{
  const char *port;
  CMotorInterface motorInterface;
  std::ofstream logOut;
  CSickLMS115Interface sickInterface;
  double timestamp;
  TSickLMS111ScanData scanData;
  int i;
  bool success;

  signal(SIGINT, mcExitHandler);

  doneG = false;

  logOut.open("laserLog.log");
  logOut << "%% 1) timestamp "
    "2) measure# "
    "3) status "
    "4) syncPhaseOffset (ticks) "
    "5) angleStepsPerRev (ticks) "
    "6) startAngle (ticks) "
    "7) endAngle (ticks) "
    "8) numPoints "
    "9) measLayer+echo "
    "10) flags "
    "11) horizAngle (ticks) "
    "12) radialDist (cm) "
    "13) echoPulseWidth (cm) "
    "9) - 13) repeated for numPoints...\n";
  char *ipAddress;
  if (argc < 2) {
    ipAddress = "192.168.1.11";
  } else {
    ipAddress = argv[1];
  }
  if (argc < 3) {
    port = "/dev/ttyUSB0";
  } else {
    port = argv[2];
  }
  infoOut << "opening Tcp port " << ipAddress << '\n';
  try {
    sickInterface.initializeTcp(ipAddress);
    sickInterface.queryScanConfig();
  } catch (CTcpIpSocket::ETcpIpSocket &e2) {
    infoOut << "Failed to open sick IP socket\n";
    return(-1);
  }
  infoOut << "opening serial port " << port << '\n';
  try {
    motorInterface.open(port);
  } catch (ESMSerial &exception) {
    errOut << "Failed to open smart motor serial port\n";
    return(-1);
  }

  int position, oldPosition, positionSame;
  try {
    infoOut << "Homing smart motor\n";
    if (!motorInterface.homeMotor()) {
      errOut << "Failed to home smart motor\n";
      return(-1);
    }
    // Wait for position to settle
    positionSame = oldPosition = 0;
    do {
      position = motorInterface.getPosition();
      infoOut << position << ' ' << oldPosition << '\n';
      if (oldPosition == position) {
    positionSame++;
      } else {
    positionSame = 0;
      }
      oldPosition = position;
    } while (positionSame < 10);
  } catch (ESMSerial &exception) {
    errOut << "Failed to home smart motor\n";
    return(-1);
  }
  infoOut << "Final Position = " << position << '\n';
  // Get a single sick measurement
  sickInterface.startSingleMeasurement();
  success = sickInterface.getNextMeasurement(timestamp, scanData);
  if (!success) {
    errOut << "Failed to read the first laser scan\n";
    return -1;
  }
  // Calculate the offset between the sick encoder and the sweep motor encoder
  int sickEncoderTransformVal;
  sickEncoderTransformVal = calculateSickEncoderTransform(position, scanData.sickEncoder[0].encoderPosition);
  infoOut << "sick encoder transform = " << position << " - " << scanData.sickEncoder[0].encoderPosition << " = " << sickEncoderTransformVal << '\n';

  // Start the motor sweeping
  motorInterface.setStart(-M_PI/2);
  motorInterface.setEnd(M_PI/2);
  motorInterface.setSpeed(M_PI/2);
  motorInterface.startSweeping();

  sickInterface.startMeasurementMode();
  // Start collecting measurements.
  // Depending on how fast your processing occurs, you
  // can either set the laser into continuous measurement mode or request measurements
  // one at a time. In continuous measurement mode you just
  // continuously call sickInterface.getNextMeasurement(...) to collect the data
  // However, if you cannot process the continuous data fast enough, you are better off
  // requesting one measurement at a time using sickInterface.startSingleMeasurement(...)
  // and then calling sickInterface.getNextMeasurement(...) to receive the measurement
  // data.
#ifdef CONTINOUS_MEASUREMENT
  sickInterface.startContinuousMeasurement(true);
#endif
  //for (i = 0; i < 10000; i++) {
  while (!doneG) {
#ifndef CONTINOUS_MEASUREMENT
    sickInterface.startSingleMeasurement();
#endif
    success = sickInterface.getNextMeasurement(timestamp, scanData);
    if (!success) {
      doneG = true;
    } else {
      // compensate for encoder offset
      for (i = 0; i < scanData.numberEncoders; i++) {
    infoOut << "encoder (Raw) = " << scanData.sickEncoder[i].encoderPosition << '\n';
    scanData.sickEncoder[i].encoderPosition = sickEncoderTransform(scanData.sickEncoder[i].encoderPosition, sickEncoderTransformVal);
      }
      logOut << std::fixed << timestamp << ' ';
      logOut << scanData << '\n';
      infoOut << '\n';
      infoOut << "messageCounter = " << scanData.messageCounter << '\n';
      infoOut << "scanCounter = " << scanData.scanCounter << '\n';
      if (scanData.numberEncoders <= 0) {
    errOut << "No encoder data from laser! Use SOPAS to verify incremental encoder configuration\n";
    doneG = true;
      }
      // Output the raw encoder values.
      // The Sick does not calculate the quadrature number of ticks, so its number of encoder ticks is
      // 1/4 the number of smart motor ticks. To calculate the encoder angle in radians, use the formula
      // sickEncoderRadians = encoderPosition/(encoderTicksPerRevolution/4)*2*M_PI
      for (i = 0; i < scanData.numberEncoders; i++) {
    infoOut << "encoder position " << ' ' << scanData.sickEncoder[i].encoderPosition << '\n';
    infoOut << "encoder speed " << ' ' << scanData.sickEncoder[i].encoderSpeed << '\n';
      }
    } // success
  }
  motorInterface.stopSweeping();
  motorInterface.close();
  //sickInterface.endMeasurementMode();

  return 0;
}