Code refactor and Angle Compensation Enabled

.gitignore

@@ -0,0 +1,44 @@
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# clion
+.idea
+
+# ignore build directories
+*build*
+
+# ignore install directory from colcon
+*install*
+
+# ros2 log
+log/

CMakeLists.txt

@@ -1,80 +1,85 @@
 cmake_minimum_required(VERSION 3.9.5)
 project(rplidar_ros)
 
+if (NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 99)
+endif ()
+
 # Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -pedantic)
-endif()
-
-set(RPLIDAR_SDK_PATH "./sdk/")
-
-FILE(GLOB RPLIDAR_SDK_SRC
-  "${RPLIDAR_SDK_PATH}/src/arch/linux/*.cpp"
-  "${RPLIDAR_SDK_PATH}/src/hal/*.cpp"
-  "${RPLIDAR_SDK_PATH}/src/*.cpp"
-)
-
-set(req_deps
-  "rclcpp"
-  "sensor_msgs"
-  "std_srvs"
-  "rclcpp_components"
-)
+if (NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif ()
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -pedantic)
+endif ()
+
+# Create sdk library
+add_library(rplidar_sdk SHARED
+        sdk/src/rplidar_driver.cpp
+
+        sdk/src/arch/linux/net_serial.cpp
+        sdk/src/arch/linux/net_socket.cpp
+        sdk/src/arch/linux/timer.cpp
+
+        sdk/src/hal/thread.cpp)
+
+target_include_directories(rplidar_sdk
+        PUBLIC
+        sdk/include
+        sdk/src
+        PRIVATE
+        sdk/src/arch/linux
+        sdk/src/hal)
 
 find_package(ament_cmake_auto REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
 
-ament_auto_find_build_dependencies(REQUIRED ${req_deps})
-
-include_directories(
-  ${RPLIDAR_SDK_PATH}/include
-  ${RPLIDAR_SDK_PATH}/src
-  include
-)
-
-# build composition node
-add_library(rplidar_composition_node SHARED
-  src/rplidar_node.cpp
-  ${RPLIDAR_SDK_SRC}
-)
-ament_target_dependencies(rplidar_composition_node ${req_deps})
+# Can't use ament_target_dependencies here, so need to manually
+# create targets for ros2 dependencies
+list(APPEND ros2_dependencies
+        rclcpp
+        sensor_msgs
+        std_srvs
+        rclcpp_components)
+
+# create targets from ros 2 packages
+foreach (dep IN LISTS ros2_dependencies)
+    find_package(${dep} REQUIRED)
+    add_library(${dep} INTERFACE)
+    target_link_libraries(${dep} INTERFACE ${${dep}_LIBRARIES})
+    target_include_directories(${dep} INTERFACE ${${dep}_INCLUDE_DIRS})
+endforeach ()
+
+ament_auto_find_build_dependencies(REQUIRED
+        rclcpp
+        sensor_msgs
+        std_srvs
+        rclcpp_components)
+
+add_library(rplidar_node SHARED src/RPLidarNode.cpp)
+target_link_libraries(rplidar_node rclcpp sensor_msgs std_srvs rclcpp_components rplidar_sdk)
+ament_target_dependencies(rplidar_node rclcpp sensor_msgs std_srvs rclcpp_components)
+target_include_directories(rplidar_node PUBLIC include)
 
 # build direct ROS 1 port
-ament_auto_add_executable(rplidarNode src/node.cpp ${RPLIDAR_SDK_SRC})
-ament_target_dependencies(rplidarNode ${req_deps})
-
-ament_auto_add_executable(rplidarNodeClient src/client.cpp)
-ament_target_dependencies(rplidarNodeClient ${req_deps})
+ament_auto_add_executable(rplidar_client src/client.cpp)
+target_link_libraries(rplidar_client rclcpp sensor_msgs std_srvs rclcpp_components)
+ament_target_dependencies(rplidar_client rclcpp sensor_msgs std_srvs rclcpp_components)
 
 # build composition node
-ament_auto_add_executable(rplidar_composition src/standalone_rplidar.cpp)
-target_link_libraries(rplidar_composition rplidar_composition_node)
-ament_target_dependencies(rplidar_composition ${req_deps})
+ament_auto_add_executable(rplidar src/rplidar.cpp)
+target_link_libraries(rplidar rplidar_node)
 
-install(
-  TARGETS rplidarNode rplidarNodeClient rplidar_composition
-  DESTINATION lib/${PROJECT_NAME}
-)
+install(TARGETS rplidar_client rplidar rplidar_sdk
+        DESTINATION lib/${PROJECT_NAME})
 
-install(
-  DIRECTORY launch rviz
-  DESTINATION share/${PROJECT_NAME}
-)
+install(DIRECTORY launch rviz config
+        DESTINATION share/${PROJECT_NAME})
 
 # Install the shared library composition node
-install(
-  TARGETS rplidar_composition_node
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-
-# TODO(hallen): port this
-# install(DIRECTORY sdk
-#   USE_SOURCE_PERMISSIONS
-# )
+install(TARGETS rplidar_node
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin)
 
 ament_auto_package()

README.md

@@ -16,7 +16,7 @@ rplidar Tutorial:  https://github.com/robopeak/rplidar_ros/wiki
 How to build rplidar ros package
 =====================================================================
     1) Clone this project to your catkin's workspace src folder
-    2) Running catkin_make to build rplidarNode and rplidarNodeClient
+    2) Running catkin_make to build rplidarNode and rplidar_client
 
 How to run rplidar ros package
 =====================================================================
@@ -40,7 +40,7 @@ roslaunch rplidar_ros rplidar_a3.launch (for RPLIDAR A3)
 or
 roslaunch rplidar_ros rplidar_s1.launch (for RPLIDAR S1)
 
-rosrun rplidar_ros rplidarNodeClient
+rosrun rplidar_ros rplidar_client
 
 You should see rplidar's scan result in the console
 

config/rplidar.yaml

@@ -0,0 +1,7 @@
+rplidar:
+  ros__parameters:
+    serial_port: /dev/ttyUSB0
+    serial_baudrate: 115200
+    frame_id: laser_frame
+    inverted: false
+    angle_compensate: true

config/rplidar_a3.yaml

@@ -0,0 +1,7 @@
+rplidar:
+  ros__parameters:
+    serial_port: /dev/ttyUSB0
+    serial_baudrate: 256000
+    frame_id: laser_frame
+    inverted: false
+    angle_compensate: true

config/rplidar_tcp.yaml

@@ -0,0 +1,8 @@
+rplidar:
+  ros__parameters:
+    serial_port: /dev/ttyUSB0
+    tcp_ip: '192.168.0.6'
+    tcp_port: 20108
+    frame_id: laser_frame
+    inverted: false
+    angle_compensate: true

include/RPLidarNode.hpp

@@ -0,0 +1,143 @@
+/*
+ *  RPLIDAR ROS NODE
+ *
+ *  Copyright (c) 2019 Hunter L. Allen
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#ifndef RPLIDAR_NODE_HPP_
+#define RPLIDAR_NODE_HPP_
+
+// Turn off warnings from sdk library.
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wall"
+#pragma GCC diagnostic ignored "-Wpedantic"
+#include <rplidar.h>
+#pragma GCC diagnostic pop
+
+#include <chrono>
+#include <rclcpp/clock.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/time_source.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <std_srvs/srv/empty.hpp>
+#include <visibility.h>
+
+namespace {
+constexpr auto M_PI{3.1415926535897932384626433832795};
+constexpr auto MAX_SAMPLE_COUNT = 360 * 8;
+constexpr float DEFAULT_FREQUENCY = 10.0; // hz
+
+using LaserScan = sensor_msgs::msg::LaserScan;
+using LaserScanPub = rclcpp::Publisher<LaserScan>::SharedPtr;
+using StartMotorService = rclcpp::Service<std_srvs::srv::Empty>::SharedPtr;
+using StopMotorService = rclcpp::Service<std_srvs::srv::Empty>::SharedPtr;
+using RPLidarDriver = rp::standalone::rplidar::RPlidarDriver;
+using RPLidarScanMode = rp::standalone::rplidar::RplidarScanMode;
+using Clock = rclcpp::Clock::SharedPtr;
+using ResponseNodeArray = std::array<rplidar_response_measurement_node_hq_t, MAX_SAMPLE_COUNT>;
+using EmptyRequest = std::shared_ptr<std_srvs::srv::Empty::Request>;
+using EmptyResponse = std::shared_ptr<std_srvs::srv::Empty::Response>;
+using Timer = rclcpp::TimerBase::SharedPtr;
+using namespace std::chrono_literals;
+} // namespace
+
+namespace rplidar_ros {
+[[nodiscard]] constexpr float getAngleInDegrees(const rplidar_response_measurement_node_hq_t& node)
+{
+   return node.angle_z_q14 * 90.f / 16384.f; // I have no clue what these values mean
+}
+
+[[nodiscard]] constexpr float degreesToRadians(float degrees)
+{
+   return degrees * M_PI / 180.0;
+}
+
+class RPLIDAR_ROS_PUBLIC RPLidarNode : public rclcpp::Node
+{
+ public:
+   explicit RPLidarNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+   virtual ~RPLidarNode();
+
+   void publish_scan(const double scan_time, const ResponseNodeArray& nodes, size_t node_count);
+
+   /* service callbacks */
+   void stop_motor(const EmptyRequest req, EmptyResponse res);
+   void start_motor(const EmptyRequest req, EmptyResponse res);
+
+ private:
+   struct RPLidarDriverDeleter
+   {
+      void operator()(RPLidarDriver* driver)
+      {
+         RPLidarDriver::DisposeDriver(driver);
+      }
+   };
+
+   using RPLidarDriverUPtr = std::unique_ptr<RPLidarDriver, RPLidarDriverDeleter>;
+
+   [[nodiscard]] bool print_device_info() const;
+   [[nodiscard]] bool is_healthy() const;
+   [[nodiscard]] bool set_scan_mode();
+   void publish_loop();
+
+   /* parameters */
+   std::string m_channel_type;
+   std::string m_tcp_ip;
+   std::string m_serial_port;
+   std::string m_scan_topic;
+   int m_tcp_port;
+   int m_serial_baudrate;
+   std::string m_frame_id;
+   bool m_inverted;
+   bool m_angle_compensate;
+   float m_angle_compensate_multiple;
+   std::string m_scan_mode;
+
+   /* Publisher */
+   LaserScanPub m_publisher;
+
+   /* Services */
+   StopMotorService m_stop_motor_service;
+   StartMotorService m_start_motor_service;
+
+   /* SDK Pointer */
+   RPLidarDriverUPtr m_driver{nullptr};
+
+   /* Timer */
+   Timer m_timer;
+
+   /* Scan Times */
+   size_t m_scan_count{0};
+   float m_min_distance{0.15f};
+   float m_max_distance{8.0};
+   float m_angle_min{degreesToRadians(0)};
+   float m_angle_max{degreesToRadians(359)};
+};
+
+} // namespace rplidar_ros
+
+#endif // RPLIDAR_NODE_HPP_