diff --git a/CMakeLists.txt b/CMakeLists.txt index c8dedc7..b403d1e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,42 +1,59 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(lms1xx) -# 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}) - -# Regular catkin package follows. -find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs) -catkin_package(CATKIN_DEPENDS roscpp) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() -include_directories(include ${catkin_INCLUDE_DIRS}) -add_executable(LMS1xx_node src/LMS1xx_node.cpp) -target_link_libraries(LMS1xx_node LMS1xx ${catkin_LIBRARIES}) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -install(TARGETS LMS1xx LMS1xx_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(xacro REQUIRED) +find_package(urdf REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(laser_geometry REQUIRED) +find_package(tf2 REQUIRED) + +add_executable(${PROJECT_NAME} + src/LMS1xx_node.cpp + src/LMS1xx.cpp) + +ament_target_dependencies( + ${PROJECT_NAME} + rclcpp + sensor_msgs + std_msgs + geometry_msgs + laser_geometry + tf2 ) -install(DIRECTORY meshes launch urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) -install(PROGRAMS scripts/find_sick scripts/set_sick_ip - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -if (CATKIN_ENABLE_TESTING) - catkin_add_gtest(test_buffer test/test_buffer.cpp) - target_link_libraries(test_buffer ${catkin_LIBRARIES}) +install(TARGETS lms1xx + DESTINATION lib/${PROJECT_NAME}) - find_package(roslint REQUIRED) - roslint_cpp() - #roslint_add_test() +install(DIRECTORY config launch meshes urdf + DESTINATION share/${PROJECT_NAME} +) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(launch/LMS1xx.launch) -endif() +ament_export_dependencies(xacro) +ament_export_dependencies(urdf) +ament_package() \ No newline at end of file diff --git a/config/lms_111.yaml b/config/lms_111.yaml new file mode 100644 index 0000000..ec8b63d --- /dev/null +++ b/config/lms_111.yaml @@ -0,0 +1,6 @@ +lms1xx: + ros_parameters: + frame_id: 'laser_link' + host: '192.168.1.2' + port: '2111' + publish_min_range_as_inf: 'false' \ No newline at end of file diff --git a/include/LMS1xx/LMS1xx_node.hpp b/include/LMS1xx/LMS1xx_node.hpp new file mode 100644 index 0000000..fc4ddc3 --- /dev/null +++ b/include/LMS1xx/LMS1xx_node.hpp @@ -0,0 +1,73 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "laser_geometry/laser_geometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/header.hpp" +#include "string" + + + +namespace LMS1xx_node +{ +class LMS1xx_node : public rclcpp::Node +{ + public: + explicit LMS1xx_node(rclcpp::NodeOptions options); + + /** + * @brief connect to the Sick LMS1xx lidar + */ + void connect_lidar(); + + private: + // laser data + LMS1xx laser_; + scanCfg cfg_; + scanOutputRange outputRange_; + scanDataCfg dataCfg_; + + // parameters + std::string host_; + std::string frame_id_; + int port_; + bool inf_range_; + int reconnect_timeout_{0}; + sensor_msgs::msg::LaserScan scan_msg_; + + // publishers + rclcpp::Publisher::SharedPtr laserscan_pub_; + /** + * @brief construct a scan message based on the + * configuration of the sick lidar + * @param scan sensor_msgs::msg::laserscan + */ + void construct_scan(); + + /** + * @brief get measurements from the lidar after it has + * been setup properly + */ + void get_measurements(); + + /** + * @brief publishes scan messages + */ + void publish_scan(); + + /** + * @brief publishes cloud messages + */ + void publish_cloud(); +}; + +} // namespace LMS1xx_node \ No newline at end of file diff --git a/include/LMS1xx/lms_buffer.h b/include/LMS1xx/lms_buffer.h index 0d72e17..503cea7 100644 --- a/include/LMS1xx/lms_buffer.h +++ b/include/LMS1xx/lms_buffer.h @@ -2,6 +2,7 @@ * lms_buffer.h * * Author: Mike Purvis + * Author: Shravan S Rai *************************************************************************** * This library is free software; you can redistribute it and/or * * modify it under the terms of the GNU Lesser General Public * @@ -23,19 +24,16 @@ #ifndef LMS1XX_LMS_BUFFER_H_ #define LMS1XX_LMS_BUFFER_H_ -#include "console_bridge/console.h" #include #include #include +#include +#include #define LMS_BUFFER_SIZE 50000 #define LMS_STX 0x02 #define LMS_ETX 0x03 -#define logWarn CONSOLE_BRIDGE_logWarn -#define logError CONSOLE_BRIDGE_logError -#define logDebug CONSOLE_BRIDGE_logDebug -#define logInform CONSOLE_BRIDGE_logInform class LMSBuffer { @@ -51,12 +49,11 @@ class LMSBuffer if (ret > 0) { total_length_ += ret; - logDebug("Read %d bytes from fd, total length is %d.", ret, total_length_); + printf("Read %d bytes from fd, total length is %d.", ret, total_length_); } else { - - logWarn("Buffer read() returned error."); + printf("Buffer read() returned error."); } } @@ -65,7 +62,7 @@ class LMSBuffer if (total_length_ == 0) { // Buffer is empty, no scan data present. - logDebug("Empty buffer, nothing to return."); + printf("Empty buffer, nothing to return."); return NULL; } @@ -76,13 +73,13 @@ class LMSBuffer if (start_of_message == NULL) { // None found, buffer reset. - logWarn("No STX found, dropping %d bytes from buffer.", total_length_); + printf("No STX found, dropping %d bytes from buffer.", total_length_); total_length_ = 0; } else if (buffer_ != start_of_message) { // Start of message found, ahead of the start of buffer. Therefore shift the buffer back. - logWarn("Shifting buffer, dropping %d bytes, %d bytes remain.", + printf("Shifting buffer, dropping %ld bytes, %ld bytes remain.", (start_of_message - buffer_), total_length_ - (start_of_message - buffer_)); shiftBuffer(start_of_message); } @@ -92,7 +89,7 @@ class LMSBuffer if (end_of_first_message_ == NULL) { // No end of message found, therefore no message to parse and return. - logDebug("No ETX found, nothing to return."); + printf("No ETX found, nothing to return."); return NULL; } diff --git a/launch/LMS1xx.launch.py b/launch/LMS1xx.launch.py new file mode 100644 index 0000000..ecac267 --- /dev/null +++ b/launch/LMS1xx.launch.py @@ -0,0 +1,40 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + # ROS packages + sick_lms1xx_pkg = get_package_share_directory('LMS1xx') + + # config + sensor_config = os.path.join(sick_lms1xx_pkg, 'config', 'lms_111.yaml') + + # Launch arguments + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + # Nodes + lms1xx_driver = Node( + package='lms1xx', + executable='lms1xx', + name='lms1xx', + parameters=[sensor_config], + output='screen', + ) + + return LaunchDescription([ + # Launch Arguments + DeclareLaunchArgument('use_sim_time', default_value='false', + description='Use simulation time if true'), + # Nodes + lms1xx_driver, + ]) \ No newline at end of file diff --git a/package.xml b/package.xml index ef58774..5b4c78f 100644 --- a/package.xml +++ b/package.xml @@ -1,22 +1,31 @@ - + lms1xx - 0.3.0 - The lms1xx package contains a basic ROS driver for the SICK LMS1xx line of LIDARs. + 0.0.1 + The lms1xx package contains a basic ROS2 driver for the SICK LMS1xx line of LIDARs. Konrad Banachowicz - Mike Purvis + Mike Purvis + Shravan S Rai + + Shravan S Rai http://ros.org/wiki/LMS1xx LGPL - catkin - rosconsole_bridge - roscpp - roscpp_serialization + ament_cmake + geometry_msgs + laser_geometry + rclcpp sensor_msgs + std_msgs + tf2_geometry_msgs + urdf + xacro + ament_lint_auto + ament_lint_common - roslaunch - roslint - rosunit + + ament_cmake + diff --git a/src/LMS1xx.cpp b/src/LMS1xx.cpp index f61d45d..1a2e7b2 100644 --- a/src/LMS1xx.cpp +++ b/src/LMS1xx.cpp @@ -34,7 +34,7 @@ #include #include "LMS1xx/LMS1xx.h" -#include "console_bridge/console.h" + LMS1xx::LMS1xx() : connected_(false) { @@ -48,7 +48,7 @@ void LMS1xx::connect(std::string host, int port) { if (!connected_) { - logDebug("Creating non-blocking socket."); + printf("Creating non-blocking socket."); socket_fd_ = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); if (socket_fd_) { @@ -57,13 +57,13 @@ void LMS1xx::connect(std::string host, int port) stSockAddr.sin_port = htons(port); inet_pton(AF_INET, host.c_str(), &stSockAddr.sin_addr); - logDebug("Connecting socket to laser."); + printf("Connecting socket to laser."); int ret = ::connect(socket_fd_, (struct sockaddr *) &stSockAddr, sizeof(stSockAddr)); if (ret == 0) { connected_ = true; - logDebug("Connected succeeded."); + printf("Connected succeeded."); } } } @@ -87,14 +87,13 @@ 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"); + printf("invalid packet recieved"); buf[len] = 0; - logDebug("RX: %s", buf); + printf("RX: %s", buf); } void LMS1xx::stopMeas() @@ -106,9 +105,9 @@ void LMS1xx::stopMeas() int len = read(socket_fd_, buf, 100); if (buf[0] != 0x02) - logWarn("invalid packet recieved"); + printf("invalid packet recieved"); buf[len] = 0; - logDebug("RX: %s", buf); + printf("RX: %s", buf); } status_t LMS1xx::queryStatus() @@ -120,9 +119,9 @@ status_t LMS1xx::queryStatus() int len = read(socket_fd_, buf, 100); if (buf[0] != 0x02) - logWarn("invalid packet recieved"); + printf("invalid packet recieved"); buf[len] = 0; - logDebug("RX: %s", buf); + printf("RX: %s", buf); int ret; sscanf((buf + 10), "%d", &ret); @@ -156,9 +155,9 @@ void LMS1xx::login() int len = read(socket_fd_, buf, 100); if (buf[0] != 0x02) - logWarn("invalid packet recieved"); + printf("invalid packet recieved"); buf[len] = 0; - logDebug("RX: %s", buf); + printf("RX: %s", buf); } scanCfg LMS1xx::getScanCfg() const @@ -171,11 +170,11 @@ scanCfg LMS1xx::getScanCfg() const int len = read(socket_fd_, buf, 100); if (buf[0] != 0x02) - logWarn("invalid packet recieved"); + printf("invalid packet recieved"); buf[len] = 0; - logDebug("RX: %s", buf); + printf("RX: %s", buf); - sscanf(buf + 1, "%*s %*s %X %*d %X %X %X", &cfg.scaningFrequency, + sscanf(buf + 1, "%*s %*s %d %*d %d %d %d", &cfg.scaningFrequency, &cfg.angleResolution, &cfg.startAngle, &cfg.stopAngle); return cfg; } @@ -201,7 +200,7 @@ void LMS1xx::setScanDataCfg(const scanDataCfg &cfg) "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); + printf("TX: %s", buf); write(socket_fd_, buf, strlen(buf)); int len = read(socket_fd_, buf, 100); @@ -218,7 +217,7 @@ scanOutputRange LMS1xx::getScanOutputRange() const int len = read(socket_fd_, buf, 100); - sscanf(buf + 1, "%*s %*s %*d %X %X %X", &outputRange.angleResolution, + sscanf(buf + 1, "%*s %*s %*d %d %d %d", &outputRange.angleResolution, &outputRange.startAngle, &outputRange.stopAngle); return outputRange; } @@ -233,10 +232,10 @@ void LMS1xx::scanContinous(int start) int len = read(socket_fd_, buf, 100); if (buf[0] != 0x02) - logError("invalid packet recieved"); + printf("invalid packet recieved"); buf[len] = 0; - logDebug("RX: %s", buf); + printf("RX: %s", buf); } bool LMS1xx::getScanData(scanData* scan_data) @@ -254,9 +253,9 @@ bool LMS1xx::getScanData(scanData* scan_data) tv.tv_sec = 0; tv.tv_usec = 100000; - logDebug("entering select()", tv.tv_usec); + printf("entering select() at %ld", tv.tv_usec); int retval = select(socket_fd_ + 1, &rfds, NULL, NULL, &tv); - logDebug("returned %d from select()", retval); + printf("returned %d from select()", retval); if (retval) { buffer_.readFrom(socket_fd_); @@ -313,7 +312,7 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) tok = strtok(NULL, " "); //NumberChannels16Bit int NumberChannels16Bit; sscanf(tok, "%d", &NumberChannels16Bit); - logDebug("NumberChannels16Bit : %d", NumberChannels16Bit); + printf("NumberChannels16Bit : %d", NumberChannels16Bit); for (int i = 0; i < NumberChannels16Bit; i++) { @@ -343,8 +342,8 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) tok = strtok(NULL, " "); //Angular step width tok = strtok(NULL, " "); //NumberData int NumberData; - sscanf(tok, "%X", &NumberData); - logDebug("NumberData : %d", NumberData); + sscanf(tok, "%d", &NumberData); + printf("NumberData : %d", NumberData); if (type == 0) { @@ -367,7 +366,7 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) { int dat; tok = strtok(NULL, " "); //data - sscanf(tok, "%X", &dat); + sscanf(tok, "%d", &dat); if (type == 0) { @@ -392,7 +391,7 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) tok = strtok(NULL, " "); //NumberChannels8Bit int NumberChannels8Bit; sscanf(tok, "%d", &NumberChannels8Bit); - logDebug("NumberChannels8Bit : %d\n", NumberChannels8Bit); + printf("NumberChannels8Bit : %d\n", NumberChannels8Bit); for (int i = 0; i < NumberChannels8Bit; i++) { @@ -422,8 +421,8 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) tok = strtok(NULL, " "); //Angular step width tok = strtok(NULL, " "); //NumberData int NumberData; - sscanf(tok, "%X", &NumberData); - logDebug("NumberData : %d\n", NumberData); + sscanf(tok, "%d", &NumberData); + printf("NumberData : %d\n", NumberData); if (type == 0) { @@ -445,7 +444,7 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) { int dat; tok = strtok(NULL, " "); //data - sscanf(tok, "%X", &dat); + sscanf(tok, "%d", &dat); if (type == 0) { @@ -477,9 +476,9 @@ void LMS1xx::saveConfig() int len = read(socket_fd_, buf, 100); if (buf[0] != 0x02) - logWarn("invalid packet recieved"); + printf("invalid packet recieved"); buf[len] = 0; - logDebug("RX: %s", buf); + printf("RX: %s", buf); } void LMS1xx::startDevice() @@ -492,7 +491,7 @@ void LMS1xx::startDevice() int len = read(socket_fd_, buf, 100); if (buf[0] != 0x02) - logWarn("invalid packet recieved"); + printf("invalid packet recieved"); buf[len] = 0; - logDebug("RX: %s", buf); + printf("RX: %s", buf); } diff --git a/src/LMS1xx_node.cpp b/src/LMS1xx_node.cpp index 03fbf28..5d7feaf 100644 --- a/src/LMS1xx_node.cpp +++ b/src/LMS1xx_node.cpp @@ -3,6 +3,7 @@ * * Created on: 09-08-2010 * Author: Konrad Banachowicz + * Author: Shravan S Rai *************************************************************************** * This library is free software; you can redistribute it and/or * * modify it under the terms of the GNU Lesser General Public * @@ -23,198 +24,195 @@ #include #include -#include -#include "ros/ros.h" -#include "sensor_msgs/LaserScan.h" #include #include +#include +#include +#include "LMS1xx/LMS1xx.h" +#include "LMS1xx/LMS1xx_node.hpp" #define DEG2RAD M_PI/180.0 -int main(int argc, char **argv) +namespace LMS1xx_node { - // laser data - LMS1xx laser; - scanCfg cfg; - scanOutputRange outputRange; - scanDataCfg dataCfg; - sensor_msgs::LaserScan scan_msg; - - // parameters - std::string host; - std::string frame_id; - bool inf_range; - int port; - - ros::init(argc, argv, "lms1xx"); - ros::NodeHandle nh; - ros::NodeHandle n("~"); - ros::Publisher scan_pub = nh.advertise("scan", 1); - - n.param("host", host, "192.168.1.2"); - n.param("frame_id", frame_id, "laser"); - n.param("publish_min_range_as_inf", inf_range, false); - n.param("port", port, 2111); - - while (ros::ok()) - { - ROS_INFO_STREAM("Connecting to laser at " << host); - laser.connect(host, port); - if (!laser.isConnected()) - { - ROS_WARN("Unable to connect, retrying."); - ros::Duration(1).sleep(); - continue; - } - - ROS_DEBUG("Logging in to laser."); - laser.login(); - cfg = laser.getScanCfg(); - outputRange = laser.getScanOutputRange(); - - if (cfg.scaningFrequency != 5000) - { - laser.disconnect(); - ROS_WARN("Unable to get laser output range. Retrying."); - ros::Duration(1).sleep(); - continue; - } - 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 output range:angleResolution %d, startAngle %d, stopAngle %d", - outputRange.angleResolution, outputRange.startAngle, outputRange.stopAngle); +LMS1xx_node::LMS1xx_node(rclcpp::NodeOptions options) : Node("LMS1xx_node", options) +{ + host_ = this->declare_parameter("host", "192.168.1.2"); + frame_id_ = this->declare_parameter("frame_id", "laser_link"); + port_ = this->declare_parameter("port", 2111); + inf_range_ = this->declare_parameter("publish_min_range_as_inf", false); + laserscan_pub_ = + this->create_publisher("/scan", 1); - 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 = static_cast(outputRange.angleResolution / 10000.0 * DEG2RAD); - scan_msg.angle_min = static_cast(outputRange.startAngle / 10000.0 * DEG2RAD - M_PI / 2); - scan_msg.angle_max = static_cast(outputRange.stopAngle / 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"); +void LMS1xx_node::connect_lidar() +{ + RCLCPP_INFO(this->get_logger(), "Connecting to Lidar"); + laser_.connect(host_, port_); - int angle_range = outputRange.stopAngle - outputRange.startAngle; - int num_values = angle_range / outputRange.angleResolution; - if (angle_range % outputRange.angleResolution == 0) - { - // Include endpoint - ++num_values; - } - scan_msg.ranges.resize(num_values); - scan_msg.intensities.resize(num_values); - - scan_msg.time_increment = - (outputRange.angleResolution / 10000.0) - / 360.0 - / (cfg.scaningFrequency / 100.0); - - ROS_DEBUG_STREAM("Time increment is " << static_cast(scan_msg.time_increment * 1000000) << " microseconds"); - - dataCfg.outputChannel = 1; - dataCfg.remission = true; - dataCfg.resolution = 1; - dataCfg.encoder = 0; - dataCfg.position = false; - dataCfg.deviceName = false; - dataCfg.outputInterval = 1; - - ROS_DEBUG("Setting scan data configuration."); - laser.setScanDataCfg(dataCfg); - - ROS_DEBUG("Starting measurements."); - laser.startMeas(); - - ROS_DEBUG("Waiting for ready status."); - ros::Time ready_status_timeout = ros::Time::now() + ros::Duration(5); - - // while(1) - // { - status_t stat = laser.queryStatus(); - ros::Duration(1.0).sleep(); - if (stat != ready_for_measurement) - { - ROS_WARN("Laser not ready. Retrying initialization."); - laser.disconnect(); - ros::Duration(1).sleep(); - continue; - } - /*if (stat == ready_for_measurement) + if (!laser_.isConnected()) + { + //Attempt to connect to the lidar 5 times before giving up + RCLCPP_INFO(this->get_logger(), "Connecting to Lidar"); + + while (!laser_.isConnected()) { - ROS_DEBUG("Ready status achieved."); - break; - } + laser_.connect(host_, port_); - if (ros::Time::now() > ready_status_timeout) + RCLCPP_WARN(this->get_logger(), "Unable to connect, retrying."); + + if (reconnect_timeout_ > 5) { - ROS_WARN("Timed out waiting for ready status. Trying again."); - laser.disconnect(); - continue; + RCLCPP_FATAL(this->get_logger(), + "Unable to connect to Lidar " + "after 5 attempts!"); + return; } - - if (!ros::ok()) + else { - laser.disconnect(); - return 1; + reconnect_timeout_++; } - }*/ + + // timer + rclcpp::sleep_for(std::chrono::seconds(5)); + } + return; + } + RCLCPP_INFO(this->get_logger(), "Logging in to laser."); + laser_.login(); + cfg_ = laser_.getScanCfg(); + outputRange_ = laser_.getScanOutputRange(); + + RCLCPP_INFO(this->get_logger(), "Connected to laser."); + construct_scan(); + get_measurements(); +} - ROS_DEBUG("Starting device."); - laser.startDevice(); // Log out to properly re-enable system after config - ROS_DEBUG("Commanding continuous measurements."); - laser.scanContinous(1); +void LMS1xx_node::construct_scan() +{ + 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 / 2) / 10000.0 * DEG2RAD; + + scan_msg_.angle_min = + ((((double)cfg_.startAngle) / 10000.0) - 90.0) * DEG2RAD; + scan_msg_.angle_max = ((((double)cfg_.stopAngle) / 10000.0) - 90.0) * DEG2RAD; + + int angle_range = outputRange_.stopAngle - outputRange_.startAngle; + int num_values = angle_range/ outputRange_.angleResolution; + + if (angle_range % outputRange_.angleResolution == 0) + { + //Include endpoints + ++num_values; + } - while (ros::ok()) - { - ros::Time start = ros::Time::now(); + scan_msg_.ranges.resize(num_values); + scan_msg_.intensities.resize(num_values); + + scan_msg_.time_increment = (outputRange_.angleResolution / 10000.0) / 360.0 / + (cfg_.scaningFrequency / 100.0); +} + +void LMS1xx_node::get_measurements() +{ + dataCfg_.outputChannel = 1; + dataCfg_.remission = true; + dataCfg_.resolution = 1; + dataCfg_.encoder = 0; + dataCfg_.position = false; + dataCfg_.deviceName = false; + dataCfg_.outputInterval = 1; + + RCLCPP_INFO(this->get_logger(), "Setting scan data configuration."); + laser_.setScanDataCfg(dataCfg_); + + RCLCPP_INFO(this->get_logger(), "Starting scan measurement."); + laser_.startMeas(); + + RCLCPP_INFO(this->get_logger(), "Waiting for ready status."); + rclcpp::Time ready_status_timeout = + this->get_clock()->now() + rclcpp::Duration::from_seconds(5); + + status_t stat = laser_.queryStatus(); + rclcpp::Duration::from_seconds(1); + if (stat != ready_for_measurement) + { + RCLCPP_ERROR(this->get_logger(), + "Laser not ready. Retrying " + "initialization."); + laser_.disconnect(); + return; + } - scan_msg.header.stamp = start; - ++scan_msg.header.seq; + RCLCPP_INFO(this->get_logger(), "Starting device."); + laser_.startDevice(); // Log out to properly re-enable system after config - scanData data; - ROS_DEBUG("Reading scan data."); - if (laser.getScanData(&data)) + RCLCPP_INFO(this->get_logger(), "Commanding continuous measurements."); + laser_.scanContinous(1); + + while (rclcpp::ok()) + { + scan_msg_.header.stamp = this->get_clock()->now(); + scanData data; + if (laser_.getScanData(&data)) + { + for (int i = 0; i < data.dist_len1; i++) { - for (int i = 0; i < data.dist_len1; i++) + float range_data = data.dist1[i] * 0.001; + if(inf_range_ && scan_msg_.range_min) { - float range_data = data.dist1[i] * 0.001; - - if (inf_range && range_data < scan_msg.range_min) - { - scan_msg.ranges[i] = std::numeric_limits::infinity(); - } - else - { - scan_msg.ranges[i] = range_data; - } + scan_msg_.ranges[i] = std::numeric_limits::infinity(); } - - for (int i = 0; i < data.rssi_len1; i++) + else { - scan_msg.intensities[i] = data.rssi1[i]; + scan_msg_.ranges[i] = range_data; } - - ROS_DEBUG("Publishing scan data."); - scan_pub.publish(scan_msg); + } - 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::spinOnce(); + RCLCPP_INFO(this->get_logger(), "Publishing the scan values"); + publish_scan(); + } + else + { + RCLCPP_ERROR(this->get_logger(), + "Laser timed out on delivering scan, " + "attempting to reinitialize."); + break; } - - laser.scanContinous(0); - laser.stopMeas(); - laser.disconnect(); } + laser_.scanContinous(0); + laser_.stopMeas(); + laser_.disconnect(); +} - return 0; +void LMS1xx_node::publish_scan() +{ + laserscan_pub_->publish(scan_msg_); } + +} //namespace LMS1xx_node + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + auto sick_lms1xx_node = std::make_shared(options); + exec.add_node(sick_lms1xx_node); + sick_lms1xx_node->connect_lidar(); + exec.spin(); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/urdf/sick_lms1xx.urdf.xacro b/urdf/sick_lms1xx.urdf.xacro index ab976cf..47af15f 100644 --- a/urdf/sick_lms1xx.urdf.xacro +++ b/urdf/sick_lms1xx.urdf.xacro @@ -43,7 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - +