migration to ros2 foxy

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
 
-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()

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'

include/LMS1xx/LMS1xx_node.hpp

@@ -0,0 +1,73 @@
+#pragma once
+
+#include <LMS1xx/LMS1xx.h>
+#include <tf2/exceptions.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <chrono>
+#include <memory>
+
+#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<sensor_msgs::msg::LaserScan>::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

include/LMS1xx/lms_buffer.h

@@ -2,6 +2,7 @@
  * lms_buffer.h
  *
  *  Author: Mike Purvis <[email protected]>
+ *  Author: Shravan S Rai <[email protected]>
  ***************************************************************************
  *   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 <stdint.h>
 #include <string.h>
 #include <unistd.h>
+#include <cstdint>
+#include <cstdio>
 
 #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;
     }
 

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,
+    ])

package.xml

@@ -1,22 +1,31 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>lms1xx</name>
-  <version>0.3.0</version>
-  <description>The lms1xx package contains a basic ROS driver for the SICK LMS1xx line of LIDARs.</description>
+  <version>0.0.1</version>
+  <description>The lms1xx package contains a basic ROS2 driver for the SICK LMS1xx line of LIDARs.</description>
 
   <author>Konrad Banachowicz</author>
-  <maintainer email="[email protected]">Mike Purvis</maintainer>
+  <author email="[email protected]">Mike Purvis</author>
+  <author email="[email protected]">Shravan S Rai</author>
+
+  <maintainer email="[email protected]">Shravan S Rai</maintainer>
   <url type="website">http://ros.org/wiki/LMS1xx</url>
 
   <license>LGPL</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <depend>rosconsole_bridge</depend>
-  <depend>roscpp</depend>
-  <depend>roscpp_serialization</depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>geometry_msgs</depend>
+  <depend>laser_geometry</depend>
+  <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>urdf</depend>
+  <depend>xacro</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
-  <test_depend>roslaunch</test_depend>
-  <test_depend>roslint</test_depend>
-  <test_depend>rosunit</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>