Guest User

sweepLaserInterface

a guest
Jul 24th, 2014
329
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /**
  2. *******************************************************************************
  3. * @file sweepLaser/sweepLaserInterface.cpp
  4. * @brief This is an example application that interfaces with the sweeping laser
  5. * sensor.
  6. *
  7. * @b PROJECT: sweepLaser
  8. *
  9. * @par ORIGINAL AUTHOR:
  10. * Stephan Roth, September 13, 2011
  11. *
  12. * @par DEPENDENCIES:
  13. * motorInterface, sickLMS115Interface, common/libcommon
  14. *
  15. * @par DESCRIPTION:
  16. * This is an example application that interfaces with the sweeping laser
  17. * sensor. You can modify this program to meet your specific needs.
  18. * The application has the following command line:
  19. * sweepLaserInterface 192.168.2.41 /dev/ttyUSB0
  20. * Here, 192.168.2.41 should be replaced by the Sick laser's IP address
  21. * /dev/ttyUSB0 should be replaced by the serial port where the smart motor is connected.
  22. *
  23. * The program follows this basic structure
  24. *
  25. * @code
  26. * main {
  27. *   Open Sick socket connection (AKA ethernet/LAN connection)
  28. *   Open smart motor serial port
  29. *   Home the smart motor
  30. *   Calculate the offset in (relative) encoder readings between the Sick and smart motor
  31. *   Set the laser in measurement mode
  32. *   while (!done) {
  33. *     Request a single measurement from the laser
  34. *     Receive a single measurement from the laser
  35. *     Log the laser data to file
  36. *   }
  37. *   Stop the smart motor
  38. * }
  39. *
  40. * @endcode
  41. *
  42. * If your processing code is fast enough, you can alternatively set the laser in
  43. * continuous measurement mode. In this mode, you do not need to request individual
  44. * measurements. You set the continuous measurement mode and then in the while loop
  45. * receive the measurements from the laser. You can select this mode by using the
  46. * macro #define CONTINOUS_MEASUREMENT
  47. *
  48. * @par NOTES:
  49. * Use the macro #define CONTINOUS_MEASUREMENT to use continuous measurement mode
  50. *
  51. * @par FUTURE WORK:
  52. * No future additions at this time
  53. *
  54. * @par REFERENCES:
  55. * The Sick LMS1xx family operating instructions .pdf
  56. * The animatics smart motor user guide
  57. *
  58. *
  59. * (c) Copyright 2011 Sensible Machines. All rights reserved
  60. ******************************************************************************/
  61.  
  62. /**
  63.  *@defgroup sweepLaser sweepLaser
  64.  *@brief The Sweep Laser group contains all of the applications and functions
  65.  * necessary to interface with the Sweeping Laser instrument
  66.  *
  67.  * The Sweeping Laser is a sensor useful in an array of applications from
  68.  * surveying to robotics. The sweeping laser system is a laser rangefinder
  69.  * mounted on a motor. This provides the ability to obtain a full 3-D point
  70.  * cloud of the area around the sensor.
  71.  * Libraries and example code are included that interface with all of the
  72.  * system's components including:
  73.  * The Sick LMS 1xx family of laser range finders
  74.  * The Animatics Smart Motor used to point the laser
  75.  */
  76.  
  77. #include "sweepLaserInterface/motorInterface.h"
  78. #include "sweepLaserInterface/sickLMS115Interface.h"
  79. #include <signal.h>
  80. #include <iostream>
  81. #include <fstream>
  82. #include <math.h>
  83.  
  84. #include <csignal>
  85. #include <cstdio>
  86. #include <LMS1xx/LMS1xx.h>
  87. #include "ros/ros.h"
  88. #include "sensor_msgs/LaserScan.h"
  89.  
  90. #include <sys/types.h>
  91. #include <sys/socket.h>
  92. #include <netinet/in.h>
  93. #include <arpa/inet.h>
  94. #include <cstdio>
  95. #include <cstdlib>
  96. #include <cstring>
  97. #include <unistd.h>
  98.  
  99. #include "LMS1xx/LMS1xx.h"
  100.  
  101. #define DEG2RAD M_PI/180.0
  102.  
  103. /// Macro for outputting error messages
  104. #define errOut std::cerr << "ERROR!!:" << __FILE__ << ':' << __LINE__ << ' '
  105. /// Macro for outputting warning messages
  106. #define warnOut std::cerr << "WARNING!!:" << __FILE__ << ':' << __LINE__ << ' '
  107. /// Macro for outputting informational messages
  108. #define infoOut std::cout
  109.  
  110.  
  111.  
  112. //I don't think I need this part cause I can use a while(ros::ok()) loop instead
  113. //bool doneG; // Global flag indicating when the application should terminate
  114.  
  115. /**
  116. *******************************************************************************
  117. * Called whenever the user presses Ctrl-c.  It signals the
  118. * application to stop by setting doneG = true;
  119. *******************************************************************************
  120. * @param[in]      sig   The signal the app received
  121. *******************************************************************************
  122. * @return  none
  123. *******************************************************************************
  124. * @note   sets doneG to true
  125. ******************************************************************************/
  126. /*
  127. void mcExitHandler(int sig)
  128. {
  129.   signal(SIGINT, SIG_DFL);
  130.   doneG = true;
  131. }
  132. */
  133.  
  134. /**
  135. *******************************************************************************
  136. * This function calculates the offset between the smart motor and Sick
  137. * encoder. Because the Sick laser does not do proper quadrature decoding,
  138. * the Sick encoder readings are 1/4 the smart motor's. Also, because
  139. * the encoder is a relative encoder, there is also an offset between the
  140. * two readings. The function calculates the value that should be subtracted
  141. * from the Sick laser's encoder reading so that a 0 smart motor encoder reading
  142. * corresponds to a 0 Sick laser encoder reading.
  143. *******************************************************************************
  144. * @param[in]      smartMotorEncoder   The value of the smart motor's encoder
  145. * @param[in]            sickEncoder   The value of the Sick's encoder
  146. *******************************************************************************
  147. * @return  The function returns the value that should be subtracted from all
  148. *          subsequent Sick encoder readings
  149. *******************************************************************************
  150. * @note   The Sick laser does not do quadrature decoding.
  151. ******************************************************************************/
  152. int calculateSickEncoderTransform(int smartMotorEncoder, int sickEncoder)
  153. {
  154.   // Sick laser does not do quadrature encoding, so its encoder values are 1/4 the smart motor's encoder values
  155.   int sickEncoderTransformVal  = ((sickINT16)(sickEncoder << 2)) - (sickINT16)smartMotorEncoder;///4;
  156.   return sickEncoderTransformVal;
  157. }
  158.  
  159. /**
  160. *******************************************************************************
  161. * This function takes the raw encoder values from the Sick laser and transforms
  162. * them so that a 0 value on the smart motor encoder corresponds to a 0 value
  163. * on the Sick encoder.
  164. *******************************************************************************
  165. * @param[in]            sickEncoder    The encoder value received from the Sick
  166. * @param[in]   sickEncoderTransform    The value to subtract from the sickEncoder
  167. *******************************************************************************
  168. * @return  The transformed Sick encoder value
  169. *******************************************************************************
  170. * @note   The Sick outputs a 14 bit encoder value.
  171. *******************************************************************************
  172. * @sa calculateSickEncoderTransform
  173. ******************************************************************************/
  174. int sickEncoderTransform(int sickEncoder, int sickEncoderTransform)
  175. {
  176.   /* Oddly, it seems that the Sick only outputs a 14 bit encoder value? In this
  177.    * case, the sickEncoder value must be sign extended if there is a 1 in the
  178.    * 14th bit.
  179.    */
  180.   //if (sickEncoder & 0x3000) {
  181.   //  sickEncoder = sickEncoder | 0xFFFFF000;
  182.   //}
  183.   //sickEncoder -= sickEncoderTransform;
  184.   sickINT16 encoder;
  185.   encoder = sickEncoder << 2;
  186.   encoder -= (sickINT16)sickEncoderTransform;
  187.   //sickEncoder = encoder - sickEncoderTransform;
  188.   sickEncoder = encoder;
  189.   return sickEncoder;
  190. }
  191.  
  192. /**
  193. *******************************************************************************
  194. * This is the main loop for the sweepLaserInterface application. It opens the
  195. * communications with the smart motor and Sick laser, receives the scan data
  196. * from the laser and logs it to file.
  197. *******************************************************************************
  198. * @param[in]      argc   The number of arguments received
  199. * @param[in]      argv   The argument values
  200. *******************************************************************************
  201. * @return  0 on success, -1 on failure
  202. ******************************************************************************/
  203. //not sure if there's an effective difference between these two, maybe C vs C++?
  204. int main(int argc, char /* *argv[] */ **argv)
  205. {
  206.   const char *port;
  207.   CMotorInterface motorInterface;
  208.   std::ofstream logOut;
  209.   CSickLMS115Interface sickInterface;
  210.   double timestamp;
  211.   //TSickLMS111ScanData scanData;
  212.   int i;
  213.   bool success;
  214.  
  215.   // laser data
  216.   LMS1xx laser;
  217.   scanCfg cfg;
  218.   scanDataCfg dataCfg;
  219.   scanData data;
  220.   // published data
  221.   sensor_msgs::LaserScan scan_msg;
  222.   // parameters
  223.   std::string host;
  224.   std::string frame_id;
  225.  
  226.   ros::init(argc, argv, "sweepLaserInterface");
  227.  
  228.   //object to interact with ROS
  229.   ros::NodeHandle n;
  230.  
  231.   //scan publisher with a max buffer size of 1000
  232.   ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 1000);
  233.  
  234.   n.param<std::string>("host", host, "192.168.1.11");
  235.   n.param<std::string>("frame_id", frame_id, "laser");
  236.  
  237.   //ros node's way of connecting to laser
  238.   laser.connect(host);
  239.  
  240.   //ros node's way of scanning after connecting
  241.   if (laser.isConnected())
  242.   {
  243.     ROS_INFO("Connected to laser.");
  244.  
  245.     laser.login();
  246.     cfg = laser.getScanCfg();
  247.  
  248.     scan_msg.header.frame_id = frame_id;
  249.  
  250.     scan_msg.range_min = 0.01;
  251.     scan_msg.range_max = 20.0;
  252.  
  253.     scan_msg.scan_time = 100.0/cfg.scaningFrequency;
  254.  
  255.     scan_msg.angle_increment = (double)cfg.angleResolution/10000.0 * DEG2RAD;
  256.     scan_msg.angle_min = (double)cfg.startAngle/10000.0 * DEG2RAD - M_PI/2;
  257.     scan_msg.angle_max = (double)cfg.stopAngle/10000.0 * DEG2RAD - M_PI/2;
  258.  
  259.     std::cout << "resolution : " << (double)cfg.angleResolution/10000.0 << " deg " << std::endl;
  260.     std::cout << "frequency : " << (double)cfg.scaningFrequency/100.0 << " Hz " << std::endl;
  261.  
  262.     int num_values;
  263.     if (cfg.angleResolution == 2500)
  264.     {
  265.       num_values = 1081;
  266.     }
  267.     else if (cfg.angleResolution == 5000)
  268.     {
  269.       num_values = 541;
  270.     }
  271.     else
  272.     {
  273.       ROS_ERROR("Unsupported resolution");
  274.       return 0;
  275.     }
  276.  
  277.     scan_msg.time_increment = scan_msg.scan_time/num_values;
  278.  
  279.     scan_msg.ranges.resize(num_values);
  280.     scan_msg.intensities.resize(num_values);
  281.  
  282.     dataCfg.outputChannel = 1;
  283.     dataCfg.remission = true;
  284.     dataCfg.resolution = 1;
  285.     dataCfg.encoder = 0;
  286.     dataCfg.position = false;
  287.     dataCfg.deviceName = false;
  288.     dataCfg.outputInterval = 1;
  289.  
  290.     laser.setScanDataCfg(dataCfg);
  291.  
  292.     laser.startMeas();
  293.  
  294.     status_t stat;
  295.     do // wait for ready status
  296.     {
  297.       stat = laser.queryStatus();
  298.       ros::Duration(1.0).sleep();
  299.     }
  300.     while (stat != ready_for_measurement);
  301.  
  302.     laser.startDevice(); // Log out to properly re-enable system after config
  303.  
  304.     laser.scanContinous(1);
  305.  
  306.     while (ros::ok())
  307.     {
  308.       ros::Time start = ros::Time::now();
  309.  
  310.       scan_msg.header.stamp = start;
  311.       ++scan_msg.header.seq;
  312.  
  313.       laser.getData(data);
  314.  
  315.       for (int i = 0; i < data.dist_len1; i++)
  316.       {
  317.         scan_msg.ranges[i] = data.dist1[i] * 0.001;
  318.       }
  319.  
  320.       for (int i = 0; i < data.rssi_len1; i++)
  321.       {
  322.         scan_msg.intensities[i] = data.rssi1[i];
  323.       }
  324.  
  325.       scan_pub.publish(scan_msg);
  326.  
  327.       ros::spinOnce();
  328.     }
  329.  
  330.     laser.scanContinous(0);
  331.     laser.stopMeas();
  332.     laser.disconnect();
  333.   }
  334.   else
  335.   {
  336.     ROS_ERROR("Connection to device failed");
  337.   }
  338.  
  339.   //signal(SIGINT, mcExitHandler);
  340.   //doneG = false;
  341.    
  342.   //log stuff, replace with ros bag if necessary?
  343. /*
  344.   logOut.open("laserLog.log");
  345.   logOut << "%% 1) timestamp "
  346.     "2) measure# "
  347.     "3) status "
  348.     "4) syncPhaseOffset (ticks) "
  349.     "5) angleStepsPerRev (ticks) "
  350.     "6) startAngle (ticks) "
  351.     "7) endAngle (ticks) "
  352.     "8) numPoints "
  353.     "9) measLayer+echo "
  354.     "10) flags "
  355.     "11) horizAngle (ticks) "
  356.     "12) radialDist (cm) "
  357.     "13) echoPulseWidth (cm) "
  358.     "9) - 13) repeated for numPoints...\n";
  359. */
  360.  
  361.   char *ipAddress;
  362.   ipAddress = "192.168.1.11";  //warning: deprecated conversion from string constant to ‘char*’ [-Wwrite-strings]
  363.   port = "/dev/ttyUSB0";  
  364.  
  365.   //(not yet sure how to rehash error out medsdages as ROS_INFO without losing information
  366.  
  367.   //original way of connecting to laser
  368.   ROS_INFO("opening Tcp port %s, \n", ipAddress);
  369.   //infoOut << "opening Tcp port " << ipAddress << '\n';
  370.   try
  371.   {
  372.     sickInterface.initializeTcp(ipAddress);
  373.     sickInterface.queryScanConfig();
  374.   } catch (CTcpIpSocket::ETcpIpSocket &e2) {
  375.     ROS_INFO("Failed to open sick IP socket\n");
  376.     //infoOut << "Failed to open sick IP socket\n";
  377.     return(-1);
  378.   }
  379.  
  380.   //original way of connecting to motor
  381.   ROS_INFO("opening serial port, %s \n", port);
  382.   //infoOut << "opening serial port " << port << '\n';
  383.   try
  384.   {
  385.     motorInterface.open(port);
  386.   } catch (ESMSerial &exception) {
  387.     errOut << "Failed to open smart motor serial port\n";
  388.     return(-1);
  389.   }
  390.  
  391.   //original way of moving motor/executing scans
  392.   int position, oldPosition, positionSame;
  393.   try
  394.   {
  395.     ROS_INFO("Homing smart motor\n");
  396.     if (!motorInterface.homeMotor()) {
  397.       errOut << "Failed to home smart motor\n";
  398.       return(-1);
  399.     }
  400.     // Wait for position to settle
  401.     positionSame = oldPosition = 0;
  402.   do
  403.   {
  404.       position = motorInterface.getPosition();
  405.       //ROS_INFO('%d %d \n', position, oldPosition);
  406.       infoOut << position << ' ' << oldPosition << '\n';
  407.       if (oldPosition == position) {
  408.     positionSame++;
  409.       } else {
  410.     positionSame = 0;
  411.       }
  412.       oldPosition = position;
  413.     } while (positionSame < 10);
  414.   } catch (ESMSerial &exception) {
  415.     errOut << "Failed to home smart motor\n";
  416.     return(-1);
  417.   }
  418.   ROS_INFO("Final Position = %i \n", position);
  419.   //infoOut << "Final Position = " << position << '\n';
  420.  
  421.   // Get a single sick measurement
  422.   sickInterface.startSingleMeasurement();
  423.   success = sickInterface.getNextMeasurement(timestamp, scanData); // error: expected primary-expression before ‘)’ token
  424.   if (!success) {
  425.     errOut << "Failed to read the first laser scan\n";
  426.     return -1;
  427.   }
  428.   // Calculate the offset between the sick encoder and the sweep motor encoder
  429.   int sickEncoderTransformVal;
  430.   sickEncoderTransformVal = calculateSickEncoderTransform(position, scanData.sickEncoder[0].encoderPosition); //error: expected primary-expression before ‘.’ token
  431.   //ROS_INFO("sick encoder transform = %d - %d = %d\n", position, scanData.sickEncoder[0].encoderPosition, sickEncoderTransformVal);
  432.   infoOut << "sick encoder transform = " << position << " - " << scanData.sickEncoder[0].encoderPosition << " = " << sickEncoderTransformVal << '\n'; //error: expected primary-expression before ‘.’ token
  433.  
  434.   // Start the motor sweeping
  435.   motorInterface.setStart(-M_PI/2);
  436.   motorInterface.setEnd(M_PI/2);
  437.   motorInterface.setSpeed(M_PI/2);
  438.   motorInterface.startSweeping();
  439.  
  440.   sickInterface.startMeasurementMode();
  441.   // Start collecting measurements.
  442.   // Depending on how fast your processing occurs, you
  443.   // can either set the laser into continuous measurement mode or request measurements
  444.   // one at a time. In continuous measurement mode you just
  445.   // continuously call sickInterface.getNextMeasurement(...) to collect the data
  446.   // However, if you cannot process the continuous data fast enough, you are better off
  447.   // requesting one measurement at a time using sickInterface.startSingleMeasurement(...)
  448.   // and then calling sickInterface.getNextMeasurement(...) to receive the measurement
  449.   // data.
  450. #ifdef CONTINOUS_MEASUREMENT
  451.   sickInterface.startContinuousMeasurement(true);
  452. #endif
  453.   //for (i = 0; i < 10000; i++) {
  454.   while (ros::ok()) {
  455. #ifndef CONTINOUS_MEASUREMENT
  456.     sickInterface.startSingleMeasurement();
  457. #endif
  458.     success = sickInterface.getNextMeasurement(timestamp, scanData); //error: expected primary-expression before ‘)’ token
  459.     if (!success) {
  460.       //doneG = true;
  461.     } else {
  462.       // compensate for encoder offset
  463.       for (i = 0; i < scanData.numberEncoders; i++) { //error: expected primary-expression before ‘.’ token
  464.     //ROS_INFO("encoder (Raw) = %d\n", scanData.sickEncoder[i].encoderPosition);
  465.     infoOut << "encoder (Raw) = " << scanData.sickEncoder[i].encoderPosition << '\n'; //error: expected primary-expression before ‘.’ token
  466.     scanData.sickEncoder[i].encoderPosition = sickEncoderTransform(scanData.sickEncoder[i].encoderPosition, sickEncoderTransformVal); //error: expected primary-expression before ‘.’ token
  467.       }
  468.       //replace logOut stuff with just regular topic publishing?
  469.       /*
  470.       logOut << std::fixed << timestamp << ' ';
  471.       logOut << scanData << '\n';
  472.       */
  473.       ROS_INFO("\n");
  474.       //infoOut << '\n';
  475.       //ROS_INFO("messageCounter = %d\n", scanData.messageCounter);
  476.       infoOut << "messageCounter = " << scanData.messageCounter << '\n'; //error: expected primary-expression before ‘.’ token
  477.       //ROS_INFO("scanCounter = %d\n", scanData.scanCounter);
  478.       infoOut << "scanCounter = " << scanData.scanCounter << '\n'; //error: expected primary-expression before ‘.’ token
  479.       if (scanData.numberEncoders <= 0) { //error: expected primary-expression before ‘.’ token
  480.     errOut << "No encoder data from laser! Use SOPAS to verify incremental encoder configuration\n";
  481.     //doneG = true;
  482.       }
  483.       // Output the raw encoder values.
  484.       // The Sick does not calculate the quadrature number of ticks, so its number of encoder ticks is
  485.       // 1/4 the number of smart motor ticks. To calculate the encoder angle in radians, use the formula
  486.       // sickEncoderRadians = encoderPosition/(encoderTicksPerRevolution/4)*2*M_PI
  487.       for (i = 0; i < scanData.numberEncoders; i++) { //error: expected primary-expression before ‘.’ token
  488.     //ROS_INFO("encoder position  %d\n", scanData.sickEncoder[i].encoderPosition);
  489.     //ROS_INFO("encoder speed  %d\n", scanData.sickEncoder[i].encoderSpeed);
  490.         infoOut << "encoder position " << ' ' << scanData.sickEncoder[i].encoderPosition << '\n'; //error: expected primary-expression before ‘.’ token
  491.     infoOut << "encoder speed " << ' ' << scanData.sickEncoder[i].encoderSpeed << '\n'; //error: expected primary-expression before ‘.’ token
  492.       }
  493.     } // success
  494.   }
  495.   motorInterface.stopSweeping();
  496.   motorInterface.close();
  497.   //sickInterface.endMeasurementMode();
  498.    
  499.   return 0;
  500. }
RAW Paste Data