From 619ff1ff42124cc9628f9c8e244aad79b0c0763d Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 3 Aug 2017 12:45:43 +0200 Subject: [PATCH] Use EasyMile's fork of lms1xx. --- CMakeLists.txt | 25 +- include/LMS1xx/LMS1xx.h | 178 -------------- src/LMS1xx.cpp | 498 ---------------------------------------- src/LMS1xx_node.cpp | 99 ++++---- 4 files changed, 65 insertions(+), 735 deletions(-) delete mode 100644 include/LMS1xx/LMS1xx.h delete mode 100644 src/LMS1xx.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c8dedc7..ee0dc03 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,22 +1,33 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(lms1xx) +find_package(Threads REQUIRED) + # Build ROS-independent library. find_package(console_bridge REQUIRED) include_directories(include ${console_bridge_INCLUDE_DIRS}) -add_library(LMS1xx src/LMS1xx.cpp) -target_link_libraries(LMS1xx ${console_bridge_LIBRARIES}) + +set(LMS1xx_TAG "77fa47e3efa87634bd5f597e0758c4d304a1d3cd") +file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include) +include(ExternalProject) +ExternalProject_Add(lms1xx_src + GIT_REPOSITORY https://github.com/EasyMile/lms1xx.git + GIT_TAG ${LMS1xx_TAG} + CMAKE_ARGS + -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX} +) # Regular catkin package follows. find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs) catkin_package(CATKIN_DEPENDS roscpp) +set(CMAKE_CXX_STANDARD 11) -include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} ${CATKIN_DEVEL_PREFIX}/include) add_executable(LMS1xx_node src/LMS1xx_node.cpp) -target_link_libraries(LMS1xx_node LMS1xx ${catkin_LIBRARIES}) - +add_dependencies(LMS1xx_node lms1xx_src) +target_link_libraries(LMS1xx_node ${catkin_LIBRARIES} ${CATKIN_DEVEL_PREFIX}/lib/liblms1xx.a ${CMAKE_THREAD_LIBS_INIT}) -install(TARGETS LMS1xx LMS1xx_node +install(TARGETS LMS1xx_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/include/LMS1xx/LMS1xx.h b/include/LMS1xx/LMS1xx.h deleted file mode 100644 index d29ed78..0000000 --- a/include/LMS1xx/LMS1xx.h +++ /dev/null @@ -1,178 +0,0 @@ -/* - * LMS1xx.h - * - * Created on: 09-08-2010 - * Author: Konrad Banachowicz - *************************************************************************** - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Lesser General Public * - * License as published by the Free Software Foundation; either * - * version 2.1 of the License, or (at your option) any later version. * - * * - * This library 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 * - * Lesser General Public License for more details. * - * * - * You should have received a copy of the GNU Lesser General Public * - * License along with this library; if not, write to the Free Software * - * Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307 USA * - * * - ***************************************************************************/ - -#ifndef LMS1XX_H_ -#define LMS1XX_H_ - -#include -#include -#include -#include - -typedef enum -{ - undefined = 0, - initialisation = 1, - configuration = 2, - idle = 3, - rotated = 4, - in_preparation = 5, - ready = 6, - ready_for_measurement = 7 -} status_t; - -/*! -* @class LMS1xx -* @brief Class responsible for communicating with LMS1xx device. -* -* @author Konrad Banachowicz -*/ - -class LMS1xx -{ -public: - LMS1xx(); - virtual ~LMS1xx(); - - /*! - * @brief Connect to LMS1xx. - * @param host LMS1xx host name or ip address. - * @param port LMS1xx port number. - */ - void connect(std::string host, int port = 2111); - - /*! - * @brief Disconnect from LMS1xx device. - */ - void disconnect(); - - /*! - * @brief Get status of connection. - * @returns connected or not. - */ - bool isConnected(); - - /*! - * @brief Start measurements. - * After receiving this command LMS1xx unit starts spinning laser and measuring. - */ - void startMeas(); - - /*! - * @brief Stop measurements. - * After receiving this command LMS1xx unit stop spinning laser and measuring. - */ - void stopMeas(); - - /*! - * @brief Get current status of LMS1xx device. - * @returns status of LMS1xx device. - */ - status_t queryStatus(); - - /*! - * @brief Log into LMS1xx unit. - * Increase privilege level, giving ability to change device configuration. - */ - void login(); - - /*! - * @brief Get current scan configuration. - * Get scan configuration : - * - scanning frequency. - * - scanning resolution. - * - start angle. - * - stop angle. - * @returns scanCfg structure. - */ - scanCfg getScanCfg() const; - - /*! - * @brief Set scan configuration. - * Get scan configuration : - * - scanning frequency. - * - scanning resolution. - * - start angle. - * - stop angle. - * @param cfg structure containing scan configuration. - */ - void setScanCfg(const scanCfg &cfg); - - /*! - * @brief Set scan data configuration. - * Set format of scan message returned by device. - * @param cfg structure containing scan data configuration. - */ - void setScanDataCfg(const scanDataCfg &cfg); - - /*! - * @brief Get current output range configuration. - * Get output range configuration : - * - scanning resolution. - * - start angle. - * - stop angle. - * @returns scanOutputRange structure. - */ - scanOutputRange getScanOutputRange() const; - - /*! - * @brief Start or stop continuous data acquisition. - * After reception of this command device start or stop continuous data stream containing scan messages. - * @param start 1 : start 0 : stop - */ - void scanContinous(int start); - - /*! - * @brief Receive single scan message. - * @return true if scan was read successfully, false if error or timeout. False implies that higher level - * logic should take correct action such as reopening the connection. - */ - bool getScanData(scanData* scan_data); - - /*! - * @brief Save data permanently. - * Parameters are saved in the EEPROM of the LMS and will also be available after the device is switched off and on again. - * - */ - void saveConfig(); - - /*! - * @brief The device is returned to the measurement mode after configuration. - * - */ - void startDevice(); - -protected: - /*! - * @brief Receive single scan message. - * @param data pointer to scanData buffer structure. - */ - static void parseScanData(char* buf, scanData* data); - - bool connected_; - LMSBuffer buffer_; - int socket_fd_; -}; - -#endif /* LMS1XX_H_ */ - diff --git a/src/LMS1xx.cpp b/src/LMS1xx.cpp deleted file mode 100644 index f61d45d..0000000 --- a/src/LMS1xx.cpp +++ /dev/null @@ -1,498 +0,0 @@ -/* - * LMS1xx.cpp - * - * Created on: 09-08-2010 - * Author: Konrad Banachowicz - *************************************************************************** - * This library is free software; you can redistribute it and/or * - * modify it under the terms of the GNU Lesser General Public * - * License as published by the Free Software Foundation; either * - * version 2.1 of the License, or (at your option) any later version. * - * * - * This library 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 * - * Lesser General Public License for more details. * - * * - * You should have received a copy of the GNU Lesser General Public * - * License along with this library; if not, write to the Free Software * - * Foundation, Inc., 59 Temple Place, * - * Suite 330, Boston, MA 02111-1307 USA * - * * - ***************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "LMS1xx/LMS1xx.h" -#include "console_bridge/console.h" - -LMS1xx::LMS1xx() : connected_(false) -{ -} - -LMS1xx::~LMS1xx() -{ -} - -void LMS1xx::connect(std::string host, int port) -{ - if (!connected_) - { - logDebug("Creating non-blocking socket."); - socket_fd_ = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); - if (socket_fd_) - { - struct sockaddr_in stSockAddr; - stSockAddr.sin_family = PF_INET; - stSockAddr.sin_port = htons(port); - inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr); - - logDebug("Connecting socket to laser."); - int ret = ::connect(socket_fd_, (struct sockaddr *) &stSockAddr, sizeof(stSockAddr)); - - if (ret == 0) - { - connected_ = true; - logDebug("Connected succeeded."); - } - } - } -} - -void LMS1xx::disconnect() -{ - if (connected_) - { - close(socket_fd_); - connected_ = false; - } -} - -bool LMS1xx::isConnected() -{ - return connected_; -} - -void LMS1xx::startMeas() -{ - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sMN LMCstartmeas", 0x03); - - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - if (buf[0] != 0x02) - logWarn("invalid packet recieved"); - buf[len] = 0; - logDebug("RX: %s", buf); -} - -void LMS1xx::stopMeas() -{ - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sMN LMCstopmeas", 0x03); - - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - if (buf[0] != 0x02) - logWarn("invalid packet recieved"); - buf[len] = 0; - logDebug("RX: %s", buf); -} - -status_t LMS1xx::queryStatus() -{ - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sRN STlms", 0x03); - - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - if (buf[0] != 0x02) - logWarn("invalid packet recieved"); - buf[len] = 0; - logDebug("RX: %s", buf); - - int ret; - sscanf((buf + 10), "%d", &ret); - - return (status_t) ret; -} - -void LMS1xx::login() -{ - char buf[100]; - int result; - sprintf(buf, "%c%s%c", 0x02, "sMN SetAccessMode 03 F4724744", 0x03); - - fd_set readset; - struct timeval timeout; - - - do //loop until data is available to read - { - timeout.tv_sec = 1; - timeout.tv_usec = 0; - - write(socket_fd_, buf, strlen(buf)); - - FD_ZERO(&readset); - FD_SET(socket_fd_, &readset); - result = select(socket_fd_ + 1, &readset, NULL, NULL, &timeout); - - } - while (result <= 0); - - int len = read(socket_fd_, buf, 100); - if (buf[0] != 0x02) - logWarn("invalid packet recieved"); - buf[len] = 0; - logDebug("RX: %s", buf); -} - -scanCfg LMS1xx::getScanCfg() const -{ - scanCfg cfg; - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sRN LMPscancfg", 0x03); - - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - if (buf[0] != 0x02) - logWarn("invalid packet recieved"); - buf[len] = 0; - logDebug("RX: %s", buf); - - sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency, - &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle); - return cfg; -} - -void LMS1xx::setScanCfg(const scanCfg &cfg) -{ - char buf[100]; - sprintf(buf, "%c%s %X +1 %X %X %X%c", 0x02, "sMN mLMPsetscancfg", - cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle, - cfg.stopAngle, 0x03); - - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - - buf[len - 1] = 0; -} - -void LMS1xx::setScanDataCfg(const scanDataCfg &cfg) -{ - char buf[100]; - sprintf(buf, "%c%s %02X 00 %d %d 0 %02X 00 %d %d 0 %d +%d%c", 0x02, - "sWN LMDscandatacfg", cfg.outputChannel, cfg.remission ? 1 : 0, - cfg.resolution, cfg.encoder, cfg.position ? 1 : 0, - cfg.deviceName ? 1 : 0, cfg.timestamp ? 1 : 0, cfg.outputInterval, 0x03); - logDebug("TX: %s", buf); - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - buf[len - 1] = 0; -} - -scanOutputRange LMS1xx::getScanOutputRange() const -{ - scanOutputRange outputRange; - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sRN LMPoutputRange", 0x03); - - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - - sscanf(buf + 1, "%*s %*s %*d %X %X %X", &outputRange.angleResolution, - &outputRange.startAngle, &outputRange.stopAngle); - return outputRange; -} - -void LMS1xx::scanContinous(int start) -{ - char buf[100]; - sprintf(buf, "%c%s %d%c", 0x02, "sEN LMDscandata", start, 0x03); - - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - - if (buf[0] != 0x02) - logError("invalid packet recieved"); - - buf[len] = 0; - logDebug("RX: %s", buf); -} - -bool LMS1xx::getScanData(scanData* scan_data) -{ - fd_set rfds; - FD_ZERO(&rfds); - FD_SET(socket_fd_, &rfds); - - // Block a total of up to 100ms waiting for more data from the laser. - while (1) - { - // Would be great to depend on linux's behaviour of updating the timeval, but unfortunately - // that's non-POSIX (doesn't work on OS X, for example). - struct timeval tv; - tv.tv_sec = 0; - tv.tv_usec = 100000; - - logDebug("entering select()", tv.tv_usec); - int retval = select(socket_fd_ + 1, &rfds, NULL, NULL, &tv); - logDebug("returned %d from select()", retval); - if (retval) - { - buffer_.readFrom(socket_fd_); - - // Will return pointer if a complete message exists in the buffer, - // otherwise will return null. - char* buffer_data = buffer_.getNextBuffer(); - - if (buffer_data) - { - parseScanData(buffer_data, scan_data); - buffer_.popLastBuffer(); - return true; - } - } - else - { - // Select timed out or there was an fd error. - return false; - } - } -} - - -void LMS1xx::parseScanData(char* buffer, scanData* data) -{ - char* tok = strtok(buffer, " "); //Type of command - tok = strtok(NULL, " "); //Command - tok = strtok(NULL, " "); //VersionNumber - tok = strtok(NULL, " "); //DeviceNumber - tok = strtok(NULL, " "); //Serial number - tok = strtok(NULL, " "); //DeviceStatus - tok = strtok(NULL, " "); //MessageCounter - tok = strtok(NULL, " "); //ScanCounter - tok = strtok(NULL, " "); //PowerUpDuration - tok = strtok(NULL, " "); //TransmissionDuration - tok = strtok(NULL, " "); //InputStatus - tok = strtok(NULL, " "); //OutputStatus - tok = strtok(NULL, " "); //ReservedByteA - tok = strtok(NULL, " "); //ScanningFrequency - tok = strtok(NULL, " "); //MeasurementFrequency - tok = strtok(NULL, " "); - tok = strtok(NULL, " "); - tok = strtok(NULL, " "); - tok = strtok(NULL, " "); //NumberEncoders - int NumberEncoders; - sscanf(tok, "%d", &NumberEncoders); - for (int i = 0; i < NumberEncoders; i++) - { - tok = strtok(NULL, " "); //EncoderPosition - tok = strtok(NULL, " "); //EncoderSpeed - } - - tok = strtok(NULL, " "); //NumberChannels16Bit - int NumberChannels16Bit; - sscanf(tok, "%d", &NumberChannels16Bit); - logDebug("NumberChannels16Bit : %d", NumberChannels16Bit); - - for (int i = 0; i < NumberChannels16Bit; i++) - { - int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2 - char content[6]; - tok = strtok(NULL, " "); //MeasuredDataContent - sscanf(tok, "%s", content); - if (!strcmp(content, "DIST1")) - { - type = 0; - } - else if (!strcmp(content, "DIST2")) - { - type = 1; - } - else if (!strcmp(content, "RSSI1")) - { - type = 2; - } - else if (!strcmp(content, "RSSI2")) - { - type = 3; - } - tok = strtok(NULL, " "); //ScalingFactor - tok = strtok(NULL, " "); //ScalingOffset - tok = strtok(NULL, " "); //Starting angle - tok = strtok(NULL, " "); //Angular step width - tok = strtok(NULL, " "); //NumberData - int NumberData; - sscanf(tok, "%X", &NumberData); - logDebug("NumberData : %d", NumberData); - - if (type == 0) - { - data->dist_len1 = NumberData; - } - else if (type == 1) - { - data->dist_len2 = NumberData; - } - else if (type == 2) - { - data->rssi_len1 = NumberData; - } - else if (type == 3) - { - data->rssi_len2 = NumberData; - } - - for (int i = 0; i < NumberData; i++) - { - int dat; - tok = strtok(NULL, " "); //data - sscanf(tok, "%X", &dat); - - if (type == 0) - { - data->dist1[i] = dat; - } - else if (type == 1) - { - data->dist2[i] = dat; - } - else if (type == 2) - { - data->rssi1[i] = dat; - } - else if (type == 3) - { - data->rssi2[i] = dat; - } - - } - } - - tok = strtok(NULL, " "); //NumberChannels8Bit - int NumberChannels8Bit; - sscanf(tok, "%d", &NumberChannels8Bit); - logDebug("NumberChannels8Bit : %d\n", NumberChannels8Bit); - - for (int i = 0; i < NumberChannels8Bit; i++) - { - int type = -1; - char content[6]; - tok = strtok(NULL, " "); //MeasuredDataContent - sscanf(tok, "%s", content); - if (!strcmp(content, "DIST1")) - { - type = 0; - } - else if (!strcmp(content, "DIST2")) - { - type = 1; - } - else if (!strcmp(content, "RSSI1")) - { - type = 2; - } - else if (!strcmp(content, "RSSI2")) - { - type = 3; - } - tok = strtok(NULL, " "); //ScalingFactor - tok = strtok(NULL, " "); //ScalingOffset - tok = strtok(NULL, " "); //Starting angle - tok = strtok(NULL, " "); //Angular step width - tok = strtok(NULL, " "); //NumberData - int NumberData; - sscanf(tok, "%X", &NumberData); - logDebug("NumberData : %d\n", NumberData); - - if (type == 0) - { - data->dist_len1 = NumberData; - } - else if (type == 1) - { - data->dist_len2 = NumberData; - } - else if (type == 2) - { - data->rssi_len1 = NumberData; - } - else if (type == 3) - { - data->rssi_len2 = NumberData; - } - for (int i = 0; i < NumberData; i++) - { - int dat; - tok = strtok(NULL, " "); //data - sscanf(tok, "%X", &dat); - - if (type == 0) - { - data->dist1[i] = dat; - } - else if (type == 1) - { - data->dist2[i] = dat; - } - else if (type == 2) - { - data->rssi1[i] = dat; - } - else if (type == 3) - { - data->rssi2[i] = dat; - } - } - } -} - -void LMS1xx::saveConfig() -{ - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sMN mEEwriteall", 0x03); - - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - - if (buf[0] != 0x02) - logWarn("invalid packet recieved"); - buf[len] = 0; - logDebug("RX: %s", buf); -} - -void LMS1xx::startDevice() -{ - char buf[100]; - sprintf(buf, "%c%s%c", 0x02, "sMN Run", 0x03); - - write(socket_fd_, buf, strlen(buf)); - - int len = read(socket_fd_, buf, 100); - - if (buf[0] != 0x02) - logWarn("invalid packet recieved"); - buf[len] = 0; - logDebug("RX: %s", buf); -} diff --git a/src/LMS1xx_node.cpp b/src/LMS1xx_node.cpp index e7ed2f7..bb7670d 100644 --- a/src/LMS1xx_node.cpp +++ b/src/LMS1xx_node.cpp @@ -23,25 +23,27 @@ #include #include -#include +#include #include "ros/ros.h" #include "sensor_msgs/LaserScan.h" #define DEG2RAD M_PI/180.0 +using namespace lms1xx; + int main(int argc, char **argv) { // laser data LMS1xx laser; - scanCfg cfg; - scanOutputRange outputRange; - scanDataCfg dataCfg; + scan_configuration cfg; + scan_output_range outputRange; + scan_data_configuration dataCfg; sensor_msgs::LaserScan scan_msg; // parameters std::string host; std::string frame_id; - int port; + std::string port; ros::init(argc, argv, "lms1xx"); ros::NodeHandle nh; @@ -50,13 +52,13 @@ int main(int argc, char **argv) n.param("host", host, "192.168.1.2"); n.param("frame_id", frame_id, "laser"); - n.param("port", port, 2111); + n.param("port", port, "2111"); while (ros::ok()) { ROS_INFO_STREAM("Connecting to laser at " << host); laser.connect(host, port); - if (!laser.isConnected()) + if (!laser.connected()) { ROS_WARN("Unable to connect, retrying."); ros::Duration(1).sleep(); @@ -65,10 +67,10 @@ int main(int argc, char **argv) ROS_DEBUG("Logging in to laser."); laser.login(); - cfg = laser.getScanCfg(); - outputRange = laser.getScanOutputRange(); + cfg = laser.get_configuration(); + outputRange = laser.get_scan_output_range(); - if (cfg.scaningFrequency != 5000) + if (cfg.scaning_frequency != 5000) { laser.disconnect(); ROS_WARN("Unable to get laser output range. Retrying."); @@ -78,25 +80,25 @@ int main(int argc, char **argv) ROS_INFO("Connected to laser."); - ROS_DEBUG("Laser configuration: scaningFrequency %d, angleResolution %d, startAngle %d, stopAngle %d", - cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle, cfg.stopAngle); + ROS_DEBUG("Laser configuration: scaning_frequency %d, angleResolution %d, startAngle %d, stopAngle %d", + cfg.scaning_frequency, cfg.angle_resolution, cfg.start_angle, cfg.stop_angle); ROS_DEBUG("Laser output range:angleResolution %d, startAngle %d, stopAngle %d", - outputRange.angleResolution, outputRange.startAngle, outputRange.stopAngle); + outputRange.angle_resolution, outputRange.start_angle, outputRange.stop_angle); scan_msg.header.frame_id = frame_id; scan_msg.range_min = 0.01; scan_msg.range_max = 20.0; - scan_msg.scan_time = 100.0 / cfg.scaningFrequency; - scan_msg.angle_increment = (double)outputRange.angleResolution / 10000.0 * DEG2RAD; - scan_msg.angle_min = (double)outputRange.startAngle / 10000.0 * DEG2RAD - M_PI / 2; - scan_msg.angle_max = (double)outputRange.stopAngle / 10000.0 * DEG2RAD - M_PI / 2; + scan_msg.scan_time = 100.0 / cfg.scaning_frequency; + scan_msg.angle_increment = (double)outputRange.angle_resolution / 10000.0 * DEG2RAD; + scan_msg.angle_min = (double)outputRange.start_angle / 10000.0 * DEG2RAD - M_PI / 2; + scan_msg.angle_max = (double)outputRange.stop_angle / 10000.0 * DEG2RAD - M_PI / 2; - ROS_DEBUG_STREAM("Device resolution is " << (double)outputRange.angleResolution / 10000.0 << " degrees."); - ROS_DEBUG_STREAM("Device frequency is " << (double)cfg.scaningFrequency / 100.0 << " Hz"); + ROS_DEBUG_STREAM("Device resolution is " << (double)outputRange.angle_resolution / 10000.0 << " degrees."); + ROS_DEBUG_STREAM("Device frequency is " << (double)cfg.scaning_frequency / 100.0 << " Hz"); - int angle_range = outputRange.stopAngle - outputRange.startAngle; - int num_values = angle_range / outputRange.angleResolution ; - if (angle_range % outputRange.angleResolution == 0) + int angle_range = outputRange.stop_angle - outputRange.start_angle; + int num_values = angle_range / outputRange.angle_resolution; + if (angle_range % outputRange.angle_resolution == 0) { // Include endpoint ++num_values; @@ -105,34 +107,34 @@ int main(int argc, char **argv) scan_msg.intensities.resize(num_values); scan_msg.time_increment = - (outputRange.angleResolution / 10000.0) + (outputRange.angle_resolution / 10000.0) / 360.0 - / (cfg.scaningFrequency / 100.0); + / (cfg.scaning_frequency / 100.0); ROS_DEBUG_STREAM("Time increment is " << static_cast(scan_msg.time_increment * 1000000) << " microseconds"); - dataCfg.outputChannel = 1; + dataCfg.output_channel = 1; dataCfg.remission = true; dataCfg.resolution = 1; dataCfg.encoder = 0; dataCfg.position = false; - dataCfg.deviceName = false; - dataCfg.outputInterval = 1; + dataCfg.device_name = false; + dataCfg.output_interval = 1; ROS_DEBUG("Setting scan data configuration."); - laser.setScanDataCfg(dataCfg); + laser.set_scan_data_configuration(dataCfg); ROS_DEBUG("Starting measurements."); - laser.startMeas(); + laser.start_measurements(); ROS_DEBUG("Waiting for ready status."); ros::Time ready_status_timeout = ros::Time::now() + ros::Duration(5); //while(1) //{ - status_t stat = laser.queryStatus(); + device_status stat = laser.status(); ros::Duration(1.0).sleep(); - if (stat != ready_for_measurement) + if (stat != device_status::ready_for_measurement) { ROS_WARN("Laser not ready. Retrying initialization."); laser.disconnect(); @@ -160,10 +162,10 @@ int main(int argc, char **argv) }*/ ROS_DEBUG("Starting device."); - laser.startDevice(); // Log out to properly re-enable system after config + laser.start_device(); // Log out to properly re-enable system after config ROS_DEBUG("Commanding continuous measurements."); - laser.scanContinous(1); + laser.scan_continous(true); while (ros::ok()) { @@ -172,34 +174,27 @@ int main(int argc, char **argv) scan_msg.header.stamp = start; ++scan_msg.header.seq; - scanData data; + scan_data data; ROS_DEBUG("Reading scan data."); - if (laser.getScanData(&data)) + data = laser.get_data(); + for (int i = 0; i < data.dist_len1; i++) { - for (int i = 0; i < data.dist_len1; i++) - { - scan_msg.ranges[i] = data.dist1[i] * 0.001; - } - - for (int i = 0; i < data.rssi_len1; i++) - { - scan_msg.intensities[i] = data.rssi1[i]; - } - - ROS_DEBUG("Publishing scan data."); - scan_pub.publish(scan_msg); + scan_msg.ranges[i] = data.dist1[i] * 0.001; } - else + + for (int i = 0; i < data.rssi_len1; i++) { - ROS_ERROR("Laser timed out on delivering scan, attempting to reinitialize."); - break; + scan_msg.intensities[i] = data.rssi1[i]; } + ROS_DEBUG("Publishing scan data."); + scan_pub.publish(scan_msg); + ros::spinOnce(); } - laser.scanContinous(0); - laser.stopMeas(); + laser.scan_continous(false); + laser.stop_measurements(); laser.disconnect(); }